aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NAvEKF: Prevent failure due to magnetometer fusion numerical errors

Browse Source 
If a badly conditioned covariance matrix causes negative innovation variances, then the filter will diverge. The previous approach of increasing process noise was not effective in some cases, so a hard reset of the covariance matrix has been adopted to guarantee recovery.
This fixes a numerical error observed using the replay on flight log which had significant periods of compass rejection.
This commit is contained in:
Paul Riseborough 2015-07-02 18:07:22 +10:00
parent bd27fed241
commit 4fb7beba4b
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2402,8 +2402,8 @@ void NavEKF::FuseMagnetometer()
faultStatus.bad_xmag = false;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we increase the state variances and try again next time
P[19][19] += 0.1f*R_MAG;
// we reset the covariance matrix and try again next measurement
CovarianceInit();
obsIndex = 1;
faultStatus.bad_xmag = true;
return;
@ -2481,8 +2481,8 @@ void NavEKF::FuseMagnetometer()
faultStatus.bad_ymag = false;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we increase the state variances and try again next time
P[20][20] += 0.1f*R_MAG;
// we reset the covariance matrix and try again next measurement
CovarianceInit();
obsIndex = 2;
faultStatus.bad_ymag = true;
return;
@ -2556,8 +2556,8 @@ void NavEKF::FuseMagnetometer()
faultStatus.bad_zmag = false;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we increase the state variances and try again next time
P[21][21] += 0.1f*R_MAG;
// we reset the covariance matrix and try again next measurement
CovarianceInit();
obsIndex = 3;
faultStatus.bad_zmag = true;
return;