This commit is contained in:
Jacob Dahl
2026-03-06 12:32:29 -09:00
parent 390c10eb55
commit 0b7cc75618
2 changed files with 3 additions and 3 deletions
@@ -69,7 +69,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
// 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
+ sq(ev_sample.pos(0)) * ev_sample.orientation_var(1); // pitch
}
}
@@ -103,9 +103,9 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
// 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
+ 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
+ sq(ev_sample.pos(2)) * ev_sample.orientation_var(0); // roll
if (_control_status.flags.gnss_pos) {
_ev_pos_b_est.setFusionActive();