ekf2: use Joseph covariance update for optical flow fusion

src/modules/ekf2/EKF/ekf.h

@@ -499,6 +499,24 @@ class Ekf final : public EstimatorInterface
 		return is_healthy;
 	}
 
+	bool measurementUpdate(VectorState &K, VectorState &H, float R, float innovation)
+	{
+		clearInhibitedStateKalmanGains(K);
+
+		SquareMatrixState I;
+		I.setIdentity();
+		const SquareMatrixState A = I - matrix::Matrix<float, State::size, 1>(K) * H.transpose();
+		P = A * P * A.transpose() + matrix::Matrix<float, State::size, 1>(K) * R * K.transpose();
+
+		constrainStateVariances();
+		forceCovarianceSymmetry();
+
+		// apply the state corrections
+		fuse(K, innovation);
+
+		return true;
+	}
+
 	void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
 
 	void updateParameters();

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -146,7 +146,7 @@ void Ekf::fuseOptFlow()
 
 		VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index];
 
-		if (measurementUpdate(Kfusion, _aid_src_optical_flow.innovation_variance[index], _aid_src_optical_flow.innovation[index])) {
+		if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) {
 			fused[index] = true;
 		}
 	}

src/modules/ekf2/test/test_EKF_flow.cpp

@@ -301,4 +301,47 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData)
 			<< "estimated vel = " << estimated_horz_velocity(0);
 	EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f)
 			<< "estimated vel = " << estimated_horz_velocity(1);
+	_ekf->state().vector().print();
+	_ekf->covariances().print();
+}
+
+TEST_F(EkfFlowTest, yawMotionNoMagFusion)
+{
+	// WHEN: fusing range finder and optical flow data in air
+	const float simulated_distance_to_ground = 5.f;
+	startRangeFinderFusion(simulated_distance_to_ground);
+	startZeroFlowFusion();
+	_ekf_wrapper.setMagFuseTypeNone();
+
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+
+	_sensor_simulator.runSeconds(5.f);
+
+	// AND WHEN: there is a pure yaw rotation
+	const Vector3f body_rate(0.f, 0.f, 3.14159f);
+	const Vector3f flow_offset(-0.15, 0.05f, 0.2f);
+	_ekf_wrapper.setFlowOffset(flow_offset);
+
+	const Vector2f simulated_horz_velocity(body_rate % flow_offset);
+	flowSample flow_sample = _sensor_simulator._flow.dataAtRest();
+	setFlowFromHorizontalVelocityAndDistance(flow_sample, simulated_horz_velocity, simulated_distance_to_ground);
+
+	// use flow sensor gyro data
+	// for clarification of the sign, see definition of flowSample
+	flow_sample.gyro_rate = -body_rate;
+
+	_sensor_simulator._flow.setData(flow_sample);
+	_sensor_simulator._imu.setGyroData(body_rate);
+	_sensor_simulator.runSeconds(10.f);
+
+	// THEN: the flow due to the yaw rotation and the offsets is canceled
+	// and the velocity estimate stays 0
+	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
+	EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.01f)
+			<< "estimated vel = " << estimated_horz_velocity(0);
+	EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f)
+			<< "estimated vel = " << estimated_horz_velocity(1);
+	_ekf->state().vector().print();
+	_ekf->covariances().print();
 }