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

ekf2-mag: turn around update_all_states condition

Browse Source 
Non-functional
This commit is contained in:
bresch
2024-05-31 10:38:17 -04:00
committed by Daniel Agar
parent c11c75d32e
commit a6007e4b93
1 changed files with 7 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
+7 -6
View File
@@ -103,7 +103,13 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
VectorState Kfusion = P * H / aid_src.innovation_variance[index]; VectorState Kfusion = P * H / aid_src.innovation_variance[index];
if (!update_all_states) { if (update_all_states) {
if (!update_tilt) {
Kfusion(State::quat_nominal.idx + 0) = 0.f;
Kfusion(State::quat_nominal.idx + 1) = 0.f;
}
} else {
// zero non-mag Kalman gains if not updating all states // zero non-mag Kalman gains if not updating all states
// copy mag_I and mag_B Kalman gains // copy mag_I and mag_B Kalman gains
@@ -116,11 +122,6 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B; Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
} }
if (!update_tilt) {
Kfusion(State::quat_nominal.idx + 0) = 0.f;
Kfusion(State::quat_nominal.idx + 1) = 0.f;
}
if (measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) { if (measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) {
fused[index] = true; fused[index] = true;
limitDeclination(); limitDeclination();