From bb5b3bb904841795596860400aff5d4072f39590 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 14 May 2024 12:23:04 -0400 Subject: [PATCH] EKF2: EV vel/pos only use EV q if enabled and valid --- .../external_vision/ev_control.cpp | 3 +- .../external_vision/ev_height_control.cpp | 8 +++-- .../external_vision/ev_pos_control.cpp | 33 +++++++++++-------- .../external_vision/ev_vel_control.cpp | 27 +++++++++------ .../ekf2/test/test_EKF_accelerometer.cpp | 2 ++ .../ekf2/test/test_EKF_externalVision.cpp | 3 ++ 6 files changed, 49 insertions(+), 27 deletions(-) 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 68022677ba64..dc56dbc4f0bc 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 440c0fe7acf6..32458393681e 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::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(); 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 590a289da394..b969fab8c0ee 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::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; 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 b94f24f45e9f..9c355da45212 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::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; diff --git a/src/modules/ekf2/test/test_EKF_accelerometer.cpp b/src/modules/ekf2/test/test_EKF_accelerometer.cpp index 255f6597b3f5..11c89902437f 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 5b1439edd9f2..47ebeb36fef5 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,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);