Extracting Yaw from a Quaternion

I have a rotation quaternion and want to extract the angle of rotation about the Up axis (the yaw). I am using XNA and as far as I can tell there is no inbuilt function for this. What is the best way to do t开发者_运维百科his?

Thanks for any help, Venatu

The quaternion representation of rotation is a variation on axis and angle. So if you rotate by r radians around axis x, y, z, then your quaternion q is:

q[0] = cos(r/2);
q[1] = sin(r/2)*x;
q[2] = sin(r/2)*y;
q[3] = sin(r/2)*z;

If you want to create a quaternion that only rotates around the y axis, you zero out the x and z axes and then re-normalize the quaternion:

q[1] = 0;
q[3] = 0;
double mag = sqrt(q[0]*q[0] + q[2]*q[2]);
q[0] /= mag;
q[2] /= mag;

If you want the resulting angle:

double ang = 2*acos(q[0]);

This assumes that the quaternion representation is stored: w,x,y,z. If both q[0] and q[2] are zero, or close to it, the resulting quaternion should just be {1,0,0,0}.

Having given a Quaternion q, you can calculate roll, pitch and yaw like this:

var yaw = atan2(2.0*(q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
var pitch = asin(-2.0*(q.x*q.z - q.w*q.y));
var roll = atan2(2.0*(q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);

This should fit for intrinsic tait-bryan rotation of xyz-order. For other rotation orders, extrinsic and proper-euler rotations other conversions have to be used.

Note: I've verified below code against Wikipedia's equations plus Pixhawk's documentation and it is correct.

If you are working with drones/aviation, below is the code (taken directly from DJI SDK). Here q0, q1, q2, q3 corresponds to w,x,y,z components of the quaternion respectively. Also note that yaw, pitch, roll may be referred to as heading, attitude and bank respectively in some literature.

float roll  = atan2(2.0 * (q.q3 * q.q2 + q.q0 * q.q1) , 1.0 - 2.0 * (q.q1 * q.q1 + q.q2 * q.q2));
float pitch = asin(2.0 * (q.q2 * q.q0 - q.q3 * q.q1));
float yaw   = atan2(2.0 * (q.q3 * q.q0 + q.q1 * q.q2) , - 1.0 + 2.0 * (q.q0 * q.q0 + q.q1 * q.q1));

If you need to calculate all 3 then you can avoid recalculating common terms by using following functions:

//Source: http://docs.ros.org/latest-lts/api/dji_sdk_lib/html/DJI__Flight_8cpp_source.html#l00152
EulerianAngle Flight::toEulerianAngle(QuaternionData data)
{
    EulerianAngle ans;

    double q2sqr = data.q2 * data.q2;
    double t0 = -2.0 * (q2sqr + data.q3 * data.q3) + 1.0;
    double t1 = +2.0 * (data.q1 * data.q2 + data.q0 * data.q3);
    double t2 = -2.0 * (data.q1 * data.q3 - data.q0 * data.q2);
    double t3 = +2.0 * (data.q2 * data.q3 + data.q0 * data.q1);
    double t4 = -2.0 * (data.q1 * data.q1 + q2sqr) + 1.0;

    t2 = t2 > 1.0 ? 1.0 : t2;
    t2 = t2 < -1.0 ? -1.0 : t2;

    ans.pitch = asin(t2);
    ans.roll = atan2(t3, t4);
    ans.yaw = atan2(t1, t0);

    return ans;
}

QuaternionData Flight::toQuaternion(EulerianAngle data)
{
    QuaternionData ans;
    double t0 = cos(data.yaw * 0.5);
    double t1 = sin(data.yaw * 0.5);
    double t2 = cos(data.roll * 0.5);
    double t3 = sin(data.roll * 0.5);
    double t4 = cos(data.pitch * 0.5);
    double t5 = sin(data.pitch * 0.5);

    ans.q0 = t2 * t4 * t0 + t3 * t5 * t1;
    ans.q1 = t3 * t4 * t0 - t2 * t5 * t1;
    ans.q2 = t2 * t5 * t0 + t3 * t4 * t1;
    ans.q3 = t2 * t4 * t1 - t3 * t5 * t0;
    return ans;
}

Note on Eigen Library

If you are using Eigen library, it has another way to do this conversion, however, this may not be as optimized as above direct code:

  Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
  yaw = euler[0]; pitch = euler[1]; roll = euler[2];

Conversion Quaternion to Euler

I hope you know that yaw, pitch and roll are not good for arbitrary rotations. Euler angles suffer from singularities (see the above link) and instability. Look at 38:25 of the presentation of David Sachs

http://www.youtube.com/watch?v=C7JQ7Rpwn2k

Good luck!

A quaternion consists of two components: a 3d vector component and a scalar component.

The vector component of the quaternion describes independent rotations about each axis, so zero'ing out the x- and y-components of the vector component and leaving z-component as-is is all you need to do in order to solve for the vector term:

// Don't modify qz
double qx = 0;
double qy = 0;  

The scalar term represents the magnitude of rotation. For a unit quaternion (such as one used to represent attitude), the entire quaternion must have a magnitude of 1. Thus, the scalar term can be solved by:

double qw = sqrt(1 - qx*qx - qy*qy - qz*qz);

Since qx and qy are zero, the scalar component is given by

double qw = sqrt(1 - qz*qz); 

Thus, the full quaternion representing yaw is given by

double qx = 0;
double qy = 0;
// Don't modify qz
double qw = sqrt(1 - qz*qz);

The transformation from quaternion to yaw, pitch, and roll depends on the conventions used to define the quaternion and the yaw, pitch, and roll. For a given convention there are many "almost correct" transformations that will work for the majority of angles but only one truly correct transformation that will work for all angles including south and north poles where the "almost correct" transformations produce gimbal locks (spurious flips and rotations).

See this tutorial for more information:

https://youtu.be/k5i-vE5rZR0