From ff3c0a613697db82dd6277f573c4c056b05e77a6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 8 Jan 2021 21:40:03 +1100 Subject: [PATCH] EKF: Fix bug preventing recovery from bad delta velocity bias estimates --- EKF/covariance.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index eac571264a..289e7ced14 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -949,7 +949,8 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) 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 * _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 if (!bad_acc_bias) {