mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-12 15:40:35 +08:00
format
This commit is contained in:
@@ -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();
|
||||
|
||||
Reference in New Issue
Block a user