From b919f3ddf41ab422d09af91eaea2b2f9729fab13 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 21 Jan 2021 10:13:28 +1100 Subject: [PATCH] EKF: Restore protection from extreme innovation values --- EKF/vel_pos_fusion.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 5aaf822ca8..19523e13b2 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -108,6 +108,10 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga const bool innov_check_pass = test_ratio(0) <= 1.0f; if (innov_check_pass || inhibit_gate) { + if (inhibit_gate && test_ratio(0) > sq(100.0f / innov_gate(0))) { + // always protect against extreme values that could result in a NaN + return false; + } if (!_fuse_hpos_as_odom) { _time_last_hor_pos_fuse = _time_last_imu;