mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-23 09:08:30 -04:00
AP_NavEKF : Reduce sensitivity on filter divergence check
Flying aerobatics with Trad Heli has shown that the divergence check can be false triggered when large magnetometer errors and GPS dropouts are present. This can also happen with multi rotors if large yaw rates are present. This was an unintended consequence of the ekfsmoothing patch which improved filter stability during high rate manoeuvres, but made the divergence test more sensitive.
This commit is contained in:
parent
36bf304f29
commit
f71aeea61d
@ -3471,7 +3471,7 @@ void NavEKF::checkDivergence()
|
||||
float tempLength = tempVec.length();
|
||||
if (tempLength != 0.0f) {
|
||||
float temp = constrain_float((P[10][10] + P[11][11] + P[12][12]),1e-12f,1e-8f);
|
||||
scaledDeltaGyrBiasLgth = (5e-7f / temp) * tempVec.length() / dtIMU;
|
||||
scaledDeltaGyrBiasLgth = (5e-8f / temp) * tempVec.length() / dtIMU;
|
||||
}
|
||||
bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f);
|
||||
lastGyroBias = state.gyro_bias;
|
||||
|
Loading…
Reference in New Issue
Block a user