AP_NavEKF3: Add velocity innovation check to use of EKF-GSF yaw

Only apply to non fly forward vehicle, eg copters, because magnitude of velocity innovation produced by plane launches is TBD.

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -524,6 +524,7 @@ private:
     const uint8_t sensorIntervalMin_ms = 50;       // The minimum allowed time between measurements from any non-IMU sensor (msec)
     const uint8_t flowIntervalMin_ms = 20;         // The minimum allowed time between measurements from optical flow sensors (msec)
     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)
 
     // time at start of current filter update
     uint64_t imuSampleTime_us;

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -646,8 +646,11 @@ void NavEKF3_core::runYawEstimatorCorrection()
 
             // 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));
+            float yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
+            bool canUseEKFGSF = yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
+                                is_positive(yawVarianceEKFGSF) &&
+                                yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
+                                (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov));
             if (canUseEKFGSF) {
                 if (EKFGSF_yaw_valid_count <  GSF_YAW_VALID_HISTORY_THRESHOLD) {
                     EKFGSF_yaw_valid_count++;

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -221,10 +221,12 @@ void NavEKF3_core::SelectMagFusion()
                 yawAngDataDelayed.type = 1;
             }
 
-            float yawEKFGSF, yawVarianceEKFGSF;
+            float yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
             bool canUseEKFGSF = yawEstimator != nullptr &&
                                 yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
-                                is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG));
+                                is_positive(yawVarianceEKFGSF) &&
+                                yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
+                                (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov));
             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
@@ -864,12 +866,12 @@ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor)
 
     // external yaw available check
     bool canUseGsfYaw = false;
-    float gsfYaw = 0.0f;
-    float gsfYawVariance = 0.0f;
+    float gsfYaw, gsfYawVariance, velInnovLength;
     if (usePredictedYaw && yawEstimator != nullptr) {
-        canUseGsfYaw = yawEstimator->getYawData(gsfYaw, gsfYawVariance)
-                        && is_positive(gsfYawVariance)
-                        && gsfYawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG));
+        canUseGsfYaw = yawEstimator->getYawData(gsfYaw, gsfYawVariance) &&
+                       is_positive(gsfYawVariance) &&
+                       gsfYawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
+                       (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov));
     }
 
     // yaw measurement error variance (rad^2)
@@ -1461,8 +1463,11 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
         return false;
     };
 
-    float yawEKFGSF, yawVarianceEKFGSF;
-    if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG))) {
+    float yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
+    if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
+        is_positive(yawVarianceEKFGSF) &&
+        yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
+        (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov))) {
 
         // keep roll and pitch and reset yaw
         rotationOrder order;