UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-27 00:48:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(ekf2): correct EV orientation variance inflation and NE aiding check (#26622)

Browse Source 
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 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
This commit is contained in:
Jacob Dahl
2026-04-19 19:28:41 -08:00
committed by GitHub
parent e7e1a7ec88
commit 52b2546046
4 changed files with 15 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp
View File

@@ -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
}
}

12
src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp
View File

@@ -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();

6
src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h
View File

@@ -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();

3
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@@ -740,7 +740,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)