A simple way of calculating Euler Angles from Rotation Matrix --- help!

Question

This is a follow up of this question :

I have the rotation matrix

$$ \left( \begin{matrix} a_{11} & a_{12} & a_{13}\\ a_{21} & a_{22} & a_{23}\\ a_{31} & a_{32} & a_{33}\\ \end{matrix}\right) $$

I'm using pre-multiplying rotation matrix (that operates on column vectors) for intrinsic rotations (i.e. I make rotations about the axes of the plane that rotates). And since the fixed frame is my reference frame ---

$$ \left( \begin{matrix} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1\\ \end{matrix}\right) $$

My rotation matrix is nothing but the column unit-vectors of the axes of the rotated frame, i.e.

$$ \left( \begin{matrix} x_{1} & x_{2} & x_{3}\\ y_{1} & y_{2} & y_{3}\\ z_{1} & z_{2} & z_{3}\\ \end{matrix}\right) $$

So therefore I have the values of a11,a12,a13,a21,a22,a23,a31,a32,a33 as x1, x2, x3, y1, y2, y3, z1, z2, z3.

Now if I consider a particular set of rotation (say X first, then Y , then Z), with the corresponding Tait-Bryan angles --- a,b and c. My rotation matrix will be the following. Rx(a)*Ry(b)*Rz(c) ---

$$ \left(\small{ \begin{matrix} \cos(b)\cos(c) & -\cos(b)\sin(c) & \sin(b)\\ \cos(a)\sin(c) + \cos(c)\sin(a)\sin(b) & \cos(a)\cos(c) - \sin(a)\sin(b)\sin(c) & -\cos(b)\sin(a)\\ \sin(a)\sin(c) - \cos(a)\cos(c)\sin(b) & \cos(c)\sin(a) + \cos(a)\sin(b)\sin(c) & \cos(a)\cos(b)\\ \end{matrix}} \right) $$

Now if I have to solve for the above angles a, b & c (pitch, yaw and roll), I basically have nine equations but three unknowns. Following are the equations ---

a11 =  cos(b)∗cos(c)
a12 = −cos(b)∗sin(c)
a13 =  sin(b)
a21 = cos(a)∗sin(c)+cos(c)∗sin(a)∗sin(b)
a22 = cos(a)∗cos(c)−sin(a)∗sin(b)∗sin(c)
a23 = −cos(b)∗sin(a)
a31 = sin(a)∗sin(c)−cos(a)∗cos(c)∗sin(b)
a32 = cos(c)∗sin(a)+cos(a)∗sin(b)∗sin(c)
a33 = cos(a)∗cos(b) 

This is where I learnt about it.

Now I am using a non-linear least squares curve fitting method to solve the above set of over-determined equations. There are two major problems that I am encountering

1. The final values of a,b, c change as I change the initial values in the iterative algorithm. I get different results if I start from [50 50 50] and different results with [0 0 0].
2. Secondly I don't think that the values obtained are correct; since the angles of pitch, yaw & roll seem pretty much different in the video. I'm using the lsqcurvefit(click for the question I asked on stackoverflow) command in Matlab. (click for documentation)

I have been on this problem of how to calculate pitch, yaw & roll for quite some time now. I think this post will give you all the details of what I have tried. I need your help to know if what I am doing is the best approach to tackle my problem. If so, please point out what is wrong in my method. If you think there are other simpler methods, please let me know about them. I'm sure there has to be a better method, since this seems like a pretty simple thing to do.

Should I change my Matlab algorithm? Anyone know any special Matlab/Mathematica toolbox that calculates the yaw, pitch, roll?

Thanks!

Answer 1

Use

$$ \begin{align} \frac{a_{23}}{a_{33}} & = \frac{ -\cos b \sin a}{\cos a \cos b} = -\tan a \\ \frac{a_{13}}{\sqrt{ a_{23}^2 + a_{33}^2 }} & = \frac{\sin b}{\cos b \sqrt{\sin^2 a +\cos^2 a}} = \tan b \\ \frac{a_{12}}{a_{11}} & = \frac{-\cos b\sin c}{\cos b \cos c} = -\tan{c} \end{align} $$

Then you get:

 a = atan2(-a23,a33)
 b = atan2(a13, sqrt(a23^2+a33^2) )
 c = atan2(-a12,a11)

Answer 2

Now if I consider a particular set of rotation (say X first, then Y , then Z), with the corresponding Tait-Bryan angles --- a,b and c. My rotation matrix will be the following ...

Look at that array. It's of the form $$\begin{array}{ccc} \cos(b) \cos(c) & -\cos(b) \sin(c) & \sin(b) \\ \cdots & \cdots & -\cos(b) \sin (a) \\ \cdots & \cdots & \phantom{-}\cos(b) \cos(a) \end{array}$$

Note that the $a_{13}$ element depends on $b$ only: $a_{13}= \sin(b)$. This immediately gives you the angle $b$: $b=\arcsin(a_{13})$. Since $\cos b$ is non-negative for $b$ in $[-\pi/2,pi/2]$, the range of Arcsine(x), the two argument arctangent can be used to compute angles $a$ and $c$: $a = \operatorname{atan2}(-a_{23}, a_{33})$ and $c=\operatorname{atan2}(-a_{12}, a_{11})$.

---

Update: Gimbal lock

The above is incomplete. Suppose that $\cos b$ is zero. This happens when $b=\pm \frac \pi 2$, making $\sin b = \pm 1$. The matrix takes on one of the following two forms in these cases.

When $\sin b=-1$, the matrix becomes $$\begin{array}{ccc} 0 & 0 & -1 \\ \phantom{-}\sin(a-c) & \phantom{-}\cos(a-c) & \phantom{-}0 \\ \phantom{-}\cos(a-c) & -\sin(a-c) & \phantom{-}0 \end{array}$$

When $\sin b=1$, the matrix becomes $$\begin{array}{ccc} 0 & 0 & \phantom{-}1 \\ \phantom{-}\sin(a+c) & \phantom{-}\cos(a+c) & \phantom{-}0 \\ -\cos(a+c) & \phantom{-}\sin(a+c) & \phantom{-}0 \end{array}$$

Note that in both cases, multiple $(a, c)$ pairs will yield the same matrix. When $\sin b=-1$, all that can be inferred from the matrix is the difference $a-c$: $\tan(a-c) = \frac {a_{21}}{a_{22}}$ in this case of gimbal lock. When $\sin b=1$, all that can be inferred is the sum $a+c$: $\tan(a+c) = \frac {a_{21}}{a_{22}}$ in this other case of gimbal lock. In both cases, the ability to uniquely identify $a$ and $c$ is lost. One solution is to arbitrarily set one of those two angles to zero. It's more convenient to set $c=0$ because then $a$ can be calculated as $a=\operatorname{atan2}(a_{21},a_{22})$ in both cases.

Gimbal lock is a problem at more than just these two singular values when the matrix is formed using finite precision arithmetic (e.g., single or double precision arithmetic). Suppose the matrix $A$ is formed by multiplying two transformation matrices $B$ and $C$ such that the $a_{13}$ element of the product matrix is close to plus or minus one. The elements nominally used to compute $a$ and $c$ will be close to zero, and these near-zero values oftentimes will be the subject of extreme loss of precision. Computing angles $a$ and $c$ via the first procedure will yield garbage values. It's better to treat such near-gimbal lock matrices as if they were indeed gimbal-locked.