From 6db228900bee813e41e6f31e4e4277ee576d0f2b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 23 Jun 2023 14:45:36 +1000
Subject: [PATCH] AP_NavEKF2: fixed velocity reset on AID_NONE

The ResetVelocity() function is only supposed to reset XY states, not
Z state. Resetting the Z state for velocity results in a large
velocity glitch if a vehicle is descending or ascending when aiding
switches to AID_NONE

this fixes #19386
---
 libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
index 5e44dfa4a5..3e7b99ee34 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
@@ -10,7 +10,7 @@ extern const AP_HAL::HAL& hal;
 *                   RESET FUNCTIONS                     *
 ********************************************************/
 
-// Reset velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute
+// Reset XY velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute
 // Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
 void NavEKF2_core::ResetVelocity(void)
 {
@@ -23,7 +23,7 @@ void NavEKF2_core::ResetVelocity(void)
     zeroCols(P,3,4);
     
     if (PV_AidingMode != AID_ABSOLUTE) {
-        stateStruct.velocity.zero();
+        stateStruct.velocity.xy().zero();
         // set the variances using the measurement noise parameter
         P[4][4] = P[3][3] = sq(frontend->_gpsHorizVelNoise);
     } else {