Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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:
Andrew Tridgell 2023-06-23 14:45:37 +10:00
parent 38a26f26a7
commit 5e657c4e7f
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -9,7 +9,7 @@
* RESET FUNCTIONS * * 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 // Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
void NavEKF3_core::ResetVelocity(resetDataSource velResetSource) void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
{ {
@ -22,7 +22,7 @@ void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
zeroCols(P,4,5); zeroCols(P,4,5);
if (PV_AidingMode != AID_ABSOLUTE) { if (PV_AidingMode != AID_ABSOLUTE) {
stateStruct.velocity.zero(); stateStruct.velocity.xy().zero();
// set the variances using the measurement noise parameter // set the variances using the measurement noise parameter
P[5][5] = P[4][4] = sq(frontend->_gpsHorizVelNoise); P[5][5] = P[4][4] = sq(frontend->_gpsHorizVelNoise);
} else { } else {