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

AP_NavEKF3: Don't try to learn gyro biases that are poorly observable

This commit is contained in:
Paul Riseborough 2021-03-14 12:13:24 +11:00 committed by Andrew Tridgell
parent 44be7161c0
commit 1498b516a3
1 changed files with 18 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -874,7 +874,24 @@ void NavEKF3_core::FuseVelPosNED()
// inhibit delta angle bias state estimation by setting Kalman gains to zero // inhibit delta angle bias state estimation by setting Kalman gains to zero
if (!inhibitDelAngBiasStates) { if (!inhibitDelAngBiasStates) {
for (uint8_t i = 10; i<=12; i++) { for (uint8_t i = 10; i<=12; i++) {
Kfusion[i] = P[i][stateIndex]*SK; // Don't try to learn gyro bias if not aiding and the axis is
// less than 45 degrees from vertical because the bias is poorly observable
bool poorObservability = false;
if (PV_AidingMode == AID_NONE) {
const uint8_t axisIndex = i - 10;
if (axisIndex == 0) {
poorObservability = fabsf(prevTnb.a.z) > M_SQRT1_2;
} else if (axisIndex == 1) {
poorObservability = fabsf(prevTnb.b.z) > M_SQRT1_2;
} else {
poorObservability = fabsf(prevTnb.c.z) > M_SQRT1_2;
}
}
if (poorObservability) {
Kfusion[i] = 0.0;
} else {
Kfusion[i] = P[i][stateIndex]*SK;
}
} }
} else { } else {
// zero indexes 10 to 12 = 3*4 bytes // zero indexes 10 to 12 = 3*4 bytes