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

AP_NavEKF3: Strengthen recovery from bad delta velocity bias learning

This commit is contained in:
Paul Riseborough 2023-05-22 18:27:31 +10:00 committed by Andrew Tridgell
parent 24a7a2ab46
commit 8cd7a035e2
1 changed files with 5 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -1935,18 +1935,14 @@ void NavEKF3_core::ConstrainVariances()
// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
if (resetRequired) { if (resetRequired) {
ftype delVelBiasVar[3];
// store all delta velocity bias variances
for (uint8_t i=0; i<=2; i++) {
delVelBiasVar[i] = P[i+13][i+13];
}
// reset all delta velocity bias covariances // reset all delta velocity bias covariances
zeroCols(P,13,15); zeroCols(P,13,15);
zeroRows(P,13,15); zeroRows(P,13,15);
// restore all delta velocity bias variances to a margin above the minimum safe value // set all delta velocity bias variances to initial values and zero bias states
for (uint8_t i=0; i<=2; i++) { P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg);
P[i+13][i+13] = MAX(delVelBiasVar[i], minSafeStateVar * 10.0F); P[14][14] = P[13][13];
} P[15][15] = P[13][13];
stateStruct.accel_bias.zero();
} }
} else { } else {