AP_NavEKF: initialise quaternion from rotation matrix

This commit is contained in:
Andrew Tridgell 2014-02-15 12:21:11 +11:00
parent 2df314799e
commit 24e1070eb2
1 changed files with 1 additions and 1 deletions

View File

@ -327,7 +327,7 @@ void NavEKF::InitialiseFilterDynamic(void)
// Calculate initial filter quaternion states from AHRS solution
Quaternion initQuat;
initQuat.from_euler(_ahrs->roll, _ahrs->pitch, _ahrs->yaw);
initQuat.from_rotation_matrix(_ahrs->get_dcm_matrix());
// Calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states