simple_algorithms

Simple 3-axis and 6-axis Algorithms

Overview

This page presents a couple simple algorithms for computing rotation/orientation with one or two sensors. Each has it's own set of limitations.

Simple 3-Axis (accelerometer or magnetometer)

The dot product and cross product operators discussed at the end of [Rotations and Orientation: Part 2](OrientationPart2.html) hold the secret to computing axis and angle based upon measurements from a single accelerometer or magnetometer. When no linear acceleration is present, an accelerometer measures the gravity vector, which is know. When there is no magnetic interference, a magnetometer measures the earth's magnetic field, which is also known (assuming you know where you are on the earth). The 3-axis computation here can be used with either sensor type so long as you know the reference sensor value at zero rotation and have the measured value at some arbitrary rotation.

Assuming you have normalized your sensor readings, the following two equations hold:

• u . v = cos α
• n = u x v / (sin α)

Re-arranging these and reducing the result to Java code, you get the function:

static public float axisAngle(float[] axis, float[] v1, float[] v2) {
    assert(v1.length==3);
    assert(v2.length==3);
    float [] cp  = new float[3]; // cross product
    float [] nv1 = new float[3]; // normalized versoin of v1
    float [] nv2 = new float[3]; // normalized version of v2
    normalize(nv1, v1);
    normalize(nv2, v2);
    float cosAngle = dotProduct(nv1, nv2);
    float angle = (float) Math.acos(cosAngle);
    computeCrossProduct(cp, nv1, nv2);
    normalize(axis, cp);
    return(angle); // Angle is in radians
}

The helper functions are:

static public float dotProduct(float[] a, float[] b) {
    // a and b are assumed to be the same length
    assert(a.length==b.length);
    float dp = 0;
    for (int i = 0; i < b.length; i++) {
            dp = dp + a[i]*b[i];
    }
    return(dp);
}
static public void computeCrossProduct(float[] cp, float[] a, float[] b) {
    // a and b are assumed to be 3x1 vectors
    assert(a.length==3);
    assert(b.length==3);
    cp[0] = a[1]*b[2]-a[2]*b[1];
    cp[1] = a[2]*b[0]-a[0]*b[2];
    cp[2] = a[0]*b[1]-a[1]*b[0];
}
static public float norm(float[] v) {
    int n = v.length;
    float norm = 0;
    for (int i = 0; i < n; i++) {
            norm = norm + v[i]*v[i];
    }
    norm = (float) Math.sqrt(norm);
    return(norm);
}
static public void normalize(float[] vout, float[] vin) {
    scalarMult(vout, vin, 1/norm(vin));
}	

Warnings:

The first issue above results from the fact that you have a single reference vector, which is insufficient to determine all possible orientations. The second results from the fact that the cross product (Equation 2) of two co-linear terms is [0;0;0], which is pretty useless as an axis of rotation. It derives from the same root cause: a single vector is insufficient to compute all possible orientations. With a single vector, you can always find an orientation that introduces problems.

6-Axis getRotationMatrix()

This algorithm combines accelerometer and magnetometer readings to compute orientation. Depending upon your selection of board/software, this algorithm may be computed locally on your Android device or it may be computed locally using Freescale's fusion library.

The 6-axis algorithm used is Android's standard [getRotationMatrix()](http://developer.android.com/reference/android/hardware/SensorManager.html#getRotationMatrix(float[], float[], float[], float[])) helper function. By helper function, we mean that getRotationMatrix() does not read sensor values directly. You must supply the measurements yourself. This allows us to run the same computation using the native sensors on your Android device as well as sensors residing on Freescale sensor fusion demo boards.

This function utilizes BOTH acceleration and magnetic field measurments. Because the two reference vectors are NOT colinear, this algorithm does not have the "blind spot" seen on the 3-axis computation in the previous section.

The reference vectors for this algorithm are magnetic north and the gravity vector. You can find the source for this function at http://grepcode.com, which offers convenient online browsing of the Android source. That source is licensed under terms of the Apache License. The listing below has been modified with additional comments.

The basic approach is to take advantage of the fact that the cross product of two vectors is a third vector which is perpendicular to the first two. A cross product of measured gravity and magnetic north give you east. A second cross product of "East" and gravity gives us "North". Components of the gravity, East and North vectors can be arranged to generate a rotation matrix which models the rotation between device and world frames of reference.

public static boolean getRotationMatrix(float[] R, float[] I, 
        float[] gravity, float[] geomagnetic) {
    float Ax = gravity[0];
    float Ay = gravity[1];
    float Az = gravity[2];
    final float Ex = geomagnetic[0];
    final float Ey = geomagnetic[1];
    final float Ez = geomagnetic[2];

    // the the cross product of magnetic vector and gravity to derive a basis
    // vector pointing East.
    float Hx = Ey*Az - Ez*Ay;
    float Hy = Ez*Ax - Ex*Az;
    float Hz = Ex*Ay - Ey*Ax;

    // Normalize & check the new basis vector
    final float normH = (float)Math.sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
    if (normH < 0.1f) {
            // device is close to free fall (or in space?), or close to
            // magnetic north pole. Typical values are  > 100.
            return false;
     }
     final float invH = 1.0f / normH;
     Hx *= invH;
     Hy *= invH;
     Hz *= invH;

    // Now normalize the acceleration (assumed = gravity) vector
    final float invA = 1.0f / (float)Math.sqrt(Ax*Ax + Ay*Ay + Az*Az);
    Ax *= invA;
    Ay *= invA;
    Az *= invA;

    // A second cross product of the newly computed East and our measured gravity
    // vector gives a north vector in the horizontal plane.
    final float Mx = Ay*Hz - Az*Hy;
    final float My = Az*Hx - Ax*Hz;
    final float Mz = Ax*Hy - Ay*Hx;

// Now create a rotation matrix based upon the newly computed basis vectors
    if (R != null) {
         if (R.length == 9) {
             R[0] = Hx;     R[1] = Hy;     R[2] = Hz;
             R[3] = Mx;     R[4] = My;     R[5] = Mz;
             R[6] = Ax;     R[7] = Ay;     R[8] = Az;
         } else if (R.length == 16) {
             R[0]  = Hx;    R[1]  = Hy;    R[2]  = Hz;   R[3]  = 0;
             R[4]  = Mx;    R[5]  = My;    R[6]  = Mz;   R[7]  = 0;
             R[8]  = Ax;    R[9]  = Ay;    R[10] = Az;   R[11] = 0;
             R[12] = 0;     R[13] = 0;     R[14] = 0;    R[15] = 1;
         }
     }
...
// Details of the inclination matrix have been omitted
    return true;
 }

getRotationMatrix() will produce inaccurate results if linear acceleration is present (because accelerometers measure the sum of acceleration and gravity). It will also produce inacurrate results if uncompensated magnetic interference is present in the magnetometer vector used as input.

And finally, remember that this algorithm references magnetic north. Not the geographic north pole.

Simple 3-axis and 6-axis Algorithms by Freescale Semiconductor is licensed under a Creative Commons Attribution 4.0 International License.