Introduce checkAndFixCovarianceUpdate(KHP) function

EKF/covariance.cpp

@@ -848,6 +848,20 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 	}
 }
 
+// if the covariance correction will result in a negative variance, then
+// the covariance matrix is unhealthy and must be corrected
+bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP) {
+	bool healthy = true;
+	for (int i = 0; i < _k_num_states; i++) {
+		if (P(i, i) < KHP(i, i)) {
+			P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
+			healthy = false;
+		}
+	}
+	return healthy;
+}
+
+
 void Ekf::resetMagRelatedCovariances()
 {
 	resetQuatCov();