From 390c10eb559ed0ffa2ec6725c4567ff9946eaed1 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 2 Mar 2026 14:17:00 -0900 Subject: [PATCH] fix(ekf2): correct EV orientation variance inflation and NE aiding check MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace dimensionally wrong max(pos/vel_var, orientation_var) floor with physically correct additive variance from the cross-product δp = δθ × p (and δv = δθ × v). The old code compared rad² directly to m² or (m/s)², ignoring lever arm / speed. Also include EV NED position (guarded by yaw_align) in isNorthEastAidingActive() so that mag fusion doesn't spuriously clear yaw_align when it stops during EV-only flight. Supersedes #25703 Co-Authored-By: Claude Opus 4.6 --- .../external_vision/ev_height_control.cpp | 7 +++---- .../aid_sources/external_vision/ev_pos_control.cpp | 12 +++++------- .../ekf2/EKF/aid_sources/external_vision/ev_vel.h | 6 +++++- src/modules/ekf2/EKF/estimator_interface.cpp | 3 ++- 4 files changed, 15 insertions(+), 13 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 5e26b059c8..3701aa2027 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 @@ -67,10 +67,9 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp 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(); - // increase minimum variance to include EV orientation variance - // TODO: do this properly - const float orientation_var_max = math::max(ev_sample.orientation_var(0), ev_sample.orientation_var(1)); - pos_cov(2, 2) = math::max(pos_cov(2, 2), orientation_var_max); + // Position variance contribution from orientation uncertainty: δp_z = δθ_roll·py - δθ_pitch·px + pos_cov(2, 2) += sq(ev_sample.pos(1)) * ev_sample.orientation_var(0) // roll + + sq(ev_sample.pos(0)) * ev_sample.orientation_var(1); // pitch } } 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 4720824e32..54bb899c85 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 @@ -101,13 +101,11 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample 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(); - // increase minimum variance to include EV orientation variance - // TODO: do this properly - const float orientation_var_max = ev_sample.orientation_var.max(); - - for (int i = 0; i < 2; i++) { - pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max); - } + // Position variance contribution from orientation uncertainty: δp = δθ × p + pos_cov(0, 0) += sq(ev_sample.pos(2)) * ev_sample.orientation_var(1) // pitch + + sq(ev_sample.pos(1)) * ev_sample.orientation_var(2); // yaw + pos_cov(1, 1) += sq(ev_sample.pos(0)) * ev_sample.orientation_var(2) // yaw + + sq(ev_sample.pos(2)) * ev_sample.orientation_var(0); // roll if (_control_status.flags.gnss_pos) { _ev_pos_b_est.setFusionActive(); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h index 10d6b64cf4..3f7cf93666 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h @@ -166,7 +166,11 @@ public: _measurement = rotation_ev_to_ekf * _sample.vel - velocity_offset_earth; _measurement_var = matrix::SquareMatrix3f(rotation_ev_to_ekf * matrix::diag( _sample.velocity_var) * rotation_ev_to_ekf.transpose()).diag(); - _min_variance = math::max(_min_variance, _sample.orientation_var.max()); + // Velocity variance contribution from orientation uncertainty: δv = δθ × v + const float vx = _sample.vel(0), vy = _sample.vel(1), vz = _sample.vel(2); + _measurement_var(0) += sq(vz) * _sample.orientation_var(1) + sq(vy) * _sample.orientation_var(2); + _measurement_var(1) += sq(vx) * _sample.orientation_var(2) + sq(vz) * _sample.orientation_var(0); + _measurement_var(2) += sq(vy) * _sample.orientation_var(0) + sq(vx) * _sample.orientation_var(1); } enforceMinimumVariance(); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index b6f71444e9..94f9c0ccfa 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -699,7 +699,8 @@ bool EstimatorInterface::isNorthEastAidingActive() const { return _control_status.flags.gnss_pos || _control_status.flags.gnss_vel - || _control_status.flags.aux_gpos; + || _control_status.flags.aux_gpos + || (_control_status.flags.ev_pos && _control_status.flags.yaw_align); } void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)