Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Improved heading and magnetic field state initialisation

This commit is contained in:
Paul Riseborough 2014-02-27 07:09:24 +11:00 committed by Andrew Tridgell
parent d83b382e59
commit 78a1cac560
1 changed files with 28 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -340,22 +340,38 @@ void NavEKF::InitialiseFilterDynamic(void)
// get initial time deltat between IMU measurements (sec) // get initial time deltat between IMU measurements (sec)
dtIMU = _ahrs->get_ins().get_delta_time(); dtIMU = _ahrs->get_ins().get_delta_time();
// Calculate initial filter quaternion states from AHRS solution // calculate initial yaw angle
float yaw;
Matrix3f Tbn;
Vector3f initMagNED;
// calculate rotation matrix from body to NED frame
Tbn.from_euler(_ahrs->roll, _ahrs->pitch, 0.0f);
// body magnetic field vector with offsets removed
Vector3f initMagXYZ;
initMagXYZ = _ahrs->get_compass()->get_field() * 0.001f; // convert from Gauss to mGauss
// rotate the magnetic field into NED axes
initMagNED = Tbn*initMagXYZ;
// calculate heading of mag field rel to body heading
float magHeading = atan2f(initMagNED.y, initMagNED.x);
// get the magnetic declination
float magDecAng = _ahrs->get_compass()->get_declination();
// calculate yaw angle rel to true north
yaw = magDecAng - magHeading;
// Calculate initial filter quaternion states
Quaternion initQuat; Quaternion initQuat;
initQuat.from_rotation_matrix(_ahrs->get_dcm_matrix()); initQuat.from_euler(_ahrs->roll, _ahrs->pitch, yaw);
// 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
Matrix3f initTbn; initQuat.rotation_matrix(Tbn);
initQuat.rotation_matrix(initTbn); initMagNED = Tbn * initMagXYZ;
Vector3f initMagNED;
Vector3f initMagXYZ;
if (useCompass)
{
readMagData();
initMagXYZ = magData - magBias;
initMagNED = initTbn * initMagXYZ;
}
// write to state vector // write to state vector
state.quat = initQuat; state.quat = initQuat;