fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 07:37:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf: fix angle wrapping in realignYawGPS

Browse Source 
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
This commit is contained in:
bresch
2020-02-21 11:48:19 +01:00
committed by Mathieu Bresciani
parent 00872fcaa3
commit 8b918563a2
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+1 -1
View File
@@ -406,7 +406,7 @@ bool Ekf::realignYawGPS()
const float ekfCOG = atan2f(_state.vel(1), _state.vel(0)); const float ekfCOG = atan2f(_state.vel(1), _state.vel(0));
// Check the EKF and GPS course over ground for consistency // 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 // 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; const bool badYawErr = fabsf(courseYawError) > 0.5f;