# Getting Euler (Tait-Bryan) Angles from Quaternion representation

Apologies if this has already been answered, but I haven't been able to get a clear answer from looking on Stack Exchange so-far.

I'm trying to solve a camera stabilization problem. I have a 2-axis actively-driven gimbal system (pitch and yaw only). I am doing all my internal mathematics for the stabilisation using quaternion representation, but to actually convert that into drive commands for the gimbal, I need to convert my internal quaternions to pitch and yaw commands for the gimbal at the end of the workflow.

I have an IMU mounted to the gimbal housing, i.e. the body frame of the vehicle carrying the gimbal. I am using quaternions to do the underlying stabilisation mathematics, as I want the final algorithm to be able to run on a relatively low computational power 32-bit microcontroller. My intended work-flow is as follows:

1. Get the desired camera pointing direction from the user in world-frame (Azimuth and Elevation)
2. Convert that target into a quaternion representing the rotation required to drive the gimbal from world-frame home to the target location.
3. Get a quaternion representing the rotation between the world frame and the body frame of the vehicle from the IMU.
4. Perform appropriate inversions and quaternion multiplications to end up with a quaternion representing the rotation from the body frame of the vehicle to the target. This step may involve an additional quaternion representing the static offset between the IMU and vehicle frame, as my IMU mountings are not perfectly aligned with the gimbal axes.
5. Convert this quaternion back into Azimuth and Elevation commanded positions for my Gimbal, and a Roll correction to be sent to the host for the software de-rotation.

The main step I'm having trouble with at the moment is (5), I can't find any sort of decent reference for converting this final quaternion back into a general Euler angle solution. At the moment I'm not 100% sure whether what I want is a 1-2-3 or a 3-2-1 Euler solution, so I want to have the ability to implement either when it comes down to it, giving me some ability to tweak/debug.

If anyone also has any gotchas they can see with this plan of attack, feel free to let me know about them. I'm an embedded control expert, and although I have some understanding of how quaternions work and what they're good for, I wouldn't be surprised to find I'd missed some crucial step in this process. My gimbal system has a limited pitch range, so given that the reference frame I'm converting back into Euler is the gimbal control frame, I don't expect gimbal lock to be an issue. I can identify the 90-degree pitch-up as higher than my motion capability and immediately discard that as an unachievable command. (I think I'm correct in saying that yaw and roll become coupled when the gimbal begins to lock looking almost directly up or down, but pitch will remain valid???)

EDIT: I've discovered since posting this question that what I was really after are apparently Tait-Bryan angles not Euler angles. The information in the question is still valid however, I'm after conversions from quaternion representation to intrinsic Roll-Pitch-Yaw and Yaw-Pitch-Roll representations.

-
When you say "get desired camera pointing direction", is this just a vector (a direction)? In other words, if the camera were to rotate about the axis of this pointing direction, does it make a difference? Is this something you would want to specify? –  Muphrid Feb 24 '14 at 4:23
The commands to the Gimbal from the user will be of the form (Target Azimuth, Target Elevation) in the world reference frame. The roll parameter is not part of the target, however a calculation of the required roll to keep the image correctly oriented (i.e. image up = world up) is part of the solution. I believe should come out in the wash along with the required AzTgt and ElTgt once the quaternion-euler transform is done. –  OcularProgrammer Feb 24 '14 at 4:25
All right; it all boils down to the quat you're getting for (2), which seems well-defined then. Do you prefer a pitch-yaw-pitch or yaw-pitch-yaw solution? –  Muphrid Feb 24 '14 at 4:28
What I'm after are Roll-Pitch-Yaw and Yaw-Pitch-Roll versions. Ideally I'd like a better understanding of what's going on under the hood, but by now I'm willing to sacrifice that understanding in exchange for correct solutions.... –  OcularProgrammer Feb 26 '14 at 4:26

After a lot of searching, I have finally found a reference that has what I needed. Although it doesn't appear in a 'Quaternion to Euler' or 'Quaternion to Tait-Bryan' google search, when I gave up and started looking for 'Quaternion to Axis-Angle' with the intention of going through that representation as an intermediate step, I came across the following wonderful document:

Technical Concepts
Orientation, Rotation, Velocity and Acceleration, and the SRM

Version 2.0, 20 June 2008
Author: Paul Berner, PhD
Contributors: Ralph Toms, PhD, Kevin Trott, Farid Mamaghani, David Shen, Craig Rollins, Edward Powell, PhD
http://www.sedris.org/wg8home/Documents/WG80485.pdf

It covers a lot of the formalisms, but most importantly, shows derivations and solutions for 3-1-3 and 3-2-1 Euler angle representation. It also seems to cover inter-conversion between pretty much every other rotation representation I'm aware of, and so I would also recommend it as a good general reference.

Oh, and the actual solution for a 3-2-1 (z-y-x) tait-bryan rotation convention from that reference: $$\phi = arctan2((q_2 q_3 + q_0 q_1),\frac{1}{2}-(q_1^2 + q_2^2)) \\ \theta = arcsin(-2(q_1 q_3 - q_0 q_2)) \\ \psi = arctan2((q_1 q_2 + q_0 q_3),\frac{1}{2}-(q_2^2 + q_3^2))$$

Note that the gimbal-lock situation occurs when $2(q_1 q_3 + q_0 q_2) = \pm1$ (which gives a $\theta$ of $\pm \frac{\pi}{2}$), so it can be clearly identified before you attempt to evaluate $\phi$ and $\psi$.

(Convention for arctan2 is arctan2(y, x), as hinted on page 3.)

-

Conversion from quaternion to Euler angle representation is covered in lots of places. For example here, and here.

Specifically, if you have a quaternion $\mathbf{q} = [q_1, q_2, q_3, q_4]^T$, then the Euler angles $\phi, \theta, \psi$ are given by $$\phi = \operatorname{arctan2}\big( q_1q_3 + q_2q_4, q_1q_4 - q_2q_3 \big) \\ \theta = \arccos\big(-q_1^2 - q_2^2 + q_3^2 + q_4^2 \big) \\ \psi = \operatorname{arctan2}\big( q_1q_3 - q_2q_4, q_2q_3 + q_1q_4 \big)$$ There are many variants of Euler angles, of course; this is just one of them. It's just copied from the second reference I cited above.