From 40349fa3dc72b626504e256ed136f7b3f71f4f17 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 10 Jul 2024 13:20:16 -0400 Subject: [PATCH] ekf2: EV velocity control should own vertical velocity reset if height failing --- .../external_vision/ev_height_control.cpp | 28 ------------------- .../external_vision/ev_vel_control.cpp | 11 ++++++++ 2 files changed, 11 insertions(+), 28 deletions(-) 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 0684536ae5..e7b6f13f11 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 @@ -149,34 +149,6 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var); bias_est.setBias(-_state.pos(2) + measurement); - // reset vertical velocity - if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast(EvCtrl::VEL))) { - - // 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 vel_offset_body = angular_velocity % pos_offset_body; - const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - - switch (ev_sample.vel_frame) { - case VelocityFrame::LOCAL_FRAME_NED: - case VelocityFrame::LOCAL_FRAME_FRD: { - const Vector3f reset_vel = ev_sample.vel - vel_offset_earth; - resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise))); - } - break; - - case VelocityFrame::BODY_FRAME_FRD: { - const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body); - const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose(); - resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise))); - } - break; - } - - } else { - resetVerticalVelocityToZero(); - } - aid_src.time_last_fuse = _time_delayed_us; } else if (is_fusion_failing) { 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 a5ba562cad..b94f24f45e 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 @@ -212,6 +212,17 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample stopEvVelFusion(); } + + } else if (isHeightResetRequired()) { + // reset vertical velocity if height is failing + if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) { + const Vector3f measurement_ekf_frame = _R_to_earth * measurement; + const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var); + resetVerticalVelocityTo(measurement_ekf_frame(2), measurement_var_ekf_frame(2)); + + } else { + resetVerticalVelocityTo(measurement(2), measurement_var(2)); + } } } else {