From 7f121e81e4a996e744acdc10cd0b7eecdba45e8c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 19 Feb 2016 16:18:07 +1100 Subject: [PATCH] EKF: Update yaw innovation calculation to match revised derivation The new derivation does not use magnetic field measurements in the observation model and instead fuses in a heading measurement directly. --- EKF/mag_fusion.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 9434266921..5c581abd56 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -532,12 +532,15 @@ void Ekf::fuseHeading() H_YAW[1] = t14 * (t15 * (q0 * q1 * 2.0f - q2 * q3 * 2.0f) + t9 * t10 * (q0 * q2 * 2.0f + q1 * q3 * 2.0f)); H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8)); - // calculate innovation - // rotate body field magnetic field measurement into earth frame and compare to published declination to get heading measurement - // TODO - enable use of an off-board heading measurement + // rotate body field magnetic field measurement into earth frame and compare horizontal vector with published declination to get yaw measurement + // TODO - enable use of an off-board yaw measurement matrix::Dcm R_to_earth(_state.quat_nominal); matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; - float innovation = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination; + float measured_yaw = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination; + + // calculate the innovation + matrix::Euler euler(_state.quat_nominal); + float innovation = euler(2) - measured_yaw; // wrap the innovation to the interval between +-pi innovation = matrix::wrap_pi(innovation);