From abc821ad3613ba174c89140356ff851ba3c9808d Mon Sep 17 00:00:00 2001
From: Paul Riseborough <priseborough@sypaq.com.au>
Date: Thu, 23 Apr 2020 16:23:12 +1000
Subject: [PATCH] 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 |  8 ++++++--
 libraries/AP_NavEKF2/AP_NavEKF2_core.cpp    | 17 +++++++++--------
 libraries/AP_NavEKF2/AP_NavEKF2_core.h      | 10 +++++++---
 3 files changed, 22 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
index 5314ab2f6c..651cb93045 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
+++ b/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
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
index 7fa2383b7b..072f8a818b 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
+++ b/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();
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index b3b03efcd7..d7dd69afca 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/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