fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 07:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "EKF: Add parameter control of individual IMU axis delta velocity bias estimation"

Browse Source 
This reverts commit 9c31632e2b.
This commit is contained in:
Paul Riseborough
2017-05-15 16:38:17 +10:00
parent 47973a4820
commit 20db74eca8
3 changed files with 58 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+2 -8
View File
@@ -549,14 +549,8 @@ void Ekf::constrainStates()
_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_ekf_avg, 0.349066f * _dt_ekf_avg);
}
float del_vel_bias_lim = _params.acc_bias_lim * _dt_ekf_avg;
for (uint8_t i=0; i < 3; i++) {
uint8_t state_index = 13 + i;
if (_params.acc_bias_learn_mask & (1<<i)) {
_state.accel_bias(state_index) = math::constrain(_state.accel_bias(state_index), -del_vel_bias_lim, del_vel_bias_lim);
} else {
_state.accel_bias(state_index) = 0.0f;
}
for (int i = 0; i < 3; i++) {
_state.accel_bias(i) = math::constrain(_state.accel_bias(i), -_params.acc_bias_lim * _dt_ekf_avg, _params.acc_bias_lim * _dt_ekf_avg);
}
for (int i = 0; i < 3; i++) {