From db0274e19b12ebce9fadf5b6f4922bad85e38a41 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 25 Apr 2022 10:24:22 +0200 Subject: [PATCH] ekf: robustify bad_acc_vertical check when the vertical pos or vel innov ratio is above the threshold, the other one needs to be significant too and not just positive to trigger the failure --- src/modules/ekf2/EKF/common.h | 3 ++- src/modules/ekf2/EKF/control.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 8f5cfead8d..b107c43d1e 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -368,7 +368,8 @@ struct parameters { float mcoef{0.1f}; ///< rotor momentum drag coefficient for the X and Y axes (1/s) // control of accel error detection and mitigation (IMU clipping) - const float vert_innov_test_lim{3.0f}; ///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed + const float vert_innov_test_lim{3.0f}; ///< Number of standard deviations of vertical vel/pos innovations allowed before triggering a vertical acceleration failure + const float vert_innov_test_min{1.0f}; ///< Minimum number of standard deviations of vertical vel/pos innovations required to trigger a vertical acceleration failure const int bad_acc_reset_delay_us{500000}; ///< Continuous time that the vertical position and velocity innovation test must fail before the states are reset (uSec) // auxiliary velocity fusion diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index dff7f37d07..72f3b9d1b8 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -785,8 +785,8 @@ void Ekf::checkVerticalAccelerationHealth() const bool using_gps_for_both = _control_status.flags.gps_hgt && _control_status.flags.gps; const bool using_ev_for_both = _control_status.flags.ev_hgt && _control_status.flags.ev_vel; are_vertical_pos_and_vel_independant = !(using_gps_for_both || using_ev_for_both); - is_inertial_nav_falling |= _vert_vel_innov_ratio > _params.vert_innov_test_lim && _vert_pos_innov_ratio > 0.0f; - is_inertial_nav_falling |= _vert_pos_innov_ratio > _params.vert_innov_test_lim && _vert_vel_innov_ratio > 0.0f; + is_inertial_nav_falling |= _vert_vel_innov_ratio > _params.vert_innov_test_lim && _vert_pos_innov_ratio > _params.vert_innov_test_min; + is_inertial_nav_falling |= _vert_pos_innov_ratio > _params.vert_innov_test_lim && _vert_vel_innov_ratio > _params.vert_innov_test_min; } else { // only height sensing available