Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix sign error in magnetic heading innovation calculation

Browse Source
This commit is contained in:
Paul Riseborough 2016-02-20 08:37:29 +11:00 committed by Andrew Tridgell
parent 7e4ae39b8e
commit 4aefe1caee
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -848,17 +848,17 @@ void NavEKF2_core::FuseDeclination()
float NavEKF2_core::calcMagHeadingInnov() float NavEKF2_core::calcMagHeadingInnov()
{ {
// rotate measured body components into earth axis and compare to declination to give a heading measurement // rotate measured body components into earth axis and compare to declination to give a heading measurement
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
Matrix3f Tbn_temp; Matrix3f Tbn_temp;
stateStruct.quat.rotation_matrix(Tbn_temp); Tbn_temp.from_euler(eulerAngles.x, eulerAngles.y, 0.0f);
Vector3f magMeasNED = Tbn_temp*magDataDelayed.mag; Vector3f magMeasNED = Tbn_temp*magDataDelayed.mag;
float measHdg = atan2f(magMeasNED.y,magMeasNED.x) - _ahrs->get_compass()->get_declination(); float measHdg = - atan2f(magMeasNED.y,magMeasNED.x) + _ahrs->get_compass()->get_declination();
// wrap the heading so it sits on the range from +-pi // wrap the heading so it sits on the range from +-pi
measHdg = wrap_PI(measHdg); measHdg = wrap_PI(measHdg);
// calculate the innovation and wrap between +-pi // calculate the innovation and wrap between +-pi
Vector3f eulerAngles;
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
float innovation = wrap_PI(eulerAngles.z - measHdg); float innovation = wrap_PI(eulerAngles.z - measHdg);
// Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift // Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift