How to get smooth orientation data in android

Question

I have an app which uses orientation data which works very well using the pre API-8 method of using a Sensor.TYPE_ORIENTAITON. Smoothing that data was relatively easy.

I am trying to update the code to avoid using this deprecated approach. The new standard approach is to replace the single Sensor.TYPE_ORIENTATION with a Sensor.TYPE_ACCELEROMETER and Sensor.TYPE_MAGENTIC_FIELD combination. As that data is received, it is sent (via SensorManager.getRotationMatrix()) to SensorManager.getOrientation(). This (theoretically) returns the same information as Sensor.TYPE_ORIENTATION did (apart from different units and axis orientation).

However, this approach seems to generate data which is much more jittery (ie noisy) than the deprecated method (which still works). So, if you compare the same information on the same device, the deprecated method provides much less noisy data than the current method.

How do I get the actual same (less noisy) data that the deprecated method used to provide?

To make my question a little clearer: I have read various answers on this subject, and I have tried all sorts of filter: simple KF / IIR low pass as you suggest; median filter between 5 and 19 points, but so far I have yet to get anywhere close to the smoothness of the data the phone supplies via TYPE_ORIENTATION.

Answer 1

Apply a low-pass filter to your sensor output.

This is my low-pass filter method:

private static final float ALPHA = 0.5f;
//lower alpha should equal smoother movement
...
private float[] applyLowPassFilter(float[] input, float[] output) {
    if ( output == null ) return input;

    for ( int i=0; i<input.length; i++ ) {
        output[i] = output[i] + ALPHA * (input[i] - output[i]);
    }
    return output;
}

Apply it like so:

float[] mGravity;
float[] mGeomagnetic;
@Override
public void onSensorChanged(SensorEvent event) {
    if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
        mGravity = applyLowPassFilter(event.values.clone(), mGravity);
    if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
        mGeomagnetic = applyLowPassFilter(event.values.clone(), mGeomagnetic);
    if (mGravity != null && mGeomagnetic != null) {
        float R[] = new float[9];
        float I[] = new float[9];

        boolean success = SensorManager.getRotationMatrix(R, I, mGravity, mGeomagnetic);
        if (success) {
            float orientation[] = new float[3];
            SensorManager.getOrientation(R, orientation);
            azimuth = -orientation[0];
            invalidate();
        }
    }
}

This is obviously code for a compass, remove what you don't need.

Also, take a look at this SE question How to implement low pass filter using java

Answer 2

It turns out that there is another, not particularly documented, way to get orientation data. Hidden in the list of sensor types is TYPE_ROTATION_VECTOR. So, set one up:

Sensor mRotationVectorSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
sensorManager.registerListener(this, mRotationVectorSensor, SensorManager.SENSOR_DELAY_GAME);

Then:

@Override
public void onSensorChanged(SensorEvent event) {
    final int eventType = event.sensor.getType();

    if (eventType != Sensor.TYPE_ROTATION_VECTOR) return;

    long timeNow            = System.nanoTime();

    float mOrientationData[] = new float[3];
    calcOrientation(mOrientationData, event.values.clone());

    // Do what you want with mOrientationData
}

The key mechanism is going from the incoming rotation data to an orientation vector via a rotation matrix. The slightly frustrating thing is the orientation vector comes from quaternion data in the first place, but I can't see how to get the quaternion delivered direct. (If you ever wondered how quaternions relate to orientatin and rotation information, and why they are used, see here.)

private void calcOrientation(float[] orientation, float[] incomingValues) {
    // Get the quaternion
    float[] quatF = new float[4];
    SensorManager.getQuaternionFromVector(quatF, incomingValues);

    // Get the rotation matrix
    //
    // This is a variant on the code presented in
    // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/
    // which has been altered for scaling and (I think) a different axis arrangement. It
    // tells you the rotation required to get from the between the phone's axis
    // system and the earth's.
    //
    // Phone axis system:
    // https://developer.android.com/guide/topics/sensors/sensors_overview.html#sensors-coords
    //
    // Earth axis system:
    // https://developer.android.com/reference/android/hardware/SensorManager.html#getRotationMatrix(float[], float[], float[], float[])
    //
    // Background information:
    // https://en.wikipedia.org/wiki/Rotation_matrix
    //
    float[][] rotMatF = new float[3][3];
    rotMatF[0][0] = quatF[1]*quatF[1] + quatF[0]*quatF[0] - 0.5f;
    rotMatF[0][1] = quatF[1]*quatF[2] - quatF[3]*quatF[0];
    rotMatF[0][2] = quatF[1]*quatF[3] + quatF[2]*quatF[0];
    rotMatF[1][0] = quatF[1]*quatF[2] + quatF[3]*quatF[0];
    rotMatF[1][1] = quatF[2]*quatF[2] + quatF[0]*quatF[0] - 0.5f;
    rotMatF[1][2] = quatF[2]*quatF[3] - quatF[1]*quatF[0];
    rotMatF[2][0] = quatF[1]*quatF[3] - quatF[2]*quatF[0];
    rotMatF[2][1] = quatF[2]*quatF[3] + quatF[1]*quatF[0];
    rotMatF[2][2] = quatF[3]*quatF[3] + quatF[0]*quatF[0] - 0.5f;

    // Get the orientation of the phone from the rotation matrix
    //
    // There is some discussion of this at
    // http://stackoverflow.com/questions/30279065/how-to-get-the-euler-angles-from-the-rotation-vector-sensor-type-rotation-vecto
    // in particular equation 451.
    //
    final float rad2deg = (float)(180.0 / PI);
    orientation[0] = (float)Math.atan2(-rotMatF[1][0], rotMatF[0][0]) * rad2deg;
    orientation[1] = (float)Math.atan2(-rotMatF[2][1], rotMatF[2][2]) * rad2deg;
    orientation[2] = (float)Math.asin ( rotMatF[2][0])                * rad2deg;
    if (orientation[0] < 0) orientation[0] += 360;
}

This seems to give data very similar in feel (I haven't run numeric tests) to the old TYPE_ORIENTATION data: it was usable for motion control of the device with marginal filtering.

There is also helpful information here, and a possible alternative solution here.

Answer 3

Here's what worked out for me using SensorManager.SENSOR_DELAY_GAME for a fast update i.e.

@Override
protected void onResume() {
    super.onResume();
    sensor_manager.registerListener(this, sensor_manager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_GAME);
    sensor_manager.registerListener(this, sensor_manager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_GAME);
}

---

MOVING AVERAGE

(less efficient)

private float[] gravity;
private float[] geomagnetic;
private float azimuth;
private float pitch;
private float roll; 

@Override
public void onSensorChanged(SensorEvent event) {
    if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
        gravity = moving_average_gravity(event.values);
    if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
        geomagnetic = moving_average_geomagnetic(event.values);

    if (gravity != null && geomagnetic != null) {
        float R[] = new float[9];
        float I[] = new float[9];

        boolean success = SensorManager.getRotationMatrix(R, I, gravity, geomagnetic);
        if (success) {
            float orientation[] = new float[3];
            SensorManager.getOrientation(R, orientation);
            azimuth = (float) Math.toDegrees(orientation[0]);
            pitch = (float) Math.toDegrees(orientation[1]);
            roll = (float) Math.toDegrees(orientation[2]);
            //if(roll>-46F && roll<46F)view.setTranslationX((roll/45F)*max_translation); //tilt from -45° to 45° to x-translate a view positioned centrally in a layout, from 0 - max_translation
            Log.i("TAG","azimuth: "+azimuth+" | pitch: "+pitch+" | roll: "+roll);
        }
    }
}




private ArrayList<Float[]> moving_gravity;
private ArrayList<Float[]> moving_geomagnetic;
private static final float moving_average_size=12;//change

private float[] moving_average_gravity(float[] gravity) {
    if(moving_gravity ==null){
        moving_gravity =new ArrayList<>();
        for (int i = 0; i < moving_average_size; i++) {
            moving_gravity.add(new Float[]{0F,0F,0F});
        }return new float[]{0F,0F,0F};
    }

    moving_gravity.remove(0);
    moving_gravity.add(new Float[]{gravity[0],gravity[1],gravity[2]});
    return moving_average(moving_gravity);
}

private float[] moving_average_geomagnetic(float[] geomagnetic) {
    if(moving_geomagnetic ==null){
        this.moving_geomagnetic =new ArrayList<>();
        for (int i = 0; i < moving_average_size; i++) {
            moving_geomagnetic.add(new Float[]{0F,0F,0F});
        }return new float[]{0F,0F,0F};
    }

    moving_geomagnetic.remove(0);
    moving_geomagnetic.add(new Float[]{geomagnetic[0],geomagnetic[1],geomagnetic[2]});
    return moving_average(moving_geomagnetic);
}

private float[] moving_average(ArrayList<Float[]> moving_values){
    float[] moving_average =new float[]{0F,0F,0F};
    for (int i = 0; i < moving_average_size; i++) {
        moving_average[0]+= moving_values.get(i)[0];
        moving_average[1]+= moving_values.get(i)[1];
        moving_average[2]+= moving_values.get(i)[2];
    }
    moving_average[0]= moving_average[0]/moving_average_size;
    moving_average[1]= moving_average[1]/moving_average_size;
    moving_average[2]= moving_average[2]/moving_average_size;
    return moving_average;
}

---

LOW PASS FILTER

(more efficient)

private float[] gravity;
private float[] geomagnetic;
private float azimuth;
private float pitch;
private float roll; 

@Override
public void onSensorChanged(SensorEvent event) {
    if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
        gravity = LPF(event.values.clone(), gravity);
    if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
        geomagnetic = LPF(event.values.clone(), geomagnetic);

    if (gravity != null && geomagnetic != null) {
        float R[] = new float[9];
        float I[] = new float[9];

        boolean success = SensorManager.getRotationMatrix(R, I, gravity, geomagnetic);
        if (success) {
            float orientation[] = new float[3];
            SensorManager.getOrientation(R, orientation);
            azimuth = (float) Math.toDegrees(orientation[0]);
            pitch = (float) Math.toDegrees(orientation[1]);
            roll = (float) Math.toDegrees(orientation[2]);
            //if(roll>-46F && roll<46F)view.setTranslationX((roll/45F)*max_translation); //tilt from -45° to 45° to x-translate a view positioned centrally in a layout, from 0 - max_translation
            Log.i("TAG","azimuth: "+azimuth+" | pitch: "+pitch+" | roll: "+roll);
        }
    }
}




private static final float ALPHA = 1/16F;//adjust sensitivity
private float[] LPF(float[] input, float[] output) {
    if ( output == null ) return input;
    for ( int i=0; i<input.length; i++ ) {
        output[i] = output[i] + ALPHA * (input[i] - output[i]);
    }return output;
}

N.B
moving average of 12 values instead as per here

low pass filter of ALPHA = 0.0625 instead as per here