From 1498b516a34d7126c1b6e7efa2c607469b5ddc77 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 14 Mar 2021 12:13:24 +1100 Subject: [PATCH] AP_NavEKF3: Don't try to learn gyro biases that are poorly observable --- .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 2a0fa2e662..ce79261c26 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -874,7 +874,24 @@ void NavEKF3_core::FuseVelPosNED() // inhibit delta angle bias state estimation by setting Kalman gains to zero if (!inhibitDelAngBiasStates) { for (uint8_t i = 10; i<=12; i++) { - Kfusion[i] = P[i][stateIndex]*SK; + // Don't try to learn gyro bias if not aiding and the axis is + // less than 45 degrees from vertical because the bias is poorly observable + bool poorObservability = false; + if (PV_AidingMode == AID_NONE) { + const uint8_t axisIndex = i - 10; + if (axisIndex == 0) { + poorObservability = fabsf(prevTnb.a.z) > M_SQRT1_2; + } else if (axisIndex == 1) { + poorObservability = fabsf(prevTnb.b.z) > M_SQRT1_2; + } else { + poorObservability = fabsf(prevTnb.c.z) > M_SQRT1_2; + } + } + if (poorObservability) { + Kfusion[i] = 0.0; + } else { + Kfusion[i] = P[i][stateIndex]*SK; + } } } else { // zero indexes 10 to 12 = 3*4 bytes