ekf2: fix direct state measurement update for suboptimal K case

The duration of a unit test had to be increased because the incorrect
covariance matrix update, was making the unit test passing faster
(over-optimistic variance).

src/modules/ekf2/EKF/ekf.h

@@ -485,9 +485,14 @@ public:
 #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"
+		// P = (I - K * H) * P * (I - K * H).T   + K * R * K.T
+		//   =      P_temp     * (I - H.T * K.T) + K * R * K.T
+		//   =      P_temp - P_temp * H.T * K.T  + K * R * K.T
 
 		// Step 1: conventional update
-		VectorState PH = P * H;
+		// Compute P_temp and store it in P to avoid allocating more memory
+		// P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major
+		VectorState PH = P * H; // H is stored as a column vector. H is in fact H.T
 
 		for (unsigned i = 0; i < State::size; i++) {
 			for (unsigned j = 0; j < State::size; j++) {
@@ -496,7 +501,7 @@ public:
 		}
 
 		// Step 2: stabilized update
-		PH = P * H;
+		PH = P * H; // H is stored as a column vector. H is in fact H.T
 
 		for (unsigned i = 0; i < State::size; i++) {
 			for (unsigned j = 0; j <= i; j++) {

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -865,18 +865,24 @@ bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, c
 #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"
+	// P = (I - K * H) * P * (I - K * H).T   + K * R * K.T
+	//   =      P_temp     * (I - H.T * K.T) + K * R * K.T
+	//   =      P_temp - P_temp * H.T * K.T  + K * R * K.T
 
 	// Step 1: conventional update
+	// Compute P_temp and store it in P to avoid allocating more memory
+	// P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major
 	VectorState PH = P.row(state_index);
 
 	for (unsigned i = 0; i < State::size; i++) {
 		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)
+			P(i, j) -= K(i) * PH(j); // P is now not symmetric if K is not optimal (e.g.: some gains have been zeroed)
 		}
 	}
 
 	// Step 2: stabilized update
-	PH = P.row(state_index);
+	// P (or "P_temp") is not symmetric so we must take the column
+	PH = P.col(state_index);
 
 	for (unsigned i = 0; i < State::size; i++) {
 		for (unsigned j = 0; j <= i; j++) {

src/modules/ekf2/test/test_EKF_initialization.cpp

@@ -346,7 +346,7 @@ TEST_F(EkfInitializationTest, initializeWithTiltNotAtRest)
 	_ekf->set_vehicle_at_rest(false);
 	_sensor_simulator.simulateOrientation(quat_sim);
 	//_sensor_simulator.runSeconds(_init_tilt_period);
-	_sensor_simulator.runSeconds(7);
+	_sensor_simulator.runSeconds(10);
 
 	EXPECT_TRUE(_ekf->control_status_flags().tilt_align);