AP_NavEKF3: Split GSF yaw estimator processing

Separate prediction and correction steps are required to provide an up to

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -597,7 +597,7 @@ void  NavEKF3_core::updateFilterStatus(void)
     filterStatus.flags.initalized = filterStatus.flags.initalized || healthy();
 }
 
-void NavEKF3_core::runYawEstimator()
+void NavEKF3_core::runYawEstimatorPrediction()
 {
     if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
         float trueAirspeed;
@@ -612,7 +612,12 @@ void NavEKF3_core::runYawEstimator()
         }
 
         yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed);
+    }
+}
 
+void NavEKF3_core::runYawEstimatorCorrection()
+{
+    if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
         if (gpsDataToFuse) {
             Vector2f gpsVelNE = Vector2f(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
             float gpsVelAcc = fmaxf(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);
@@ -624,7 +629,6 @@ void NavEKF3_core::runYawEstimator()
             EKFGSF_resetMainFilterYaw();
         }
     }
-
 }
 
 // request a reset the yaw to the GSF estimate

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -631,20 +631,21 @@ void NavEKF3_core::UpdateFilter(bool predict)
         // Predict the covariance growth
         CovariancePrediction();
 
+        // Run the IMU prediction step for the GSF yaw estimator algorithm
+        // using IMU and optionally true airspeed data.
+        // Must be run before SelectMagFusion() to provide an up to date yaw estimate
+        runYawEstimatorPrediction();
+
         // Update states using  magnetometer or external yaw sensor data
         SelectMagFusion();
 
         // Update states using GPS and altimeter data
         SelectVelPosFusion();
 
-        // Generate an alternative yaw estimate used for inflight
+        // Run the GPS velocity correction step for the GSF yaw estimator algorithm
-        // recovery from bad compass data requires horizontal GPS
+        // and use the yaw estimate to reset the main EKF yaw if requested
-        // velocity. We only do this when posVelFusionDelayed is false
+        // Muat be run after SelectVelPosFusion() so that fresh GPS data is available
-        // as otherwise there will be no new GPS data, as the
+        runYawEstimatorCorrection();
-        // sttoredGPS recall will have been skipped
-        if (!posVelFusionDelayed) {
-            runYawEstimator();
-        }
 
         // Update states using range beacon data
         SelectRngBcnFusion();

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -887,9 +887,13 @@ private:
     // correct GPS data for antenna position
     void CorrectGPSForAntennaOffset(gps_elements &gps_data) const;
 
-    // Run an independent yaw estimator algorithm that uses IMU, GPs horizontal velocity
+    // Runs the IMU prediction step for an independent GSF yaw estimator algorithm
-    // and optionally true airspeed data.
+    // that uses IMU, GPS horizontal velocity and optionally true airspeed data.
-    void runYawEstimator(void);
+    void runYawEstimatorPrediction(void);
+
+    // Run the GPS velocity correction step for the GSF yaw estimator and use the
+    // yaw estimate to reset the main EKF yaw if requested
+    void runYawEstimatorCorrection(void);
 
     // reset the quaternion states using the supplied yaw angle, maintaining the previous roll and pitch
     // also reset the body to nav frame rotation matrix