diff --git a/EKF/common.h b/EKF/common.h index 2cef4a5644..0b05d7921d 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -169,6 +169,11 @@ struct extVisionSample { #define RNG_MAX_INTERVAL 2e5 #define EV_MAX_INTERVAL 2e5 +// 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 { // measurement source control int fusion_mode; // bitmasked integer that selects which of the GPS and optical flow aiding sources will be used @@ -501,6 +506,7 @@ union ekf_solution_status { uint16_t pred_pos_horiz_rel : 1; // 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate uint16_t pred_pos_horiz_abs : 1; // 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate uint16_t gps_glitch : 1; // 10 - True if the EKF has detected a GPS glitch + uint16_t accel_error : 1; // 11 - True if the EKF has detected bad accelerometer data } flags; uint16_t value; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 4e69c7ab0c..43af6e137c 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -468,9 +468,26 @@ void Ekf::controlHeightSensorTimeouts() // record time of last bad vert accel if (bad_vert_accel) { _time_bad_vert_accel = _time_last_imu; + } else { + _time_good_vert_accel = _time_last_imu; } - if ((P[9][9] > sq(_params.hgt_reset_lim)) && ((_time_last_imu - _time_last_hgt_fuse) > 5e6)) { + // declare a bad vertical acceleration measurement and make the declaration persist + // for a minimum of 10 seconds + if (_bad_vert_accel_detected) { + _bad_vert_accel_detected = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION); + } else { + _bad_vert_accel_detected = bad_vert_accel; + } + + // 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); + + // check if height has been inertial deadreckoning for too long + bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > 5e6); + + // reset the vertical position and velocity states + if ((P[9][9] > sq(_params.hgt_reset_lim)) && (hgt_fusion_timeout || continuous_bad_accel_hgt)) { // boolean that indicates we will do a height reset bool reset_height = false; @@ -484,7 +501,7 @@ void Ekf::controlHeightSensorTimeouts() bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); // check for inertial sensing errors in the last 10 seconds - bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < 10E6); + bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < BADACC_PROBATION); // reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data bool reset_to_gps = gps_hgt_available && gps_hgt_accurate && !_gps_hgt_faulty && !prev_bad_vert_accel; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 461199c068..eb9b7b0b91 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -154,11 +154,13 @@ void Ekf::predictCovariance() // convert rate of change of accelerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f); - // inhibit learning of imu acccel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities + // inhibit learning of imu acccel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected float alpha = 1.0f - math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); _ang_rate_mag_filt = fmax(_imu_sample_delayed.delta_ang.norm(), alpha * _ang_rate_mag_filt); _accel_mag_filt = fmax(_imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt); - if (_ang_rate_mag_filt > dt * _params.acc_bias_learn_gyr_lim || _accel_mag_filt > dt * _params.acc_bias_learn_acc_lim) { + if (_ang_rate_mag_filt > dt * _params.acc_bias_learn_gyr_lim + || _accel_mag_filt > dt * _params.acc_bias_learn_acc_lim + || _bad_vert_accel_detected) { // store the bias state variances to be reinstated later if (!_accel_bias_inhibit) { _prev_dvel_bias_var(0) = P[13][13]; @@ -232,6 +234,11 @@ void Ekf::predictCovariance() float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f); daxVar = dayVar = dazVar = sq(dt * gyro_noise); float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f); + if (_bad_vert_accel_detected) { + // Increase accelerometer process noise if bad accel data is detected. Measurement errors due to + // vibration induced clipping commonly reach an equivalent 0.5g offset. + accel_noise = BADACC_BIAS_PNOISE; + } dvxVar = dvyVar = dvzVar = sq(dt * accel_noise); // predict the covariance diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index c449c3f558..de965c7716 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -123,7 +123,9 @@ Ekf::Ekf(): _gps_hgt_faulty(false), _rng_hgt_faulty(false), _primary_hgt_source(VDIST_SENSOR_BARO), - _time_bad_vert_accel(0) + _time_bad_vert_accel(0), + _time_good_vert_accel(0), + _bad_vert_accel_detected(false) { _state = {}; _last_known_posNE.setZero(); diff --git a/EKF/ekf.h b/EKF/ekf.h index ddf4bab24a..f14a34ef45 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -349,6 +349,8 @@ private: // imu fault status uint64_t _time_bad_vert_accel; // last time a bad vertical accel was detected (usec) + uint64_t _time_good_vert_accel; // last time a good vertical accel was detected (usec) + bool _bad_vert_accel_detected; // true when bad vertical accelerometer data has been detected // update the real time complementary filter states. This includes the prediction // and the correction step diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index ffff704b7f..6f1acc217f 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -862,6 +862,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) bool gps_pos_innov_bad = (_vel_pos_test_ratio[3] > 1.0f) || (_vel_pos_test_ratio[4] > 1.0f); bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f); soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good; + soln_status.flags.accel_error = _bad_vert_accel_detected; *status = soln_status.value; }