From d7960093024a31db0c97730a4fa26bb974464e7e Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 16 May 2024 16:32:22 +0200 Subject: [PATCH] mag_ctrl: do not fuse synthetic mag but do not zero the innovation --- .../ekf2/EKF/aid_sources/magnetometer/mag_control.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 5bf9965040..90ed1f6704 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -121,11 +121,6 @@ void Ekf::controlMagFusion() VectorState 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++) { aid_src.observation[i] = mag_sample.mag(i); aid_src.observation_variance[i] = R_MAG;