From 4499006744fc7533c7d6c78e9b17da38d1c9bcea Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Sat, 27 Aug 2016 21:18:27 -0400
Subject: [PATCH] Condition LPE covariance to avoid blowing up in edge cases.
 (#5377)

Merging this as it is definitely more robust than what we have now.
---
 .../BlockLocalPositionEstimator.cpp           | 45 ++++++++++++++++---
 .../BlockLocalPositionEstimator.hpp           |  1 +
 2 files changed, 40 insertions(+), 6 deletions(-)

diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 61bb6a982d..5d08da6f4e 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -17,6 +17,8 @@ static const uint32_t 		EST_STDDEV_Z_VALID = 2.0; // 2.0 m
 static const uint32_t 		EST_STDDEV_TZ_VALID = 2.0; // 2.0 m
 static const bool integrate = true; // use accel for integrating
 
+static const float P_MAX = 1.0e6f; // max allowed value in state covariance
+
 BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// this block has no parent, and has name LPE
 	SuperBlock(NULL, "LPE"),
@@ -390,15 +392,28 @@ void BlockLocalPositionEstimator::update()
 		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
 	}
 
-	// reinitialize P if necessary
+	// force P symmetry and reinitialize P if necessary
 	bool reinit_P = false;
 
 	for (int i = 0; i < n_x; i++) {
-		for (int j = 0; j < n_x; j++) {
+		for (int j = 0; j <= i; j++) {
 			if (!PX4_ISFINITE(_P(i, j))) {
 				reinit_P = true;
-				break;
 			}
+
+			if (i == j) {
+				// make sure diagonal elements are positive
+				if (_P(i, i) <= 0) {
+					reinit_P = true;
+				}
+
+			} else {
+				// copy elememnt from upper triangle to force
+				// symmetry
+				_P(j, i) = _P(i, j);
+			}
+
+			if (reinit_P) { break; }
 		}
 
 		if (reinit_P) { break; }
@@ -600,6 +615,22 @@ void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
 	if (std::abs(bz) > BIAS_MAX) { bz = BIAS_MAX * bz / std::abs(bz); }
 }
 
+
+void BlockLocalPositionEstimator::covPropagationLogic(Matrix<float, n_x, n_x> &dP)
+{
+	for (int i = 0; i < n_x; i++) {
+		if (_P(i, i) > P_MAX) {
+			// if diagonal element greater than max, stop propagating
+			dP(i, i) = 0;
+
+			for (int j = 0; j < n_x; j++) {
+				dP(i, j) = 0;
+				dP(j, i) = 0;
+			}
+		}
+	}
+}
+
 void BlockLocalPositionEstimator::detectDistanceSensors()
 {
 	for (int i = 0; i < N_DIST_SUBS; i++) {
@@ -847,9 +878,11 @@ void BlockLocalPositionEstimator::predict()
 	// propagate
 	correctionLogic(dx);
 	_x += dx;
-	_P += (_A * _P + _P * _A.transpose() +
-	       _B * _R * _B.transpose() +
-	       _Q) * getDt();
+	Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
+				      _B * _R * _B.transpose() + _Q) * getDt();
+	covPropagationLogic(dP);
+	_P += dP;
+
 	_xLowPass.update(_x);
 	_aglLowPass.update(agl());
 }
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 761251b0de..7f3ddc29b5 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -204,6 +204,7 @@ private:
 	// misc
 	float agl();
 	void correctionLogic(Vector<float, n_x> &dx);
+	void covPropagationLogic(Matrix<float, n_x, n_x> &dP);
 	void detectDistanceSensors();
 
 	// publications