diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index ddf2becf73..f850a38a19 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -9,7 +9,7 @@ * 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 NavEKF3_core::ResetVelocity(resetDataSource velResetSource) { @@ -22,7 +22,7 @@ void NavEKF3_core::ResetVelocity(resetDataSource velResetSource) zeroCols(P,4,5); if (PV_AidingMode != AID_ABSOLUTE) { - stateStruct.velocity.zero(); + stateStruct.velocity.xy().zero(); // set the variances using the measurement noise parameter P[5][5] = P[4][4] = sq(frontend->_gpsHorizVelNoise); } else {