fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 06:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

mag_ctrl: do not fuse synthetic mag but do not zero the innovation

Browse Source
This commit is contained in:
bresch
2024-05-17 11:19:04 +02:00
committed by Mathieu Bresciani
parent bb5dfc7d51
commit d796009302
1 changed files with 0 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
-5
View File
@@ -121,11 +121,6 @@ void Ekf::controlMagFusion()
VectorState H; VectorState H;
sym::ComputeMagInnovInnovVarAndHx(_state.vector(), P, mag_sample.mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H); sym::ComputeMagInnovInnovVarAndHx(_state.vector(), P, mag_sample.mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
if (_control_status.flags.synthetic_mag_z) {
mag_innov(2) = 0.0f;
}
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
aid_src.observation[i] = mag_sample.mag(i); aid_src.observation[i] = mag_sample.mag(i);
aid_src.observation_variance[i] = R_MAG; aid_src.observation_variance[i] = R_MAG;