dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/ekf_helper.cpp
View File

@ -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;