diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 933fc37248..83e4783078 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -1135,7 +1135,7 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) ftype _gyrNoise = constrain_ftype(frontend->_gyrNoise, 0.0f, 1.0f); daxVar = dayVar = dazVar = sq(dt*_gyrNoise); } - ftype _accNoise = constrain_ftype(frontend->_accNoise, 0.0f, 10.0f); + ftype _accNoise = badIMUdata ? BAD_IMU_DATA_ACC_P_NSE : constrain_ftype(frontend->_accNoise, 0.0f, 10.0f); dvxVar = dvyVar = dvzVar = sq(dt*_accNoise); if (!inhibitDelVelBiasStates) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 2a89f1a5ab..1337083683 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -96,6 +96,8 @@ #define EK3_POSXY_STATE_LIMIT 1.0e6 #endif +#define BAD_IMU_DATA_ACC_P_NSE 5.0f + class NavEKF3_core : public NavEKF_core_common { public: