From 8b918563a2abb242d84c48fef58623dd37f503a1 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 21 Feb 2020 11:48:19 +0100 Subject: [PATCH] ekf: fix angle wrapping in realignYawGPS To obtain the correct difference between two angles, we need to wrap the result between -pi and pi. Otherwise, the difference between two angles close to 180 degrees but with opposite signs will produce a large error. For example if a = -179 and b = 179, b - a = 258 instead of -2 degrees --- EKF/ekf_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 00ad25062c..34ab910cb1 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -406,7 +406,7 @@ bool Ekf::realignYawGPS() const float ekfCOG = atan2f(_state.vel(1), _state.vel(0)); // Check the EKF and GPS course over ground for consistency - const float courseYawError = gpsCOG - ekfCOG; + const float courseYawError = wrap_pi(gpsCOG - ekfCOG); // If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad const bool badYawErr = fabsf(courseYawError) > 0.5f;