0

This is something what probably was asked before, but I could not find a working answer.

I am trying to convert Euler angles into a quaternion (easier and faster calucations, no gimbel lock), but for some reason it does not work. I am using a tinkerforge IMUV2, which does (amoung others) deliver me with the attitude quaternion, attitude Euler angles and attitude change in Euler angle.

I am using the quaternion and the Euler angle attitude to determine if my transformation from Euler angles to quaternions is correct. Strangely, so far, only the scalar part I calculate is correct; the vector part is utter nonsense. I am simply unable to find my mistake.

The Formula is

q=[cos(w/2),v_x sin(w/2),v_y sin(w/2),v_z sin(w/2)]

w is the vector angle ||v||; and v the normalized vector v/||v||.

    public static Quaternion eulerToQuaternion(short[] eulerR,double dt){

    double[] euler = new double[] {eulerR[0]/16*(Math.PI/180),eulerR[1]/16*(Math.PI/180),eulerR[2]/16*(Math.PI/180)};

    double scalar = Math.sqrt(euler[0]*euler[0]+euler[1]*euler[1]+euler[2]*euler[2]);
    double[] vector = new double[]{euler[0]/scalar,euler[1]/scalar,euler[2]/scalar};
    scalar = scalar*dt;
    double a = Math.sin(scalar/2);
    return new Quaternion(Math.cos(scalar/2),vector[0]*a,vector[1]*a,vector[2]*a);

}

I convert the input with 1/16*(Math.PI/180) because the tinkerforge output is in 1/16° and i need it in radians. I would be really surprised if that is the issue. The dt part is for the future and i keep dt=1. The constructor simply arranges the values in an array for a Quaternion class I wrote to make calculations.

The approach from wikipedia does not work either (scalar and vector part are both wrong).

Edit: Balazs Vago spotted the mistake of me using the first euler angle three times. With switched the second and third value of the resulting quaternion and taking the congruent I was able to make the wikipedia version work (edited version)

    public static Quaternion toQuaternion(short[] euler, double dt)
{
    double yaw=euler[0]/16*(Math.PI/180);
    double roll=euler[1]/16*(Math.PI/180);
    double pitch=euler[2]/16*(Math.PI/180);
    double t0 = Math.cos(yaw * 0.5);
    double t1 = Math.sin(yaw * 0.5);
    double t2 = Math.cos(roll * 0.5);
    double t3 = Math.sin(roll * 0.5);
    double t4 = Math.cos(pitch * 0.5);
    double t5 = Math.sin(pitch * 0.5);

    return new Quaternion(t0 * t2 * t5 + t1 * t3 * t4,-(t0 * t2 * t4 + t1 * t3 * t5),-(t0 * t3 * t4 - t1 * t2 * t5),-(t1 * t2 * t4 - t0 * t3 * t5));
}

This version is somewhat inefficient and I still need help with eulerToQuaternion to understand my mistake.

DwB
  • 37,124
  • 11
  • 56
  • 82
Pyrphoros
  • 1
  • 2
  • Might not be relevant, but in function `toQuaternion` you use the first array element 3 times. – Balazs Vago Mar 09 '17 at 13:43
  • i did not want to use toQuaternion and did not pay it mutch attention. But yes, fixing that and for some reason switching the secound and third value of the result i get the kongruent of the wanted Quaternion. Thank your for sharp eyes and common sense ;) – Pyrphoros Mar 09 '17 at 14:08
  • Well you could store a "constant" with pi/180, and also divide by 32, so you don't have to "multiply" by 0.5. – Balazs Vago Mar 09 '17 at 14:21

0 Answers0