diff --git a/EKF/common.h b/EKF/common.h index 45d9b485f2..bb3b8953b8 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -354,7 +354,7 @@ struct parameters { float bcoef_y{25.0f}; ///< ballistic coefficient along the Y-axis (kg/m**2) // control of accel error detection and mitigation (IMU clipping) - float vert_innov_test_lim{4.5f}; ///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed + float vert_innov_test_lim{3.0f}; ///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed 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/EKF/control.cpp b/EKF/control.cpp index 9b8651cf87..01b0c4a995 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -893,8 +893,8 @@ void Ekf::checkVerticalAccelerationHealth() { // Check for IMU accelerometer vibration induced clipping as evidenced by the vertical // innovations being positive and not stale. - // Clipping causes the average accel reading to move towards zero which makes the INS - // think it is falling and produces positive vertical innovations + // Clipping usually causes the average accel reading to move towards zero which makes the INS + // think it is falling and produces positive vertical innovations. // Don't use stale innovation data. bool is_inertial_nav_falling = false; bool are_vertical_pos_and_vel_independant = false; @@ -905,13 +905,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); - if (are_vertical_pos_and_vel_independant) { - if (_vert_pos_innov_ratio > 0.0f && _vert_vel_innov_ratio > 0.0f) { - is_inertial_nav_falling = _vert_pos_innov_ratio * _vert_vel_innov_ratio > 0.5f * sq(_params.vert_innov_test_lim); - } - } else { - is_inertial_nav_falling = _vert_vel_innov_ratio > _params.vert_innov_test_lim || _vert_pos_innov_ratio > _params.vert_innov_test_lim; - } + 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; } else { // only height sensing available is_inertial_nav_falling = _vert_pos_innov_ratio > _params.vert_innov_test_lim; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 25c30bbe0a..1e0111611e 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -152,7 +152,7 @@ void Ekf::predictCovariance() // When on ground, only consider an accel bias observable if aligned with the gravity vector const bool is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.8f) || _control_status.flags.in_air; - const bool do_inhibit_axis = do_inhibit_all_axes || !is_bias_observable; + const bool do_inhibit_axis = do_inhibit_all_axes || !is_bias_observable || _imu_sample_delayed.delta_vel_clipping[index]; if (do_inhibit_axis) { // store the bias state variances to be reinstated later