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

AP_NavEKF: explicitly zeroed covariance matrix priro to setting initial values

Browse Source
This commit is contained in:
Paul Riseborough 2014-01-04 07:57:52 +11:00 committed by Andrew Tridgell
parent 6e6e3d923e
commit 7e026d41df
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1808,7 +1808,15 @@ void NavEKF::OnGroundCheck()
void NavEKF::CovarianceInit()
{
// Calculate the initial covariance matrix P
// zero the matrix
for (uint8_t i=1; i<=20; i++)
{
for (uint8_t j=0; j<=20; j++)
{
P[i][j] = 0.0f;
}
}
// Set the diagonals
P[1][1] = 0.25f*sq(1.0f*DEG_TO_RAD);
P[2][2] = 0.25f*sq(1.0f*DEG_TO_RAD);
P[3][3] = 0.25f*sq(10.0f*DEG_TO_RAD);