Combine two pitch, yaw, roll rotations

[Ziks]

Firstly, I apologise if this is the wrong section, but as far as I could tell this was the most relevant.

I have a pair of triplets $(p1, y1, r1)$ and $(p2, y2, r2)$. Each one describes a set of roll, yaw, and pitch rotations in that order, by the angles given by each component in respective order. In my scenario, pitch is a rotation about the Y axis, yaw about Z, and therefore roll about X. Also, the angles are in degrees for my application.

My overall aim is to produce a third triplet $(p3, y3, r3)$ that describes the combined transformation of the first triplet and the second, in that order.

My initial naive solution was to simply add each component like so:

$(p3, y3, r3) = (p1 + p2, y1 + y2, r1 + r2)$

This works for some very simple inputs, such as when all the components of the first or second rotation are all 0, or both rotations are 0 on all but the same axis. My second approach was to convert both rotations to matrices, multiply the matrices, and then extract the new pitch, yaw and roll angles from that. This is my working, although I may have made an error:

$\begin{array}{rl} pitch(p) &= \begin{bmatrix} cos(p) & 0 & sin(p) \\ 0 & 1 & 0 \\ -sin(p) & 0 & cos(p) \end{bmatrix} \\ \\ yaw(y) &= \begin{bmatrix} cos(y) & -sin(y) & 0 \\ sin(y) & cos(y) & 0 \\ 0 & 0 & 1 \end{bmatrix} \\ \\ roll(r) &= \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(y) & -sin(y) \\ 0 & sin(y) & cos(y) \end{bmatrix} \end{array}$

To find the composite transformation $T$ from a given $(p, y, r)$ triplet:

$T = pitch(p) \times yaw(y) \times roll(r) = \begin{bmatrix} \color{blue}{cos(p)cos(y)} & -cos(p)sin(y)cos(r) + sin(p)sin(r) & cos(p)sin(y)sin(r) + sin(p)cos(r) \\ \color{green}{sin(y)} & \color{red}{cos(y)cos(r)} & \color{red}{-cos(y)sin(r)} \\ \color{blue}{-sin(p)cos(y)} & sin(p)sin(y)cos(r) + cos(p)sin(r) & -sin(p)sin(y)sin(r) + cos(p)cos(r)\end{bmatrix}$

Now I want to extract a $(p, y, r)$ triplet from a given transformation $T$:

$(p, y, r) = (atan2(-\color{blue}{T_{31}}, \color{blue}{T_{11}}), arcsin(\color{green}{T_{21}}), atan2(-\color{red}{T_{23}}, \color{red}{T_{22}}))$

Where $atan2$ is defined as (from wikipedia):

This only works for inputs where $-90 \lt y \lt 90$, because $sin(y) = sin(180-y)$ and so $arcsin(sin(y)) \ne y$ when $y \lt -90$ or $90 \lt y$. Let's try a simple example input where this fails:

$T = pitch(0) \times yaw(135) \times roll(0)$

$(p, y, r) = (atan2(-T_{31}, T_{11}), arcsin(T_{21}), atan2(-T_{23}, T_{22})) = (180, 45, 180)$

This is almost a valid answer, because $(180, -45, 180)$ is the same transformation as $(0, 135, 0)$.

Is there a simpler way of combining two pitch, yaw, roll transformations? If not, is there a nice way of preserving the sign of $y$ and fixing my method?

I am by no means a mathematician or physicist, but I've made an attempt using what little trigonometry and linear algebra I know. Despite the apparent simplicity of the problem, I couldn't find anything using Google.

Thank you in advance.

 

[Spinnor]

This might work,

http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations

From,

https://www.google.com/webhp?hl=en#...44,d.dmg&fp=14e0e787545be73c&biw=1093&bih=491

See also,

https://www.google.com/search?hl=en...44,d.dmQ&fp=a2c640d0b52bae48&biw=1093&bih=491

 

[Ziks]

Are you suggesting I should convert both of them into quaternions, then find the pitch, yaw, roll from the combined quaternion? Or find the quaternion representation from the combined matrix, then find the pitch, yaw, roll from that quaternion? Would either of those methods be easier than what I am attempting?

 

[I like Serena]

Welcome to PF, Ziks!

Your problem is that you are applying atan2 to an x and y of which you do not properly know the sign yet, meaning chances are fifty-fifty that you are off by an angle of 180 degrees.
The challenge is to deduce those signs or otherwise somehow eliminate them.

I think we can assume that the pitch is always between -90 and +90 degrees.
That is, since the pitch identifies how much an airplane goes angles up or down.
Or even if it was not originally, we still want it in this range.

With a pitch between -90 and +90 degrees, cos(p) is always positive.
To find it we can use T11 and T31.
First we need to get rid of the cos(y), or more specifically of its unknown sign.
We can do that by dividing T11 and T31 on each other.
There is a simple solution here: just use the arctan(-T31/T11) instead of atan2.

$$\text{pitch: } p = \arctan(-T31/T11) \qquad \text{ if } T11 \ne 0$$

When you have the pitch, you can deduce what you need for the yaw.
To properly find the yaw, you need both cos(y) and sin(y).
You already have sin(y)=T21. and you can find cos(y) from T11/cos(p).

$$\text{yaw: } y = \text{atan2}(T21, T11/\cos(p)) \qquad \text{ if } \cos(p) \ne 0$$

That leaves the roll, which is a bit tricky.
You can apply the same trick, since you have the yaw now.
That is to divide by cos(yaw) to get rid of its sign, and then take the atan2.

$$\text{roll: } r = \text{atan2}(-T23/\cos(y), T22/\cos(y)) \qquad \text{ if } \cos(y) \ne 0$$

However, that only works if cos(y) does not happen to be (close to) zero.

These formulas will work if:
1. The pitch is not straight up or down.
2. The yaw is not exactly to the left or exactly to the right.

Especially case 2 will be annoying, since that is a very normal situation.
To deal with it, I think you should calculate the matrices for pitch and yaw in reverse, and apply those to the matrix T.
That will leave you with the roll matrix.
In the roll matrix you have a cos(r) and a sin(r) that you can use with atan2 to find the roll.

$$R = yaw(-y) \times pitch(-p) \times T$$
$$\text{roll: } r = \text{atan2}(R32, R22)$$

 

[Ziks]

Thank you I like Serena, your help lead me towards the answer.

After some more investigating, it turned out the environment I was solving this for was actually using $Z \times Y \times X$ rotations and not $Y \times Z \times X$, so my final solution was this:

Rotation matrix construction:

$(y, p, r) = \text{yaw}(y) \times \text{pitch}(p) \times \text{roll}(r) \\ (y, p, r) = \begin{bmatrix} \text{cos}(y)\text{cos}(p) & \text{cos}(y)\text{sin}(p)\text{sin}(r)-\text{sin}(y)\text{cos}(r) & \text{cos}(y)\text{sin}(p)\text{cos}(r)+\text{sin}(y)\text{sin}(r) \\ \text{sin}(y)\text{cos}(p) & \text{sin}(y)\text{sin}(p)\text{sin}(r)+\text{cos}(y)\text{cos}(r) & \text{sin}(y)\text{sin}(p)\text{cos}(r)-\text{cos}(y)\text{sin}(r) \\ -\text{sin}(p) & \text{cos}(p)\text{sin}(r) & \text{cos}(p)\text{cos}(r) \end{bmatrix}$

The two input rotations in triplet form and how their rotation matrices are constructed:

$(y_1, p_1, r_1) = \text{yaw}(y_1) \times \text{pitch}(p_1) \times \text{roll}(r_1) \\ (y_2, p_2, r_2) = \text{yaw}(y_2) \times \text{pitch}(p_2) \times \text{roll}(r_2)$

The combined transformation:

$R = (y_2, p_2, r_2) \times (y_1, p_1, r_1) \\ R = \text{yaw}(y_2) \times \text{pitch}(p_2) \times \text{roll}(r_2) \times \text{yaw}(y_1) \times \text{pitch}(p_1) \times \text{roll}(r_1) \\ R = (y_3, p_3, r_3) = \text{yaw}(y_3) \times \text{pitch}(p_3) \times \text{roll}(r_3) \\$

Find the new yaw $y_3$ from the combined transformation, keeping the value $-90 \lt y_3 \lt 90$:

$y_3 = \text{atan2}(R_{21}, R_{11}) + \begin{cases} 180 & \mbox{if } R_{11} \lt 0 \\ 0 & \mbox{otherwise}. \end{cases}$

I use this in the place of just $\text{arctan}(R_{21} / R_{11})$ to fix the case where $R_{11} = 0$, and the conditional addition of $180$ keeps it in the correct range.

Now, since I know the yaw, I can reduce the transformation into just a $Y \times X$ one:

$S = \text{yaw}(-y_3) \times R = \text{yaw}(-y_3) \times \text{yaw}(y_3) \times \text{pitch}(p_3) \times \text{roll}(r_3) = \text{pitch}(p_3) \times \text{roll}(r_3) \\ S = \begin{bmatrix} \text{cos}(p_3) & \text{sin}(p_3)\text{sin}(r_3) & \text{sin}(p_3)\text{cos}(r_3) \\ 0 & \text{cos}(r_3) & -\text{sin}(r_3) \\ -\text{sin}(p_3) & \text{cos}(p_3)\text{sin}(r_3) & \text{cos}(p_3)\text{cos}(r_3) \end{bmatrix}$

Now finding $p_3$ and $r_3$ is trivial:

$p_3 = \text{atan2}(-S_{31}, S_{11}) \\ r_3 = \text{atan2}(-S_{23}, S_{22})$

 

[I like Serena]

Looks good!
I'm pleased to see my previous comment made sense to you!

You may have a problem though from a physical point of view.
I mentioned that pitch would likely be restricted to -90 to +90 degrees.
However, yaw would not.

A pitch of +90 means you go straight up, which is the maximum that you can go up.
A yaw of +90 means that you are heading towards the left, which is perfectly normal.
Heading more than 90 degrees to the left is also normal.
(A roll of more than 90 degrees also makes sense - it means you are going upside down.)

 

[Ziks]

Ah yes, I realize now that the conditional is unnecessary.

Although restricting the range of the pitch makes sense, the application of this solution doesn't require it, just any valid set of angles.

Thanks again!