From f0ee11e95115e202e657df4e0e24451b1b713b6a Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Wed, 17 Sep 2014 04:52:23 +1000
Subject: [PATCH] AP_NavEKF : Fix bug in reset of position, height and velocity
 states

If the inertial solution velocity or position needs to be reset to the GPS or baro, the stored state history for the corresponding states should also be reset.
Otherwise the next GPS or baro measurement will be compared to an invalid previous state and will be rejected. This is particularly a problem if IMU saturation or timeout has occurred because the previous states could be out by a large amount
The position state should be reset to a GPS position corrected for velocity and measurement latency. This will make a noticeable difference for high speed flight vehicles, eg 11m at 50m/s.
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 46 ++++++++++++++++++++-----------
 1 file changed, 30 insertions(+), 16 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 74a42866f1..9a7e415fb0 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -407,12 +407,17 @@ void NavEKF::ResetPosition(void)
 
         // read the GPS
         readGpsData();
-        // write to state vector
-        states[7] = gpsPosNE.x + gpsPosGlitchOffsetNE.x;
-        states[8] = gpsPosNE.y + gpsPosGlitchOffsetNE.y;
+        // write to state vector and compensate for GPS latency
+        states[7] = gpsPosNE.x + gpsPosGlitchOffsetNE.x + 0.001f*velNED.x*float(_msecPosDelay);
+        states[8] = gpsPosNE.y + gpsPosGlitchOffsetNE.y + 0.001f*velNED.y*float(_msecPosDelay);
     }
     // reset the glitch ofset correction states
     gpsPosGlitchOffsetNE.zero();
+    // stored horizontal position states to prevent subsequent GPS measurements from being rejected
+    for (uint8_t i=0; i<=49; i++){
+        storedStates[i].position[0] = state.position[0];
+        storedStates[i].position[1] = state.position[1];
+    }
 }
 
 // resets velocity states to last GPS measurement or to zero if in static mode
@@ -425,20 +430,25 @@ void NavEKF::ResetVelocity(void)
     } else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
         // read the GPS
         readGpsData();
-        // reset horizontal velocity states
-        if (_fusionModeGPS <= 1) {
-            states[4]  = velNED[0]; // north velocity from blended accel data
-            states[5]  = velNED[1]; // east velocity from blended accel data
-            states[23] = velNED[0]; // north velocity from IMU1 accel data
-            states[24] = velNED[1]; // east velocity from IMU1 accel data
-            states[27] = velNED[0]; // north velocity from IMU2 accel data
-            states[28] = velNED[1]; // east velocity from IMU2 accel data
+        // Set vertical GPS velocity to 0 if mode > 0 (assume 0 if no VZ measurement)
+        if (_fusionModeGPS >= 1) {
+            velNED[2] = 0.0f;
         }
-        // reset vertical velocity states
-        if (_fusionModeGPS <= 0) {
-            states[6]  = velNED[2]; // down velocity from blended accel data
-            states[25] = velNED[2]; // down velocity from IMU1 accel data
-            states[29] = velNED[2]; // down velocity from IMU2 accel data
+        // reset filter velocity states
+        states[4]  = velNED[0]; // north velocity from blended accel data
+        states[5]  = velNED[1]; // east velocity from blended accel data
+        states[23] = velNED[0]; // north velocity from IMU1 accel data
+        states[24] = velNED[1]; // east velocity from IMU1 accel data
+        states[27] = velNED[0]; // north velocity from IMU2 accel data
+        states[28] = velNED[1]; // east velocity from IMU2 accel data
+        states[6]  = velNED[2]; // down velocity from blended accel data
+        states[25] = velNED[2]; // down velocity from IMU1 accel data
+        states[29] = velNED[2]; // down velocity from IMU2 accel data
+        // reset stored velocity states to prevent subsequent GPS measurements from being rejected
+        for (uint8_t i=0; i<=49; i++){
+            storedStates[i].velocity[0] = velNED[0];
+            storedStates[i].velocity[1] = velNED[1];
+            storedStates[i].velocity[2] = velNED[2];
         }
     }
 }
@@ -452,6 +462,10 @@ void NavEKF::ResetHeight(void)
     states[9]   = -hgtMea; // down position from blended accel data
     state.posD1 = -hgtMea; // down position from IMU1 accel data
     state.posD2 = -hgtMea; // down position from IMU2 accel data
+    // reset stored vertical position states to prevent subsequent GPS measurements from being rejected
+    for (uint8_t i=0; i<=49; i++){
+        storedStates[i].position[2] = -hgtMea;
+    }
 }
 
 // this function is used to initialise the filter whilst moving, using the AHRS DCM solution