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::YAW))
+					    && 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::YAW))
+				    && 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::YAW))
+				    && 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
+			vel = ev_sample.vel - vel_offset_earth;
+			vel_cov = matrix::diag(ev_sample.velocity_var);
+
+		} else {
+			continuing_conditions_passing = false;
 		}
 
 		break;

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,6 +154,7 @@ 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();
 	_sensor_simulator.startExternalVision();
 	_ekf->set_vehicle_at_rest(false);