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

AP_NavEKF3: Increase lower state variance limit when vibration affected 

This makes state corrections from GPS and baro observations more aggressive
This commit is contained in:
Paul Riseborough 2021-07-16 16:29:40 +10:00 committed by Andrew Tridgell
parent c3f38ec159
commit ca00da81cd
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -1853,8 +1853,12 @@ void NavEKF3_core::ConstrainVariances()
for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_ftype(P[i][i],0.0,1.0); // attitude error
for (uint8_t i=4; i<=5; i++) P[i][i] = constrain_ftype(P[i][i], VEL_STATE_MIN_VARIANCE, 1.0e3); // NE velocity
// check for collapse of the vertical velocity variance
if (P[6][6] < VEL_STATE_MIN_VARIANCE) {
// if vibration affected use sensor observation variances to set a floor on the state variances
if (badIMUdata) {
P[6][6] = fmaxF(P[6][6], sq(frontend->_gpsVertVelNoise));
P[9][9] = fmaxF(P[9][9], sq(frontend->_baroAltNoise));
} else if (P[6][6] < VEL_STATE_MIN_VARIANCE) {
// handle collapse of the vertical velocity variance
P[6][6] = VEL_STATE_MIN_VARIANCE;
// this counter is decremented by 1 each prediction cycle in CovariancePrediction
// resulting in the count from each clip event fading to zero over 1 second which