From 65f63341d7dd6155447e319af6fa5b0cd5940773 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 16 May 2016 16:16:00 +1000
Subject: [PATCH] AP_NavEKF2: Improve numerical error protection in optical
 flow fusion

Abort fusion of data if variances will become negative and publish the fault
---
 .../AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp   | 66 +++++++++++++------
 libraries/AP_NavEKF2/AP_NavEKF2_core.h        |  4 +-
 2 files changed, 49 insertions(+), 21 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
index ef79f0fb74..17a327ded8 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
@@ -428,9 +428,12 @@ void NavEKF2_core::FuseOptFlow()
             // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
             if (t61 > R_LOS) {
                 t62 = 1.0f/t61;
+                faultStatus.bad_yflow = false;
             } else {
                 t61 = 0.0f;
                 t62 = 1.0f/R_LOS;
+                faultStatus.bad_yflow = true;
+                return;
             }
             varInnovOptFlow[0] = t61;
 
@@ -579,9 +582,12 @@ void NavEKF2_core::FuseOptFlow()
             // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
             if (t61 > R_LOS) {
                 t62 = 1.0f/t61;
+                faultStatus.bad_yflow = false;
             } else {
                 t61 = 0.0f;
                 t62 = 1.0f/R_LOS;
+                faultStatus.bad_yflow = true;
+                return;
             }
             varInnovOptFlow[1] = t61;
 
@@ -634,18 +640,6 @@ void NavEKF2_core::FuseOptFlow()
             // record the last time observations were accepted for fusion
             prevFlowFuseTime_ms = imuSampleTime_ms;
 
-            // zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
-            stateStruct.angErr.zero();
-
-            // correct the state vector
-            for (uint8_t j= 0; j<=stateIndexLim; j++) {
-                statesArray[j] = statesArray[j] - Kfusion[j] * innovOptFlow[obsIndex];
-            }
-
-            // the first 3 states represent the angular misalignment vector. This is
-            // is used to correct the estimated quaternion on the current time step
-            stateStruct.quat.rotate(stateStruct.angErr);
-
             // correct the covariance P = (I - K*H)*P
             // take advantage of the empty columns in KH to reduce the
             // number of operations
@@ -674,17 +668,49 @@ void NavEKF2_core::FuseOptFlow()
                     KHP[i][j] = res;
                 }
             }
-            for (unsigned i = 0; i<=stateIndexLim; i++) {
-                for (unsigned j = 0; j<=stateIndexLim; j++) {
-                    P[i][j] = P[i][j] - KHP[i][j];
+
+            // Check that we are not going to drive any variances negative and skip the update if so
+            bool healthyFusion = true;
+            for (uint8_t i= 0; i<=stateIndexLim; i++) {
+                if (KHP[i][i] > P[i][i]) {
+                    healthyFusion = false;
                 }
             }
+
+            if (healthyFusion) {
+                // update the covariance matrix
+                for (uint8_t i= 0; i<=stateIndexLim; i++) {
+                    for (uint8_t j= 0; j<=stateIndexLim; j++) {
+                        P[i][j] = P[i][j] - KHP[i][j];
+                    }
+                }
+
+                // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
+                ForceSymmetry();
+                ConstrainVariances();
+
+                // zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
+                stateStruct.angErr.zero();
+
+                // correct the state vector
+                for (uint8_t j= 0; j<=stateIndexLim; j++) {
+                    statesArray[j] = statesArray[j] - Kfusion[j] * innovOptFlow[obsIndex];
+                }
+
+                // the first 3 states represent the angular misalignment vector. This is
+                // is used to correct the estimated quaternion on the current time step
+                stateStruct.quat.rotate(stateStruct.angErr);
+
+            } else {
+                // record bad axis
+                if (obsIndex == 0) {
+                    faultStatus.bad_xflow = true;
+                } else if (obsIndex == 1) {
+                    faultStatus.bad_yflow = true;
+                }
+
+            }
         }
-
-        // fix basic numerical errors
-        ForceSymmetry();
-        ConstrainVariances();
-
     }
 }
 
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index 0192226e2a..f108a32b3f 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
@@ -871,7 +871,7 @@ private:
     uint32_t touchdownExpectedSet_ms; // system time at which expectGndEffectTouchdown was set
     float meaHgtAtTakeOff;            // height measured at commencement of takeoff
 
-    // flags indicating severw numerical errors in innovation variance calculation for different fusion operations
+    // flags indicating severe numerical errors in innovation variance calculation for different fusion operations
     struct {
         bool bad_xmag:1;
         bool bad_ymag:1;
@@ -886,6 +886,8 @@ private:
         bool bad_dpos:1;
         bool bad_yaw:1;
         bool bad_decl:1;
+        bool bad_xflow:1;
+        bool bad_yflow:1;
     } faultStatus;
 
     // flags indicating which GPS quality checks are failing