From 9c5f564dc5877944dcd0bc083592d14de0181f87 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sat, 29 Mar 2014 19:46:19 +1100
Subject: [PATCH] AP_NavEKF : Fixes bug in initial earth magnetic field states

The calculation for these states was not being bias corrected
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 7bf1b0462b..d637b40cea 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -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;