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

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:
priseborough 2014-09-15 18:09:10 +10:00 committed by Randy Mackay
parent 36bf304f29
commit f71aeea61d
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -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;