Extracting Yaw from a Quaternion

Question

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 this?

Thanks for any help, Venatu

Evolute

Answer 1

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}.

Caper

Does this also work to 'only' extract pitch (or roll) from a Quaternion, e.g. by setting x and y to zero. If not, why not? – Taxaceous

@AlexanderPacha It doesn't quite work for extracting pitch or roll. The reason is that 'yaw' is typically defined as rotation around the world's 'up' axis. But pitch and roll are defined relative to object's internal axes. When a craft pitches, it does so around its own wings, regardless of what world axis the wings are aligned with. Since the quaternion is typically in world coordinates, it takes extra steps to convert things to/from the object local frame of reference. – Caper

Forgive my stupidity, but I can not get out of place here, I have the quaternion matrix wxyz, where does this value of R? – Osmond

This fails if r is negative – Hogtie

@Hogtie It should still work when r is negative. Rotating around x,y,z by -r is the same as rotating around -x,-y,-z by r. If you negate all elements of the quaternion, you have the same rotation. acos() Always returns a positive value. So you'll get 0 <= ang < 2*pi back. But if you want a negative angle (e.g., if 0 means forward and +/- angles are left/right turns) you just do if (ang > pi) ang -= 2*pi – Caper

My mistake, your quaternion order is different to the one I was assuming, and your answer is correct – Hogtie

N.B. If you are just interested in ang, then instead of computing 2*acos(q0/sqrt(q0*q0+q2*q2)), you can directly compute atan(q[2],q[0])*2 which is also already normalized to [-pi, pi]. – Princessprinceton

@Caper The topic is quite old but still. The definition of yaw is a rotation around the z axis in body frame and will it match with z axis in inertial frame or not depends on the EA rotation order (i.e. 321, 323, etc.) The solution you provided gives the azimuth - a projection of a heading vector to an inertial XY plane. – Madaras

Answer 2

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.

Susannahsusanne

I just double checked, but this is actually my working code transforming Quaternion-Values to Yaw, Pitch, Roll for usage in Maya. Tried the commonly accepted method first, but it does not work as expected! You might better first try, before voting down... – Susannahsusanne

I say yes (testet at least for some [f.e. Autodesk Maya] implementation of yaw, pitch, roll) - others vote it down but don't comment anything. Make up your own mind, or just try if it fits your needs. It is just 3 lines of code, if it does not work for you try to look for something differend and drop me a line what was wrong. If it works for you please confirm it with a comment and vote up :D – Susannahsusanne

Hmm, it returns me some weird results at the moment: If I yaw my camera a bit, only the yaw changes, which is what I expect. If I then pitch it, every component returned changes, which is unexpected. E.g. if I yaw and pitch my cam by 0.3 radians, I would expect this to return 0.3 for botch and 0 roll, but every component has a weird number for me then ;S – Leatherwood

Strange, I have testes the usual implementations in Maya, and it resulted in strange rotations when looking upwards beyond 'north-pole' (like it would try to work around some gimbal), where this exact implementation works perfect for all conditions (f.e. looking 180° upwards results in a upside-down look backwards, were the ususal implementation results in just looking backwards because the horizon flips back to normal). Thanks for your comment :) – Susannahsusanne

You can convert it from radiant to degree by multiplying each value by "180.0/M_PI" - pitch has a value range from -90° to +90° and then jumps, where yaw and roll switch from -180° to + 180° what it no jump because it is the same... maybe it is your quaternion values you start with? – Susannahsusanne

some might use different order of assignment: roll, yaw, pitch - depending on coordinate system. – Susannahsusanne

You're right. Yaw and Pitch was swapped for me, and I had to negate every value. – Leatherwood

maybe you like to rate it up now? :D – Susannahsusanne

Yeah, even though it still doesn't work as intended :S – Leatherwood

Are you shure you want a yaw, pitch, roll conversion and no conversion to euler angles? euclideanspace.com/maths/geometry/rotations/conversions/… – Susannahsusanne

Yeah, thats needed here – Leatherwood

But now you are doing yaw, roll, and yaw again - not yaw, pitch, roll: math.stackexchange.com/questions/147028/… – Susannahsusanne

I posted my specific question here: #23310799 The termination "euler angles" put me in big confusion until Wikipedia clarified it I am searching for "Tait-Bryan" angles (XYZ rotation, not XYX). – Leatherwood

This should fit for XYZ rotate order assuming yaw=X, pitch=Y, roll=Z. Afaik "Tait-Bryan" is ZYX order by convention. – Susannahsusanne

Best explanaition here: answers.com/topic/euler-angles#Tait.E2.80.93Bryan_angles Maya default convention: download.autodesk.com/global/docs/maya2014/en_us/… – Susannahsusanne

Wait, "aircraft principal axes" are again something different? I thought it's the same. God... who made up these terms... – Leatherwood

This seems to work with normalized invensense DMP data except that I had to swap the ordering of the results from yaw,pitch, roll to pitch, roll, yaw to have the angles make sense – Grannias

Of course this can happen, depending on the coordinate system orientation for your destination the axes are also changing on yaw, pitch, roll. – Susannahsusanne

Answer 3

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];

Brinna

@Susannahsusanne - It's not "your" answer. Above code is from DJI SDK as I've mentioned which should give people more confidence in using. Also I've added the version of code which avoids doing same dot products again if you wanted to calculate all 3. It's very bad taste to vote down because you think you own a math formula AND the fact that you didn't wrote efficient code in first place which is very important because these calculations needs to be done 100s of times per second. – Brinna

@Susannahsusanne The reason I wrote this answer is to help others save some time for the code that I'd to write myself. The toEularianAngle() does eliminate one redundancy (q2sqr), its more clean and its far from "shameless copy" that you claim to be. The toQuternion() makes it more complete. – Brinna

since someone deleted half of my answers, I deleted the remaining ones. great work eliminating that very single one multiplication. – Susannahsusanne

Dude, I have no idea what you are talking about. As far as I know, only you can delete your own answers. Someone else can't just delete other people's answers. That would be very scary. I've also filed issue with DJI folks to see if their code is correct: github.com/dji-sdk/Onboard-SDK/issues/59#issuecomment-225851991. That's how you fix problems, not by creating and protecting your "turfs". Anyway, whatever. Best of luck. – Brinna

Answer 4

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!

Channa

Euler Angels don't always have this Gimbal-Lock problem, if you handle them correctly (and this means in first case no usage of workarounds like in the video - but a correct handling of multiplication order). Euler can even have advantages like Rotation with >360°. But I also prefer Quaternions in most cases. – Susannahsusanne

I'm working with implementations that don't cause singularities in working with euler angles... for example, extraction with asin causes gimbal lock... I'm currently working on an implementation using 3 atan2 functions to avoid such a circumstance. – Preestablish

Answer 5

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);

Geology

Answer 6

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

Anamorphic

If there are key-concept or key-element in the link you provid, please consider summarizing it in your answer(unless it's already in you first paragraph). Do not forget that link or videos are note eternal! – Tabb

Or just look at my answer, it was tested to work correctly, not just almost correct under most circumstances. I tried a dozen of these almost correct ones before figuring out a combination of them that doesn't have these problems. – Susannahsusanne