Question

I’m creating 3D Compass application.

I’m using getOrientation method to get orientation (almost same implementation like here). If I place phone on the table it works well, but when top of the phone points to the sky (minus Z axis on the picture; sphere is the Earth) getOrientation starts giving really bad results. It gives values for Z axis between 0 to 180 degrees in a few real degrees. Is there any way how to suppress this behavior? I created a little video what describes problem (sorry for bad quality). Thanks in advance.

Solution:
When you rotating model, there is difference between:

gl.glRotatef(_angleY, 0f, 1f, 0f); //ROLL
gl.glRotatef(_angleX, 1f, 0f, 0f); //ELEVATION
gl.glRotatef(_angleZ, 0f, 0f, 1f); //AZIMUTH


gl.glRotatef(_angleX, 1f, 0f, 0f); //ELEVATION
gl.glRotatef(_angleY, 0f, 1f, 0f); //ROLL
gl.glRotatef(_angleZ, 0f, 0f, 1f); //AZIMUTH

Answer 1

Well, I can see at least 1 problem with this approach of yours.

I assume that you combine a 3D vector corresponding to your magnetometer with an averaging low pass filter to smoothen the data. Although such approach would work great for a sensor value which varies without discontinuities, such as raw data from accelerometer, it doesn’t work so great verbatim with angular variables fetched from your magnetometer. Why, one might ask?

Because those angular variables (azimuth, pitch, roll) have an upper-bound and a lower-bound, which means that any value above 180 degrees, say 181 degrees, would wrap around to 181-360 = -179 degrees, and any variable below -180 degrees would wrap around in the other direction. So when one of those angular variables get close to those thresholds (180 or -180), this variable will tend to oscillate to values close to those 2 extremes. When you blindly apply a low-pass filter to those values, you get either a smooth decreasing from 180 degrees towards -180 degrees, or a smooth increasing from -180 towards 180 degrees. Either way, the result would look quite like your video above… As long as one directly applies an averaging buffer onto the raw angle data from getOrientation(...), this problem will be present (and should be present not only for the case where the phone is upright, but also in the cases where there are azimuth angle wraparounds too… Maybe you could test for those bugs as well…).

You say that you tested this with a buffer size of 1. Theoretically, the problem should not be present if there is no averaging at all, although in some implementations of a circular buffer I’ve seen in the past, it could mean that there is still averaging done with at least 1 past value, not that there is no averaging at all. If this is your case, we have found the root cause of your bug.

Unfortunately, there isn’t much of an elegant solution that could be implemented while sticking with your standard averaging filter. What I usually do in this case is switch to another type of low pass filter, which doesn’t need any deep buffer to operate: a simple IIR filter (order 1):

diff = x[n] – y[n-1]

y[n] – y[n-1] = alpha * (x[n] – y[n-1]) = alpha * diff

…where y is the filtered angle, x is the raw angle, and alpha<1 is analogous to a time constant, as alpha=1 corresponds to the no-filter case, and the frequency cutoff of the low-pass filter gets lowered as alpha approaches zero. An acute eye would probably have noticed by now that this corresponds to a simple Proportional Controller.

Such a filter allows the compensation of the wraparound of the angle value because we can add or subtract 360 to diff so as to ensure that abs(diff)<=180, which in turn ensures that the filtered angle value will always increase/decrease in the optimal direction to reach its “setpoint”.

An example function call, which is to be scheduled periodically, that calculates a filtered angle value y for a given raw angle value x, could be something like this:

private float restrictAngle(float tmpAngle){
    while(tmpAngle>=180) tmpAngle-=360;
    while(tmpAngle<-180) tmpAngle+=360;
    return tmpAngle;
}

//x is a raw angle value from getOrientation(...)
//y is the current filtered angle value
private float calculateFilteredAngle(float x, float y){ 
    final float alpha = 0.1f;
    float diff = x-y;

    //here, we ensure that abs(diff)<=180
    diff = restrictAngle(diff);

    y += alpha*diff;
    //ensure that y stays within [-180, 180[ bounds
    y = restrictAngle(y);

    return y;
}

The function calculateFilteredAngle(float x, float y) can then be called periodically using something like this (example for azimuth angle from getOrientation(...) function:

filteredAzimuth = calculateFilteredAngle(azimuth, filteredAzimuth);

Using this method, the filter would not misbehave like the averaging filter as mentioned by the OP.

As I could not load the .apk uploaded by the OP, I decided to implement my own test project in order to see if the corrections work. Here is the entire code (it does not use a .XML for the main layout, so I did not include it). Simply copy it to a test project to see if it works on a specific device (tested functional on a HTC Desire w/ Android v. 2.1):

File 1: Compass3DActivity.java:

package com.epichorns.compass3D;

import android.app.Activity;
import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.view.ViewGroup;
import android.widget.LinearLayout;
import android.widget.TextView;

public class Compass3DActivity extends Activity {
    //Textviews for showing angle data
    TextView mTextView_azimuth;
    TextView mTextView_pitch;
    TextView mTextView_roll;

    TextView mTextView_filtered_azimuth;
    TextView mTextView_filtered_pitch;
    TextView mTextView_filtered_roll;


    float mAngle0_azimuth=0;
    float mAngle1_pitch=0;
    float mAngle2_roll=0;

    float mAngle0_filtered_azimuth=0;
    float mAngle1_filtered_pitch=0;
    float mAngle2_filtered_roll=0;

    private Compass3DView mCompassView;

    private SensorManager sensorManager;
    //sensor calculation values
    float[] mGravity = null;
    float[] mGeomagnetic = null;
    float Rmat[] = new float[9];
    float Imat[] = new float[9];
    float orientation[] = new float[3];
    SensorEventListener mAccelerometerListener = new SensorEventListener(){
        public void onAccuracyChanged(Sensor sensor, int accuracy) {}

        public void onSensorChanged(SensorEvent event) {
            if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER){
                mGravity = event.values.clone();
                processSensorData();
            }
        }   
    };
    SensorEventListener mMagnetometerListener = new SensorEventListener(){
        public void onAccuracyChanged(Sensor sensor, int accuracy) {}

        public void onSensorChanged(SensorEvent event) {
            if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD){
                mGeomagnetic = event.values.clone();
                processSensorData();                
                update();
            }
        }   
    };

    private float restrictAngle(float tmpAngle){
        while(tmpAngle>=180) tmpAngle-=360;
        while(tmpAngle<-180) tmpAngle+=360;
        return tmpAngle;
    }

    //x is a raw angle value from getOrientation(...)
    //y is the current filtered angle value
    private float calculateFilteredAngle(float x, float y){ 
        final float alpha = 0.3f;
        float diff = x-y;

        //here, we ensure that abs(diff)<=180
        diff = restrictAngle(diff);

        y += alpha*diff;
        //ensure that y stays within [-180, 180[ bounds
        y = restrictAngle(y);

        return y;
    }



    public void processSensorData(){
        if (mGravity != null && mGeomagnetic != null) { 
            boolean success = SensorManager.getRotationMatrix(Rmat, Imat, mGravity, mGeomagnetic);
            if (success) {              
                SensorManager.getOrientation(Rmat, orientation);
                mAngle0_azimuth = (float)Math.toDegrees((double)orientation[0]); // orientation contains: azimut, pitch and roll
                mAngle1_pitch = (float)Math.toDegrees((double)orientation[1]); //pitch
                mAngle2_roll = -(float)Math.toDegrees((double)orientation[2]); //roll               
                mAngle0_filtered_azimuth = calculateFilteredAngle(mAngle0_azimuth, mAngle0_filtered_azimuth);
                mAngle1_filtered_pitch = calculateFilteredAngle(mAngle1_pitch, mAngle1_filtered_pitch);
                mAngle2_filtered_roll = calculateFilteredAngle(mAngle2_roll, mAngle2_filtered_roll);    
            }           
            mGravity=null; //oblige full new refresh
            mGeomagnetic=null; //oblige full new refresh
        }
    }

    /** Called when the activity is first created. */
    @Override
    public void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);     
        LinearLayout ll = new LinearLayout(this);       
        LinearLayout.LayoutParams llParams = new LinearLayout.LayoutParams(LinearLayout.LayoutParams.FILL_PARENT, LinearLayout.LayoutParams.FILL_PARENT);      
        ll.setLayoutParams(llParams);      
        ll.setOrientation(LinearLayout.VERTICAL);      
        ViewGroup.LayoutParams txtParams = new ViewGroup.LayoutParams(ViewGroup.LayoutParams.WRAP_CONTENT, ViewGroup.LayoutParams.WRAP_CONTENT);        
        mTextView_azimuth = new TextView(this);
        mTextView_azimuth.setLayoutParams(txtParams);
        mTextView_pitch = new TextView(this);
        mTextView_pitch.setLayoutParams(txtParams);
        mTextView_roll = new TextView(this);
        mTextView_roll.setLayoutParams(txtParams);      
        mTextView_filtered_azimuth = new TextView(this);
        mTextView_filtered_azimuth.setLayoutParams(txtParams);
        mTextView_filtered_pitch = new TextView(this);
        mTextView_filtered_pitch.setLayoutParams(txtParams);
        mTextView_filtered_roll = new TextView(this);
        mTextView_filtered_roll.setLayoutParams(txtParams);

        mCompassView = new Compass3DView(this);        
        ViewGroup.LayoutParams compassParams = new ViewGroup.LayoutParams(200,200);
        mCompassView.setLayoutParams(compassParams);

        ll.addView(mCompassView);
        ll.addView(mTextView_azimuth);
        ll.addView(mTextView_pitch);
        ll.addView(mTextView_roll);
        ll.addView(mTextView_filtered_azimuth);
        ll.addView(mTextView_filtered_pitch);
        ll.addView(mTextView_filtered_roll);

        setContentView(ll);

        sensorManager = (SensorManager) this.getSystemService(Context.SENSOR_SERVICE);
        sensorManager.registerListener(mAccelerometerListener, sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_UI); 
        sensorManager.registerListener(mMagnetometerListener, sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_UI);
        update();       
    }


    @Override
    public void onDestroy(){
        super.onDestroy();
        sensorManager.unregisterListener(mAccelerometerListener);
        sensorManager.unregisterListener(mMagnetometerListener);
    }


    private void update(){
        mCompassView.changeAngles(mAngle1_filtered_pitch,  mAngle2_filtered_roll, mAngle0_filtered_azimuth);

        mTextView_azimuth.setText("Azimuth: "+String.valueOf(mAngle0_azimuth));
        mTextView_pitch.setText("Pitch: "+String.valueOf(mAngle1_pitch));
        mTextView_roll.setText("Roll: "+String.valueOf(mAngle2_roll));

        mTextView_filtered_azimuth.setText("Azimuth: "+String.valueOf(mAngle0_filtered_azimuth));
        mTextView_filtered_pitch.setText("Pitch: "+String.valueOf(mAngle1_filtered_pitch));
        mTextView_filtered_roll.setText("Roll: "+String.valueOf(mAngle2_filtered_roll));

    }
}

File 2: Compass3DView.java:

package com.epichorns.compass3D;

import android.content.Context;
import android.opengl.GLSurfaceView;

public class Compass3DView extends GLSurfaceView {
    private Compass3DRenderer mRenderer;

    public Compass3DView(Context context) {
        super(context);
        mRenderer = new Compass3DRenderer(context);
        setRenderer(mRenderer);
    }

    public void changeAngles(float angle0, float angle1, float angle2){
        mRenderer.setAngleX(angle0);
        mRenderer.setAngleY(angle1);
        mRenderer.setAngleZ(angle2);
    }

}

File 3: Compass3DRenderer.java:

package com.epichorns.compass3D;


import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.FloatBuffer;
import java.nio.ShortBuffer;

import javax.microedition.khronos.egl.EGLConfig;
import javax.microedition.khronos.opengles.GL10;

import android.content.Context;
import android.opengl.GLSurfaceView;


public class Compass3DRenderer implements GLSurfaceView.Renderer {
    Context mContext;

    // a raw buffer to hold indices
    ShortBuffer _indexBuffer;    
    // raw buffers to hold the vertices
    FloatBuffer _vertexBuffer0;
    FloatBuffer _vertexBuffer1;
    FloatBuffer _vertexBuffer2;
    FloatBuffer _vertexBuffer3;
    FloatBuffer _vertexBuffer4;
    FloatBuffer _vertexBuffer5;
    int _numVertices = 3; //standard triangle vertices = 3

    FloatBuffer _textureBuffer0123;



    //private FloatBuffer _light0Position;
    //private FloatBuffer _light0Ambient;
    float _light0Position[] = new float[]{10.0f, 10.0f, 10.0f, 0.0f};
    float _light0Ambient[] = new float[]{0.05f, 0.05f, 0.05f, 1.0f};
    float _light0Diffuse[] = new float[]{0.5f, 0.5f, 0.5f, 1.0f};
    float _light0Specular[] = new float[]{0.7f, 0.7f, 0.7f, 1.0f};
    float _matAmbient[] = new float[] { 0.6f, 0.6f, 0.6f, 1.0f };
    float _matDiffuse[] = new float[] { 0.6f, 0.6f, 0.6f, 1.0f };




    private float _angleX=0f;
    private float _angleY=0f;
    private float _angleZ=0f;


    Compass3DRenderer(Context context){
        super();
        mContext = context;
    }

    public void setAngleX(float angle) {
        _angleX = angle;
    }

    public void setAngleY(float angle) {
        _angleY = angle;
    }

    public void setAngleZ(float angle) {
        _angleZ = angle;
    }

    FloatBuffer InitFloatBuffer(float[] src){
        ByteBuffer bb = ByteBuffer.allocateDirect(4*src.length);
        bb.order(ByteOrder.nativeOrder());
        FloatBuffer inBuf = bb.asFloatBuffer();
        inBuf.put(src);
        return inBuf;
    }

    ShortBuffer InitShortBuffer(short[] src){
        ByteBuffer bb = ByteBuffer.allocateDirect(2*src.length);
        bb.order(ByteOrder.nativeOrder());
        ShortBuffer inBuf = bb.asShortBuffer();
        inBuf.put(src);
        return inBuf;
    }

    //Init data for our rendered pyramid
    private void initTriangles() {

        //Side faces triangles
        float[] coords = {
            -0.25f, -0.5f, 0.25f,
            0.25f, -0.5f, 0.25f,
            0f, 0.5f, 0f
        };

        float[] coords1 = {
            0.25f, -0.5f, 0.25f,
            0.25f, -0.5f, -0.25f,
            0f, 0.5f, 0f
        };

        float[] coords2 = {
            0.25f, -0.5f, -0.25f,
            -0.25f, -0.5f, -0.25f,
            0f, 0.5f, 0f
        };

        float[] coords3 = {
            -0.25f, -0.5f, -0.25f,
            -0.25f, -0.5f, 0.25f,
            0f, 0.5f, 0f
        };

        //Base triangles
        float[] coords4 = {
            -0.25f, -0.5f, 0.25f,
            0.25f, -0.5f, -0.25f,
            0.25f, -0.5f, 0.25f
        };

        float[] coords5 = {
            -0.25f, -0.5f, 0.25f,
            -0.25f, -0.5f, -0.25f, 
            0.25f, -0.5f, -0.25f
        };


        float[] textures0123 = {
                // Mapping coordinates for the vertices (UV mapping CW)
                0.0f, 0.0f,     // bottom left                    
                1.0f, 0.0f,     // bottom right
                0.5f, 1.0f,     // top ctr              
        };


        _vertexBuffer0 = InitFloatBuffer(coords);
        _vertexBuffer0.position(0);

        _vertexBuffer1 = InitFloatBuffer(coords1);
        _vertexBuffer1.position(0);    

        _vertexBuffer2 = InitFloatBuffer(coords2);
        _vertexBuffer2.position(0);

        _vertexBuffer3 = InitFloatBuffer(coords3);
        _vertexBuffer3.position(0);

        _vertexBuffer4 = InitFloatBuffer(coords4);
        _vertexBuffer4.position(0);

        _vertexBuffer5 = InitFloatBuffer(coords5);
        _vertexBuffer5.position(0);

        _textureBuffer0123 = InitFloatBuffer(textures0123);
        _textureBuffer0123.position(0);

        short[] indices = {0, 1, 2};
        _indexBuffer = InitShortBuffer(indices);        
        _indexBuffer.position(0);

    }


    public void onSurfaceCreated(GL10 gl, EGLConfig config) {

        gl.glEnable(GL10.GL_CULL_FACE); // enable the differentiation of which side may be visible 
        gl.glShadeModel(GL10.GL_SMOOTH);

        gl.glFrontFace(GL10.GL_CCW); // which is the front? the one which is drawn counter clockwise
        gl.glCullFace(GL10.GL_BACK); // which one should NOT be drawn

        initTriangles();

        gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
        gl.glEnableClientState(GL10.GL_TEXTURE_COORD_ARRAY);
    }

    public void onDrawFrame(GL10 gl) {


        gl.glPushMatrix();

        gl.glClearColor(0, 0, 0, 1.0f); //clipping backdrop color
        // clear the color buffer to show the ClearColor we called above...
        gl.glClear(GL10.GL_COLOR_BUFFER_BIT);

        // set rotation       
        gl.glRotatef(_angleY, 0f, 1f, 0f); //ROLL
        gl.glRotatef(_angleX, 1f, 0f, 0f); //ELEVATION
        gl.glRotatef(_angleZ, 0f, 0f, 1f); //AZIMUTH

        //Draw our pyramid

        //4 side faces
        gl.glColor4f(0.5f, 0f, 0f, 0.5f);
        gl.glVertexPointer(3, GL10.GL_FLOAT, 0, _vertexBuffer0);
        gl.glDrawElements(GL10.GL_TRIANGLES, _numVertices, GL10.GL_UNSIGNED_SHORT, _indexBuffer);

        gl.glColor4f(0.5f, 0.5f, 0f, 0.5f);
        gl.glVertexPointer(3, GL10.GL_FLOAT, 0, _vertexBuffer1);
        gl.glDrawElements(GL10.GL_TRIANGLES, _numVertices, GL10.GL_UNSIGNED_SHORT, _indexBuffer);

        gl.glColor4f(0f, 0.5f, 0f, 0.5f);
        gl.glVertexPointer(3, GL10.GL_FLOAT, 0, _vertexBuffer2);
        gl.glDrawElements(GL10.GL_TRIANGLES, _numVertices, GL10.GL_UNSIGNED_SHORT, _indexBuffer);

        gl.glColor4f(0f, 0.5f, 0.5f, 0.5f);
        gl.glVertexPointer(3, GL10.GL_FLOAT, 0, _vertexBuffer3);
        gl.glDrawElements(GL10.GL_TRIANGLES, _numVertices, GL10.GL_UNSIGNED_SHORT, _indexBuffer);

        //Base face
        gl.glColor4f(0f, 0f, 0.5f, 0.5f);
        gl.glVertexPointer(3, GL10.GL_FLOAT, 0, _vertexBuffer4);
        gl.glDrawElements(GL10.GL_TRIANGLES, _numVertices, GL10.GL_UNSIGNED_SHORT, _indexBuffer);
        gl.glVertexPointer(3, GL10.GL_FLOAT, 0, _vertexBuffer5);
        gl.glDrawElements(GL10.GL_TRIANGLES, _numVertices, GL10.GL_UNSIGNED_SHORT, _indexBuffer);

        gl.glPopMatrix();
    }

    public void onSurfaceChanged(GL10 gl, int w, int h) {
        gl.glViewport(0, 0, w, h);
        gl.glViewport(0, 0, w, h);

    }



}

Please note that this code does not compensate for tablet default landscape orientation, so it is only expected to work correctly on a phone (I didn’t have a tablet close by to test any correction code).