From 774b6ed3b8f7992f4a959b597e98213237a79196 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 22 May 2024 10:44:55 +0200 Subject: [PATCH] ekf2-mag: do not use yaw emergency estimator to reset mag states On slowly moving vehicles (e.g.: boats, rovers), the yaw estimator has worse convergence than the main EKF. Resetting the mag states using the yaw estimator as reference can lead to poor heading. Also, the EKF can recover really well from initially incorrect mag states. --- .../EKF/aid_sources/magnetometer/mag_control.cpp | 16 ---------------- 1 file changed, 16 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 0077736bb0..5d857100fa 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -349,23 +349,7 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) * Vector3f(_mag_strength_gps, 0, 0); // mag_B: reset -#if defined(CONFIG_EKF2_GNSS) - - if (isYawEmergencyEstimateAvailable()) { - - const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth); - const Dcmf R_to_body = R_to_earth.transpose(); - - // mag_B: reset using WMM and yaw estimator - _state.mag_B = mag - (R_to_body * mag_earth_pred); - - ECL_INFO("resetMagStates using yaw estimator"); - - } else if (!reset_heading && _control_status.flags.yaw_align) { -#else - if (!reset_heading && _control_status.flags.yaw_align) { -#endif // mag_B: reset using WMM const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); _state.mag_B = mag - (R_to_body * mag_earth_pred);