Android Compass that can Compensate for Tilt and Pitch

Question

I'm trying to make a application on my Android phone (Nexus 4), which will be used in a model boat. I've added low pass filters to filter out the gitter from the sensors.

However, the compass is only stable when the phone is flat on its back. If I tilt it up, (such as turning a page of a booK), then the compass heading goes way off - as much as 50*.

I've tried this with Sensor.TYPE_MAGNETIC_FIELD with either Sensor.TYPE_GRAVITY and Sensor.TYPE_ACCELEROMETER and the effect is the same.

I've used the solution mentioned here, and many other places. My maths is not great but this must be a common problem and I find it frustrating that there is not an API to deal with it.

I've been working on this problem for 3 days and have still not found any solution, but when I use the Compass from Catch, theirs stays stable no matter how much the phone is inclined. So I know it must be possible.

All I want to do is create a compass that if the phone is pointing say north, then the compass will read north, and not jump around when the phone is moved through any other axis (roll or pitch).

Can anyone please help before I have to abandon my project.

Thanks, Adam

Answer 1

By co-incidence I've been thinking about this problem for several weeks, because

1. As a mathematician, I haven't been satisfied by any of the answers that I've seen suggested elsewhere; and
2. I need a good answer for an app that I'm working on.

So over the last couple of days I've come up with my own way of calculating the azimuth value for use in a compass.

I've put that maths that I'm using here on math.stackexchange.com, and I've pasted the code I've used below. The code calculates the azimuth and pitch from the raw TYPE_GRAVITY and TYPE_MAGNETIC_FIELD sensor data, without any API calls to e.g. SensorManager.getRotationMatrix(...) or SensorManager.getOrientation(...). The code could probably be improved e.g. by using a low pass filter if the inputs turn out to be a bit erratic. Note that the code records the accuracy of the sensors via the method onAccuracyChanged(Sensor sensor, int accuracy), so if the azimuth seems unstable another thing to check is how accurate each sensor is. In any case, with all the calculations explicitly visible in this code, if there are instability problems (when the sensor accuracy is reasonable) then they could be tackled by looking at the instabilities in the inputs or in the direction vectors m_NormGravityVector[], m_NormEastVector[] or m_NormNorthVector[].

I'd be very interested in any feedback that anyone has for me on this method. I find that it works like a dream in my own app, as long as the device is flat face up, vertical, or somewhere in between. However, as I mention in the math.stackexchange.com article, there are issues that arise as the device gets close to being turned upside down. In that situation, one would need to define carefully what behaviour one wants.

    import android.app.Activity;
    import android.hardware.Sensor;
    import android.hardware.SensorEvent;
    import android.hardware.SensorEventListener;
    import android.hardware.SensorManager;
    import android.view.Surface;

    public static class OrientationSensor implements  SensorEventListener {

    public final static int SENSOR_UNAVAILABLE = -1;

    // references to other objects
    SensorManager m_sm;
    SensorEventListener m_parent;   // non-null if this class should call its parent after onSensorChanged(...) and onAccuracyChanged(...) notifications
    Activity m_activity;            // current activity for call to getWindowManager().getDefaultDisplay().getRotation()

    // raw inputs from Android sensors
    float m_Norm_Gravity;           // length of raw gravity vector received in onSensorChanged(...).  NB: should be about 10
    float[] m_NormGravityVector;    // Normalised gravity vector, (i.e. length of this vector is 1), which points straight up into space
    float m_Norm_MagField;          // length of raw magnetic field vector received in onSensorChanged(...). 
    float[] m_NormMagFieldValues;   // Normalised magnetic field vector, (i.e. length of this vector is 1)

    // accuracy specifications. SENSOR_UNAVAILABLE if unknown, otherwise SensorManager.SENSOR_STATUS_UNRELIABLE, SENSOR_STATUS_ACCURACY_LOW, SENSOR_STATUS_ACCURACY_MEDIUM or SENSOR_STATUS_ACCURACY_HIGH
    int m_GravityAccuracy;          // accuracy of gravity sensor
    int m_MagneticFieldAccuracy;    // accuracy of magnetic field sensor

    // values calculated once gravity and magnetic field vectors are available
    float[] m_NormEastVector;       // normalised cross product of raw gravity vector with magnetic field values, points east
    float[] m_NormNorthVector;      // Normalised vector pointing to magnetic north
    boolean m_OrientationOK;        // set true if m_azimuth_radians and m_pitch_radians have successfully been calculated following a call to onSensorChanged(...)
    float m_azimuth_radians;        // angle of the device from magnetic north
    float m_pitch_radians;          // tilt angle of the device from the horizontal.  m_pitch_radians = 0 if the device if flat, m_pitch_radians = Math.PI/2 means the device is upright.
    float m_pitch_axis_radians;     // angle which defines the axis for the rotation m_pitch_radians

    public OrientationSensor(SensorManager sm, SensorEventListener parent) {
        m_sm = sm;
        m_parent = parent;
        m_activity = null;
        m_NormGravityVector = m_NormMagFieldValues = null;
        m_NormEastVector = new float[3];
        m_NormNorthVector = new float[3];
        m_OrientationOK = false;
    }

    public int Register(Activity activity, int sensorSpeed) {
        m_activity = activity;  // current activity required for call to getWindowManager().getDefaultDisplay().getRotation()
        m_NormGravityVector = new float[3];
        m_NormMagFieldValues = new float[3];
        m_OrientationOK = false;
        int count = 0;
        Sensor SensorGravity = m_sm.getDefaultSensor(Sensor.TYPE_GRAVITY);
        if (SensorGravity != null) {
            m_sm.registerListener(this, SensorGravity, sensorSpeed);
            m_GravityAccuracy = SensorManager.SENSOR_STATUS_ACCURACY_HIGH;
            count++;
        } else {
            m_GravityAccuracy = SENSOR_UNAVAILABLE;
        }
        Sensor SensorMagField = m_sm.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
        if (SensorMagField != null) {
            m_sm.registerListener(this, SensorMagField, sensorSpeed);
            m_MagneticFieldAccuracy = SensorManager.SENSOR_STATUS_ACCURACY_HIGH;     
            count++;
        } else {
            m_MagneticFieldAccuracy = SENSOR_UNAVAILABLE;
        }
        return count;
    }

    public void Unregister() {
        m_activity = null;
        m_NormGravityVector = m_NormMagFieldValues = null;
        m_OrientationOK = false;
        m_sm.unregisterListener(this);
    }

    @Override
    public void onSensorChanged(SensorEvent evnt) {
        int SensorType = evnt.sensor.getType();
        switch(SensorType) {
            case Sensor.TYPE_GRAVITY:
                if (m_NormGravityVector == null) m_NormGravityVector = new float[3];
                System.arraycopy(evnt.values, 0, m_NormGravityVector, 0, m_NormGravityVector.length);                   
                m_Norm_Gravity = (float)Math.sqrt(m_NormGravityVector[0]*m_NormGravityVector[0] + m_NormGravityVector[1]*m_NormGravityVector[1] + m_NormGravityVector[2]*m_NormGravityVector[2]);
                for(int i=0; i < m_NormGravityVector.length; i++) m_NormGravityVector[i] /= m_Norm_Gravity;
                break;
            case Sensor.TYPE_MAGNETIC_FIELD:
                if (m_NormMagFieldValues == null) m_NormMagFieldValues = new float[3];
                System.arraycopy(evnt.values, 0, m_NormMagFieldValues, 0, m_NormMagFieldValues.length);
                m_Norm_MagField = (float)Math.sqrt(m_NormMagFieldValues[0]*m_NormMagFieldValues[0] + m_NormMagFieldValues[1]*m_NormMagFieldValues[1] + m_NormMagFieldValues[2]*m_NormMagFieldValues[2]);
                for(int i=0; i < m_NormMagFieldValues.length; i++) m_NormMagFieldValues[i] /= m_Norm_MagField;  
                break;
        }
        if (m_NormGravityVector != null && m_NormMagFieldValues != null) {
            // first calculate the horizontal vector that points due east
            float East_x = m_NormMagFieldValues[1]*m_NormGravityVector[2] - m_NormMagFieldValues[2]*m_NormGravityVector[1];
            float East_y = m_NormMagFieldValues[2]*m_NormGravityVector[0] - m_NormMagFieldValues[0]*m_NormGravityVector[2];
            float East_z = m_NormMagFieldValues[0]*m_NormGravityVector[1] - m_NormMagFieldValues[1]*m_NormGravityVector[0];
            float norm_East = (float)Math.sqrt(East_x * East_x + East_y * East_y + East_z * East_z);
            if (m_Norm_Gravity * m_Norm_MagField * norm_East < 0.1f) {  // Typical values are  > 100.
                m_OrientationOK = false; // device is close to free fall (or in space?), or close to magnetic north pole.
            } else {
                m_NormEastVector[0] = East_x / norm_East; m_NormEastVector[1] = East_y / norm_East; m_NormEastVector[2] = East_z / norm_East;

                // next calculate the horizontal vector that points due north                   
                float M_dot_G = (m_NormGravityVector[0] *m_NormMagFieldValues[0] + m_NormGravityVector[1]*m_NormMagFieldValues[1] + m_NormGravityVector[2]*m_NormMagFieldValues[2]);
                float North_x = m_NormMagFieldValues[0] - m_NormGravityVector[0] * M_dot_G;
                float North_y = m_NormMagFieldValues[1] - m_NormGravityVector[1] * M_dot_G;
                float North_z = m_NormMagFieldValues[2] - m_NormGravityVector[2] * M_dot_G;
                float norm_North = (float)Math.sqrt(North_x * North_x + North_y * North_y + North_z * North_z);
                m_NormNorthVector[0] = North_x / norm_North; m_NormNorthVector[1] = North_y / norm_North; m_NormNorthVector[2] = North_z / norm_North;

                // take account of screen rotation away from its natural rotation
                int rotation = m_activity.getWindowManager().getDefaultDisplay().getRotation();
                float screen_adjustment = 0;
                switch(rotation) {
                    case Surface.ROTATION_0:   screen_adjustment =          0;         break;
                    case Surface.ROTATION_90:  screen_adjustment =   (float)Math.PI/2; break;
                    case Surface.ROTATION_180: screen_adjustment =   (float)Math.PI;   break;
                    case Surface.ROTATION_270: screen_adjustment = 3*(float)Math.PI/2; break;
                }
                // NB: the rotation matrix has now effectively been calculated. It consists of the three vectors m_NormEastVector[], m_NormNorthVector[] and m_NormGravityVector[]

                // calculate all the required angles from the rotation matrix
                // NB: see https://math.stackexchange.com/questions/381649/whats-the-best-3d-angular-co-ordinate-system-for-working-with-smartfone-apps
                float sin = m_NormEastVector[1] -  m_NormNorthVector[0], cos = m_NormEastVector[0] +  m_NormNorthVector[1];
                m_azimuth_radians = (float) (sin != 0 && cos != 0 ? Math.atan2(sin, cos) : 0);
                m_pitch_radians = (float) Math.acos(m_NormGravityVector[2]);
                sin = -m_NormEastVector[1] -  m_NormNorthVector[0]; cos = m_NormEastVector[0] -  m_NormNorthVector[1];
                float aximuth_plus_two_pitch_axis_radians = (float)(sin != 0 && cos != 0 ? Math.atan2(sin, cos) : 0);
                m_pitch_axis_radians = (float)(aximuth_plus_two_pitch_axis_radians - m_azimuth_radians) / 2;
                m_azimuth_radians += screen_adjustment;
                m_pitch_axis_radians += screen_adjustment;
                m_OrientationOK = true;                                 
            }
        }
        if (m_parent != null) m_parent.onSensorChanged(evnt);
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int accuracy) {
        int SensorType = sensor.getType();
        switch(SensorType) {
            case Sensor.TYPE_GRAVITY: m_GravityAccuracy = accuracy; break;
            case Sensor.TYPE_MAGNETIC_FIELD: m_MagneticFieldAccuracy = accuracy; break;
        }
        if (m_parent != null) m_parent.onAccuracyChanged(sensor, accuracy);
    }
}

Answer 2

OK, think I solved it.

Rather than using Sensor.TYPE_ACCELEROMETER (or TYPE_GRAVITY) and Sensor.TYPE_MAGNETIC_FIELD, I used Sensor.TYPE_ROTATION_VECTOR with:

float[] roationV = new float[16];
SensorManager.getRotationMatrixFromVector(roationV, rotationVector);

float[] orientationValuesV = new float[3];
SensorManager.getOrientation(roationV, orientationValuesV);

This returned a stable azimuth no matter the roll or pitch of the phone.

If you look here at the Android Motion Sensors, just beneath Table 1, it says that the ROTATION sensor is ideal for compass, augmented reality etc.

So easy when you know how.... However, I've yet to test this over time to see if errors are introduced.

Answer 3

The problem you've got is probably Gimbal lock. If you think about it, when the phone is upright so the pitch is plus or minus 90 degrees, then azimuth and roll are the same thing. If you look into the maths, you'll see that in that situation either azimuth+roll or azimuth-roll is well defined, but they're not defined individually. So when the pitch gets close to plus or minus 90 degrees the readings become unstable. Some people choose to remap the co-ordinate system tom try and get round this, see e.g. How should I calculate azimuth, pitch, orientation when my Android device isn't flat?, so perhaps that might work for you.

Answer 4

This is an other way to get the magnetic heading without being affected by pitch or roll.

private final static double PI = Math.PI;
private final static double TWO_PI = PI*2;

 case Sensor.TYPE_ROTATION_VECTOR:
                float[] orientation = new float[3];
                float[] rotationMatrix = new float[9];

                SensorManager.getRotationMatrixFromVector(rotationMatrix, rawValues);
                SensorManager.getOrientation(rotationMatrix, orientation);

                float heading = mod(orientation[0] + TWO_PI,TWO_PI);//important
                //do something with the heading
                break;



private double mod(double a, double b){
        return a % b;
    }

Answer 5

Take a look at Mixare, an open source augmented reality tool for android and iphone, it has some great stuff in there about compensating the phones orientation/position to show things correctly on the screen.

EDIT: in particular take a look at the MixView java class which handles the sensor events.

Answer 6

i found that on certain smartphone models , the activation of the camera can change COMPASS datas... 1/10 grads... (related to light of the scene)

black scene... 1/2 .... very white scene (10 or more grads)