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

AP_NavEKF: Added improved covariance matrix error fix

This commit is contained in:
Paul Riseborough 2014-01-03 19:00:15 +11:00 committed by Andrew Tridgell
parent 4a56ea84b5
commit 43dc9bc055
1 changed files with 2 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -978,17 +978,8 @@ void NavEKF::CovariancePrediction()
}
}
// Force symmetry on the covariance matrix to prevent ill-conditioning
// of the matrix which would cause the filter to blow-up
for (uint8_t i=0; i<=20; i++) P[i][i] = nextP[i][i];
for (uint8_t i=1; i<=20; i++)
{
for (uint8_t j=0; j<=i-1; j++)
{
P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]);
P[j][i] = P[i][j];
}
}
FixCovarianceErrors();
perf_end(_perf_CovariancePrediction);
}