Asked by Florian Mayerle
on 12 Aug 2019

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 by Jim Riggs
on 12 Aug 2019

Edited by Jim Riggs
on 12 Aug 2019

Accepted Answer

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.

Jim Riggs
on 12 Aug 2019

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
on 12 Aug 2019

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
on 12 Aug 2019

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.

Sign in to comment.

Answer by Chris
on 12 Aug 2019

Florian Mayerle
on 12 Aug 2019

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.

Sign in to comment.

Opportunities for recent engineering grads.

Apply Today
## 0 Comments

Sign in to comment.