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

AP_NavEKF : Fixes bug in initial earth magnetic field states

Browse Source 
The calculation for these states was not being bias corrected
This commit is contained in:
priseborough 2014-03-29 19:46:19 +11:00 committed by Andrew Tridgell
parent 7bff8e9312
commit 9c5f564dc5
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3010,7 +3010,7 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
readMagData(); readMagData();
// rotate the magnetic field into NED axes // rotate the magnetic field into NED axes
initMagNED = Tbn*magData; initMagNED = Tbn*(magData - magBias);
// calculate heading of mag field rel to body heading // calculate heading of mag field rel to body heading
float magHeading = atan2f(initMagNED.y, initMagNED.x); float magHeading = atan2f(initMagNED.y, initMagNED.x);
@ -3028,7 +3028,7 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
// calculate initial Tbn matrix and rotate Mag measurements into NED // calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states // to set initial NED magnetic field states
initQuat.rotation_matrix(Tbn); initQuat.rotation_matrix(Tbn);
initMagNED = Tbn * magData; initMagNED = Tbn * (magData - magBias);
// write to earth magnetic field state vector // write to earth magnetic field state vector
state.earth_magfield = initMagNED; state.earth_magfield = initMagNED;