AP_NavEKF2: fixed reset of coviariance matrix

2017-04-27 16:34:32 +10:00 · 2017-04-27 16:34:32 +10:00 · d799bacd7d
parent 888edcd709
commit d799bacd7d
1 changed files with 2 additions and 7 deletions
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
@ -413,13 +413,8 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
 void NavEKF2_core::CovarianceInit()
 {
    // zero the matrix
-    for (uint8_t i=1; i<=stateIndexLim; i++)
-    {
-        for (uint8_t j=0; j<=stateIndexLim; j++)
-        {
-            P[i][j] = 0.0f;
-        }
-    }
+    memset(P, 0, sizeof(P));
+
    // attitude error
    P[0][0]   = 0.1f;
    P[1][1]   = 0.1f;