From 2f2ac5be434ad7a528b9c2954b68e1e8636c4fc2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 17 Mar 2017 10:45:24 +1100 Subject: [PATCH 1/3] EKF: Improve protection against severe IMU accel errors Use vertical velocity and position innovation failure to detect bad accelerometer data caused by clipping or aliasing which can cause large vertical acceleration errors and loss of height estimation. When bad accel data is detected: 1) Inhibit accelerometer bias learning 2) Force fusion of vertical velocity and height data 3) Increase accelerometer process noise --- EKF/common.h | 5 +++++ EKF/control.cpp | 21 +++++++++++++++++++-- EKF/covariance.cpp | 10 ++++++++-- EKF/ekf.cpp | 4 +++- EKF/ekf.h | 2 ++ 5 files changed, 37 insertions(+), 5 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 2cef4a5644..653ce9df60 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_MULT 2.0f // The delta velocity process noise is multiplied by this value when accel data is declared bad + struct parameters { // measurement source control int fusion_mode; // bitmasked integer that selects which of the GPS and optical flow aiding sources will be used 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..de85cf0ae8 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,10 @@ 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 + accel_noise *= BADACC_BIAS_PNOISE_MULT; + } 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 From f0bbbc8dc6348f514535d7b7614a8d6a12cfd614 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 17 Mar 2017 16:35:18 +1100 Subject: [PATCH 2/3] EKF: Add reporting of bad accel status --- EKF/common.h | 1 + EKF/ekf_helper.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/EKF/common.h b/EKF/common.h index 653ce9df60..4cca1ec5d1 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -506,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/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; } From 7b996c597295ef54d494aad79bb5f081ec4ee987 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 7 Apr 2017 16:37:27 +1000 Subject: [PATCH 3/3] EKF: Increase measurement error allowance for accelerometer clipping --- EKF/common.h | 2 +- EKF/covariance.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 4cca1ec5d1..0b05d7921d 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -172,7 +172,7 @@ struct extVisionSample { // 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_MULT 2.0f // The delta velocity process noise is multiplied by this value when accel data is declared bad +#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 diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index de85cf0ae8..eb9b7b0b91 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -235,8 +235,9 @@ void Ekf::predictCovariance() 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 - accel_noise *= BADACC_BIAS_PNOISE_MULT; + // 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);