fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 02:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: EV overhaul yaw and position fusion (#20501)

Browse Source 
- move EV yaw and EV position to new state machines
 - EV yaw and EV pos now configured via EKF2_EV_CTRL (migrated from EKF2_AID_MASK)
 - new EV position offset estimator to enable EV position while GPS position is active (no more EV pos delta fusion)
 - yaw_align now strictly means north (no more rotate external vision aid mask)
 - automatic switching between EV yaw, and yaw align north based on GPS quality
This commit is contained in:
Daniel Agar
2022-12-20 10:23:56 -05:00
committed by GitHub
parent 20342216e2
commit 54a32eb2f7
34 changed files with 1019 additions and 553 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ev_height_control.cpp
+6 -4
View File
@@ -53,7 +53,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
// rotate measurement into correct earth frame if required
Vector3f pos{ev_sample.pos};
Matrix3f pos_cov{matrix::diag(ev_sample.posVar)};
Matrix3f pos_cov{matrix::diag(ev_sample.position_var)};
// rotate EV to the EKF reference frame unless we're operating entirely in vision frame
// TODO: only necessary if there's a roll/pitch offset between VIO and EKF
@@ -65,7 +65,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
const Dcmf R_ev_to_ekf(q_error);
pos = R_ev_to_ekf * ev_sample.pos;
pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.posVar) * R_ev_to_ekf.transpose();
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
@@ -98,9 +98,11 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
}
const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS)) && measurement_valid;
const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
&& measurement_valid;
const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing;
const bool starting_conditions_passing = common_starting_conditions_passing
&& continuing_conditions_passing;
if (_control_status.flags.ev_hgt) {
aid_src.fusion_enabled = true;