From 421f13e4b5550a3aa13f6fdd7684b2a0dccd6660 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 6 Mar 2024 17:25:06 +0100
Subject: [PATCH] ekf2: fix joseph covariance update for Schmidt-Kalman filter

If part of the Kalman gain is zeroed, the first step of the joseph
update does not produce a symmetrical matrix.
---
 src/lib/matrix/matrix/Matrix.hpp    | 18 ++++++++++++++++++
 src/modules/ekf2/EKF/ekf.h          | 20 +++++++++++++++-----
 src/modules/ekf2/EKF/ekf_helper.cpp | 22 +++++++++++++++++-----
 3 files changed, 50 insertions(+), 10 deletions(-)

diff --git a/src/lib/matrix/matrix/Matrix.hpp b/src/lib/matrix/matrix/Matrix.hpp
index 33d2e6cbf0..14400683f0 100644
--- a/src/lib/matrix/matrix/Matrix.hpp
+++ b/src/lib/matrix/matrix/Matrix.hpp
@@ -162,6 +162,24 @@ public:
 		return res;
 	}
 
+	// Using this function reduces the number of temporary variables needed to compute A * B.T
+	template<size_t P>
+	Matrix<Type, M, M> multiplyByTranspose(const Matrix<Type, P, N> &other) const
+	{
+		Matrix<Type, M, P> res;
+		const Matrix<Type, M, N> &self = *this;
+
+		for (size_t i = 0; i < M; i++) {
+			for (size_t k = 0; k < P; k++) {
+				for (size_t j = 0; j < N; j++) {
+					res(i, k) += self(i, j) * other(k, j);
+				}
+			}
+		}
+
+		return res;
+	}
+
 	// Element-wise multiplication
 	Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const
 	{
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 11b98f4173..edffd17974 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -472,6 +472,17 @@ public:
 	{
 		clearInhibitedStateKalmanGains(K);
 
+#if false
+		// Matrix implementation of the Joseph stabilized covariance update
+		// This is extremely expensive to compute. Use for debugging purposes only.
+		auto A = matrix::eye<float, State::size>();
+		A -= K.multiplyByTranspose(H);
+		P = A * P;
+		P = P.multiplyByTranspose(A);
+
+		const VectorState KR = K * R;
+		P += KR.multiplyByTranspose(K);
+#else
 		// Efficient implementation of the Joseph stabilized covariance update
 		// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
 
@@ -479,9 +490,8 @@ public:
 		VectorState PH = P * H;
 
 		for (unsigned i = 0; i < State::size; i++) {
-			for (unsigned j = 0; j <= i; j++) {
-				P(i, j) = P(i, j) - K(i) * PH(j);
-				P(j, i) = P(i, j);
+			for (unsigned j = 0; j < State::size; j++) {
+				P(i, j) -= K(i) * PH(j); // P is now not symmetrical if K is not optimal (e.g.: some gains have been zeroed)
 			}
 		}
 
@@ -490,11 +500,11 @@ public:
 
 		for (unsigned i = 0; i < State::size; i++) {
 			for (unsigned j = 0; j <= i; j++) {
-				float s = .5f * (P(i, j) - PH(i) * K(j) + P(i, j) - PH(j) * K(i));
-				P(i, j) = s + K(i) * R * K(j);
+				P(i, j) = P(i, j) - PH(i) * K(j) + K(i) * R * K(j);
 				P(j, i) = P(i, j);
 			}
 		}
+#endif
 
 		constrainStateVariances();
 
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index 3a030cf632..032d35fb31 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -850,6 +850,19 @@ bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, c
 
 	clearInhibitedStateKalmanGains(K);
 
+#if false
+	// Matrix implementation of the Joseph stabilized covariance update
+	// This is extremely expensive to compute. Use for debugging purposes only.
+	auto A = matrix::eye<float, State::size>();
+	VectorState H;
+	H(state_index) = 1.f;
+	A -= K.multiplyByTranspose(H);
+	P = A * P;
+	P = P.multiplyByTranspose(A);
+
+	const VectorState KR = K * R;
+	P += KR.multiplyByTranspose(K);
+#else
 	// Efficient implementation of the Joseph stabilized covariance update
 	// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
 
@@ -857,9 +870,8 @@ bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, c
 	VectorState PH = P.row(state_index);
 
 	for (unsigned i = 0; i < State::size; i++) {
-		for (unsigned j = 0; j <= i; j++) {
-			P(i, j) = P(i, j) - K(i) * PH(j);
-			P(j, i) = P(i, j);
+		for (unsigned j = 0; j < State::size; j++) {
+			P(i, j) -= K(i) * PH(j); // P is now not symmetrical if K is not optimal (e.g.: some gains have been zeroed)
 		}
 	}
 
@@ -868,11 +880,11 @@ bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, c
 
 	for (unsigned i = 0; i < State::size; i++) {
 		for (unsigned j = 0; j <= i; j++) {
-			float s = .5f * (P(i, j) - PH(i) * K(j) + P(i, j) - PH(j) * K(i));
-			P(i, j) = s + K(i) * R * K(j);
+			P(i, j) = P(i, j) - PH(i) * K(j) + K(i) * R * K(j);
 			P(j, i) = P(i, j);
 		}
 	}
+#endif
 
 	constrainStateVariances();