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);