From 44c50ab2dfa6b362bb52546109797a0272d61b9a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 9 Nov 2017 22:22:08 +1100 Subject: [PATCH] EKF: Correct magnetic yaw measurement using learned mag biases Don't apply bias corrections when biases are being learned to avoid possible circular data dependency. --- EKF/mag_fusion.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 1c0740b05a..632bda0c67 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -469,7 +469,12 @@ void Ekf::fuseHeading() Dcmf R_to_earth(euler321); // rotate the magnetometer measurements into earth frame using a zero yaw angle - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + if (_control_status.flags.mag_3D) { + // don't apply bias corrections if we are learning them + mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + } else { + mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); + } // the angle of the projection onto the horizontal gives the yaw angle measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; @@ -556,7 +561,12 @@ void Ekf::fuseHeading() R_to_earth(2,2) = cp*cr; // rotate the magnetometer measurements into earth frame using a zero yaw angle - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + if (_control_status.flags.mag_3D) { + // don't apply bias corrections if we are learning them + mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + } else { + mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B); + } // the angle of the projection onto the horizontal gives the yaw angle measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;