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: turn around update_all_states condition

Browse Source 
Non-functional
This commit is contained in:
bresch 2024-05-22 10:17:42 +02:00 committed by Daniel Agar
parent c11c75d32e
commit a6007e4b93
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
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];
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
// 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;
}
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])) {
fused[index] = true;
limitDeclination();