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

EKF2: vision attitude error filter (#21791)

Browse Source 
* ekf2-ev: filter q_error for frame correction
* ekf2: filter EV attitude error centrally
This commit is contained in:
Mathieu Bresciani
2023-07-03 11:13:59 -04:00
committed by GitHub
parent 24665f10f2
commit 288e3ae74a
5 changed files with 24 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ev_height_control.cpp
+1 -2
View File
@@ -56,10 +56,9 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
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
if (!(_control_status.flags.ev_yaw && _control_status.flags.ev_pos)) {
const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
const Quatf q_error(_ev_q_error_filt.getState());
if (q_error.isAllFinite()) {
const Dcmf R_ev_to_ekf(q_error);