From 047e9fabafa8a1869456ad27983c3acd744a50f1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 19 Feb 2016 17:17:21 +1100 Subject: [PATCH] AP_NavEKF2: Fix bug in simple heading fusion The innovation calculation should have been updated when the heading fusion maths was updated. We now use a direct heading or yaw angle measurement in the derivation, not the difference between observed and published declination. --- libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 3580a3dc20..7bc3ed32f4 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -847,21 +847,19 @@ void NavEKF2_core::FuseDeclination() // Calculate magnetic heading innovation float NavEKF2_core::calcMagHeadingInnov() { - // rotate predicted earth components into body axes and calculate - // predicted measurements + // rotate measured body components into earth axis and compare to declination to give a heading measurement Matrix3f Tbn_temp; stateStruct.quat.rotation_matrix(Tbn_temp); Vector3f magMeasNED = Tbn_temp*magDataDelayed.mag; + float measHdg = atan2f(magMeasNED.y,magMeasNED.x) - _ahrs->get_compass()->get_declination(); - // calculate the innovation where the predicted measurement is the angle wrt magnetic north of the horizontal component of the measured field - float innovation = atan2f(magMeasNED.y,magMeasNED.x) - _ahrs->get_compass()->get_declination(); + // wrap the heading so it sits on the range from +-pi + measHdg = wrap_PI(measHdg); - // wrap the innovation so it sits on the range from +-pi - if (innovation > M_PI_F) { - innovation = innovation - 2*M_PI_F; - } else if (innovation < -M_PI_F) { - innovation = innovation + 2*M_PI_F; - } + // calculate the innovation and wrap between +-pi + Vector3f eulerAngles; + stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); + float innovation = wrap_PI(eulerAngles.z - measHdg); // Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift if (innovation - lastInnovation > M_PI_F) {