EKF2: vision attitude error filter (#21791)

* ekf2-ev: filter q_error for frame correction * ekf2: filter EV attitude error centrally
PX4 · Jul 3, 2023 · 288e3ae · 288e3ae
1 parent 24665f1
commit 288e3ae
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 6 deletions.
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
@@ -1003,6 +1003,7 @@ class Ekf final : public EstimatorInterface
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// control fusion of external vision observations
 	void controlExternalVisionFusion();
+	void updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset);
 	void controlEvHeightFusion(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 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);
@@ -1160,6 +1161,8 @@ class Ekf final : public EstimatorInterface
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
 	PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
+	AlphaFilter<AxisAnglef> _ev_q_error_filt{0.001f};
+	bool _ev_q_error_initialized{false};
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator

diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/ev_control.cpp
@@ -58,6 +58,7 @@ void Ekf::controlExternalVisionFusion()
 				&& ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
 				&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
 
+		updateEvAttitudeErrorFilter(ev_sample, ev_reset);
 		controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
 		controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
 		controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
@@ -84,3 +85,20 @@ void Ekf::controlExternalVisionFusion()
 		ECL_WARN("vision data stopped");
 	}
 }
+
+void Ekf::updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset)
+{
+	const AxisAnglef q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
+
+	if (!q_error.isAllFinite()) {
+		return;
+	}
+
+	if (!_ev_q_error_initialized || ev_reset) {
+		_ev_q_error_filt.reset(q_error);
+		_ev_q_error_initialized = true;
+
+	} else {
+		_ev_q_error_filt.update(q_error);
+	}
+}
diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp
@@ -56,10 +56,9 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 	Matrix3f pos_cov{matrix::diag(ev_sample.position_var)};
 
 	// rotate EV to the EKF reference frame unless we're operating entirely in vision frame
-	// TODO: only necessary if there's a roll/pitch offset between VIO and EKF
 	if (!(_control_status.flags.ev_yaw && _control_status.flags.ev_pos)) {
 
-		const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
+		const Quatf q_error(_ev_q_error_filt.getState());
 
 		if (q_error.isAllFinite()) {
 			const Dcmf R_ev_to_ekf(q_error);

diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp
@@ -95,8 +95,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 
 		} else {
 			// rotate EV to the EKF reference frame
-			const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
-			const Dcmf R_ev_to_ekf = Dcmf(q_error);
+			const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
 
 			pos = R_ev_to_ekf * ev_sample.pos - pos_offset_earth;
 			pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose();

diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp
@@ -80,8 +80,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 
 		} else {
 			// rotate EV to the EKF reference frame
-			const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
-			const Dcmf R_ev_to_ekf = Dcmf(q_error);
+			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();