From 36ead940d6e1f968452859c6227c18c7889b7e27 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 16 Jul 2021 07:32:58 +1000 Subject: [PATCH] AP_NavEKF3: Use large accel process noise when IMU data is bad --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) 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: