Hi all,
First off, I apologise for the length of this query. I've double checked everything, and tried to be as clear as possible. It may even turn out to be an error with Wikipedia's article, so some other good can come out of this if we find the problem.
I'm trying to combine two Euler angle sets (named say, "E1" and "E2", comprising x,y,z angles for each of course) by converting them to DCM (Direct cosine matrix), multiplying the matrices and then converting them back to Euler angles again (say, "E3"). The calcs are going screwy somewhere, but I'm not sure where. I'm only using one source for now - Wikipedia, to prevent possible convention mixup of Euler angle types:
http://en.wikipedia.org/wiki/Rotation_representation_(mathematics)
Before I show my working out, maybe there's a more direct way of combining Euler angles without converting to DCM etc. Unfortunately, it's not as easy as merely adding the angles by calculating:
E3.x = E1.angx + E2.angx
E3.y = E1.angy + E2.angy
E3.z = E1.angz + E2.angz
...because that will only work if we rotate on the x axis, then the y next, and then the z axis last. We may want to rotate the y axis first etc. Hence why we 'need to' (afaik) convert to DCM and back.
Below is my entire working out. At the end of the post is the entire code, which is fully compilable. Sometimes I will display simple pseudo-code for speed, (and partially because it'll be a lot quicker me to do, and also shorten what I write).
The variables E1 and E2 are the Euler angles (each branch off into three components: angx, angy, and angz angles)
Okay so first we convert from Euler angle set 1 (E1) to DCM. (for reference see http://en.wikipedia.org/wiki/Rotation_representation_(mathematics)#Euler_angles_.E2.86.92_DCM )
sx = sin(E1.angx); sy = sin(E1.angy); sz = sin(E1.angz);
cx = cos(E1.angx); cy = cos(E1.angy); cz = cos(E1.angz);
The three matrices for x,y,z are as follows:
[1 0 0 ] [cy 0 -sy] [cz sz 0]
[0 cx sx] [0 1 0 ] [-sz cz 0]
[0 -sx cx] [sy 0 cy ] [0 0 1]
We now multiply these matrices, and end up with the matrix ABC (convert E1 to DCM matrix):
[ (cy*cz) (cy*sz) (-sy) ]
[ (sx*sy*cz + cx*-sz) (sx*sy*sz + cx*cz) (sx*cy) ]
[ (cx*sy*cz+(-sx*-sz)) (cx*sy*sz + -sx*cz) (cx*cy) ]
I'll assign this matrix to temporary variables:
a1 = (cy*cz); a2=(cy*sz); a3=(-sy);
b1 = (sx*sy*cz + cx*-sz); b2=(sx*sy*sz + cx*cz); b3=(sx*cy);
c1 = (cx*sy*cz+(-sx*-sz)); c2=(cx*sy*sz + -sx*cz); c3=(cx*cy);
Okay, now we do the exactly the same with the other set of Euler angles (E2), and end up with:
sx = sin(E2.angx); sy = sin(E2.angy); sz = sin(E2.angz);
cx = cos(E2.angx); cy = cos(E2.angy); cz = cos(E2.angz);
....creating this matrix MNO (convert E2 to DCM matrix):
m1 = (cy*cz); m2=(cy*sz); m3=(-sy);
n1 = (sx*sy*cz + cx*-sz); n2=(sx*sy*sz + cx*cz); n3=(sx*cy);
o1 = (cx*sy*cz+(-sx*-sz)); o2=(cx*sy*sz + -sx*cz); o3=(cx*cy);
Next, we multiply the ABC matrix with the MNO matrix, ending up with:
[ (a1*m1 + a2*n1 + a3*o1) (a1*m2 + a2*n2 + a3*o2) (a1*m3 + a2*n3 + a3*o3) ]
[ (b1*m1 + b2*n1 + b3*o1) (b1*m2 + b2*n2 + b3*o2) (b1*m3 + b2*n3 + b3*o3) ]
[ (c1*m1 + c2*n1 + c3*o1) (c1*m2 + c2*n2 + c3*o2) (c1*m3 + c2*n3 + c3*o3) ]
And finally we convert some of these entries back to a single set of Euler angles using the formula given at Wikipedia here:
http://en.wikipedia.org/wiki/Rotation_representation_(mathematics)#DCM_.E2.86.92_Euler_angles ):
E3.angx = acos ( (c1*m3 + c2*n3 + c3*o3)) ;
E3.angy = - atan2( (a1*m3 + a2*n3 + a3*o3) , (b1*m3 + b2*n3 + b3*o3));
E3.angz = atan2( (c1*m1 + c2*n1 + c3*o1) , (c1*m2 + c2*n2 + c3*o2) );
The above three assigns are the only ones I'm not sure about. But even assuming I mixed up say, E3.angx with E3.angy above, the results as shown below would still show any combintation to still be garbage.
Okay finished. Unfortunately, as said above, when I enter some numbers for the initial two Euler angle sets, I end up with garbage. For example, if I try some very simple ones:
INPUT:
E1.angx=0, E1.angy=0.05, E1.angz=0
E2.angx=0, E2.angy=0, E2.angz=0
... we would expect E3 to be the same as E1 (since the angles are all adding zero in E2). However, we get:
OUTPUT: E3.angx=0.05, E3.angy=0.5pi, E3.angz=0.5pi
Let's try different ones (E2 angles will stay at zero as before):
INPUT: E1.angx=0.05, E1.angy=0, E1.angz=0
OUTPUT: E3.angx=0.05, E3.angy=0, E3.angz=1pi
INPUT: E1.angx=0, E1.angy=0, E1.angz=0.05
OUTPUT: E3.angx=0, E3.angy=0, E3.angz=0
Not just slightly wrong, those answers aren't even consistently wrong. What's going on?
Here's the full compilable code for testing purposes:
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <sys/timeb.h>
#include <time.h>
#include <math.h>
void combineEuler(double E1angx, double E1angy, double E1angz,
double E2angx, double E2angy, double E2angz )
{
double sx = sin(E1angx); double sy = sin(E1angy); double sz = sin(E1angz);
double cx = cos(E1angx); double cy = cos(E1angy); double cz = cos(E1angz);
// ABC matrix (convert E1 to DCM):
double a1 = (cy*cz); double a2=(cy*sz); double a3=(-sy) ;
double b1 = (sx*sy*cz + cx*-sz); double b2=(sx*sy*sz + cx*cz); double b3=(sx*cy) ;
double c1 = (cx*sy*cz+(-sx*-sz)); double c2=(cx*sy*sz + -sx*cz); double c3=(cx*cy) ;
// Okay, now we do the exactly the same with the other set of Euler angles (E2), and end up with:
sx = sin(E2angx); sy = sin(E2angy); sz = sin(E2angz);
cx = cos(E2angx); cy = cos(E2angy); cz = cos(E2angz);
//matrix MNO (convert E2 to DCM):
double m1 = (cy*cz); double m2=(cy*sz); double m3=(-sy) ;
double n1 = (sx*sy*cz + cx*-sz); double n2=(sx*sy*sz + cx*cz); double n3=(sx*cy) ;
double o1 = (cx*sy*cz+(-sx*-sz)); double o2=(cx*sy*sz + -sx*cz); double o3=(cx*cy) ;
/*Next, we multiply the ABC matrix with the MNO matrix, ending up with:
[ (a1*m1 + a2*n1 + a3*o1) (a1*m2 + a2*n2 + a3*o2) (a1*m3 + a2*n3 + a3*o3) ]
[ (b1*m1 + b2*n1 + b3*o1) (b1*m2 + b2*n2 + b3*o2) (b1*m3 + b2*n3 + b3*o3) ]
[ (c1*m1 + c2*n1 + c3*o1) (c1*m2 + c2*n2 + c3*o2) (c1*m3 + c2*n3 + c3*o3) ]*/
double E3angx = acos( (c1*m3 + c2*n3 + c3*o3)) ;
double E3angy = - atan2( (a1*m3 + a2*n3 + a3*o3) , (b1*m3 + b2*n3 + b3*o3));
double E3angz = atan2( (c1*m1 + c2*n1 + c3*o1) , (c1*m2 + c2*n2 + c3*o2));
printf("E3xang: %f ",E3angx);
printf(" E3yang: %f ",E3angy);
printf(" E3zang: %f\n", E3angz);
}
int main(int argc, char *argv[])
{
// Output should be same as first 3 parameters of input.
combineEuler(0,0.05,0 ,0,0,0); // OUTPUT (WRONG): E3.angx=0.05, E3.angy=0.5pi, E3.angz=0.5pi
combineEuler(0.05,0,0 ,0,0,0); // OUTPUT (WRONG): E3.angx=0.05, E3.angy=0, E3.angz=1pi
combineEuler(0,0,0.05 ,0,0,0); // OUTPUT (WRONG): E3.angx=0, E3.angy=0, E3.angz=0
}
[Edited by - TwinbeeUK on January 21, 2010 6:37:25 PM]
