From 0eac1d7b4a4c59f5f1fffa4a209a496c75a80977 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 25 Jun 2023 09:16:49 +1000 Subject: [PATCH] AP_NavEKF3: Use a smaller airspeed observation gate when dead reckoning --- libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 8e9e3ad7645bc..6cd4aff92220e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -72,8 +72,11 @@ void NavEKF3_core::FuseAirspeed() // When dead reckoning, scale up innovation variance if innovations are larger than the gate // and do not reject the observation to reduce the impact of bad airspeed data or sudden // wind speed changes on the ability to continue dead reckoning navigation. - tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f)) * varInnovVtas); const bool doingAirDataDeadReckoning = posTimeout || (PV_AidingMode != AID_ABSOLUTE); + // Use a smaller gate when dead reckoning to prevent rapid changes to velocity states + // due to gusts or airspeed errors. + const ftype gateStdDev = doingAirDataDeadReckoning ? 1.0f : MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f); + tasTestRatio = sq(innovVtas) / (sq(gateStdDev) * varInnovVtas); if (doingAirDataDeadReckoning && tasTestRatio > 1.0f) { // Adjust the Kalman gains equivalent to increasing the observation noise variance to the // smallest value that would result in the observation being accepted.