fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:18:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2-mag: do not use yaw emergency estimator to reset mag states

Browse Source 
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.
This commit is contained in:
bresch
2024-05-31 10:38:17 -04:00
committed by Daniel Agar
parent c3d984703c
commit 774b6ed3b8
1 changed files with 0 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
-16
View File
@@ -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);