diff --git a/EKF/common.h b/EKF/common.h index 8bca806a66..6e86ad8d4d 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -177,7 +177,6 @@ struct dragSample { // bad accelerometer detection and mitigation #define BADACC_PROBATION 10E6 // Number of usec that accel data declared bad must continuously pass checks to be declared good -#define BADACC_HGT_RESET 1E6 // Number of usec that accel data must continuously fail checks to trigger a height reset #define BADACC_BIAS_PNOISE 4.9f // The delta velocity process noise is set to this when accel data is declared bad (m/s**2) struct parameters { @@ -296,6 +295,10 @@ struct parameters { float bcoef_x{25.0f}; // ballistic coefficient along the X-axis (kg/m**2) 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 + 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) + }; struct stateSample { diff --git a/EKF/control.cpp b/EKF/control.cpp index b394348a60..db1dfcc5c9 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -458,13 +458,14 @@ void Ekf::controlHeightSensorTimeouts() * source failed if we have switched. */ - // check for inertial sensing errors as evidenced by the vertical innovations having the same sign and not stale + // 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 + float var_product_lim = sq(_params.vert_innov_test_lim) * sq(_params.vert_innov_test_lim); bool bad_vert_accel = (_control_status.flags.baro_hgt && // we can only run this check if vertical position and velocity observations are indepedant - (_vel_pos_innov[5] * _vel_pos_innov[2] > 0.0f) && // vertical position and velocity sensors are in agreement + (sq(_vel_pos_innov[5] * _vel_pos_innov[2]) > var_product_lim * (_vel_pos_innov_var[5] * _vel_pos_innov_var[2])) && // vertical position and velocity sensors are in agreement that we have a significant error + (_vel_pos_innov[2] > 0.0f) && // positive innovation indicates that the inertial nav thinks it is falling ((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && // vertical position data is fresh - ((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) && // vertical velocity data is freshs - _vel_pos_test_ratio[2] > 1.0f && // vertical velocty innovations have failed innovation consistency checks - _vel_pos_test_ratio[5] > 1.0f); // vertical position innovations have failed innovation consistency checks + ((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL)); // vertical velocity data is fresh // record time of last bad vert accel if (bad_vert_accel) { @@ -482,7 +483,7 @@ void Ekf::controlHeightSensorTimeouts() } // check if height is continuously failing becasue of accel errors - bool continuous_bad_accel_hgt = ((_time_last_imu - _time_good_vert_accel) > BADACC_HGT_RESET); + bool continuous_bad_accel_hgt = ((_time_last_imu - _time_good_vert_accel) > (unsigned)_params.bad_acc_reset_delay_us); // check if height has been inertial deadreckoning for too long bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > 5e6);