From 1a969aed079d726fe631f359578264d5de28b8ff Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Wed, 21 Nov 2018 15:02:42 +0100 Subject: [PATCH] EKF: use low pass filtered mag measurements for heading initialization --- EKF/ekf_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 3ddd7fdaba..0e84690115 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -591,7 +591,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { // rotate the magnetometer measurements into earth frame using a zero yaw angle - Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + Vector3f mag_earth_pred = R_to_earth * mag_init; // the angle of the projection onto the horizontal gives the yaw angle euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; @@ -649,7 +649,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) { // rotate the magnetometer measurements into earth frame using a zero yaw angle - Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + Vector3f mag_earth_pred = R_to_earth * mag_init; // the angle of the projection onto the horizontal gives the yaw angle euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;