From 145d7037190d6af4d0222c0f989147038d1dda3d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 8 Jun 2023 08:43:07 +1000 Subject: [PATCH] AP_NavEKF3: When dead reckoning, clip but don't reject airspeed --- libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp index 38508e44a9a15c..81aa5d9489d646 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp @@ -134,8 +134,16 @@ void NavEKF3_core::FuseAirspeed() // calculate the innovation consistency test ratio tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f)) * varInnovVtas); - // fail if the ratio is > 1, but don't fail if bad IMU data - const bool isConsistent = (tasTestRatio < 1.0f) || badIMUdata; + // when dead reckoning using airspeed measurements we cannot allow measurements to be rejected because + // this will cause the EKF to stop navigating so instead clip the measurement and accept it + const bool doingAirDataDeadReckoning = posTimeout || (PV_AidingMode != AID_ABSOLUTE); + ftype innovScaleFactor = 1.0f; + if (doingAirDataDeadReckoning && tasTestRatio > 1.0f) { + innovScaleFactor = sqrtF(1.0f / tasTestRatio); + } + + // fail if the ratio is > 1, but don't fail if bad IMU data or doing air data dead reckoning + const bool isConsistent = (tasTestRatio < 1.0f) || badIMUdata || doingAirDataDeadReckoning; tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms; if (!isConsistent) { lastTasFailTime_ms = imuSampleTime_ms; @@ -151,7 +159,7 @@ void NavEKF3_core::FuseAirspeed() // correct the state vector for (uint8_t j= 0; j<=stateIndexLim; j++) { - statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas; + statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas * innovScaleFactor; } stateStruct.quat.normalize();