By co-incidence I’ve been thinking about this problem for several weeks, because
- As a mathematician, I haven’t been satisfied by any of the answers that I’ve seen suggested elsewhere; and
- 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);
}
}