From 9ddac762e397556de6210173fd4d1e4b5f37ed35 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 4 Oct 2024 13:08:20 -0400 Subject: [PATCH] ekf2: stricter GNSS vel rejected innovation override - in the special case of bad vertical acceleration detected (bad_acc_vertical) only allow overriding rejected GNSS 3D velocity if horizontal innovations are accepted --- src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index bf3d086c6a..9799c8bb71 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -204,7 +204,13 @@ void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_samp // vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable, // but limit innovation to prevent spikes that could destabilise the filter - if (aid_src.innovation_rejected && _fault_status.flags.bad_acc_vertical && (gnss_sample.sacc < _params.req_sacc)) { + bool bad_acc_vz_rejected = _fault_status.flags.bad_acc_vertical + && (aid_src.test_ratio[2] > 1.f) // vz rejected + && (aid_src.test_ratio[0] < 1.f) && (aid_src.test_ratio[1] < 1.f); // vx & vy accepted + + if (bad_acc_vz_rejected + && (gnss_sample.sacc < _params.req_sacc) + ) { const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]); aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit); aid_src.innovation_rejected = false;