EKF2: EV vel/pos only use EV q if enabled and valid

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

@@ -92,7 +92,8 @@ void Ekf::updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset)
 {
 	const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
 
-	if (!q_error.isAllFinite()) {
+	if (!ev_sample.quat.isAllFinite() || !q_error.isAllFinite()) {
+		_ev_q_error_initialized = false;
 		return;
 	}
 

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

@@ -59,10 +59,12 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 	// rotate EV to the EKF reference frame unless we're operating entirely in vision frame
 	if (!(_control_status.flags.ev_yaw && _control_status.flags.ev_pos)) {
 
-		const Quatf q_error(_ev_q_error_filt.getState());
+		const bool ev_q_available = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::ORIENTATION))
+					    && ev_sample.quat.isAllFinite();
 
-		if (q_error.isAllFinite()) {
-			const Dcmf R_ev_to_ekf(q_error);
+		if (ev_q_available && _ev_q_error_filt.getState().isAllFinite()) {
+			// rotate EV to the EKF reference frame
+			const Dcmf R_ev_to_ekf(_ev_q_error_filt.getState());
 
 			pos = R_ev_to_ekf * ev_sample.pos;
 			pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose();

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

@@ -45,15 +45,19 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 			     const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
 			     estimator_aid_source2d_s &aid_src)
 {
-	const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
-					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
-
 	// determine if we should use EV position aiding
 	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
 					     && _control_status.flags.tilt_align
 					     && PX4_ISFINITE(ev_sample.pos(0))
 					     && PX4_ISFINITE(ev_sample.pos(1));
 
+	const bool ev_q_available = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::ORIENTATION))
+				    && ev_sample.quat.isAllFinite();
+
+	const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
+					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
+
+
 	// correct position for offset relative to IMU
 	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
 	const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
@@ -86,17 +90,9 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 		break;
 
 	case PositionFrame::LOCAL_FRAME_FRD:
-		if (_control_status.flags.ev_yaw) {
-			// using EV frame
-			pos = ev_sample.pos - pos_offset_earth;
-			pos_cov = matrix::diag(ev_sample.position_var);
-
-			_ev_pos_b_est.setFusionInactive();
-			_ev_pos_b_est.reset();
-
-		} else {
+		if (ev_q_available && !_control_status.flags.ev_yaw) {
 			// rotate EV to the EKF reference frame
-			const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
+			const Dcmf R_ev_to_ekf(_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();
@@ -115,6 +111,17 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 			} else {
 				_ev_pos_b_est.setFusionInactive();
 			}
+
+		} else if (!_control_status.flags.yaw_align) {
+			// using EV frame
+			pos = ev_sample.pos - pos_offset_earth;
+			pos_cov = matrix::diag(ev_sample.position_var);
+
+			_ev_pos_b_est.setFusionInactive();
+			_ev_pos_b_est.reset();
+
+		} else {
+			continuing_conditions_passing = false;
 		}
 
 		break;

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

@@ -47,14 +47,18 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample
 {
 	static constexpr const char *AID_SRC_NAME = "EV velocity";
 
-	const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
-					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
-
 	// determine if we should use EV velocity aiding
 	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
 					     && _control_status.flags.tilt_align
 					     && ev_sample.vel.isAllFinite();
 
+	const bool ev_q_available = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::ORIENTATION))
+				    && ev_sample.quat.isAllFinite();
+
+	const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
+					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
+
+
 	// correct velocity for offset relative to IMU
 	const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
 	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
@@ -80,19 +84,22 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample
 		break;
 
 	case VelocityFrame::LOCAL_FRAME_FRD:
-		if (_control_status.flags.ev_yaw) {
-			// using EV frame
-			measurement = ev_sample.vel - vel_offset_earth;
-			measurement_var = ev_sample.velocity_var;
-
-		} else {
+		if (ev_q_available && !_control_status.flags.ev_yaw) {
 			// rotate EV to the EKF reference frame
-			const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
+			const Dcmf R_ev_to_ekf(_ev_q_error_filt.getState());
 
 			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());
+
+		} else if (!_control_status.flags.yaw_align) {
+			// using EV frame
+			measurement = ev_sample.vel - vel_offset_earth;
+			measurement_var = ev_sample.velocity_var;
+
+		} else {
+			continuing_conditions_passing = false;
 		}
 
 		break;

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

@@ -66,7 +66,7 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
 	}
 
 	// determine if we should use EV yaw aiding
-	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
+	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::ORIENTATION))
 					     && _control_status.flags.tilt_align
 					     && !_control_status.flags.ev_yaw_fault
 					     && PX4_ISFINITE(aid_src.observation)

src/modules/ekf2/EKF/common.h

@@ -155,10 +155,10 @@ enum class RngCtrl : uint8_t {
 };
 
 enum class EvCtrl : uint8_t {
-	HPOS = (1 << 0),
-	VPOS = (1 << 1),
-	VEL  = (1 << 2),
-	YAW  = (1 << 3)
+	HPOS        = (1 << 0),
+	VPOS        = (1 << 1),
+	VEL         = (1 << 2),
+	ORIENTATION = (1 << 3)
 };
 
 enum class MagCheckMask : uint8_t {

src/modules/ekf2/EKF2.cpp

@@ -300,7 +300,7 @@ void EKF2::AdvertiseTopics()
 			_estimator_aid_src_ev_vel_pub.advertise();
 		}
 
-		if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::YAW)) {
+		if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::ORIENTATION)) {
 			_estimator_aid_src_ev_yaw_pub.advertise();
 		}
 

src/modules/ekf2/module.yaml

@@ -508,13 +508,13 @@ parameters:
       description:
         short: External vision (EV) sensor aiding
         long: 'Set bits in the following positions to enable: 0 : Horizontal position
-          fusion 1 : Vertical position fusion 2 : 3D velocity fusion 3 : Yaw'
+          fusion 1 : Vertical position fusion 2 : 3D velocity fusion 3 : Orientation'
       type: bitmask
       bit:
         0: Horizontal position
         1: Vertical position
         2: 3D velocity
-        3: Yaw
+        3: Orientation
       default: 0
       min: 0
       max: 15

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -187,12 +187,12 @@ bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
 
 void EkfWrapper::enableExternalVisionHeadingFusion()
 {
-	_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::YAW);
+	_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::ORIENTATION);
 }
 
 void EkfWrapper::disableExternalVisionHeadingFusion()
 {
-	_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::YAW);
+	_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::ORIENTATION);
 }
 
 bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const

src/modules/ekf2/test/test_EKF_accelerometer.cpp

@@ -207,6 +207,7 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionBaroEvVel)
 {
 	// GIVEN: baro and EV vel fusion
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
+	_ekf_wrapper.enableExternalVisionHeadingFusion();
 	_sensor_simulator.startExternalVision();
 	_sensor_simulator.runSeconds(1);
 
@@ -227,6 +228,7 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionEvVelHgt)
 {
 	// GIVEN: EV height and vel fusion
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
+	_ekf_wrapper.enableExternalVisionHeadingFusion();
 	_ekf_wrapper.enableExternalVisionHeightFusion();
 	_sensor_simulator.startExternalVision();
 	_ekf_wrapper.disableBaroHeightFusion();

src/modules/ekf2/test/test_EKF_externalVision.cpp

@@ -89,6 +89,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
 	EXPECT_FALSE(_ekf->global_position_is_valid());
 
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
+	_sensor_simulator._vio.setVelocityFrameToLocalNED();
 	_sensor_simulator.runSeconds(2);
 
 	EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
@@ -119,6 +120,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityReset)
 
 	_sensor_simulator._vio.setVelocity(simulated_velocity);
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
+	_sensor_simulator._vio.setVelocityFrameToLocalNED();
 	_sensor_simulator.startExternalVision();
 	// Note: test duration needs to allow time for tilt alignment to complete
 	_ekf->set_vehicle_at_rest(false);
@@ -152,8 +154,11 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
 	const Vector3f simulated_velocity_in_ekf_frame =
 		Dcmf(vision_to_ekf) * simulated_velocity_in_vision_frame;
 	_sensor_simulator._vio.setVelocity(simulated_velocity_in_vision_frame);
+	_sensor_simulator._vio.setVelocityFrameToLocalFRD();
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
+	_ekf_wrapper.enableGpsFusion();
 	_sensor_simulator.startExternalVision();
+	_sensor_simulator.startGps();
 	_ekf->set_vehicle_at_rest(false);
 	_sensor_simulator.runMicroseconds(2e5);
 
@@ -203,6 +208,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
 		Dcmf(vision_to_ekf) * simulated_position_in_vision_frame;
 	_sensor_simulator._vio.setPosition(simulated_position_in_vision_frame);
 	_ekf_wrapper.enableExternalVisionPositionFusion();
+	_ekf_wrapper.setMagFuseTypeNone();
 	_sensor_simulator.startExternalVision();
 	_sensor_simulator.runMicroseconds(2e5);
 
@@ -240,12 +246,15 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
 	// the y EKF frame axis
 	_sensor_simulator._vio.setVelocityVariance(Vector3f{2.0f, 0.01f, 0.01f});
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
+	_ekf_wrapper.enableExternalVisionHeadingFusion();
+	_ekf_wrapper.enableGpsFusion();
 	_sensor_simulator.startExternalVision();
+	_sensor_simulator.startGps();
 
 	const Vector3f velVar_init = _ekf->getVelocityVariance();
 	EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
 
-	_sensor_simulator.runSeconds(4);
+	_sensor_simulator.runSeconds(20);
 
 	// THEN: velocity uncertainty in y should be bigger
 	const Vector3f velVar_new = _ekf->getVelocityVariance();

src/modules/ekf2/test/test_EKF_fusionLogic.cpp

@@ -271,6 +271,7 @@ TEST_F(EkfFusionLogicTest, doVisionPositionFusion)
 {
 	// WHEN: allow vision position to be fused and we send vision data
 	_ekf_wrapper.enableExternalVisionPositionFusion();
+	_ekf_wrapper.setMagFuseTypeNone();
 	_sensor_simulator.startExternalVision();
 	_sensor_simulator.runSeconds(4);
 
@@ -299,6 +300,7 @@ TEST_F(EkfFusionLogicTest, doVisionVelocityFusion)
 {
 	// WHEN: allow vision position to be fused and we send vision data
 	_ekf_wrapper.enableExternalVisionVelocityFusion();
+	_ekf_wrapper.setMagFuseTypeNone();
 	_sensor_simulator.startExternalVision();
 	_sensor_simulator.runSeconds(4);