dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: update _R_to_earth when the state quat changed through fusion

This commit is contained in:
bresch 2022-10-26 16:14:20 +02:00 committed by Daniel Agar
parent 28458340e6
commit 4dbdf23346
1 changed files with 2 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -856,6 +856,8 @@ void Ekf::fuse(const Vector24f &K, float innovation)
{
_state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation;
_state.quat_nominal.normalize();
_R_to_earth = Dcmf(_state.quat_nominal);
_state.vel -= K.slice<3, 1>(4, 0) * innovation;
_state.pos -= K.slice<3, 1>(7, 0) * innovation;
_state.delta_ang_bias -= K.slice<3, 1>(10, 0) * innovation;