From 918606e71d5eceabe3b3e6a292d917ab4e6fc01f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 14 Mar 2021 08:02:13 +1100 Subject: [PATCH] AP_NavEKF3: Don't use fake measurements to learn dvel bias --- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index c8fff42d04..2a0fa2e662 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -881,8 +881,11 @@ void NavEKF3_core::FuseVelPosNED() memset(&Kfusion[10], 0, 12); } - // inhibit delta velocity bias state estimation by setting Kalman gains to zero - if (!inhibitDelVelBiasStates) { + // Inhibit delta velocity bias state estimation by setting Kalman gains to zero + // Don't use 'fake' horizontal measurements used to constrain attitude drift during + // periods of non-aiding to learn bias as these can give incorrect esitmates. + const bool horizInhibit = PV_AidingMode == AID_NONE && obsIndex != 2 && obsIndex != 5; + if (!horizInhibit && !inhibitDelVelBiasStates) { for (uint8_t i = 13; i<=15; i++) { if (!dvelBiasAxisInhibit[i-13]) { Kfusion[i] = P[i][stateIndex]*SK;