Fusioning magnetometer into kalman filter