ekf2: EV fusion in body frame (PX4#23191)

ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu

@@ -7,6 +7,7 @@
 #
 # @maintainer
 # @board px4_fmu-v2 exclude
+# @board px4_fmu-v6x exclude
 #
 
 . ${R}etc/init.d/rc.mc_defaults

ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21

@@ -13,6 +13,7 @@
 # @board px4_fmu-v4pro exclude
 # @board px4_fmu-v5 exclude
 # @board px4_fmu-v5x exclude
+# @board px4_fmu-v6x exclude
 # @board diatone_mamba-f405-mk2 exclude
 #
 . ${R}etc/init.d/rc.mc_defaults

src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp

@@ -37,6 +37,9 @@
  */
 
 #include "ekf.h"
+#include <ekf_derivation/generated/compute_ev_body_vel_hx.h>
+#include <ekf_derivation/generated/compute_ev_body_vel_hy.h>
+#include <ekf_derivation/generated/compute_ev_body_vel_hz.h>
 
 void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
 			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src)
@@ -57,14 +60,16 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 
 	// rotate measurement into correct earth frame if required
-	Vector3f vel{NAN, NAN, NAN};
-	Matrix3f vel_cov{};
+	Vector3f measurement{};
+	Vector3f measurement_var{};
+
+	float minimum_variance = math::max(sq(0.01f), sq(_params.ev_vel_noise));
 
 	switch (ev_sample.vel_frame) {
 	case VelocityFrame::LOCAL_FRAME_NED:
 		if (_control_status.flags.yaw_align) {
-			vel = ev_sample.vel - vel_offset_earth;
-			vel_cov = matrix::diag(ev_sample.velocity_var);
+			measurement = ev_sample.vel - vel_offset_earth;
+			measurement_var = ev_sample.velocity_var;
 
 		} else {
 			continuing_conditions_passing = false;
@@ -75,31 +80,28 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	case VelocityFrame::LOCAL_FRAME_FRD:
 		if (_control_status.flags.ev_yaw) {
 			// using EV frame
-			vel = ev_sample.vel - vel_offset_earth;
-			vel_cov = matrix::diag(ev_sample.velocity_var);
+			measurement = ev_sample.vel - vel_offset_earth;
+			measurement_var = ev_sample.velocity_var;
 
 		} else {
 			// rotate EV to the EKF reference frame
 			const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
 
-			vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
-			vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * R_ev_to_ekf.transpose();
-
-			// increase minimum variance to include EV orientation variance
-			// TODO: do this properly
-			const float orientation_var_max = ev_sample.orientation_var.max();
-
-			for (int i = 0; i < 2; i++) {
-				vel_cov(i, i) = math::max(vel_cov(i, i), orientation_var_max);
-			}
+			measurement = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
+			measurement_var = matrix::SquareMatrix3f(R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) *
+					  R_ev_to_ekf.transpose()).diag();
+			minimum_variance = math::max(minimum_variance, ev_sample.orientation_var.max());
 		}
 
 		break;
 
-	case VelocityFrame::BODY_FRAME_FRD:
-		vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
-		vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
-		break;
+	case VelocityFrame::BODY_FRAME_FRD: {
+
+			// currently it is assumed that the orientation of the EV frame and the body frame are the same
+			measurement = ev_sample.vel - vel_offset_body;
+			measurement_var = ev_sample.velocity_var;
+			break;
+		}
 
 	default:
 		continuing_conditions_passing = false;
@@ -111,48 +113,56 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	// increase minimum variance if GPS active (position reference)
 	if (_control_status.flags.gps) {
 		for (int i = 0; i < 2; i++) {
-			vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise));
+			measurement_var(i) = math::max(measurement_var(i), sq(_params.gps_vel_noise));
 		}
 	}
 
 #endif // CONFIG_EKF2_GNSS
 
-	const Vector3f measurement{vel};
-
-	const Vector3f measurement_var{
-		math::max(vel_cov(0, 0), sq(_params.ev_vel_noise), sq(0.01f)),
-		math::max(vel_cov(1, 1), sq(_params.ev_vel_noise), sq(0.01f)),
-		math::max(vel_cov(2, 2), sq(_params.ev_vel_noise), sq(0.01f))
+	measurement_var = Vector3f{
+		math::max(measurement_var(0), minimum_variance),
+		math::max(measurement_var(1), minimum_variance),
+		math::max(measurement_var(2), minimum_variance)
 	};
+	continuing_conditions_passing &= measurement.isAllFinite() && measurement_var.isAllFinite();
+
+	if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
+		const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
+		const Vector3f measurement_ekf_frame = _R_to_earth * measurement;
+		const uint64_t t = aid_src.timestamp_sample;
+		updateAidSourceStatus(aid_src,
+				      ev_sample.time_us,					// sample timestamp
+				      measurement_ekf_frame,					// observation
+				      measurement_var_ekf_frame,				// observation variance
+				      _state.vel - measurement_ekf_frame,			// innovation
+				      getVelocityVariance() + measurement_var_ekf_frame,	// innovation variance
+				      math::max(_params.ev_vel_innov_gate, 1.f));		// innovation gate
+		aid_src.timestamp_sample = t;
+		measurement.copyTo(aid_src.observation);
+		measurement_var.copyTo(aid_src.observation_variance);
 
-	const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
-
-	updateAidSourceStatus(aid_src,
-				 ev_sample.time_us,                          // sample timestamp
-				 measurement,                                // observation
-				 measurement_var,                            // observation variance
-				 _state.vel - measurement,                   // innovation
-				 getVelocityVariance() + measurement_var,    // innovation variance
-				 math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate
-
-	if (!measurement_valid) {
-		continuing_conditions_passing = false;
+	} else {
+		updateAidSourceStatus(aid_src,
+				      ev_sample.time_us,				// sample timestamp
+				      measurement,					// observation
+				      measurement_var,					// observation variance
+				      _state.vel - measurement,				// innovation
+				      getVelocityVariance() + measurement_var,		// innovation variance
+				      math::max(_params.ev_vel_innov_gate, 1.f));	// innovation gate
 	}
 
+
 	const bool starting_conditions_passing = common_starting_conditions_passing
 			&& continuing_conditions_passing
 			&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());
 
 	if (_control_status.flags.ev_vel) {
-
 		if (continuing_conditions_passing) {
-
 			if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) {
-
 				if (quality_sufficient) {
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					_information_events.flags.reset_vel_to_vision = true;
-					resetVelocityTo(measurement, measurement_var);
+					resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
 					resetAidSourceStatusZeroInnovation(aid_src);
 
 				} else {
@@ -163,7 +173,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 				}
 
 			} else if (quality_sufficient) {
-				fuseVelocity(aid_src);
+				fuseEvVelocity(aid_src, ev_sample);
 
 			} else {
 				aid_src.innovation_rejected = true;
@@ -177,24 +187,27 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 					// Data seems good, attempt a reset
 					_information_events.flags.reset_vel_to_vision = true;
 					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
-					resetVelocityTo(measurement, measurement_var);
+					resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
 					resetAidSourceStatusZeroInnovation(aid_src);
 
 					if (_control_status.flags.in_air) {
 						_nb_ev_vel_reset_available--;
 					}
 
-				} else if (starting_conditions_passing) {
-					// Data seems good, but previous reset did not fix the issue
-					// something else must be wrong, declare the sensor faulty and stop the fusion
-					//_control_status.flags.ev_vel_fault = true;
-					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
-					stopEvVelFusion();
-
 				} else {
-					// A reset did not fix the issue but all the starting checks are not passing
-					// This could be a temporary issue, stop the fusion without declaring the sensor faulty
-					ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
+					// differ warning message based on whether the starting conditions are passing
+					if (starting_conditions_passing) {
+						// Data seems good, but previous reset did not fix the issue
+						// something else must be wrong, declare the sensor faulty and stop the fusion
+						//_control_status.flags.ev_vel_fault = true;
+						ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
+
+					} else {
+						// A reset did not fix the issue but all the starting checks are not passing
+						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+						ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
+					}
+
 					stopEvVelFusion();
 				}
 			}
@@ -211,15 +224,13 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 			if (!isHorizontalAidingActive() || yaw_alignment_changed) {
 				ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME,
 					 (double)measurement(0), (double)measurement(1), (double)measurement(2));
-
 				_information_events.flags.reset_vel_to_vision = true;
-
-				resetVelocityTo(measurement, measurement_var);
+				resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
 				resetAidSourceStatusZeroInnovation(aid_src);
 
 				_control_status.flags.ev_vel = true;
 
-			} else if (fuseVelocity(aid_src)) {
+			} else if (fuseEvVelocity(aid_src, ev_sample)) {
 				ECL_INFO("starting %s fusion", AID_SRC_NAME);
 				_control_status.flags.ev_vel = true;
 			}
@@ -232,10 +243,87 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	}
 }
 
+bool Ekf::fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample)
+{
+	if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
+
+		VectorState H;
+		estimator_aid_source1d_s current_aid_src;
+		const auto state_vector = _state.vector();
+
+		for (uint8_t index = 0; index <= 2; index++) {
+			current_aid_src.timestamp_sample = aid_src.timestamp_sample;
+
+			if (index == 0) {
+				sym::ComputeEvBodyVelHx(state_vector, &H);
+
+			} else if (index == 1) {
+				sym::ComputeEvBodyVelHy(state_vector, &H);
+
+			} else {
+				sym::ComputeEvBodyVelHz(state_vector, &H);
+			}
+
+			const float innov_var = (H.T() * P * H)(0, 0) + aid_src.observation_variance[index];
+			const float innov = (_R_to_earth.transpose() * _state.vel - Vector3f(aid_src.observation))(index, 0);
+
+			updateAidSourceStatus(current_aid_src,
+					      ev_sample.time_us,				// sample timestamp
+					      aid_src.observation[index],			// observation
+					      aid_src.observation_variance[index],		// observation variance
+					      innov,						// innovation
+					      innov_var,    					// innovation variance
+					      math::max(_params.ev_vel_innov_gate, 1.f));	// innovation gate
+
+			if (!current_aid_src.innovation_rejected) {
+				fuseBodyVelocity(current_aid_src, current_aid_src.innovation_variance, H);
+
+			}
+
+			aid_src.innovation[index] = current_aid_src.innovation;
+			aid_src.innovation_variance[index] = current_aid_src.innovation_variance;
+			aid_src.test_ratio[index] = current_aid_src.test_ratio;
+			aid_src.fused = current_aid_src.fused;
+			aid_src.innovation_rejected |= current_aid_src.innovation_rejected;
+
+			if (aid_src.fused) {
+				aid_src.time_last_fuse = _time_delayed_us;
+			}
+
+		}
+
+		aid_src.timestamp_sample = current_aid_src.timestamp_sample;
+		return !aid_src.innovation_rejected;
+
+	} else {
+		return fuseVelocity(aid_src);
+	}
+}
+
 void Ekf::stopEvVelFusion()
 {
 	if (_control_status.flags.ev_vel) {
 
 		_control_status.flags.ev_vel = false;
 	}
 }
+
+void Ekf::resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var,
+			    const VelocityFrame &vel_frame)
+{
+	if (vel_frame == VelocityFrame::BODY_FRAME_FRD) {
+		const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
+		resetVelocityTo(_R_to_earth * measurement, measurement_var_ekf_frame);
+
+	} else {
+		resetVelocityTo(measurement, measurement_var);
+	}
+
+}
+
+Vector3f Ekf::rotateVarianceToEkf(const Vector3f &measurement_var)
+{
+	// rotate the covariance matrix into the EKF frame
+	const matrix::SquareMatrix<float, 3> R_cov = _R_to_earth * matrix::diag(measurement_var) * _R_to_earth.transpose();
+	return R_cov.diag();
+}

src/modules/ekf2/EKF/ekf.h

@@ -921,13 +921,21 @@ class Ekf final : public EstimatorInterface
 	void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
 	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
 	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
+	void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame);
+	Vector3f rotateVarianceToEkf(const Vector3f &measurement_var);
 
 	void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
 	void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);
 	void stopEvPosFusion();
 	void stopEvHgtFusion();
 	void stopEvVelFusion();
 	void stopEvYawFusion();
+	bool fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample);
+	void fuseBodyVelocity(estimator_aid_source1d_s &aid_src, float &innov_var, VectorState &H)
+	{
+		VectorState Kfusion = P * H / innov_var;
+		aid_src.fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation);
+	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_GNSS)

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -362,6 +362,40 @@ def compute_sideslip_h_and_k(
 
     return (H.T, K)
 
+def predict_vel_body(
+        state: VState
+) -> (sf.V3):
+    vel = state["vel"]
+    R_to_body = state["quat_nominal"].inverse()
+    return R_to_body * vel
+
+def compute_ev_body_vel_hx(
+        state: VState,
+) -> (VTangent):
+
+    state = vstate_to_state(state)
+    meas_pred = predict_vel_body(state)
+    Hx = jacobian_chain_rule(meas_pred[0], state)
+    return (Hx.T)
+
+def compute_ev_body_vel_hy(
+        state: VState,
+) -> (VTangent):
+
+    state = vstate_to_state(state)
+    meas_pred = predict_vel_body(state)[1]
+    Hy = jacobian_chain_rule(meas_pred, state)
+    return (Hy.T)
+
+def compute_ev_body_vel_hz(
+        state: VState,
+) -> (VTangent):
+
+    state = vstate_to_state(state)
+    meas_pred = predict_vel_body(state)[2]
+    Hz = jacobian_chain_rule(meas_pred, state)
+    return (Hz.T)
+
 def predict_mag_body(state) -> sf.V3:
     mag_field_earth = state["mag_I"]
     mag_bias_body = state["mag_B"]
@@ -697,5 +731,8 @@ def compute_gravity_z_innov_var_and_h(
 generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"])
 generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"])
 generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"])
+generate_px4_function(compute_ev_body_vel_hx, output_names=["H"])
+generate_px4_function(compute_ev_body_vel_hy, output_names=["H"])
+generate_px4_function(compute_ev_body_vel_hz, output_names=["H"])
 
 generate_px4_state(State, tangent_idx)