Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -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();
// rotate the magnetic field into NED axes
initMagNED = Tbn*magData;
initMagNED = Tbn*(magData - magBias);
// calculate heading of mag field rel to body heading
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
// to set initial NED magnetic field states
initQuat.rotation_matrix(Tbn);
initMagNED = Tbn * magData;
initMagNED = Tbn * (magData - magBias);
// write to earth magnetic field state vector
state.earth_magfield = initMagNED;