From b08817554e6ff6fe9bb6f9a083207b4355f7e76f Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Wed, 21 Oct 2015 16:20:52 +1100
Subject: [PATCH] AP_NavEKF2: Ensure consistent position and velocity fusion
 rates when not using GPS

This sets the fusion of the synthetic position and velocity to occur at the same time as the barometer

This makes filter tuning more consistent between GPS and non-GPS useage
---
 .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp    | 17 ---------
 .../AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp    | 38 +++++++++++--------
 2 files changed, 23 insertions(+), 32 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index 62055d44c6..acf6823175 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
@@ -599,23 +599,6 @@ void NavEKF2_core::readGpsData()
             }
         }
     }
-
-    // If not aiding we synthesise the GPS measurements at the last known position
-    if (PV_AidingMode == AID_NONE) {
-        if (imuSampleTime_ms - gpsDataNew.time_ms > 200) {
-            gpsDataNew.pos.x = lastKnownPositionNE.x;
-            gpsDataNew.pos.y = lastKnownPositionNE.y;
-            gpsDataNew.time_ms = imuSampleTime_ms-frontend._gpsDelay_ms;
-            // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
-            // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
-            gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend.fusionTimeStep_ms);
-            // Prevent time delay exceeding age of oldest IMU data in the buffer
-            gpsDataNew.time_ms = max(gpsDataNew.time_ms,imuDataDelayed.time_ms);
-            // save measurement to buffer to be fused later
-            StoreGPS();
-        }
-    }
-
 }
 
 
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
index 9d3428b968..c2b4ee12e4 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
@@ -119,11 +119,9 @@ void NavEKF2_core::SelectVelPosFusion()
     readGpsData();
 
     // Determine if we need to fuse position and velocity data on this time step
-    if (RecallGPS() && PV_AidingMode != AID_RELATIVE) {
+    if (RecallGPS() && PV_AidingMode == AID_ABSOLUTE) {
         // Don't fuse velocity data if GPS doesn't support it
-        // If no aiding is avaialble, then we use zeroed GPS position and  elocity data to constrain
-        // tilt errors assuming that the vehicle is not accelerating
-        if (frontend._fusionModeGPS <= 1 || PV_AidingMode == AID_NONE) {
+        if (frontend._fusionModeGPS <= 1) {
             fuseVelData = true;
         } else {
             fuseVelData = false;
@@ -151,11 +149,26 @@ void NavEKF2_core::SelectVelPosFusion()
         fuseHgtData = true;
     }
 
+    // If we are operating without any aiding, fuse in the last known position and zero velocity
+    // to constrain tilt drift. This assumes a non-manoeuvring vehicle
+    // Do this to coincide with the height fusion
+    if (fuseHgtData && PV_AidingMode == AID_NONE) {
+        gpsDataDelayed.vel.zero();
+        gpsDataDelayed.pos.x = lastKnownPositionNE.x;
+        gpsDataDelayed.pos.y = lastKnownPositionNE.y;
+        fuseVelData = true;
+        fusePosData = true;
+    }
+
     // perform fusion
     if (fuseVelData || fusePosData || fuseHgtData) {
         // ensure that the covariance prediction is up to date before fusing data
         if (!covPredStep) CovariancePrediction();
         FuseVelPosNED();
+        // clear the flags to prevent repeated fusion of the same data
+        fuseVelData = false;
+        fuseHgtData = false;
+        fusePosData = false;
     }
 }
 
@@ -198,17 +211,12 @@ void NavEKF2_core::FuseVelPosNED()
         if (useAirspeed()) gpsRetryTime = frontend.gpsRetryTimeUseTAS_ms;
         else gpsRetryTime = frontend.gpsRetryTimeNoTAS_ms;
 
-        // form the observation vector and zero velocity and horizontal position observations if in constant position mode
-        // If in constant velocity mode, hold the last known horizontal velocity vector
-        if (PV_AidingMode == AID_ABSOLUTE) {
-            observation[0] = gpsDataDelayed.vel.x + gpsVelGlitchOffset.x;
-            observation[1] = gpsDataDelayed.vel.y + gpsVelGlitchOffset.y;
-            observation[2] = gpsDataDelayed.vel.z;
-            observation[3] = gpsDataDelayed.pos.x + gpsPosGlitchOffsetNE.x;
-            observation[4] = gpsDataDelayed.pos.y + gpsPosGlitchOffsetNE.y;
-        } else if (PV_AidingMode == AID_NONE) {
-            for (uint8_t i=0; i<=4; i++) observation[i] = 0.0f;
-        }
+        // form the observation vector
+        observation[0] = gpsDataDelayed.vel.x + gpsVelGlitchOffset.x;
+        observation[1] = gpsDataDelayed.vel.y + gpsVelGlitchOffset.y;
+        observation[2] = gpsDataDelayed.vel.z;
+        observation[3] = gpsDataDelayed.pos.x + gpsPosGlitchOffsetNE.x;
+        observation[4] = gpsDataDelayed.pos.y + gpsPosGlitchOffsetNE.y;
         observation[5] = -baroDataDelayed.hgt;
 
         // calculate additional error in GPS position caused by manoeuvring