mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: Don't try to learn gyro biases that are poorly observable
This commit is contained in:
parent
44be7161c0
commit
1498b516a3
|
@ -874,8 +874,25 @@ 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++) {
|
||||||
|
// 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;
|
Kfusion[i] = P[i][stateIndex]*SK;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
} else {
|
} else {
|
||||||
// zero indexes 10 to 12 = 3*4 bytes
|
// zero indexes 10 to 12 = 3*4 bytes
|
||||||
memset(&Kfusion[10], 0, 12);
|
memset(&Kfusion[10], 0, 12);
|
||||||
|
|
Loading…
Reference in New Issue