From 556a195320dad6f5f13a51a9ba6f897fee9cd1d0 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 19 Jan 2021 21:04:52 +1100 Subject: [PATCH] EKF: Improve handling of non position mode large position innovations These changes fix a bug that allowed IMU gyro errors that cause large tilt errors prior to start of aiding not resulting in large innovation test ratios. --- EKF/control.cpp | 9 ++++----- EKF/ekf.h | 2 +- EKF/vel_pos_fusion.cpp | 4 ++-- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index b35ca6f842..9148d0cf84 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -300,7 +300,7 @@ void Ekf::controlExternalVisionFusion() // innovation gate size ev_pos_innov_gates(0) = fmaxf(_params.ev_pos_innov_gate, 1.0f); - fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gates, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio); + fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gates, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio, false); } // determine if we should use the velocity observations @@ -712,7 +712,7 @@ void Ekf::controlGpsFusion() // fuse GPS measurement fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates,gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); - fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio); + fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio, false); } } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { @@ -1289,11 +1289,10 @@ void Ekf::controlFakePosFusion() _gps_pos_innov.xy() = Vector2f(_state.pos) - _last_known_posNE; - // glitch protection is not required so set gate to a large value - const Vector2f fake_pos_innov_gate(100.0f, 100.0f); + const Vector2f fake_pos_innov_gate(3.0f, 3.0f); fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var, - _gps_pos_innov_var, _gps_pos_test_ratio); + _gps_pos_innov_var, _gps_pos_test_ratio, true); } } else { diff --git a/EKF/ekf.h b/EKF/ekf.h index 2f84e6436d..9b1532e493 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -622,7 +622,7 @@ private: Vector3f &innov_var, Vector2f &test_ratio); bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, - Vector3f &innov_var, Vector2f &test_ratio); + Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate); bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 911a065d07..5aaf822ca8 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -97,7 +97,7 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate } bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, - Vector3f &innov_var, Vector2f &test_ratio) + Vector3f &innov_var, Vector2f &test_ratio, bool inhibit_gate) { innov_var(0) = P(7, 7) + obs_var(0); @@ -107,7 +107,7 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga const bool innov_check_pass = test_ratio(0) <= 1.0f; - if (innov_check_pass) { + if (innov_check_pass || inhibit_gate) { if (!_fuse_hpos_as_odom) { _time_last_hor_pos_fuse = _time_last_imu;