fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 17:03:35 +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_pos_control.cpp
+1 -2
View File
@@ -95,8 +95,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
} else {
// rotate EV to the EKF reference frame
const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
const Dcmf R_ev_to_ekf = Dcmf(q_error);
const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
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();