From fd1398d64ee3f84c52e7639d184ba2e2cb97c12e Mon Sep 17 00:00:00 2001
From: Paul Riseborough <priseborough@sypaq.com.au>
Date: Tue, 21 Feb 2023 11:31:38 +1100
Subject: [PATCH 01/19] AP_NavEKF3: Apply GPS quality checks following loss of
 3D fix if velocity error is bounded

---
 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp       |  6 ++++--
 libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp  | 14 ++++++++++++++
 libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp  |  5 +++--
 libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp |  4 ++++
 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp          |  2 ++
 libraries/AP_NavEKF3/AP_NavEKF3_core.h            |  2 ++
 6 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index c886f48ab6328..0489aee688579 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -337,8 +337,10 @@ void NavEKF3_core::setAidingMode()
             // Check if attitude drift has been constrained by a measurement source
             bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed;
 
-            // check if velocity drift has been constrained by a measurement source
-            bool velAiding = gpsVelUsed || airSpdUsed || dragUsed || optFlowUsed || bodyOdmUsed;
+            // Check if velocity drift has been constrained by a measurement source
+            // Currently these are all the same source as will stabilise attitude because we do not currently have
+            // a sensor that only observes attitude
+            velAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed;
 
             // check if position drift has been constrained by a measurement source
             bool posAiding = posUsed || rngBcnUsed;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index 1440a18a49828..1b25e84e655bf 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -542,6 +542,20 @@ void NavEKF3_core::readGpsData()
     // check for new GPS data
     const auto &gps = dal.gps();
 
+    if (gps.status(selected_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) {
+        // The GPS has dropped lock so force quality checks to restart
+        gpsGoodToAlign = false;
+        lastGpsVelFail_ms = imuSampleTime_ms;
+        lastGpsVelPass_ms = 0;
+        if (filterStatus.flags.horiz_pos_rel && !filterStatus.flags.horiz_pos_abs) {
+            // If we can do dead reckoning with a data source other than GPS there is time to wait
+            // for GPS alignment checks to pass before using GPS inside the EKF.
+            waitingForGpsChecks = true;
+        } else {
+            waitingForGpsChecks = false;
+        }
+    }
+
     // limit update rate to avoid overflowing the FIFO buffer
     if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) {
         return;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
index d8a28e15ad4f8..13e086e0d5456 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
@@ -494,7 +494,8 @@ void NavEKF3_core::SelectVelPosFusion()
     readGpsYawData();
 
     // get data that has now fallen behind the fusion time horizon
-    gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
+    gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms) && !waitingForGpsChecks;
+
     if (gpsDataToFuse) {
         CorrectGPSForAntennaOffset(gpsDataDelayed);
         // calculate innovations and variances for reporting purposes only
@@ -794,7 +795,7 @@ void NavEKF3_core::FuseVelPosNED()
                 // if timed out or outside the specified uncertainty radius, reset to the external sensor
                 // if velocity drift is being constrained, dont reset until gps passes quality checks
                 const bool posVarianceIsTooLarge = (frontend->_gpsGlitchRadiusMax > 0) && (P[8][8] + P[7][7]) > sq(ftype(frontend->_gpsGlitchRadiusMax));
-                if (posTimeout || posVarianceIsTooLarge) {
+                if ((posTimeout || posVarianceIsTooLarge) && (!velAiding || gpsGoodToAlign)) {
                     // reset the position to the current external sensor position
                     ResetPosition(resetDataSource::DEFAULT);
 
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
index 47e42773613ff..cb9a8598d6097 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
@@ -230,6 +230,10 @@ void NavEKF3_core::calcGpsGoodToAlign(void)
     } else if (gpsGoodToAlign && imuSampleTime_ms - lastGpsVelPass_ms > 5000) {
         gpsGoodToAlign = false;
     }
+
+    if (gpsGoodToAlign && waitingForGpsChecks) {
+        waitingForGpsChecks = false;
+    }
 }
 
 // update inflight calculaton that determines if GPS data is good enough for reliable navigation
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
index 6c750ff63a891..d1b4ba86c8e1f 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -258,6 +258,8 @@ void NavEKF3_core::InitialiseVariables()
     PV_AidingModePrev = AID_NONE;
     posTimeout = true;
     velTimeout = true;
+    velAiding = false;
+    waitingForGpsChecks = false;
     memset(&faultStatus, 0, sizeof(faultStatus));
     hgtRate = 0.0f;
     onGround = true;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index eef68881dd4a6..8f3fd2d057066 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -1037,6 +1037,8 @@ class NavEKF3_core : public NavEKF_core_common
     bool tasTimeout;                // boolean true if true airspeed measurements have failed for too long and have timed out
     bool dragTimeout;               // boolean true if drag measurements have failed for too long and have timed out
     bool badIMUdata;                // boolean true if the bad IMU data is detected
+    bool velAiding;                 // boolean true if the velocity drift is constrained by observations
+    bool waitingForGpsChecks;          // boolean true if the EKF should not retrieve GPS data from the buffer until quality checks have passed
     uint32_t badIMUdata_ms;         // time stamp bad IMU data was last detected
     uint32_t goodIMUdata_ms;        // time stamp good IMU data was last detected
     uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit

From 092b8d876f50878e76a95cf0c54efb22c1944ca2 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <priseborough@sypaq.com.au>
Date: Mon, 6 Mar 2023 08:39:40 +1100
Subject: [PATCH 02/19] AP_NavEKF3: Use last observed wind states to enable
 dead reckoning

These changes enable the EKF to use the last observable wind velocity vector estimate to synthesise an airspeed measurement if operating without an airspeed sensor and when all other measurement types that can constrain velocoty drift are lost. This enables the EKF to use dead reckoning to continue after loss of GPS when there is no air speed sensor fitted and without the need to set a default airspeed value.
The logic used to fuse a default airspeed value has also been cleaned up and the call to FuseAirSpeed() from inside SelectBetaDragFusion() has been removed.

AP_NavEKF3: Fix error in default airspeed observation variance

AP_NavEKF3: Enable shadow fusion of airspeed when sensor is disabled
---
 .../AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp   |  4 ---
 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp   | 12 ++++++-
 .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp    | 35 ++++++++++++-------
 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp      |  3 ++
 libraries/AP_NavEKF3/AP_NavEKF3_core.h        |  4 ++-
 5 files changed, 40 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
index 38508e44a9a15..79e18027acefe 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
@@ -256,10 +256,6 @@ void NavEKF3_core::SelectBetaDragFusion()
             // we are required to correct only wind states
             airDataFusionWindOnly = true;
         }
-        // Fuse estimated airspeed to aid wind estimation
-        if (usingDefaultAirspeed) {
-            FuseAirspeed();
-        }
         FuseSideslip();
         prevBetaDragStep_ms = imuSampleTime_ms;
     }
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index 0489aee688579..c76d650aae1ab 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -64,6 +64,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
                                  PV_AidingMode != AID_NONE;
     if (!inhibitWindStates && !canEstimateWind) {
         inhibitWindStates = true;
+        windStateLastObsIsValid = false;
         updateStateIndexLim();
     } else if (inhibitWindStates && canEstimateWind &&
                (sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f) || dragFusionEnabled)) {
@@ -77,7 +78,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
             Vector3F tempEuler;
             stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
             ftype trueAirspeedVariance;
-            const bool haveAirspeedMeasurement = usingDefaultAirspeed || (tasDataDelayed.allowFusion && (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500) && useAirspeed());
+            const bool haveAirspeedMeasurement = (tasDataDelayed.allowFusion && (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500) && useAirspeed());
             if (haveAirspeedMeasurement) {
                 trueAirspeedVariance = constrain_ftype(tasDataDelayed.tasVariance, WIND_VEL_VARIANCE_MIN, WIND_VEL_VARIANCE_MAX);
                 const ftype windSpeed =  sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
@@ -342,6 +343,15 @@ void NavEKF3_core::setAidingMode()
             // a sensor that only observes attitude
             velAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed;
 
+            // Store the last wind states whose errors were constrained by measurements
+            // This will be used to synthesise an airspeed measurement to enable dead reckoning
+            bool newWindStateIsObservable = !inhibitWindStates && (posUsed || gpsVelUsed || optFlowUsed || rngBcnUsed || bodyOdmUsed);
+            if (!inhibitWindStates && !newWindStateIsObservable && windStateIsObservable) {
+                windStateLastObs = stateStruct.wind_vel;
+                windStateLastObsIsValid = true;
+            }
+            windStateIsObservable = newWindStateIsObservable;
+
             // check if position drift has been constrained by a measurement source
             bool posAiding = posUsed || rngBcnUsed;
 
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index 1b25e84e655bf..8533490443788 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -861,18 +861,29 @@ void NavEKF3_core::readAirSpdData()
     // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
     tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
 
-    float easErrVar = sq(MAX(frontend->_easNoise, 0.5f));
-    // Allow use of a default value if enabled
-    if (!useAirspeed() &&
-        imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200 &&
-        is_positive(defaultAirSpeed)) {
-        tasDataDelayed.tas = defaultAirSpeed * EAS2TAS;
-        tasDataDelayed.tasVariance = sq(MAX(defaultAirSpeedVariance, easErrVar));
-        tasDataDelayed.allowFusion = true;
-        tasDataDelayed.time_ms = 0;
-        usingDefaultAirspeed = true;
-    } else {
-        usingDefaultAirspeed = false;
+    // Allow use of a default value or a value synthesised from a stored wind velocity vector
+    if (!useAirspeed()) {
+        if (is_positive(defaultAirSpeed)) {
+            if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200) {
+                tasDataDelayed.tas = defaultAirSpeed * EAS2TAS;
+                tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f)));
+                tasDataToFuse = true;
+                tasDataDelayed.allowFusion = true;
+            } else {
+                tasDataToFuse = false;
+            }
+        } else if (windStateLastObsIsValid && !windStateIsObservable) {
+            if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200) {
+                // use stored wind state to synthesise an airspeed measurement
+                const Vector3F windRelVel = stateStruct.velocity - Vector3F(windStateLastObs.x, windStateLastObs.y, 0.0F);
+                tasDataDelayed.tas = windRelVel.length();
+                tasDataDelayed.tasVariance = sq(MAX(frontend->_easNoise, 0.5f));
+                tasDataToFuse = true;
+                tasDataDelayed.allowFusion = true;
+            } else {
+                tasDataToFuse = false;
+            }
+        }
     }
 }
 
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
index d1b4ba86c8e1f..f608f6d4d2cc0 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -268,6 +268,9 @@ void NavEKF3_core::InitialiseVariables()
     prevInFlight = false;
     manoeuvring = false;
     inhibitWindStates = true;
+    windStateLastObs.zero();
+    windStateIsObservable = false;
+    windStateLastObsIsValid = false;
     windStatesAligned = false;
     inhibitDelVelBiasStates = true;
     inhibitDelAngBiasStates = true;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 8f3fd2d057066..69cbfb5acf7a1 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -1110,6 +1110,9 @@ class NavEKF3_core : public NavEKF_core_common
     Vector3F magTestRatio;          // sum of squares of magnetometer innovations divided by fail threshold
     ftype tasTestRatio;             // sum of squares of true airspeed innovation divided by fail threshold
     bool inhibitWindStates;         // true when wind states and covariances are to remain constant
+    Vector2F windStateLastObs;      // wind states from the last time wind states were constrained using observations (m/s)
+    bool windStateLastObsIsValid;
+    bool windStateIsObservable;     // true when wind states are observable from measurements.
     bool windStatesAligned;         // true when wind states have been aligned
     bool inhibitMagStates;          // true when magnetic field states are inactive
     bool lastInhibitMagStates;      // previous inhibitMagStates
@@ -1145,7 +1148,6 @@ class NavEKF3_core : public NavEKF_core_common
     range_elements rangeDataDelayed;// Range finder data at the fusion time horizon
     tas_elements tasDataNew;        // TAS data at the current time horizon
     tas_elements tasDataDelayed;    // TAS data at the fusion time horizon
-    bool usingDefaultAirspeed;      // true when a default airspeed is being used instead of a measured value
     mag_elements magDataDelayed;    // Magnetometer data at the fusion time horizon
     gps_elements gpsDataNew;        // GPS data at the current time horizon
     gps_elements gpsDataDelayed;    // GPS data at the fusion time horizon

From 3439de82c2238bcda037eac5601d3d1768a4c115 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <priseborough@sypaq.com.au>
Date: Tue, 21 Mar 2023 20:03:58 +1100
Subject: [PATCH 03/19] AP_NavEKF3: Strengthen protection against GPS jamming

These changes prevent the EKF from consuming GPS data too soon when it is recovering from jamming if the EKF is able to navigate using dead reckoning.
---
 libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index 8533490443788..dbb60d9bac1e3 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -547,7 +547,11 @@ void NavEKF3_core::readGpsData()
         gpsGoodToAlign = false;
         lastGpsVelFail_ms = imuSampleTime_ms;
         lastGpsVelPass_ms = 0;
-        if (filterStatus.flags.horiz_pos_rel && !filterStatus.flags.horiz_pos_abs) {
+        const bool doingBodyVelNav = (imuSampleTime_ms - prevBodyVelFuseTime_ms < 1000);
+        const bool doingFlowNav = (imuSampleTime_ms - prevFlowFuseTime_ms < 1000);;
+        const bool doingWindRelNav = (!tasTimeout && assume_zero_sideslip());
+        const bool canDeadReckon = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingBodyVelNav);
+        if (canDeadReckon) {
             // If we can do dead reckoning with a data source other than GPS there is time to wait
             // for GPS alignment checks to pass before using GPS inside the EKF.
             waitingForGpsChecks = true;

From 2b086440e62ae3f14005c98a69184133521fb580 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <priseborough@sypaq.com.au>
Date: Tue, 21 Feb 2023 11:34:14 +1100
Subject: [PATCH 04/19] Tools: re-work copter and plane loss of GPS auto tests

Explicitly test time taken to reset to GPS loss and regain of lock for copter without and plane with dead reckoning assistance.
---
 Tools/autotest/arducopter.py | 16 ++++++++++++++++
 Tools/autotest/arduplane.py  | 20 ++++++++++++++++++++
 2 files changed, 36 insertions(+)

diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 6df913906b75a..b646f909a7fb6 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -1922,6 +1922,22 @@ def GPSGlitchAuto(self, timeout=180):
                 self.show_gps_and_sim_positions(False)
             raise e
 
+        # stop and test loss of GPS for a short time - it should resume GPS use without falling back into a non aiding mode
+        self.change_mode("LOITER")
+        self.set_parameters({
+            "SIM_GPS_DISABLE": 1,
+        })
+        self.delay_sim_time(2)
+        self.set_parameters({
+            "SIM_GPS_DISABLE": 0,
+        })
+        # regaining GPS should not result in it falling back to a non-navigation mode
+        self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1)
+        # It should still be navigating after enougnh time has passed for any pending timeouts to activate.
+        self.delay_sim_time(10)
+        self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1)
+        self.change_mode("AUTO")
+
         # record time and position
         tstart = self.get_sim_time()
 
diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index ba7a2eed20db4..c92a57f215470 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -2195,8 +2195,28 @@ def validate_global_position_int_against_simstate(mav, m):
             self.delay_sim_time(20)
             self.change_mode("RTL")
             self.wait_distance_to_home(100, 200, timeout=200)
+            # go into LOITER to create additonal time for a GPS re-enable test
+            self.change_mode("LOITER")
             self.set_parameter("SIM_GPS_DISABLE", 0)
+            t_enabled = self.get_sim_time()
+            # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning
+            # to prevent bad GPS being used when coming back after loss of lock due to interence.
+            self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=15)
+            if self.get_sim_time() < (t_enabled+9):
+                raise NotAchievedException("GPS use re-started too quickly")
+            # wait for EKF and vehicle position to stabilise, then test response to jamming
+            self.delay_sim_time(20)
+
+            self.set_parameter("AHRS_OPTIONS", 1)
+            self.set_parameter("SIM_GPS_JAM", 1)
             self.delay_sim_time(10)
+            self.set_parameter("SIM_GPS_JAM", 0)
+            t_enabled = self.get_sim_time()
+            # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning
+            # to prevent bad GPS being used when coming back after loss of lock due to interence.
+            self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=15)
+            if self.get_sim_time() < (t_enabled+9):
+                raise NotAchievedException("GPS use re-started too quickly after jamming")
             self.set_rc(3, 1000)
             self.fly_home_land_and_disarm()
             self.progress("max-divergence: %fm" % (self.max_divergence,))

From 09e84c701763e2d6e21f6c1d8ef7779c36a915a7 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Tue, 26 Sep 2023 19:36:39 +1000
Subject: [PATCH 05/19] AP_NavEKF3: Fix bug preventing use of default or
 synthetic airspeed

---
 libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index dbb60d9bac1e3..ef465549afee0 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -868,7 +868,7 @@ void NavEKF3_core::readAirSpdData()
     // Allow use of a default value or a value synthesised from a stored wind velocity vector
     if (!useAirspeed()) {
         if (is_positive(defaultAirSpeed)) {
-            if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200) {
+            if (imuDataDelayed.time_ms - lastTasPassTime_ms > 200) {
                 tasDataDelayed.tas = defaultAirSpeed * EAS2TAS;
                 tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f)));
                 tasDataToFuse = true;
@@ -877,7 +877,7 @@ void NavEKF3_core::readAirSpdData()
                 tasDataToFuse = false;
             }
         } else if (windStateLastObsIsValid && !windStateIsObservable) {
-            if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200) {
+            if (imuDataDelayed.time_ms - lastTasPassTime_ms > 200) {
                 // use stored wind state to synthesise an airspeed measurement
                 const Vector3F windRelVel = stateStruct.velocity - Vector3F(windStateLastObs.x, windStateLastObs.y, 0.0F);
                 tasDataDelayed.tas = windRelVel.length();

From e602a46a98d98dda34ca8a03db993f9cc02b1d08 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Wed, 27 Sep 2023 08:18:07 +1000
Subject: [PATCH 06/19] AP_NavEKF3: Treat wind as truth when deadreckoning with
 no airspeed sensor

---
 libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp |  4 ++--
 libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp     |  8 ++++----
 libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp |  4 ++--
 libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp  |  8 ++++----
 libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp  |  2 +-
 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp          | 11 ++++++++---
 libraries/AP_NavEKF3/AP_NavEKF3_core.h            |  3 ++-
 7 files changed, 23 insertions(+), 17 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
index 79e18027acefe..dab013b5e690c 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
@@ -120,7 +120,7 @@ void NavEKF3_core::FuseAirspeed()
             zero_range(&Kfusion[0], 16, 21);
         }
 
-        if (tasDataDelayed.allowFusion && !inhibitWindStates) {
+        if (tasDataDelayed.allowFusion && !inhibitWindStates && !treatWindStatesAsTruth) {
             Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]);
             Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]);
         } else {
@@ -420,7 +420,7 @@ void NavEKF3_core::FuseSideslip()
             zero_range(&Kfusion[0], 16, 21);
         }
 
-        if (!inhibitWindStates) {
+        if (!inhibitWindStates && !treatWindStatesAsTruth) {
             Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]);
             Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]);
         } else {
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
index e8b635e560fed..19dfd91b1f8f0 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
@@ -642,7 +642,7 @@ void NavEKF3_core::FuseMagnetometer()
             }
 
             // zero Kalman gains to inhibit wind state estimation
-            if (!inhibitWindStates) {
+            if (!inhibitWindStates && !treatWindStatesAsTruth) {
                 Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]);
                 Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]);
             } else {
@@ -725,7 +725,7 @@ void NavEKF3_core::FuseMagnetometer()
             }
 
             // zero Kalman gains to inhibit wind state estimation
-            if (!inhibitWindStates) {
+            if (!inhibitWindStates && !treatWindStatesAsTruth) {
                 Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
                 Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]);
             } else {
@@ -809,7 +809,7 @@ void NavEKF3_core::FuseMagnetometer()
             }
 
             // zero Kalman gains to inhibit wind state estimation
-            if (!inhibitWindStates) {
+            if (!inhibitWindStates && !treatWindStatesAsTruth) {
                 Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]);
                 Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]);
             } else {
@@ -1345,7 +1345,7 @@ void NavEKF3_core::FuseDeclination(ftype declErr)
         zero_range(&Kfusion[0], 16, 21);
     }
 
-    if (!inhibitWindStates) {
+    if (!inhibitWindStates && !treatWindStatesAsTruth) {
         Kfusion[22] = -t4*t13*(P[22][16]*magE-P[22][17]*magN);
         Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
     } else {
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
index e457bf7a03e52..84dd164bac161 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
@@ -491,7 +491,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
                 zero_range(&Kfusion[0], 16, 21);
             }
 
-            if (!inhibitWindStates) {
+            if (!inhibitWindStates && !treatWindStatesAsTruth) {
                 Kfusion[22] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
                 Kfusion[23] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
             } else {
@@ -668,7 +668,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
                 zero_range(&Kfusion[0], 16, 21);
             }
 
-            if (!inhibitWindStates) {
+            if (!inhibitWindStates && !treatWindStatesAsTruth) {
                 Kfusion[22] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
                 Kfusion[23] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
             } else {
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
index 13e086e0d5456..e995a9e30c76b 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
@@ -1046,7 +1046,7 @@ void NavEKF3_core::FuseVelPosNED()
                 }
 
                 // inhibit wind state estimation by setting Kalman gains to zero
-                if (!inhibitWindStates) {
+                if (!inhibitWindStates && !treatWindStatesAsTruth) {
                     Kfusion[22] = P[22][stateIndex]*SK;
                     Kfusion[23] = P[23][stateIndex]*SK;
                 } else {
@@ -1563,7 +1563,7 @@ void NavEKF3_core::FuseBodyVel()
                 zero_range(&Kfusion[0], 16, 21);
             }
 
-            if (!inhibitWindStates) {
+            if (!inhibitWindStates && !treatWindStatesAsTruth) {
                 Kfusion[22] = t77*(P[22][5]*t4+P[22][4]*t9+P[22][0]*t14-P[22][6]*t11+P[22][1]*t18-P[22][2]*t21+P[22][3]*t24);
                 Kfusion[23] = t77*(P[23][5]*t4+P[23][4]*t9+P[23][0]*t14-P[23][6]*t11+P[23][1]*t18-P[23][2]*t21+P[23][3]*t24);
             } else {
@@ -1740,7 +1740,7 @@ void NavEKF3_core::FuseBodyVel()
                 zero_range(&Kfusion[0], 16, 21);
             }
 
-            if (!inhibitWindStates) {
+            if (!inhibitWindStates && !treatWindStatesAsTruth) {
                 Kfusion[22] = t77*(-P[22][4]*t3+P[22][5]*t8+P[22][0]*t15+P[22][6]*t12+P[22][1]*t18+P[22][2]*t22-P[22][3]*t25);
                 Kfusion[23] = t77*(-P[23][4]*t3+P[23][5]*t8+P[23][0]*t15+P[23][6]*t12+P[23][1]*t18+P[23][2]*t22-P[23][3]*t25);
             } else {
@@ -1918,7 +1918,7 @@ void NavEKF3_core::FuseBodyVel()
                 zero_range(&Kfusion[0], 16, 21);
             }
 
-            if (!inhibitWindStates) {
+            if (!inhibitWindStates && !treatWindStatesAsTruth) {
                 Kfusion[22] = t77*(P[22][4]*t4+P[22][0]*t14+P[22][6]*t9-P[22][5]*t11-P[22][1]*t17+P[22][2]*t20+P[22][3]*t24);
                 Kfusion[23] = t77*(P[23][4]*t4+P[23][0]*t14+P[23][6]*t9-P[23][5]*t11-P[23][1]*t17+P[23][2]*t20+P[23][3]*t24);
             } else {
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
index 92a2ba4eeca5f..f3b332dbf1e8a 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
@@ -234,7 +234,7 @@ void NavEKF3_core::FuseRngBcn()
             zero_range(&Kfusion[0], 16, 21);
         }
 
-        if (!inhibitWindStates) {
+        if (!inhibitWindStates && !treatWindStatesAsTruth) {
             Kfusion[22] = -t26*(P[22][7]*t4*t9+P[22][8]*t3*t9+P[22][9]*t2*t9);
             Kfusion[23] = -t26*(P[23][7]*t4*t9+P[23][8]*t3*t9+P[23][9]*t2*t9);
         } else {
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
index f608f6d4d2cc0..428130028fdeb 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -270,6 +270,7 @@ void NavEKF3_core::InitialiseVariables()
     inhibitWindStates = true;
     windStateLastObs.zero();
     windStateIsObservable = false;
+    treatWindStatesAsTruth = false;
     windStateLastObsIsValid = false;
     windStatesAligned = false;
     inhibitDelVelBiasStates = true;
@@ -1085,8 +1086,8 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
 
     if (!inhibitWindStates) {
         const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout;
-        if (isDragFusionDeadReckoning) {
-            // when dead reckoning using drag fusion stop learning wind states to provide a more stable velocity estimate
+        treatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable;
+        if (treatWindStatesAsTruth) {
             P[23][23] = P[22][22] = 0.0f;
         } else {
 	        ftype windVelVar  = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate)));
@@ -1946,7 +1947,11 @@ void NavEKF3_core::ConstrainVariances()
     }
 
     if (!inhibitWindStates) {
-        for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,WIND_VEL_VARIANCE_MAX);
+        if (treatWindStatesAsTruth) {
+            P[23][23] = P[22][22] = 0.0f;
+        } else {
+            for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,WIND_VEL_VARIANCE_MAX);
+        }
     } else {
         zeroCols(P,22,23);
         zeroRows(P,22,23);
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 69cbfb5acf7a1..1282dd73f1514 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -1109,10 +1109,11 @@ class NavEKF3_core : public NavEKF_core_common
     ftype hgtTestRatio;             // sum of squares of baro height innovation divided by fail threshold
     Vector3F magTestRatio;          // sum of squares of magnetometer innovations divided by fail threshold
     ftype tasTestRatio;             // sum of squares of true airspeed innovation divided by fail threshold
-    bool inhibitWindStates;         // true when wind states and covariances are to remain constant
+    bool inhibitWindStates;         // true when wind states and covariances should not be used
     Vector2F windStateLastObs;      // wind states from the last time wind states were constrained using observations (m/s)
     bool windStateLastObsIsValid;
     bool windStateIsObservable;     // true when wind states are observable from measurements.
+    bool treatWindStatesAsTruth;    // true when wind states should be used as a truth reference
     bool windStatesAligned;         // true when wind states have been aligned
     bool inhibitMagStates;          // true when magnetic field states are inactive
     bool lastInhibitMagStates;      // previous inhibitMagStates

From 115057d6144c92c8bbcd2db999f507412e0e6512 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Wed, 27 Sep 2023 08:21:06 +1000
Subject: [PATCH 07/19] Tools: Allow dead reckoning test longer to learn wind
 if no aspd sensor

---
 Tools/autotest/arduplane.py | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index c92a57f215470..90d9971919075 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -2188,10 +2188,14 @@ def validate_global_position_int_against_simstate(mav, m):
                 frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
             )
             self.wait_location(loc, accuracy=100)
-            self.progress("Stewing")
-            self.delay_sim_time(20)
+            self.progress("Orbit with GPS and learn wind")
+            # allow longer to learn wind if there is no airspeed sensor
+            if disable_airspeed_sensor:
+                self.delay_sim_time(60)
+            else:
+                self.delay_sim_time(20)
             self.set_parameter("SIM_GPS_DISABLE", 1)
-            self.progress("Roasting")
+            self.progress("Continue orbit without GPS")
             self.delay_sim_time(20)
             self.change_mode("RTL")
             self.wait_distance_to_home(100, 200, timeout=200)

From 409d0c54a6b59925592436f17fe17b60057c367f Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Wed, 27 Sep 2023 09:01:58 +1000
Subject: [PATCH 08/19] AP_NavEKF3: Allow wind to relearn rapidly when GPS is
 re-enabled

---
 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
index 428130028fdeb..435ba04dcb701 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -1086,10 +1086,18 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
 
     if (!inhibitWindStates) {
         const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout;
-        treatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable;
-        if (treatWindStatesAsTruth) {
+        const bool newTreatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable;
+        if (newTreatWindStatesAsTruth) {
+            treatWindStatesAsTruth = true;
             P[23][23] = P[22][22] = 0.0f;
         } else {
+            if (treatWindStatesAsTruth) {
+                treatWindStatesAsTruth = false;
+                if (windStateIsObservable) {
+                    // allow EKF to relearn wind states rapidly
+                    P[23][23] = P[22][22] = sq(WIND_VEL_VARIANCE_MAX);
+                }
+            }
 	        ftype windVelVar  = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate)));
 	        if (!tasDataDelayed.allowFusion) {
 	            // Allow wind states to recover faster when using sideslip fusion with a failed airspeed sesnor

From 810125c0c73732dc5ae792545e6b0433a12e0e90 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Wed, 27 Sep 2023 10:33:01 +1000
Subject: [PATCH 09/19] AP_NavEKF3: Fix bug causing in flight yaw align to not
 complete

---
 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp   |  6 ++--
 libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 32 +++++++++----------
 libraries/AP_NavEKF3/AP_NavEKF3_core.h        |  4 +--
 3 files changed, 21 insertions(+), 21 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index c76d650aae1ab..430be502a8c60 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -696,9 +696,11 @@ bool NavEKF3_core::setOrigin(const Location &loc)
     return true;
 }
 
-// record a yaw reset event
-void NavEKF3_core::recordYawReset()
+// record all requested yaw resets completed
+void NavEKF3_core::recordYawResetsCompleted()
 {
+    gpsYawResetRequest = false;
+    magYawResetRequest = false;
     yawAlignComplete = true;
     if (inFlight) {
         finalInflightYawInit = true;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
index 19dfd91b1f8f0..402cfd97867bc 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
@@ -154,6 +154,15 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset)
         // If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
         bool badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete;
 
+        // get yaw variance from GPS speed uncertainty
+        const ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise));
+        const ftype gps_yaw_variance = sq(asinF(constrain_float(gpsVelAcc/gpsDataDelayed.vel.xy().length(), -1.0F, 1.0F)));
+        if (gps_yaw_variance < sq(radians(GPS_VEL_YAW_ALIGN_MAX_ANG_ERR))) {
+            yawAlignGpsValidCount++;
+        } else {
+            yawAlignGpsValidCount = 0;
+        }
+
         // correct yaw angle using GPS ground course if compass yaw bad
         if (badMagYaw) {
             // attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches
@@ -163,16 +172,6 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset)
                 return;
             }
 
-            // get yaw variance from GPS speed uncertainty
-            const ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise));
-            const ftype gps_yaw_variance = sq(asinF(constrain_float(gpsVelAcc/gpsDataDelayed.vel.xy().length(), -1.0F, 1.0F)));
-
-            if (gps_yaw_variance < sq(radians(GPS_VEL_YAW_ALIGN_MAX_ANG_ERR))) {
-                yawAlignGpsValidCount++;
-            } else {
-                yawAlignGpsValidCount = 0;
-            }
-
             if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) {
                 yawAlignGpsValidCount = 0;
                 // keep roll and pitch and reset yaw
@@ -194,6 +193,10 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset)
                     allMagSensorsFailed = false;
                 }
             }
+        } else if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) {
+                // There is no need to do a yaw reset
+                yawAlignGpsValidCount = 0;
+                recordYawResetsCompleted();
         }
     } else {
         yawAlignGpsValidCount = 0;
@@ -1579,7 +1582,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw(bool emergency_reset)
         }
 
         // record the yaw reset event
-        recordYawReset();
+        recordYawResetsCompleted();
 
         // reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
         ResetVelocity(resetDataSource::DEFAULT);
@@ -1655,10 +1658,5 @@ void NavEKF3_core::resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationO
     lastYawReset_ms = imuSampleTime_ms;
 
     // record the yaw reset event
-    recordYawReset();
-
-    // clear all pending yaw reset requests
-    gpsYawResetRequest = false;
-    magYawResetRequest = false;
-    
+    recordYawResetsCompleted();
 }
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 1282dd73f1514..29d631851b998 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -957,8 +957,8 @@ class NavEKF3_core : public NavEKF_core_common
     // zero attitude state covariances, but preserve variances
     void zeroAttCovOnly();
 
-    // record a yaw reset event
-    void recordYawReset();
+    // record all requested yaw resets completed
+    void recordYawResetsCompleted();
 
     // record a magnetic field state reset event
     void recordMagReset();

From 6d95ec0909c4b417fc8bf1b5c222aaae69d3cd46 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Wed, 27 Sep 2023 11:04:35 +1000
Subject: [PATCH 10/19] AP_NavEKF3: Don't block no compass planes from running
 GPS alignment checks

---
 libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
index cb9a8598d6097..e6876b07a4484 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
@@ -12,7 +12,7 @@
 */
 void NavEKF3_core::calcGpsGoodToAlign(void)
 {
-    if (inFlight && assume_zero_sideslip() && !use_compass()) {
+    if (inFlight && !finalInflightYawInit && assume_zero_sideslip() && !use_compass()) {
         // this is a special case where a plane has launched without magnetometer
         // is now in the air and needs to align yaw to the GPS and start navigating as soon as possible
         gpsGoodToAlign = true;

From 420fb5cbe9c7f9408f170fe8e3f48af375ca8276 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Wed, 27 Sep 2023 13:26:59 +1000
Subject: [PATCH 11/19] AP_NavEKF3: Log gpsGoodToAlign

---
 libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 3 ++-
 libraries/AP_NavEKF3/LogStructure.h         | 6 ++++--
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
index 93e3db98d5b90..062bd4a637ebb 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
@@ -100,7 +100,8 @@ void NavEKF3_core::Log_Write_XKFS(uint64_t time_us) const
         baro_index     : selected_baro,
         gps_index      : selected_gps,
         airspeed_index : getActiveAirspeed(),
-        source_set     : frontend->sources.getPosVelYawSourceSet()
+        source_set     : frontend->sources.getPosVelYawSourceSet(),
+        gps_good_to_align : gpsGoodToAlign
     };
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
 }
diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h
index 8e124462fcf0d..0ecf37dd13deb 100644
--- a/libraries/AP_NavEKF3/LogStructure.h
+++ b/libraries/AP_NavEKF3/LogStructure.h
@@ -344,6 +344,7 @@ struct PACKED log_XKQ {
 // @Field: GI: GPS selection index
 // @Field: AI: airspeed selection index
 // @Field: SS: Source Set (primary=0/secondary=1/tertiary=2)
+// @Field: GPS_GTA: GPS good to align
 struct PACKED log_XKFS {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -353,6 +354,7 @@ struct PACKED log_XKFS {
     uint8_t gps_index;
     uint8_t airspeed_index;
     uint8_t source_set;
+    uint8_t gps_good_to_align;
 };
 
 // @LoggerMessage: XKTV
@@ -439,8 +441,8 @@ struct PACKED log_XKV {
     { LOG_XKFM_MSG, sizeof(log_XKFM),   \
       "XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s#-----", "F------", true }, \
     { LOG_XKFS_MSG, sizeof(log_XKFS), \
-      "XKFS","QBBBBBB","TimeUS,C,MI,BI,GI,AI,SS", "s#-----", "F------" , true }, \
-    { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#----", "F-0000" , true }, \
+      "XKFS","QBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA", "s#------", "F-------" , true }, \
+    { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#????", "F-????" , true }, \
     { LOG_XKT_MSG, sizeof(log_XKT),   \
       "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000", true }, \
     { LOG_XKTV_MSG, sizeof(log_XKTV),                         \

From 5e3a063fe6749dee4b76dd592f36a04664799dd8 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Wed, 27 Sep 2023 15:35:31 +1000
Subject: [PATCH 12/19] AP_NavEKF3: Rework GPS jamming resiliency

Make it user selectable.
Remove potential for a race condition between decisions based on latest data and the EKF fusion processing which operates on a delayed time horizon. This is achieved by preventing data entering the buffer if awaiting checks to pass ensuring that no EKF fusion time horizon processes can use data that hasn't passed checks.
Log the waitingForGpsChecks class variable
---
 libraries/AP_NavEKF3/AP_NavEKF3.cpp           |  7 ++++
 libraries/AP_NavEKF3/AP_NavEKF3.h             |  7 ++++
 libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp   |  3 +-
 .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp    | 42 ++++++++++---------
 libraries/AP_NavEKF3/AP_NavEKF3_core.h        |  2 +-
 libraries/AP_NavEKF3/LogStructure.h           |  4 +-
 6 files changed, 43 insertions(+), 22 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
index 6b26791589446..2021b69ba2195 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
@@ -734,6 +734,13 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = {
     // @Units: m
     AP_GROUPINFO("GPS_VACC_MAX", 10, NavEKF3, _gpsVAccThreshold, 0.0f),
 
+    // @Param: OPTIONS
+    // @DisplayName: Optional EKF behaviour
+    // @Description: This controls optional EKF behaviour. Setting JammingExpected will change the EKF nehaviour such that if dead reckoning navigation is possible it will require the preflight alignment GPS quality checks controlled by EK3_GPS_CHECK and EK3_CHECK_SCALE to pass before resuming GPS use if GPS lock is lost for more than 2 seconds to prevent bad
+    // @Bitmask: 0:JammingExpected
+    // @User: Advanced
+    AP_GROUPINFO("OPTIONS",  11, NavEKF3, _options, 0),
+
     AP_GROUPEND
 };
 
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h
index 9d3303b40df2a..f47f96e8ac157 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.h
@@ -448,6 +448,12 @@ class NavEKF3 {
     AP_Int8 _primary_core;          // initial core number
     AP_Enum<LogLevel> _log_level;   // log verbosity level
     AP_Float _gpsVAccThreshold;     // vertical accuracy threshold to use GPS as an altitude source
+    AP_Int32 _options;              // bit mask of processing options
+
+    // enum for processing options
+    enum class Options {
+        JammingExpected     = (1<<0),
+    };
 
 // Possible values for _flowUse
 #define FLOW_USE_NONE    0
@@ -487,6 +493,7 @@ class NavEKF3 {
     const uint8_t extNavIntervalMin_ms = 20;       // The minimum allowed time between measurements from external navigation sensors (msec)
     const float maxYawEstVelInnov = 2.0f;          // Maximum acceptable length of the velocity innovation returned by the EKF-GSF yaw estimator (m/s)
     const uint16_t deadReckonDeclare_ms = 1000;    // Time without equivalent position or velocity observation to constrain drift before dead reckoning is declared (msec)
+    const uint16_t gpsNoFixTimeout_ms = 2000;      // Time without a fix required to reset GPS alignment checks when EK3_OPTIONS bit 0 is set (msec)
 
     // time at start of current filter update
     uint64_t imuSampleTime_us;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
index 062bd4a637ebb..726b091b3e71b 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
@@ -101,7 +101,8 @@ void NavEKF3_core::Log_Write_XKFS(uint64_t time_us) const
         gps_index      : selected_gps,
         airspeed_index : getActiveAirspeed(),
         source_set     : frontend->sources.getPosVelYawSourceSet(),
-        gps_good_to_align : gpsGoodToAlign
+        gps_good_to_align : gpsGoodToAlign,
+        wait_for_gps_checks : waitingForGpsChecks
     };
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
 }
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index ef465549afee0..107c71db33554 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -542,24 +542,6 @@ void NavEKF3_core::readGpsData()
     // check for new GPS data
     const auto &gps = dal.gps();
 
-    if (gps.status(selected_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) {
-        // The GPS has dropped lock so force quality checks to restart
-        gpsGoodToAlign = false;
-        lastGpsVelFail_ms = imuSampleTime_ms;
-        lastGpsVelPass_ms = 0;
-        const bool doingBodyVelNav = (imuSampleTime_ms - prevBodyVelFuseTime_ms < 1000);
-        const bool doingFlowNav = (imuSampleTime_ms - prevFlowFuseTime_ms < 1000);;
-        const bool doingWindRelNav = (!tasTimeout && assume_zero_sideslip());
-        const bool canDeadReckon = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingBodyVelNav);
-        if (canDeadReckon) {
-            // If we can do dead reckoning with a data source other than GPS there is time to wait
-            // for GPS alignment checks to pass before using GPS inside the EKF.
-            waitingForGpsChecks = true;
-        } else {
-            waitingForGpsChecks = false;
-        }
-    }
-
     // limit update rate to avoid overflowing the FIFO buffer
     if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) {
         return;
@@ -651,6 +633,27 @@ void NavEKF3_core::readGpsData()
         useGpsVertVel = false;
     }
 
+    if ((frontend->_options & (int32_t)NavEKF3::Options::JammingExpected) &&
+        (lastTimeGpsReceived_ms - secondLastGpsTime_ms) > frontend->gpsNoFixTimeout_ms) {
+        const bool doingBodyVelNav = (imuSampleTime_ms - prevBodyVelFuseTime_ms < 1000);
+        const bool doingFlowNav = (imuSampleTime_ms - prevFlowFuseTime_ms < 1000);;
+        const bool canDoWindRelNav = assume_zero_sideslip();
+        const bool canDeadReckon = ((doingFlowNav && gndOffsetValid) || canDoWindRelNav || doingBodyVelNav);
+        if (canDeadReckon) {
+            // If we can do dead reckoning with a data source other than GPS there is time to wait
+            // for GPS alignment checks to pass before using GPS inside the EKF.
+            // this gets set back to false in calcGpsGoodToAlign() when GPS checks pass
+            waitingForGpsChecks = true;
+            // force GPS checks to restart
+            lastPreAlignGpsCheckTime_ms = 0;
+            lastGpsVelFail_ms = imuSampleTime_ms;
+            lastGpsVelPass_ms = 0;
+            gpsGoodToAlign = false;
+        } else {
+            waitingForGpsChecks = false;
+        }
+    }
+
     // Monitor quality of the GPS velocity data before and after alignment
     calcGpsGoodToAlign();
 
@@ -699,7 +702,8 @@ void NavEKF3_core::readGpsData()
     }
 
     // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
-    if (validOrigin) {
+    // and are not waiting for GPs checks to pass
+    if (validOrigin && !waitingForGpsChecks) {
         gpsDataNew.lat = gpsloc.lat;
         gpsDataNew.lng = gpsloc.lng;
         if ((frontend->_originHgtMode & (1<<2)) == 0) {
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 29d631851b998..985882373fc41 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -1038,7 +1038,7 @@ class NavEKF3_core : public NavEKF_core_common
     bool dragTimeout;               // boolean true if drag measurements have failed for too long and have timed out
     bool badIMUdata;                // boolean true if the bad IMU data is detected
     bool velAiding;                 // boolean true if the velocity drift is constrained by observations
-    bool waitingForGpsChecks;          // boolean true if the EKF should not retrieve GPS data from the buffer until quality checks have passed
+    bool waitingForGpsChecks;       // boolean true if the EKF should write GPS data to the buffer until quality checks have passed
     uint32_t badIMUdata_ms;         // time stamp bad IMU data was last detected
     uint32_t goodIMUdata_ms;        // time stamp good IMU data was last detected
     uint32_t vertVelVarClipCounter; // counter used to control reset of vertical velocity variance following collapse against the lower limit
diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h
index 0ecf37dd13deb..59ea98f9e42b5 100644
--- a/libraries/AP_NavEKF3/LogStructure.h
+++ b/libraries/AP_NavEKF3/LogStructure.h
@@ -345,6 +345,7 @@ struct PACKED log_XKQ {
 // @Field: AI: airspeed selection index
 // @Field: SS: Source Set (primary=0/secondary=1/tertiary=2)
 // @Field: GPS_GTA: GPS good to align
+// @Field: GPS_CHK_WAIT: Waiting for GPS checks to pass
 struct PACKED log_XKFS {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -355,6 +356,7 @@ struct PACKED log_XKFS {
     uint8_t airspeed_index;
     uint8_t source_set;
     uint8_t gps_good_to_align;
+    uint8_t wait_for_gps_checks;
 };
 
 // @LoggerMessage: XKTV
@@ -441,7 +443,7 @@ struct PACKED log_XKV {
     { LOG_XKFM_MSG, sizeof(log_XKFM),   \
       "XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s#-----", "F------", true }, \
     { LOG_XKFS_MSG, sizeof(log_XKFS), \
-      "XKFS","QBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA", "s#------", "F-------" , true }, \
+      "XKFS","QBBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA,GPS_CHK_WAIT", "s#-------", "F--------" , true }, \
     { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#????", "F-????" , true }, \
     { LOG_XKT_MSG, sizeof(log_XKT),   \
       "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000", true }, \

From c716578c3af1ba53a292499efbfcd58170f995d0 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Wed, 27 Sep 2023 15:48:59 +1000
Subject: [PATCH 13/19] Tools: Use GPS jamming option in EKF dead reckoning
 autotests

---
 Tools/autotest/arduplane.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 90d9971919075..7fbffdd6e6dd8 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -2136,6 +2136,7 @@ def AIRSPEED_AUTOCAL(self):
         self.fly_home_land_and_disarm()
 
     def deadreckoning_main(self, disable_airspeed_sensor=False):
+        self.set_parameter("EK3_OPTIONS", 1)
         self.reboot_sitl()
         self.wait_ready_to_arm()
         self.gpi = None

From 7cd6200f1f905258acb10c9294e26ba13b04377e Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Thu, 28 Sep 2023 07:57:34 +1000
Subject: [PATCH 14/19] AP_NavEKF3: Rework method of synthesising airspeed for
 dead reckoning

The previous method resulted in data incest and fusion of predicted airspeed on every EKF internal time step.
This was not apparent during flight where the vehicle was turning, but during long straight legs did not constrain along track drift.
---
 .../AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp   |  2 +
 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp   | 14 ++---
 .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp    | 60 ++++++++++---------
 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp      |  3 +-
 libraries/AP_NavEKF3/AP_NavEKF3_core.h        |  4 +-
 5 files changed, 43 insertions(+), 40 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
index dab013b5e690c..a6b71e4a03407 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
@@ -212,8 +212,10 @@ void NavEKF3_core::SelectTasFusion()
     readAirSpdData();
 
     // if the filter is initialised, wind states are not inhibited and we have data to fuse, then perform TAS fusion
+
     if (tasDataToFuse && statesInitialised && !inhibitWindStates) {
         FuseAirspeed();
+        tasDataToFuse = false;
         prevTasStep_ms = imuSampleTime_ms;
     }
 }
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index 430be502a8c60..2f858548721ca 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -64,7 +64,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
                                  PV_AidingMode != AID_NONE;
     if (!inhibitWindStates && !canEstimateWind) {
         inhibitWindStates = true;
-        windStateLastObsIsValid = false;
+        lastAspdEstIsValid = false;
         updateStateIndexLim();
     } else if (inhibitWindStates && canEstimateWind &&
                (sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f) || dragFusionEnabled)) {
@@ -343,14 +343,12 @@ void NavEKF3_core::setAidingMode()
             // a sensor that only observes attitude
             velAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed;
 
-            // Store the last wind states whose errors were constrained by measurements
-            // This will be used to synthesise an airspeed measurement to enable dead reckoning
-            bool newWindStateIsObservable = !inhibitWindStates && (posUsed || gpsVelUsed || optFlowUsed || rngBcnUsed || bodyOdmUsed);
-            if (!inhibitWindStates && !newWindStateIsObservable && windStateIsObservable) {
-                windStateLastObs = stateStruct.wind_vel;
-                windStateLastObsIsValid = true;
+            // Store the last valid airspeed estimate
+            windStateIsObservable = !inhibitWindStates && (posUsed || gpsVelUsed || optFlowUsed || rngBcnUsed || bodyOdmUsed);
+            if (windStateIsObservable) {
+                lastAirspeedEstimate = (stateStruct.velocity - Vector3F(stateStruct.wind_vel.x, stateStruct.wind_vel.y, 0.0F)).length();
+                lastAspdEstIsValid = true;
             }
-            windStateIsObservable = newWindStateIsObservable;
 
             // check if position drift has been constrained by a measurement source
             bool posAiding = posUsed || rngBcnUsed;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index 107c71db33554..b5a933645c790 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
@@ -850,46 +850,50 @@ void NavEKF3_core::readAirSpdData()
     // we take a new reading, convert from EAS to TAS and set the flag letting other functions
     // know a new measurement is available
 
-    const auto *airspeed = dal.airspeed();
-    if (airspeed &&
-        (airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
-        tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS;
-        timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed);
-        tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
-        tasDataNew.tasVariance = sq(MAX(frontend->_easNoise * EAS2TAS, 0.5f));
-        tasDataNew.allowFusion = airspeed->healthy(selected_airspeed) && airspeed->use(selected_airspeed);
-
-        // Correct for the average intersampling delay due to the filter update rate
-        tasDataNew.time_ms -= localFilterTimeStep_ms/2;
-
-        // Save data into the buffer to be fused when the fusion time horizon catches up with it
-        storedTAS.push(tasDataNew);
-    }
-
-    // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
-    tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
-
-    // Allow use of a default value or a value synthesised from a stored wind velocity vector
-    if (!useAirspeed()) {
+    if (useAirspeed()) {
+        const auto *airspeed = dal.airspeed();
+        if (airspeed &&
+            (airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
+            tasDataNew.allowFusion = airspeed->healthy(selected_airspeed) && airspeed->use(selected_airspeed);
+            if (tasDataNew.allowFusion) {
+                tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS;
+                timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed);
+                tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
+                tasDataNew.tasVariance = sq(MAX(frontend->_easNoise * EAS2TAS, 0.5f));
+                // Correct for the average intersampling delay due to the filter update rate
+                tasDataNew.time_ms -= localFilterTimeStep_ms/2;
+                // Save data into the buffer to be fused when the fusion time horizon catches up with it
+                storedTAS.push(tasDataNew);
+            }
+        }
+        // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
+        tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
+    } else {
         if (is_positive(defaultAirSpeed)) {
-            if (imuDataDelayed.time_ms - lastTasPassTime_ms > 200) {
+            // this is the preferred method with the autopilot providing a model based airspeed estimate
+            if (imuDataDelayed.time_ms - prevTasStep_ms > 200 ) {
                 tasDataDelayed.tas = defaultAirSpeed * EAS2TAS;
                 tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f)));
                 tasDataToFuse = true;
                 tasDataDelayed.allowFusion = true;
+                tasDataDelayed.time_ms = imuDataDelayed.time_ms;
             } else {
                 tasDataToFuse = false;
+                tasDataDelayed.allowFusion = false;
             }
-        } else if (windStateLastObsIsValid && !windStateIsObservable) {
-            if (imuDataDelayed.time_ms - lastTasPassTime_ms > 200) {
-                // use stored wind state to synthesise an airspeed measurement
-                const Vector3F windRelVel = stateStruct.velocity - Vector3F(windStateLastObs.x, windStateLastObs.y, 0.0F);
-                tasDataDelayed.tas = windRelVel.length();
-                tasDataDelayed.tasVariance = sq(MAX(frontend->_easNoise, 0.5f));
+        } else if (lastAspdEstIsValid && !windStateIsObservable) {
+            // this uses the last airspeed estimated before dead reckoning started and
+            // wind states became unobservable
+            if (lastAspdEstIsValid && imuDataDelayed.time_ms - prevTasStep_ms > 200) {
+                tasDataDelayed.tas = lastAirspeedEstimate;
+                // this airspeed estimate has a lot of uncertainty
+                tasDataDelayed.tasVariance = sq(MAX(MAX(frontend->_easNoise, 0.5f), 0.5f * lastAirspeedEstimate));
                 tasDataToFuse = true;
                 tasDataDelayed.allowFusion = true;
+                tasDataDelayed.time_ms = imuDataDelayed.time_ms;
             } else {
                 tasDataToFuse = false;
+                tasDataDelayed.allowFusion = false;
             }
         }
     }
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
index 435ba04dcb701..008c9e2c01c7e 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -268,10 +268,9 @@ void NavEKF3_core::InitialiseVariables()
     prevInFlight = false;
     manoeuvring = false;
     inhibitWindStates = true;
-    windStateLastObs.zero();
     windStateIsObservable = false;
     treatWindStatesAsTruth = false;
-    windStateLastObsIsValid = false;
+    lastAspdEstIsValid = false;
     windStatesAligned = false;
     inhibitDelVelBiasStates = true;
     inhibitDelAngBiasStates = true;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index 985882373fc41..2049e7278fad6 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -1110,8 +1110,8 @@ class NavEKF3_core : public NavEKF_core_common
     Vector3F magTestRatio;          // sum of squares of magnetometer innovations divided by fail threshold
     ftype tasTestRatio;             // sum of squares of true airspeed innovation divided by fail threshold
     bool inhibitWindStates;         // true when wind states and covariances should not be used
-    Vector2F windStateLastObs;      // wind states from the last time wind states were constrained using observations (m/s)
-    bool windStateLastObsIsValid;
+    ftype lastAirspeedEstimate;     // last true airspeed estimate (m/s)
+    bool lastAspdEstIsValid;        // true when the last true airspeed estimate is valid (m/s)
     bool windStateIsObservable;     // true when wind states are observable from measurements.
     bool treatWindStatesAsTruth;    // true when wind states should be used as a truth reference
     bool windStatesAligned;         // true when wind states have been aligned

From 576fb394e7739f5280e8e369c91fd276470fae4b Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Thu, 28 Sep 2023 09:25:22 +1000
Subject: [PATCH 15/19] Tools: Disable DCM fallback for plane dead reckoning
 tests

---
 Tools/autotest/arduplane.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 7fbffdd6e6dd8..adce4c66d83b9 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -2137,6 +2137,7 @@ def AIRSPEED_AUTOCAL(self):
 
     def deadreckoning_main(self, disable_airspeed_sensor=False):
         self.set_parameter("EK3_OPTIONS", 1)
+        self.set_parameter("AHRS_OPTIONS",1)
         self.reboot_sitl()
         self.wait_ready_to_arm()
         self.gpi = None

From de698c37875e8482aebef2edbadb4420ca523962 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Thu, 28 Sep 2023 09:26:10 +1000
Subject: [PATCH 16/19] Tools: relax req accuracy for plane dead reckoning when
 not using airspeed

---
 Tools/autotest/arduplane.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index adce4c66d83b9..8601f6b171486 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -2155,7 +2155,10 @@ def validate_global_position_int_against_simstate(mav, m):
             if self.simstate is None:
                 return
             divergence = self.get_distance_int(self.gpi, self.simstate)
-            max_allowed_divergence = 200
+            if disable_airspeed_sensor:
+                max_allowed_divergence = 300
+            else:
+                max_allowed_divergence = 150
             if (time.time() - self.last_print > 1 or
                     divergence > self.max_divergence):
                 self.progress("position-estimate-divergence=%fm" % (divergence,))

From 314110c84e420a5fc1f3b86e8cb0aa7484130df9 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <gncsolns@gmail.com>
Date: Fri, 29 Sep 2023 19:42:19 +1000
Subject: [PATCH 17/19] Tools: update AHRS_OPTIONS for dead reckoning test

---
 Tools/autotest/arduplane.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 8601f6b171486..40ccbeca81832 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -2137,7 +2137,7 @@ def AIRSPEED_AUTOCAL(self):
 
     def deadreckoning_main(self, disable_airspeed_sensor=False):
         self.set_parameter("EK3_OPTIONS", 1)
-        self.set_parameter("AHRS_OPTIONS",1)
+        self.set_parameter("AHRS_OPTIONS", 3)
         self.reboot_sitl()
         self.wait_ready_to_arm()
         self.gpi = None

From dbfbb0e4411a8b21af9450e4506ccde6af67bcc5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 4 Nov 2023 18:10:24 +1100
Subject: [PATCH 18/19] autotest: enable LOG_REPLAY in deadreckoning test

---
 Tools/autotest/arduplane.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 40ccbeca81832..62578e06cad3b 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -2138,6 +2138,7 @@ def AIRSPEED_AUTOCAL(self):
     def deadreckoning_main(self, disable_airspeed_sensor=False):
         self.set_parameter("EK3_OPTIONS", 1)
         self.set_parameter("AHRS_OPTIONS", 3)
+        self.set_parameter("LOG_REPLAY", 1)
         self.reboot_sitl()
         self.wait_ready_to_arm()
         self.gpi = None

From bffbda2b68d22bd2f3b2d7a13fee43f3609e584c Mon Sep 17 00:00:00 2001
From: Paul Riseborough <priseborough@sypaq.com.au>
Date: Wed, 29 May 2024 10:02:04 +1000
Subject: [PATCH 19/19] autotest: Reduce time threshold used in plane
 deadreckoning test

---
 Tools/autotest/arduplane.py | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 62578e06cad3b..3f714260b008f 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -2224,9 +2224,12 @@ def validate_global_position_int_against_simstate(mav, m):
             t_enabled = self.get_sim_time()
             # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning
             # to prevent bad GPS being used when coming back after loss of lock due to interence.
+            # The EKF_STATUS_REPORT does not tell us when the good to align check passes, so the minimum time
+            # value of 3.0 seconds is an arbitrary value set on inspection of dataflash logs from this test
             self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=15)
-            if self.get_sim_time() < (t_enabled+9):
-                raise NotAchievedException("GPS use re-started too quickly after jamming")
+            time_since_jamming_stopped = self.get_sim_time() - t_enabled
+            if time_since_jamming_stopped < 3:
+                raise NotAchievedException("GPS use re-started %f sec after jamming stopped" % time_since_jamming_stopped)
             self.set_rc(3, 1000)
             self.fly_home_land_and_disarm()
             self.progress("max-divergence: %fm" % (self.max_divergence,))