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;