AP_NavEKF3: Require GSF yaw history for reset when not using a yaw sensor

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -642,6 +642,18 @@ void NavEKF3_core::runYawEstimatorCorrection()
             Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
             float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);
             yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc);
+
+            // after velocity data has been fused the yaw variance esitmate will have been refreshed and
+            // is used maintain a history of validity
+            float yawEKFGSF, yawVarianceEKFGSF;
+            bool canUseEKFGSF = yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG));
+            if (canUseEKFGSF) {
+                if (EKFGSF_yaw_valid_count <  GSF_YAW_VALID_HISTORY_THRESHOLD) {
+                    EKFGSF_yaw_valid_count++;
+                }
+            } else {
+                EKFGSF_yaw_valid_count = 0;
+            }
         }
 
         // action an external reset request

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -264,7 +264,9 @@ void NavEKF3_core::SelectMagFusion()
     if ((effectiveMagCal == MagCal::GSF_YAW || !use_compass()) &&
         effectiveMagCal != MagCal::EXTERNAL_YAW &&
         effectiveMagCal != MagCal::EXTERNAL_YAW_FALLBACK) {
-        if (!yawAlignComplete) {
+
+        // because this type of reset event is not as time critical, require a continuous history of valid estimates
+        if (!yawAlignComplete && EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD) {
             yawAlignComplete = EKFGSF_resetMainFilterYaw();
         }
 
@@ -278,7 +280,8 @@ void NavEKF3_core::SelectMagFusion()
             }
 
             float yawEKFGSF, yawVarianceEKFGSF;
-            bool canUseEKFGSF = yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f));
+            bool canUseEKFGSF = yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
+                                is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG));
             if (yawAlignComplete && canUseEKFGSF && !assume_zero_sideslip()) {
                 // use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement
                 // for non fixed wing platform types
@@ -933,7 +936,7 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor)
     if (usePredictedYaw && yawEstimator != nullptr) {
         canUseGsfYaw = yawEstimator->getYawData(gsfYaw, gsfYawVariance)
                         && is_positive(gsfYawVariance)
-                        && gsfYawVariance < sq(radians(15.0f));
+                        && gsfYawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG));
     }
 
     // yaw measurement error variance (rad^2)
@@ -1483,7 +1486,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
     };
 
     float yawEKFGSF, yawVarianceEKFGSF;
-    if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f))) {
+    if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG))) {
 
         // keep roll and pitch and reset yaw
         resetQuatStateYawOnly(yawEKFGSF, yawVarianceEKFGSF);

libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp

@@ -416,6 +416,7 @@ void NavEKF3_core::detectFlight()
         EKFGSF_yaw_reset_ms = 0;
         EKFGSF_yaw_reset_request_ms = 0;
         EKFGSF_yaw_reset_count = 0;
+        EKFGSF_yaw_valid_count = 0;
         EKFGSF_run_filterbank = true;
         Vector3f gyroBias;
         getGyroBias(gyroBias);

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -439,6 +439,7 @@ void NavEKF3_core::InitialiseVariables()
     EKFGSF_yaw_reset_request_ms = 0;
     EKFGSF_yaw_reset_count = 0;
     EKFGSF_run_filterbank = false;
+    EKFGSF_yaw_valid_count = 0;
 
     effectiveMagCal = effective_magCal();
 }

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -79,6 +79,12 @@
 // number of seconds a request to reset the yaw to the GSF estimate is active before it times out
 #define YAW_RESET_TO_GSF_TIMEOUT_MS 5000
 
+// accuracy threshold applied to GSF yaw estimate use
+#define GSF_YAW_ACCURACY_THRESHOLD_DEG 15.0f
+
+// number of continuous valid GSF yaw estimates required to confirm valid hostory
+#define GSF_YAW_VALID_HISTORY_THRESHOLD 5
+
 class AP_AHRS;
 
 class NavEKF3_core : public NavEKF_core_common
@@ -1483,4 +1489,5 @@ private:
     uint32_t EKFGSF_yaw_reset_request_ms;   // timestamp of last emergency yaw reset request (uSec)
     uint8_t EKFGSF_yaw_reset_count;         // number of emergency yaw resets performed
     bool EKFGSF_run_filterbank;             // true when the filter bank is active
+    uint8_t EKFGSF_yaw_valid_count;         // number of updates since the last invalid yaw estimate
 };