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