AP_NavEKF2: Split GSF yaw estimator processing

Separate prediction and correction steps are required to provide an up to date yaw estimate using IMU prediction before it may be required by SelectMagFusion() whilst still doing the velocity update after GPS data haw been pulled from the buffer by SelectVelPosFusion()

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -539,7 +539,7 @@ void  NavEKF2_core::updateFilterStatus(void)
     filterStatus.flags.initalized = filterStatus.flags.initalized || healthy();
 }
 
-void NavEKF2_core::runYawEstimator()
+void NavEKF2_core::runYawEstimatorPrediction()
 {
     if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
         float trueAirspeed;
@@ -554,7 +554,12 @@ void NavEKF2_core::runYawEstimator()
         }
 
         yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed);
+    }
+}
 
+void NavEKF2_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);
@@ -566,7 +571,6 @@ void NavEKF2_core::runYawEstimator()
             EKFGSF_resetMainFilterYaw();
         }
     }
-
 }
 
 // request a reset the yaw to the GSF estimate

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -588,20 +588,21 @@ void NavEKF2_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 data
         SelectMagFusion();
 
         // Update states using GPS and altimeter data
         SelectVelPosFusion();
 
-        // Generate an alternative yaw estimate used for inflight
-        // recovery from bad compass data requires horizontal GPS
-        // velocity. We only do this when posVelFusionDelayed is false
-        // as otherwise there will be no new GPS data, as the
-        // sttoredGPS recall will have been skipped
-        if (!posVelFusionDelayed) {
-            runYawEstimator();
-        }
+        // Run the GPS velocity correction step for the GSF yaw estimator algorithm
+        // and use the yaw estimate to reset the main EKF yaw if requested
+        // Muat be run after SelectVelPosFusion() so that fresh GPS data is available
+        runYawEstimatorCorrection();
 
         // Update states using range beacon data
         SelectRngBcnFusion();

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -801,9 +801,13 @@ private:
     // correct external navigation earth-frame position using sensor body-frame offset
     void CorrectExtNavForSensorOffset(Vector3f &ext_position) const;
     
-    // Run an independent yaw estimator algorithm that uses IMU, GPs horizontal velocity
-    // and optionally true airspeed data.
-    void runYawEstimator(void);
+    // Runs the IMU prediction step for an independent GSF yaw estimator algorithm
+    // that uses IMU, GPS horizontal velocity and optionally true airspeed data.
+    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