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

ekf2: mag_fusion remove direct state index usage

Browse Source
This commit is contained in:
Daniel Agar 2023-10-05 12:12:56 -04:00
parent cf4c565e4a
commit b5f3d089c4
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -177,13 +177,16 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index];
if (!update_all_states) {
for (unsigned row = 0; row <= 15; row++) {
Kfusion(row) = 0.f;
}
// zero non-mag Kalman gains if not updating all states
for (unsigned row = 22; row <= 23; row++) {
Kfusion(row) = 0.f;
}
// copy mag_I and mag_B Kalman gains
const Vector3f K_mag_I = Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0);
const Vector3f K_mag_B = Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0);
// zero all Kalman gains, then restore mag
Kfusion.setZero();
Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = K_mag_I;
Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
}
if (measurementUpdate(Kfusion, aid_src_mag.innovation_variance[index], aid_src_mag.innovation[index])) {