From 43dc9bc05559f8af413d417eece127957694b006 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 3 Jan 2014 19:00:15 +1100
Subject: [PATCH] AP_NavEKF: Added improved covariance matrix error fix

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 13 ++-----------
 1 file changed, 2 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index ca8a48f680..571a592a2f 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -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);
 }