From 8cd7a035e2920809ebb64dee6570164bd853904c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 22 May 2023 18:27:31 +1000 Subject: [PATCH] AP_NavEKF3: Strengthen recovery from bad delta velocity bias learning --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 8e1628df23..50dfd58c11 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -1935,18 +1935,14 @@ void NavEKF3_core::ConstrainVariances() // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero if (resetRequired) { - ftype delVelBiasVar[3]; - // store all delta velocity bias variances - for (uint8_t i=0; i<=2; i++) { - delVelBiasVar[i] = P[i+13][i+13]; - } // reset all delta velocity bias covariances zeroCols(P,13,15); zeroRows(P,13,15); - // restore all delta velocity bias variances to a margin above the minimum safe value - for (uint8_t i=0; i<=2; i++) { - P[i+13][i+13] = MAX(delVelBiasVar[i], minSafeStateVar * 10.0F); - } + // set all delta velocity bias variances to initial values and zero bias states + P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg); + P[14][14] = P[13][13]; + P[15][15] = P[13][13]; + stateStruct.accel_bias.zero(); } } else {