Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 17:18:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Scale divergence check with covariance

Browse Source 
This provides consistent behaviour for a range of gyro bias process
noise settings and automatically adjusts sensitivity as filter learns bias
This commit is contained in:
priseborough 2014-05-16 21:13:30 +10:00 committed by Andrew Tridgell
parent 0337d44b2e
commit a19015ed61
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3398,7 +3398,8 @@ void NavEKF::checkDivergence()
Vector3f tempVec = state.gyro_bias - lastGyroBias; Vector3f tempVec = state.gyro_bias - lastGyroBias;
float tempLength = tempVec.length(); float tempLength = tempVec.length();
if (tempLength != 0.0f) { if (tempLength != 0.0f) {
scaledDeltaGyrBiasLgth = 5e4f*tempVec.length()/dtIMU; float temp = constrain_float((P[10][10] + P[11][11] + P[12][12]),1e-12f,1e-8f);
scaledDeltaGyrBiasLgth = (2e-6f / temp) * tempVec.length() / dtIMU;
} }
bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f); bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f);
lastGyroBias = state.gyro_bias; lastGyroBias = state.gyro_bias;