MATLAB Answers

calculate difference of euler angles between two dynamic moving objects

30 views (last 30 days)
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.

Accepted Answer

Jim Riggs
Jim Riggs on 12 Aug 2019
Edited: Jim Riggs on 12 Aug 2019
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
Jim Riggs
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
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
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.

More Answers (1)

Chris
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

Florian Mayerle
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.

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by