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

@@ -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) {
+        // Set vertical GPS velocity to 0 if mode > 0 (assume 0 if no VZ measurement)
+        if (_fusionModeGPS >= 1) {
+            velNED[2] = 0.0f;
+        }
+        // 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
-        }
-        // 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 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