mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: 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
This commit is contained in:
parent
38a26f26a7
commit
5e657c4e7f
|
@ -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 {
|
||||
|
|
Loading…
Reference in New Issue