0

I have a Quaternion Q = new Quaternion( 0.342,-0.704,-0.431, 0.449) in earth coordinate system and I want to remove the Azimuth from it. I also have a function getGravityFromQuarternion(Quaternion, boolean) that gives me the Gravity pull vector on a quaternion.

To remove Azimuth means (correct me if I'm wrong) means that I have to find the Azimuth correcting Quaternion A (w,x,y,z) = Quaternion(Math.cos(azimuth * 0.5),0,0,Math.sin(azimuth * 0.5)) that than can be rotated with Q (A.multiply(Q)) into Qnew. Qnew needs to either have Y or Z to zero:

  • If getGravityFromQuarternion(Q).[2] (the z gravity pull) >= 0 I want Qnew to be (w, x, y, 0)
  • If getGravityFromQuarternion(Q).[2] (the z gravity pull) < 0 I want Qnew to be (w, x, 0, z)

How to find Quaternion A (or more precise, how to find the azimuth value used in creating A) that does this rotation resulting in a Qnew that has either Y or Z to be zero, depending on object facing up or down?

Below my current method that accomplishes this - but it's not perfect.

My current method: convert Q to Gravity[] - gravity in itself does not have Azimuth - and convert Gravity back to Q

I have a function that gets me the gravity pull vector of a quaternion. For example this line of code: getGravityFromQuarternion(new Quaternion(1,0,0,0), false); would result in: [0,0,9.81]

And if I run this line of code: getGravityFromQuarternion(Q, false) I get (rounded) double[] gravVector = [-3.31, -8.52, -3.56]. I Also have a function to create a Quaternion from a Gravity vector and if I run that on this gravVector I get another quaternion back. So getQuaternionFromGravVector(getGravityFromQuarternion(Q, false)) results in a new Quaternion (w,x,y,z) : (0.53, -0.83, 0, 0.204). This result is the same orientation but without the Azimuth. In this case Y = 0 because gravVector[2] < 0.

The only problem with this approach is that it's a work around and it has some rounding issues which at some point (when integrating data) become an issue for the results.

---- Some used code (Java)

The getGravity function:

public static double[] getGravityFromQuarternion(Quaternion q, boolean skipGrav) {
    double[] gravityGyro= new double[3];
    double q0 = q.getQ0();
    double q1 = q.getQ1();
    double q2 = q.getQ2();
    double q3 = q.getQ3();

    gravityGyro[0] = 2 * (q1*q3 - q0*q2);
    gravityGyro[1] = 2 * (q0*q1 + q2*q3);
    gravityGyro[2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;

    if(!skipGrav) {
        gravityGyro[0] *= SensorReader.GRAVITY_EARTH;
        gravityGyro[1] *= SensorReader.GRAVITY_EARTH;
        gravityGyro[2] *= SensorReader.GRAVITY_EARTH;
    }
    return gravityGyro;
}

The function to convert gravity vector to Quaternion:

public static Quaternion   getQuaternionFromGravVector(double[] input) {
    double Q1, Q2, Q3, Q4;

    Vector3D vectorInput = new Vector3D(input[0], input[1], input[2]);
    Vector3D vector = vectorInput.normalize();

    double[] input2 = new double[3];
    input2[0] = vector.getX();
    input2[1] = vector.getY();
    input2[2] = vector.getZ();

    if(input2[2] >= 0){
        Q1 = -Math.sqrt(((input2[2]) + 1f)  / 2f);
        Q2 = -(input2[1] / Math.sqrt(2*(input2[2]+1)));
        Q3 = input2[0]  /  Math.sqrt(2*(input2[2]+1));
        Q4 = 0;
    }
    else {
        Q1 = -(input2[1] / Math.sqrt(2*(1-input2[2])));
        Q2 = -Math.sqrt((1-input2[2]) / 2);
        Q3 = 0;
        Q4 = -input2[0] / Math.sqrt(2*(1-input2[2]));

    }
    Quaternion Qa = new Quaternion(Q1, Q2, Q3, Q4);

    return Qa;
}
Herbert
  • 83
  • 1
  • 1
  • 6
  • What is your 4th dimension? My vague recollection of quaternions was that it should be zero except during transformations, but you've got all four dimensions filled in? – RBarryYoung Oct 28 '21 at 12:32
  • Thanks! It's basically a random quaternion I just generated on https://quaternions.online to have an example. But my orientation quaternions that I really use have a 4th dimension indeed. Context: it's a sensor fusion algoritm (gyroscope + accelerometer) - NO magnetometer. Since I don't have a magnetometer I don't want to quaternion to drift away from Azimuth = 0. Accelerometer is correcting pitch and yaw, but can't correct azimuth. Obviously I dont know where north is which is why I just want the Y (when facing down) or Z (when facing up) to be zero. – Herbert Oct 28 '21 at 13:03
  • So to continue with the context, imagine a flat orientation in earth gravity (0,0, 9.81) and the gyroscope gives a positive yaw movement of 90 degrees, then my gyroQuaternion rotates to the right and become something like (0.707, 0,0, 0.707). My gravity vector according to accelerometer is still 0,0,9.81 so the movement is not detected by accelerometer (since the movement is perpendicular to gravity). So I want to find the Quaternion that moves my gyroQuaternion back to (1,0,0,0). I cant simply multiply with it's inverse - that doesnt work when there is also pitch and roll in the orientation – Herbert Oct 28 '21 at 13:13

0 Answers0