From 7e026d41dfe1a9cef76b95d51dce20e62481d92c Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 4 Jan 2014 07:57:52 +1100
Subject: [PATCH] AP_NavEKF: explicitly zeroed covariance matrix priro to
 setting initial values

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 8d7f57313c..5f1fcd82ce 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -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);