AP_NavEKF2: add error checking and isolation to 3D amg fusion

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -559,24 +559,6 @@ void NavEKF2_core::FuseMagnetometer()
 
     hal.util->perf_begin(_perf_test[5]);
 
-    // 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] * innovMag[obsIndex];
-    }
-
-    // Inhibit corrections to tilt if requested. This enables mag states to settle after a reset without causing sudden changes in roll and pitch
-    if (magFuseTiltInhibit) {
-        stateStruct.angErr.x = 0.0f;
-        stateStruct.angErr.y = 0.0f;
-    }
-
-    // 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
@@ -609,15 +591,54 @@ void NavEKF2_core::FuseMagnetometer()
             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();
+
+        // update the states
+        // 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] * innovMag[obsIndex];
+        }
+
+        // Inhibit corrections to tilt if requested. This enables mag states to settle after a reset without causing sudden changes in roll and pitch
+        if (magFuseTiltInhibit) {
+            stateStruct.angErr.x = 0.0f;
+            stateStruct.angErr.y = 0.0f;
+        }
+
+        // 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_xmag = true;
+        } else if (obsIndex == 1) {
+            faultStatus.bad_ymag = true;
+        } else if (obsIndex == 2) {
+            faultStatus.bad_zmag = true;
         }
     }
-     // force the covariance matrix to be symmetrical and limit the variances to prevent
-    // ill-condiioning.
-    ForceSymmetry();
-    ConstrainVariances();
 
     hal.util->perf_end(_perf_test[5]);