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

EKF: Fix bug preventing recovery from bad delta velocity bias estimates

Browse Source
This commit is contained in:
Paul Riseborough
2021-01-08 21:40:03 +11:00
committed by Daniel Agar
parent c25c97f8b5
commit ff3c0a6136
1 changed files with 2 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+2 -1
View File
@@ -949,7 +949,8 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim
&& ( (down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps) && ( (down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps)
|| (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel) ) || (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel) )
&& down_dvel_bias * _gps_pos_innov(2) < 0.0f); && ( (down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt)
|| (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt) ) );
// record the pass/fail // record the pass/fail
if (!bad_acc_bias) { if (!bad_acc_bias) {