From f6ce25df2a84f1b9dcb674b7b2f2ee0670701312 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sat, 13 Dec 2014 18:30:45 +1100
Subject: [PATCH] AP_NavEKF: Do not reset vertical velocity state from GPS

Doing this can cause large height and height rate errors if large GPS velocity errors cause the GPS tn be rejected for long enough to cause a timeout and reset of states.
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 34 +++++++++++--------------------
 1 file changed, 12 insertions(+), 22 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index a6de14ad42..aab58723ee 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -488,40 +488,30 @@ void NavEKF::ResetPosition(void)
     }
 }
 
-// resets velocity states to last GPS measurement or to zero if in static mode
+// Reset velocity states to last GPS measurement if available or to zero if in static mode
+// Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
 void NavEKF::ResetVelocity(void)
 {
     if (staticMode) {
          state.velocity.zero();
          state.vel1.zero();
          state.vel2.zero();
-    } else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
+    } else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D && _fusionModeGPS <= 1) {
         // 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
-            // over write stored horizontal 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];
-            }
-        }
-        // reset filter velocity states
-        state.velocity = velNED;
-        state.vel1 = velNED;
-        state.vel2 = velNED;
-        // reset stored velocity states to prevent subsequent GPS measurements from being rejected
+        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
+        // over write stored horizontal velocity states to prevent subsequent GPS measurements from being rejected
         for (uint8_t i=0; i<=49; i++){
-            storedStates[i].velocity = velNED;
+            storedStates[i].velocity[0] = velNED[0];
+            storedStates[i].velocity[1] = velNED[1];
         }
     }
-
 }
 
 // reset the vertical position state using the last height measurement