diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp index 68022677ba..dc56dbc4f0 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp +++ b/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; } diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index 440c0fe7ac..b16d71682b 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/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(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(); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp index 590a289da3..fe2872579a 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp +++ b/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(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(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; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp index b94f24f45e..a4322782b1 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp +++ b/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(EvCtrl::VEL)) && _control_status.flags.tilt_align && ev_sample.vel.isAllFinite(); + const bool ev_q_available = (_params.ev_ctrl & static_cast(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; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp index ad15c4f6bb..5edb7d2f4f 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp +++ b/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(EvCtrl::YAW)) + bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::ORIENTATION)) && _control_status.flags.tilt_align && !_control_status.flags.ev_yaw_fault && PX4_ISFINITE(aid_src.observation) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index af03d2a8db..3057506cf6 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/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 { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index bceb4d3dcc..dffe5e031b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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(EvCtrl::YAW)) { + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::ORIENTATION)) { _estimator_aid_src_ev_yaw_pub.advertise(); } diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml index 8ed6e700db..6ecfb51bec 100644 --- a/src/modules/ekf2/module.yaml +++ b/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 diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 9fd62885e3..6dd5d3579a 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/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(EvCtrl::YAW); + _ekf_params->ev_ctrl |= static_cast(EvCtrl::ORIENTATION); } void EkfWrapper::disableExternalVisionHeadingFusion() { - _ekf_params->ev_ctrl &= ~static_cast(EvCtrl::YAW); + _ekf_params->ev_ctrl &= ~static_cast(EvCtrl::ORIENTATION); } bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const diff --git a/src/modules/ekf2/test/test_EKF_accelerometer.cpp b/src/modules/ekf2/test/test_EKF_accelerometer.cpp index 255f6597b3..11c8990243 100644 --- a/src/modules/ekf2/test/test_EKF_accelerometer.cpp +++ b/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(); diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index 5b1439edd9..98999c89de 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/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(); diff --git a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp index 9367b86da3..0a96fc3db7 100644 --- a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp +++ b/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);