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?
Best Answer
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*
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...