From 99575d73e10aad06eb7d5b6415ea2db4151c5045 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 9 Jan 2021 08:18:18 +1100 Subject: [PATCH] EKF: Add bad accel bias checking for EV and range finder height use cases --- EKF/covariance.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 289e7ced14..25c30bbe0a 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -949,8 +949,10 @@ 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 && _control_status.flags.gps_hgt) - || (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt) ) ); + && ( (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) + || (down_dvel_bias * _rng_hgt_innov(2) < 0.0f && _control_status.flags.rng_hgt) + || (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt) ) ); // record the pass/fail if (!bad_acc_bias) {