From 0ad5329caff79720efa2891a87e8a08793678530 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 24 Feb 2016 08:20:29 +1100 Subject: [PATCH] EKF: Update comments in heading fusion to clarify calculation of magnetic heading --- EKF/mag_fusion.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index c31ad0cf58..857d1c019c 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -610,9 +610,11 @@ void Ekf::fuseHeading() // TODO - enable use of an off-board heading measurement - // rotate the magnetometer measurement into earth frame using an assumed zero yaw angle + // rotate the magnetometer measurement into earth frame matrix::Euler euler(_state.quat_nominal); float predicted_hdg = euler(2); // we will need the predicted heading to calculate the innovation + + // Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle euler(2) = 0.0f; matrix::Dcm R_to_earth(euler); matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;