MATLAB Answers

0

calculate difference of euler angles between two dynamic moving objects

Asked by Florian Mayerle on 12 Aug 2019
Latest activity Commented on 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

  0 Comments

Sign in to comment.

2 Answers

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.

  4 Comments

Show 1 older comment
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)
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
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

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.

  1 Comment

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.