From 9e53ff2f8087c983b204fd3b5b26023c357eb80d Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 7 May 2016 14:23:56 +1000
Subject: [PATCH] EKF: Improve protection against bad optical flow fusion

---
 EKF/optflow_fusion.cpp | 49 +++++++++++++++++++++++++++++++++---------
 1 file changed, 39 insertions(+), 10 deletions(-)

diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp
index 5345f18310..eadf3f7d65 100644
--- a/EKF/optflow_fusion.cpp
+++ b/EKF/optflow_fusion.cpp
@@ -431,9 +431,6 @@ void Ekf::fuseOptFlow()
 			gain[row] = Kfusion[row][obs_index];
 		}
 
-		// Update the state vector
-		fuse(gain, _flow_innov[obs_index]);
-
 		// apply covariance correction via P_new = (I -K*H)*P
 		// first calculate expression for KHP
 		// then calculate P - KHP
@@ -456,16 +453,48 @@ void Ekf::fuseOptFlow()
 			}
 		}
 
-		for (unsigned row = 0; row < _k_num_states; row++) {
-			for (unsigned column = 0; column < _k_num_states; column++) {
-				P[row][column] -= KHP[row][column];
+		// if the covariance correction will result in a negative variance, then
+		// the covariance marix is unhealthy and must be corrected
+		bool healthy = true;
+		_fault_status.bad_optflow_X = false;
+		_fault_status.bad_optflow_Y = false;
+		for (int i = 0; i < _k_num_states; i++) {
+			if (P[i][i] < KHP[i][i]) {
+				// zero rows and columns
+				zeroRows(P,i,i);
+				zeroCols(P,i,i);
+
+				//flag as unhealthy
+				healthy = false;
+
+				// update individual measurement health status
+				if (obs_index == 0) {
+					_fault_status.bad_optflow_X = true;
+				} else if (obs_index == 1) {
+					_fault_status.bad_optflow_Y = true;
+				}
 			}
 		}
 
-		_time_last_of_fuse = _time_last_imu;
-		_gps_check_fail_status.value = 0;
-		makeSymmetrical();
-		limitCov();
+		// only apply covariance and state corrrections if healthy
+		if (healthy) {
+			// apply the covariance corrections
+			for (unsigned row = 0; row < _k_num_states; row++) {
+				for (unsigned column = 0; column < _k_num_states; column++) {
+					P[row][column] = P[row][column] - KHP[row][column];
+				}
+			}
+
+			// correct the covariance marix for gross errors
+			makeSymmetrical();
+			limitCov();
+
+			// apply the state corrections
+			fuse(gain, _flow_innov[obs_index]);
+
+			_time_last_of_fuse = _time_last_imu;
+			_gps_check_fail_status.value = 0;
+		}
 	}
 }