Getting Tait-Bryan Angles from Quaternion for a Non-Standard, Left-Handed Coordinate System I am trying to write a autopilot script for Kerbal Space Program, which requires me to do some conversions between Tait-Bryan angles and quaternions. Unfortunately, KSP uses a left-handed coordinate system and different identities for each axis than anywhere else I've seen and I am stymied.
There are two important reference frames for this problem: the space vessel's orbital reference frame ("vorf") and the vessel's intrinsic reference frame ("vrf"). Pictures are worth a thousand words, so here are representations of these coordinates: vessel orbital reference frame, vessel intrinsic reference frame
It is possible to obtain the current rotation in the orbital reference frame as a quaternion of the form $(x, y, z, w)$ where $w$ is the real part.
Here's what I need math to do:


*

*Convert a desired pointing (yaw, pitch, roll) in the vorf to a quaternion $d$ in the vorf.

*Determine the difference between the desired rotation quaternion, $d$, and the current rotation quaternion $c$. I can do this by multiplying $c^{-1}\cdot d = diff$

*Determine the (yaw, pitch, roll) values for the quaternion $diff$. It doesn't much matter what reference frame they're in (I think...), since they are effectively differences/errors.

*Pass these angular values to the controls of the vessel. I have a PID controller all set up for this.


I need assistance with (1) and (3). No matter how I try and formulate my conversions, I can't get a conversion (yaw, pitch, roll) $\rightarrow$ quaternion $\rightarrow$ (yaw, pitch, roll) to give the same output as I give input. (This is not just a case of identity mod $\pi/2$ or something, the values seem nonsensical.)
The best I've done so far is (yaw, 0, 0) $\rightarrow$ quaternion $\rightarrow$ (yaw, 0, 0) and similar for the other axes, but once I add a second or third non-zero value, everything falls apart.
Can someone point me to a reference that explains the conversion between angles and quaternions this way or explain the procedure to transform one of the more standard representations into the one I require?
 A: I think I have solved my problem. The main reference that finally clicked with me was this page, but I will continue and lay out my work here.
Here is a badly-drawn version of the coordinate system I'm using here (it looks better in my notes, I swear...)
bad plane drawing*


*

*$\psi$ represents a rotation around the $z$-axis, yaw

*$\theta$ represents a rotation around the $x$-axis, pitch

*$\phi$ represents a rotation around the $y$-axis, roll


The names of the angles are the traditional ones ($\psi$ for yaw, etc.) but the axes they are paired with are not.
Tait-Bryan rotations here will be performed in the order yaw, pitch, roll in the body-fixed reference frame. To apply them in the body-fixed frame, they must be applied in the reverse order: roll, pitch, yaw.
Angles to Quaternion
To convert from these three angles to a quaternion, I will use the definition of quaternion for each axis and then multiply them to get an overall quaternion.
$\begin{align}
  Q_p &= \left(\cos\frac{\theta}{2}, \sin\frac{\theta}{2}\left(\begin{array}{ccc}1\\0\\0\end{array}\right)\right)\\
  Q_r &= \left(\cos\frac{\phi}{2}, \sin\frac{\phi}{2}\left(\begin{array}{ccc}0\\1\\0\end{array}\right)\right)\\
  Q_y &= \left(\cos\frac{\psi}{2}, \sin\frac{\psi}{2}\left(\begin{array}{ccc}0\\0\\1\end{array}\right)\right)
\end{align}$
Thus the overall quaternion I seek is $Q_R = Q_y\times Q_p\times Q_r$. Since the action of quaternions is applied right-to-left, this series of multiplications corresponds to the desired roll, pitch, yaw order.
Substituting and using the associative property:
$Q_R = \left(\cos\frac{\psi}{2} + \sin\frac{\psi}{2}\hat{k}\right)\times \left(\left(\cos\frac{\theta}{2} + \sin\frac{\theta}{2}\hat{i}\right)\times\left(\cos\frac{\phi}{2} + \sin\frac{\phi}{2}\hat{j}\right)\right)$
where $\hat{i}, \hat{j}, \hat{k}$ represent the three basis elements of the quaternions corresponding to rotations of $\pi/2$ about the $x$, $y$, and $z$ axes, respectively.
Performing the multiplication:
$\begin{align}
Q_R =& \left(\cos\frac{\psi}{2} + \sin\frac{\psi}{2}\hat{k}\right)\times\left(\cos\frac{\theta}{2}\cos\frac{\phi}{2} + \cos\frac{\theta}{2}\sin\frac{\phi}{2}\hat{j} + \sin\frac{\theta}{2}\cos\frac{\phi}{2}\hat{i} + \sin\frac{\theta}{2}\sin\frac{\phi}{2}\hat{i}\hat{j}\right) \\
=& \cos\frac{\psi}{2}\cos\frac{\theta}{2}\cos\frac{\phi}{2} +
   \cos\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2}\hat{j} +
   \cos\frac{\psi}{2}\sin\frac{\theta}{2}\cos\frac{\phi}{2}\hat{i} +
   \cos\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2}\hat{i}\hat{j} +\\
&  \sin\frac{\psi}{2}\cos\frac{\theta}{2}\cos\frac{\phi}{2}\hat{k} +
   \sin\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2}\hat{k}\hat{j} +
   \sin\frac{\psi}{2}\sin\frac{\theta}{2}\cos\frac{\phi}{2}\hat{k}\hat{i} +
   \sin\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2}\hat{k}\hat{i}\hat{j}
\end{align}$
Using the standard results for multiplication of quaternion elements,
$\begin{array}{ccccc}
\times & 1 & \hat{i} & \hat{j} & \hat{k} \\
1 & 1 & \hat{i} & \hat{j} & \hat{k} \\
\hat{i} & \hat{i} & -1 & \hat{k} & -\hat{j} \\
\hat{j} & \hat{j} & -\hat{k} & -1 & \hat{i} \\
\hat{k} & \hat{k} & \hat{j} & -\hat{i} & -1
\end{array}$
these terms can be collected and placed in a column vector
$Q_R = \left(\begin{array}{c}w\\x\\y\\z\end{array}\right)
=\left(\begin{array}{c}
\cos\frac{\psi}{2}\cos\frac{\theta}{2}\cos\frac{\phi}{2} - \sin\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2} \\
\cos\frac{\psi}{2}\sin\frac{\theta}{2}\cos\frac{\phi}{2} - \sin\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2} \\
\cos\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2} - \sin\frac{\psi}{2}\sin\frac{\theta}{2}\cos\frac{\phi}{2} \\
\cos\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2} - \sin\frac{\psi}{2}\cos\frac{\theta}{2}\cos\frac{\phi}{2}
\end{array}\right)$
and this is my first desired result!
Quaternion to Angles
The next step is to get back from a quaternion $(w,x,y,z)^T$ to the angles $\psi, \theta, \phi$. This eventually condenses down to three simple formulae, but the derivation takes a few steps.
First, realize that the rotation described by the quaternion can also be described by the product of three rotation matrices
$M_R = M_y\times M_p\times M_r =
\left[\begin{array}{ccc}
\cos\psi & -\sin\psi & 0 \\
\sin\psi & \cos\psi & 0 \\
0 & 0 & 1
\end{array}\right]\times
\left[\begin{array}{ccc}
1 & 0 & 0 \\
0 & \cos\theta & -\sin\theta \\
0 & \sin\theta & \cos\theta
\end{array}\right]\times
\left[\begin{array}{ccc}
\cos\phi & 0 & \sin\phi \\
0 & 1 & 0 \\
-\sin\phi & 0 & \cos\phi
\end{array}\right]$
$M_R = \left[\begin{array}{ccc}
\cos\psi\cos\phi-\sin\psi\sin\theta\sin\phi & -\sin\psi\cos\theta & \cos\psi\sin\phi+\sin\psi\sin\theta\cos\phi \\
\cos\psi\sin\theta\sin\phi+\sin\psi\cos\phi & \cos\psi\cos\theta & \sin\psi\sin\phi-\cos\psi\sin\theta\cos\phi \\
-\cos\theta\sin\phi & \sin\theta & \cos\theta\cos\phi
\end{array}\right]$
The elements of this matrix can be used to extract expressions for the angles:
$\frac{M_{R21}}{M_{R22}} = \frac{-\sin\psi\cos\theta}{\cos\psi\cos\theta} = -\tan\psi, M_{R32} = \sin\theta, \frac{M_{R31}}{M_{R33}} = \frac{-\cos\theta\sin\phi}{\cos\theta\cos\phi} = -\tan\phi$
A quaternion can also be represented as a rotation matrix (some elements omitted since they are unncessary for this derivation--full matrix here: H-T-T-P://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm):
$M_Q = \left[\begin{array}{ccc}
- & 2xy-2wz & - \\
- & 1-2x^2-2z^2 & - \\
2xz-2wy & 2yz+2wx & 1-2x^2-2y^2
\end{array}\right]$
Since, $M_R=M_Q$, the above expressions involving the elements of $M_R$ can be written with the elements of $M_Q$:
$\begin{align}
-\tan\psi &= \frac{2xy-2wz}{1-2x^2-2z^2} &\Rightarrow& \psi = \arctan\left(\frac{-2xy+2wz}{1-2x^2-2z^2}\right) \\
\sin\theta &= 2yz+2wx &\Rightarrow& \theta = \arcsin\left(2yz+2wx\right)\\
-\tan\phi &= \frac{2xz-2wy}{1-2x^2-2y^2} &\Rightarrow& \phi = \arctan\left(\frac{-2xz+2wy}{1-2x^2-2y^2}\right)
\end{align}$
Implementation Notes
There are two difficulties with implementing the conversion from quaternion to Tait-Bryan angles in a program. The first is what happens when the denominator in an $\arctan$ expression is 0. Most programming languages have solved this problem with a function arctan2(y,x) which computes $\arctan(y/x)$ while avoiding the possible divide-by-zero error. Check the documentation of your particular language, because some use the convention $\arctan(y/x)$ and some $\arctan(x/y)$.
The second difficulty involves the singularities of Tait-Bryan/Euler angles (gimbal lock), which are well-documented elsewhere. In this formulation, the singularities occur when $\theta = \pm \pi/2$. This can be solved with the technique described here under "Derivation of Equations": H-T-T-P://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm
*Anyone with the proper reputation, feel free to place that image inline and fix my extra links...
