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

@@ -162,6 +162,24 @@ class Matrix
 		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
 	{

src/modules/ekf2/EKF/ekf.h

@@ -472,16 +472,26 @@ class Ekf final : public EstimatorInterface
 	{
 		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_true = A * P_true;
+		P_true = P_true.multiplyByTranspose(A);
+
+		const VectorState KR = K * R;
+		P_true += 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"
 
 		// Step 1: conventional update
 		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 @@ class Ekf final : public EstimatorInterface
 
 		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();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -850,16 +850,26 @@ 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>();
+	A -= K.multiplyByTranspose(H);
+	P_true = A * P_true;
+	P_true = P_true.multiplyByTranspose(A);
+
+	const VectorState KR = K * R;
+	P_true += 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"
 
 	// Step 1: conventional update
 	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 +878,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();