[Math] Isolate yaw-pitch-roll from rotation

matricestransformation

I have a transformation that acts as such:

$$RX=Y$$

Where $R_{3\times3}$ is a yaw-pitch-roll rotation matrix, and $X, Y$ are 3D vectors.

Yaw ($\alpha$)-pitch ($\beta$)-roll ($\gamma$) rotations are the product of the 3 rotation matrices:

$$R_x(\gamma)R_y(\beta)R_z(\alpha)$$

Given $X$ and $Y$, how can I find $\alpha$, $\beta$ and $\gamma$?

Best Answer

The first answer to any question on how to find Euler angles is: don't; use quaternions instead.

If external factors force you to use Euler angles, you could proceed like this: If $X$ and $Y$ are identical, the rotation can be a rotation around $X$ through any angle. Otherwise, the rotation can be a rotation through any axis $A$ in the plane $A\cdot X=A\cdot Y$. If $X$ and $Y$ are antipodal, the rotation must be through an angle $\pi$. Otherwise, that plane is spanned by the two vectors $X+Y$ and $X\times Y$, so you can parametrize the possible axes as

$$A(\xi)=\cos\xi\frac{X+Y}{|X+Y|}+\sin\xi\frac{X\times Y}{|X\times Y|}$$ with $\xi\in[0,\pi[$. The rotation angle must be the angle between the perpendicular projections of $X$ and $Y$ into the plane perpendicular to the axis, which are

$$ \begin{eqnarray} X_\perp&=& X - (A\cdot X)A\\ &=&X-\cos\xi\frac{X^2+X\cdot Y}{|X+Y|}A\\ &=&X-\frac12\cos\xi\frac{(X+Y)^2}{|X+Y|}A\\ &=&X-\frac12\cos\xi|X+Y|A \end{eqnarray}$$

(where I used $X^2=Y^2$), and likewise for $Y_\perp$, so the angle between them is

$$ \begin{eqnarray} \phi(\xi) &=&\arccos\frac{X_\perp\cdot Y_\perp}{|X_\perp||Y_\perp|}\\ &=&\arccos\frac{X_\perp\cdot Y_\perp}{X_\perp\cdot X_\perp}\\ &=&\arccos\frac{X\cdot Y -\frac12\cos\xi|X+Y|A\cdot(X+Y)+\frac14\cos^2\xi |X+Y|^2}{X\cdot X -\frac12\cos\xi|X+Y|A\cdot(X+X)+\frac14\cos^2\xi |X+Y|^2}\\ &=&\arccos\frac{X\cdot Y -\frac14\cos^2\xi (X+Y)^2}{X\cdot X -\frac14\cos^2\xi (X+Y)^2}\\ &=&\arccos\frac{\cos^2\xi (X+Y)^2-4X\cdot Y}{\cos^2\xi (X+Y)^2-4X\cdot X}\\ &=&\arccos\frac{\sin^2\theta-\cos^2\theta\sin^2\xi}{\cos^2\theta\cos^2\xi-1}\;,\\ \end{eqnarray} $$

where $\theta$ is the angle between $X$ or $Y$ and $X+Y$, that is, half the angle between $X$ and $Y$.

Using $A(\xi)$ and $\phi(\xi)$, you can construct the rotation matrices $R(\xi)$ and extract the Euler angles.