fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 16:53:33 +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-10 09:18:04 -04:00
parent cf4c565e4a
commit b5f3d089c4
1 changed files with 9 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/mag_fusion.cpp
+9 -6
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]; VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index];
if (!update_all_states) { if (!update_all_states) {
for (unsigned row = 0; row <= 15; row++) { // zero non-mag Kalman gains if not updating all states
Kfusion(row) = 0.f;
}
for (unsigned row = 22; row <= 23; row++) { // copy mag_I and mag_B Kalman gains
Kfusion(row) = 0.f; 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])) { if (measurementUpdate(Kfusion, aid_src_mag.innovation_variance[index], aid_src_mag.innovation[index])) {