From bab95e7ca83008c1617d7c968b6e1bdaee8f7863 Mon Sep 17 00:00:00 2001
From: Corvus Corax <corvuscorax@cybertrench.com>
Date: Thu, 6 Jun 2013 00:07:11 +0200
Subject: [PATCH] check variances for validity and reinitialize if invalid

---
 flight/modules/StateEstimation/filterekf.c | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c
index af5ffcd77..dc4fa38d3 100644
--- a/flight/modules/StateEstimation/filterekf.c
+++ b/flight/modules/StateEstimation/filterekf.c
@@ -422,6 +422,13 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     EKFStateVarianceGet(&vardata);
     INSGetP(vardata.P);
     EKFStateVarianceSet(&vardata);
+    int t;
+    for (t = 0; t < EKFSTATEVARIANCE_P_NUMELEM; t++) {
+        if (invalid_var(vardata.P[t])) {
+            INSResetP(this->ekfConfiguration.P);
+            break;
+        }
+    }
 
     // all sensor data has been used, reset!
     this->work.updated = 0;