2
$\begingroup$

I use the following code to build a rotation matrix

 function C = build3Drot(yaw,pitch,roll)    X = [[1,0,0];[0,cos(roll),-sin(roll)];[0,sin(roll),cos(roll)]]   Y = [[cos(pitch),0,sin(pitch)];[0,1,0];[-sin(pitch),0,cos(pitch)]]   Z = [[cos(yaw),-sin(yaw),0];[sin(yaw),cos(yaw),0];[0,0,1]]  C = Z * Y * X; 

and the inverse function to get euleur angles given a rotation matrix

function [yaw,pitch,roll] = unbuild3DRot(X)  yaw = atan2(X(2,1),X(1,1));  pitch = atan(-X(3,1) / sqrt(X(3,2) * X(3,2) + X(3,3) * X(3,3)));  roll = atan2(X(3,2),X(3,3)); 

The two functions look to be correct, but I get very, very weird results when extracting angles from combined matrixes. For example

A= build3Drot(0.052041,0.663198,-0.014)  A =     0.7870   -0.0606    0.6140     0.0410    0.9981    0.0460    -0.6156   -0.0110    0.7880  B = build3Drot(0.085,0.737,0.049) B =     0.7378   -0.0520    0.6730     0.0629    0.9980    0.0082    -0.6721    0.0363    0.7396 C = A * B C =     0.1642   -0.0791    0.9833     0.0621    0.9956    0.0698    -0.9845    0.0496    0.1684  [y,p,r] = unbuild3Drot(C) y =     0.3615 p =     1.3944 r =     0.2864 

So I started from realtively small yaw and roll angles for both matrixes A and B and ended up with a final yaw of (0.3615 * 180 /pi) = 20.7 degrees and a roll of (0.2864 * 180 / pi) = 16.4 degrees. Is this normal ? It looks very strange to me, since initial angles in degrees for A were yaw = 2.9817 pitch = 37.9984 roll = -0.8021 and for B yaw = 4.8701 pitch = 42.2270 roll = 2.8075.

I also wonder why, if I chain/multiply several matrixes together at a certain point the pitch decreases. For example

[y,p,r]=unbuild3Drot(A * A * A)  [y,p,r]  =     3.1174    1.1478    2.9611  while  [y,p,r]=unbuild3Drot(A * A * A * A) y,p,r]  =     -3.0863    0.4847    2.9904 

The resulting pitch of A * A * A * A is smaller than that of A

I feel like I'm doing something horribly wrong...but can't understand what.. thanks.

  • 0
    Tha$n$ks for the much clearer example. I'll be giving an answer shortly. I suggest you edit this into the question in case anyone else wants to have a go at this.2011-10-04

1 Answers 1

1

I can't speak to what's happening in the $A$, $B$, $C$ example, but you're right that the powers of $A$ point to an error in the pitch calculation. The problem is that by taking the square root you're losing the sign of the cosine of the pitch, and this is causing the angles to be reflected at $\pi/2$. Since you have the roll, you can use that to undo the roll without a square root:

roll = atan2(X(3,2),X(3,3)); pitch = atan(-X(3,1) / (X(3,2) * sin (roll) + X(3,3) * cos (roll))); 

This should cause the pitch to cycle through to $-\pi/2$ instead of being reflected at $\pi/2$, which is correct, since the yaw and roll are jumping by $\pi$ at the same time. If you don't understand that equivalence, you might want to have a look at this: How can I find equivalent Euler angles?

Note that the division can cause problems if the denominator becomes zero; to avoid that, you could use the atan2 function as for the other two angles and discard the spurious extra information on the pitch that it yields.

But I can't bear to write this answer without repeating that if at all possible you should try to avoid all these complications by working with quaternions instead.

  • 0
    @Alfio: No, your problems don't all come from the fact that Euler angles are not unique. The angles reflected at $\pi/2$ describe the wrong rotation; they're not equivalent to the correct angles. Equivalent angles would all yield the same rotation when you plug them into `build3Drot`; whereas these angles would lead to the wrong sign on the cosine. Yes, resorting to quaternions would help you get rid of this problem. Yes, you could then get correct Euler (not "euleur") angles from the quaternions (if you still need them). No, you couldn't get unique Euler angles, since there's no such thing.2011-10-04