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.