From c80dfebadf9757277f0f174e930a6d1ba20f480c Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Thu, 8 Jun 2023 08:43:07 +1000
Subject: [PATCH] AP_NavEKF3: Scale innovation variance instead of clipping
 aspd innovation

---
 .../AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp   | 57 ++++++++++---------
 1 file changed, 29 insertions(+), 28 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
index 81aa5d9489d64..8e9e3ad7645bc 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
@@ -54,10 +54,11 @@ void NavEKF3_core::FuseAirspeed()
         H_TAS[6] = vd*SH_TAS[0];
         H_TAS[22] = -SH_TAS[2];
         H_TAS[23] = -SH_TAS[1];
-        // calculate Kalman gains
-        ftype temp = (tasDataDelayed.tasVariance + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
-        if (temp >= tasDataDelayed.tasVariance) {
-            SK_TAS[0] = 1.0f / temp;
+
+        // calculate innovation variance
+        varInnovVtas = (tasDataDelayed.tasVariance + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
+        if (varInnovVtas >= tasDataDelayed.tasVariance) {
+            SK_TAS[0] = 1.0f / varInnovVtas;
             faultStatus.bad_airspeed = false;
         } else {
             // the calculation is badly conditioned, so we cannot perform fusion on this step
@@ -66,6 +67,29 @@ void NavEKF3_core::FuseAirspeed()
             faultStatus.bad_airspeed = true;
             return;
         }
+
+        // Calculate the innovation consistency test ratio.
+        // 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);
+        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.
+            SK_TAS[0] /= tasTestRatio;
+        }
+
+        // Reject the observation if the test 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;
+        } else {
+            lastTasFailTime_ms = 0;
+        }
+
+        // Calculate Kalman gains
         SK_TAS[1] = SH_TAS[1];
 
         if (tasDataDelayed.allowFusion && !airDataFusionWindOnly) {
@@ -128,29 +152,6 @@ void NavEKF3_core::FuseAirspeed()
             zero_range(&Kfusion[0], 22, 23);
         }
 
-        // calculate measurement innovation variance
-        varInnovVtas = 1.0f/SK_TAS[0];
-
-        // calculate the innovation consistency test ratio
-        tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (ftype)frontend->_tasInnovGate, 1.0f)) * varInnovVtas);
-
-        // 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;
-        } else {
-            lastTasFailTime_ms = 0;
-        }
-
         // test the ratio before fusing data, forcing fusion if airspeed and position are timed out as we have no choice but to try and use airspeed to constrain error growth
         if (tasDataDelayed.allowFusion && (isConsistent || (tasTimeout && posTimeout))) {
 
@@ -159,7 +160,7 @@ void NavEKF3_core::FuseAirspeed()
 
             // correct the state vector
             for (uint8_t j= 0; j<=stateIndexLim; j++) {
-                statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas * innovScaleFactor;
+                statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas;
             }
             stateStruct.quat.normalize();