From 610a9306122528486f1f9ae73d0ef4a0c2367cab Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 21 Apr 2014 15:24:16 +1000
Subject: [PATCH] AP_NavEKF: catch covarience errors and reset filter

this catches covariance values beyond a reasonable limit and resets
the filter is they happen
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 22 ++++++++++++++++++++--
 1 file changed, 20 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 77c6f23609..0accaa268b 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -95,6 +95,9 @@ extern const AP_HAL::HAL& hal;
 
 #define earthRate 0.000072921f // earth rotation rate (rad/sec)
 
+// maximum value for any element in the covariance matrix
+#define EKF_COVARIENCE_MAX 1.0e8f
+
 // when the wind estimation first starts with no airspeed sensor,
 // assume 3m/s to start
 #define STARTUP_WIND_SPEED 3.0f
@@ -2641,6 +2644,10 @@ bool NavEKF::getPosNED(Vector3f &pos) const
 // return body axis gyro bias estimates in rad/sec
 void NavEKF::getGyroBias(Vector3f &gyroBias) const
 {
+    if (dtIMU == 0) {
+        gyroBias.zero();
+        return;
+    }
     gyroBias.x = states[10] / dtIMU;
     gyroBias.y = states[11] / dtIMU;
     gyroBias.z = states[12] / dtIMU;
@@ -2650,8 +2657,13 @@ void NavEKF::getGyroBias(Vector3f &gyroBias) const
 void NavEKF::getAccelBias(Vector3f &accelBias) const
 {
     accelBias.x = IMU1_weighting;
-    accelBias.y = states[22] / dtIMU;
-    accelBias.z = states[13] / dtIMU;
+    if (dtIMU == 0) {
+        accelBias.y = 0;
+        accelBias.z = 0;
+    } else {
+        accelBias.y = states[22] / dtIMU;
+        accelBias.z = states[13] / dtIMU;
+    }
 }
 
 // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
@@ -2778,6 +2790,12 @@ void NavEKF::ForceSymmetry()
     {
         for (uint8_t j=0; j<=i-1; j++)
         {
+            if (fabsf(P[i][j]) > EKF_COVARIENCE_MAX ||
+                fabsf(P[j][i]) > EKF_COVARIENCE_MAX) {
+                // re-initialise the filter from scratch
+                InitialiseFilterDynamic();
+                return;
+            }
             float temp = 0.5f*(P[i][j] + P[j][i]);
             P[i][j] = temp;
             P[j][i] = temp;