[Physics] A simple way of calculating Euler Angles from Rotation Matrix — help!

coordinate systemsgeometryreference framesrotationvectors

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!

Best Answer

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)