calculate difference of euler angles between two dynamic moving objects

Hi,

I have two Objekts in same coordinate System. I want to calculate the difference between object1 and object2 in yaw, pitch and roll by comparing the rotation matrix .

If the objects dont move and if it has almoast the same orientation I can calculate each position relative to the origin and compare the angles, this already works verry good.

But if the objcts move and the orientation differs a lot I get allways a wrong angle differnce on one of the axises.

I dont know hot two fix this Problem.

Is there any sollution to calculate the difference between Rotation Matrixes/Quaterions/Axis and Angle and get solid results without singularitys?

Greetings

Florian

Answer 1

[Jim Riggs]

You have two objects described in a common reference frame. Lets call the objects A and B, and the frame is the I frame. So if Direction cosine matrix [I -> A] represents the transformation from the I frame to body A frame, and [I -> B] is the DCM which transforms coordinates from I to body B, then the transformation from A to B is:

[A -> B] = [I -> B] [I -> A]^T (where [I -> A]^T is the transpose of [I -> A]

This is the same as [A -> B] = [I -> B] [A -> I].

Now you can extract the Euler angles from [A -> B] which represent the rotation to get from A to B.

[Florian Mayerle]

I Use the code above now and after first test it seems to work better.

Just to make clear, DCM is the same as the Rotation matrix I get from rotationVectorToMatrix ?

% Rotation Matrix A = Object1, B= Object2

A=[ -0.9513 0.2892 -0.1069; B=[-0.7589 0.6003 -0.2525;

0.2795 0.6625 -0.6949; 0.6428 0.6284 -0.4380;

-0.1302 -0.6909 -0.7111] -0.1043 -0.4947 -0.8628]

% [A -> B] = [I -> A]^T [I -> B]

AB= transpose(A)*B

R=AB;

%calculate Euler angles in rad

E2 = - asin(R(1,3));

E1 = atan2(R(2,3)/cos(E2), R(3,3)/cos(E2));

E3 = atan2(R(1,2)/cos(E2), R(1,1)/cos(E2));

[Jim Riggs]

Check the order of multiplication. (I made an edit to my answer a few minutes after I posted it)

The first operation is to go from A to I. This is [I -> A]^T or transpose(A).

The result of this is in the I frame. This result is then transposed to the B frame by multiplying it by [I -> B]. (Using matrix pre-multiplication, the second operation preceeds the first, i.e, it pre-multiplies)

So it should be R = AB = [I -> B]*[I -> A]^T = B*transpose(A).

Next, it appears from your equations that you are performing a Z-Y-X rotation sequence. In this case, the correct calculation for the Euler angles is as follows:

Angle Y = asin(-R(1,3) (this is correct)

But, there is no need to use this angle to calculate X and Z; Siimply use the following:

Angle X = atan2(R(2,3), R(3,3))

Angle Z = atan2(-R(1,2), R(1,1))

(note the "-" sign in the Angle Z term)

[Jim Riggs]

Also note that if it seems like things are going the wrong way, it is possible that the A and B matrix that you get from the rotationVectorToMatrix function are defined opposite what I have assumed, i.e. the A and B matrix might be [A -> I] (in stead of [I -> A] and [B -> I] (in stead of [I -> B]).

If this is the case, then [A -> B] is given by:

[A -> B] = [B -> I]^T [A -> I] = transpose(B) * A

[Florian Mayerle]

B*transpose(A) seems to give me the right output, I have to do some validatioin first to be sure.

But anyway it looks way better than before now.

Jim Riggs, thanks a lot for your time and effort.

Answer 2

[Chris]

Are you trying to compare the rotation matries directly or are you compairing the euler angles? I am not sure you can use the matricies directly but looking at the euler angles is a needed first step to debug your code before doing anything more complicated. Also remember to account for heading wrap around at north.

[Florian Mayerle]

I try to smehow compare the positon between the two objekts.

I have a rotation vector (rodrigues) as input and convert it to rotation matrix with:

rotationVectorToMatrix

Then i calculated euler angles relative to the Origin between each object and compare the output but sometimes I get values whitch seems to be wrong and I cant find the error.