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

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;
     }

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;
 

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 or a value synthesised from a stored wind velocity vector
-    // Allow use of a default value if enabled
+    if (!useAirspeed()) {
-    if (!useAirspeed() &&
+        if (is_positive(defaultAirSpeed)) {
-        imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200 &&
+            if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200) {
-        is_positive(defaultAirSpeed)) {
+                tasDataDelayed.tas = defaultAirSpeed * EAS2TAS;
-        tasDataDelayed.tas = defaultAirSpeed * EAS2TAS;
+                tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f)));
-        tasDataDelayed.tasVariance = sq(MAX(defaultAirSpeedVariance, easErrVar));
+                tasDataToFuse = true;
-        tasDataDelayed.allowFusion = true;
+                tasDataDelayed.allowFusion = true;
-        tasDataDelayed.time_ms = 0;
+            } else {
-        usingDefaultAirspeed = true;
+                tasDataToFuse = false;
-    } else {
+            }
-        usingDefaultAirspeed = 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;
+            }
+        }
     }
 }
 

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;

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -1110,6 +1110,9 @@ private:
     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 @@ private:
     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