Develop Reference

2D values from Quaternion

i'm working in a AR application in android with the Epson Moverio BT-200.
I have a quaternion that change his values with my sensor fusion algorithm.
In my application i'm trying to move a 2D item changing his margin left and margin top values when I move my head.
I'd like to know how can I extract, from the quaternion values, only the "horizontal" and "vertical" movements.
I could extract from the quaternion the pitch and roll values, but I read that there are several problems with euler angle. Could I do this only working with quaternions?

This is my actual code. I solved the problem using the Quaternions for the algorithm, and at the end I extract the euler angles from the rotation matrix.
This is the algorithm for take the values from the sensors:
private static final float NS2S = 1.0f / 1000000000.0f;
private final Quaternion deltaQuaternion = new Quaternion();
private Quaternion quaternionGyroscope = new Quaternion();
private Quaternion quaternionRotationVector = new Quaternion();
private long timestamp;
private static final double EPSILON = 0.1f;
private double gyroscopeRotationVelocity = 0;
private boolean positionInitialised = false;
private int panicCounter;
private static final float DIRECT_INTERPOLATION_WEIGHT = 0.005f;
private static final float OUTLIER_THRESHOLD = 0.85f;
private static final float OUTLIER_PANIC_THRESHOLD = 0.65f;
private static final int PANIC_THRESHOLD = 60;
#Override
public void onSensorChanged(SensorEvent event) {
if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) {
// Process rotation vector (just safe it)
float[] q = new float[4];
// Calculate angle. Starting with API_18, Android will provide this value as event.values[3], but if not, we have to calculate it manually.
SensorManager.getQuaternionFromVector(q, event.values);
// Store in quaternion
quaternionRotationVector.setXYZW(q[1], q[2], q[3], -q[0]);
if (!positionInitialised) {
// Override
quaternionGyroscope.set(quaternionRotationVector);
positionInitialised = true;
}
} else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
// Process Gyroscope and perform fusion
// This timestep's delta rotation to be multiplied by the current rotation
// after computing it from the gyro sample data.
if (timestamp != 0) {
final float dT = (event.timestamp - timestamp) * NS2S;
// Axis of the rotation sample, not normalized yet.
float axisX = event.values[0];
float axisY = event.values[1];
float axisZ = event.values[2];
// Calculate the angular speed of the sample
gyroscopeRotationVelocity = Math.sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);
// Normalize the rotation vector if it's big enough to get the axis
if (gyroscopeRotationVelocity > EPSILON) {
axisX /= gyroscopeRotationVelocity;
axisY /= gyroscopeRotationVelocity;
axisZ /= gyroscopeRotationVelocity;
}
// Integrate around this axis with the angular speed by the timestep
// in order to get a delta rotation from this sample over the timestep
// We will convert this axis-angle representation of the delta rotation
// into a quaternion before turning it into the rotation matrix.
double thetaOverTwo = gyroscopeRotationVelocity * dT / 2.0f;
double sinThetaOverTwo = Math.sin(thetaOverTwo);
double cosThetaOverTwo = Math.cos(thetaOverTwo);
deltaQuaternion.setX((float) (sinThetaOverTwo * axisX));
deltaQuaternion.setY((float) (sinThetaOverTwo * axisY));
deltaQuaternion.setZ((float) (sinThetaOverTwo * axisZ));
deltaQuaternion.setW(-(float) cosThetaOverTwo);
// Move current gyro orientation
deltaQuaternion.multiplyByQuat(quaternionGyroscope, quaternionGyroscope);
// Calculate dot-product to calculate whether the two orientation sensors have diverged
// (if the dot-product is closer to 0 than to 1), because it should be close to 1 if both are the same.
float dotProd = quaternionGyroscope.dotProduct(quaternionRotationVector);
// If they have diverged, rely on gyroscope only (this happens on some devices when the rotation vector "jumps").
if (Math.abs(dotProd) < OUTLIER_THRESHOLD) {
// Increase panic counter
if (Math.abs(dotProd) < OUTLIER_PANIC_THRESHOLD) {
panicCounter++;
}
// Directly use Gyro
setOrientationQuaternionAndMatrix(quaternionGyroscope);
} else {
// Both are nearly saying the same. Perform normal fusion.
// Interpolate with a fixed weight between the two absolute quaternions obtained from gyro and rotation vector sensors
// The weight should be quite low, so the rotation vector corrects the gyro only slowly, and the output keeps responsive.
Quaternion interpolate = new Quaternion();
quaternionGyroscope.slerp(quaternionRotationVector, interpolate, DIRECT_INTERPOLATION_WEIGHT);
// Use the interpolated value between gyro and rotationVector
setOrientationQuaternionAndMatrix(interpolate);
// Override current gyroscope-orientation
quaternionGyroscope.copyVec4(interpolate);
// Reset the panic counter because both sensors are saying the same again
panicCounter = 0;
}
if (panicCounter > PANIC_THRESHOLD) {
Log.d("Rotation Vector",
"Panic counter is bigger than threshold; this indicates a Gyroscope failure. Panic reset is imminent.");
if (gyroscopeRotationVelocity < 3) {
Log.d("Rotation Vector",
"Performing Panic-reset. Resetting orientation to rotation-vector value.");
// Manually set position to whatever rotation vector says.
setOrientationQuaternionAndMatrix(quaternionRotationVector);
// Override current gyroscope-orientation with corrected value
quaternionGyroscope.copyVec4(quaternionRotationVector);
panicCounter = 0;
} else {
Log.d("Rotation Vector",
String.format(
"Panic reset delayed due to ongoing motion (user is still shaking the device). Gyroscope Velocity: %.2f > 3",
gyroscopeRotationVelocity));
}
}
}
timestamp = event.timestamp;
}
}
private void setOrientationQuaternionAndMatrix(Quaternion quaternion) {
Quaternion correctedQuat = quaternion.clone();
// We inverted w in the deltaQuaternion, because currentOrientationQuaternion required it.
// Before converting it back to matrix representation, we need to revert this process
correctedQuat.w(-correctedQuat.w());
synchronized (syncToken) {
// Use gyro only
currentOrientationQuaternion.copyVec4(quaternion);
// Set the rotation matrix as well to have both representations
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuat.ToArray());
}
}
And this is how I take the euler angles rotation values:
/**
* #return Returns the current rotation of the device in the Euler-Angles
*/
public EulerAngles getEulerAngles() {
float[] angles = new float[3];
float[] remappedOrientationMatrix = new float[16];
SensorManager.remapCoordinateSystem(currentOrientationRotationMatrix.getMatrix(), SensorManager.AXIS_X,
SensorManager.AXIS_Z, remappedOrientationMatrix);
SensorManager.getOrientation(remappedOrientationMatrix, angles);
return new EulerAngles(angles[0], angles[1], angles[2]);
}
I solved my problem with this solution. Now won't be difficult to move my 2d Object with this sensors values. Sorry for lenght of my answer, but I hope that it could be useful for someone :)

Android AR orientation

I'm making program for showing objects from map on camera and this works almost well except few degrees to left and right from vertical orientation (like in 80-110 and 260-280 degrees). In other +-320 degrees it works well. I've tried to use TYPE_ROTATION_VECTOR and accelerometer with magnetometer and they have the same result. Does anybody know any solution?
with TYPE_ROTATION_VECTOR:
if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR)
{
float[] roationV = new float[16];
SensorManager.getRotationMatrixFromVector(roationV, event.values);
float[] orientationValuesV = new float[3];
SensorManager.getOrientation(roationV, orientationValuesV);
tvHeading.setText(String.format(
"Coordinates: lat = %1$.2f, lon = %2$.2f, time = %3$.2f",
orientationValuesV[0], orientationValuesV[1], orientationValuesV[2]));
float[] rotationMatrix=new float[16];
mSensorManager.getRotationMatrixFromVector(rotationMatrix, event.values);
float[] orientationValues = new float[3];
SensorManager.getOrientation(rotationMatrix, orientationValues);
double azimuth = Math.toDegrees(orientationValues[0]);
double pitch = Math.toDegrees(orientationValues[1]);
double roll = Math.toDegrees(orientationValues[2]);
tvOrientation.setText(String.format(
"Coordinates: lat = %1$.2f, lon = %2$.2f, time = %3$.2f",
azimuth,pitch,roll));
}
with accelerometer+magnetometer
if (event.sensor == mAccelerometer) {
System.arraycopy(event.values, 0, mLastAccelerometer, 0, event.values.length);
mLastAccelerometer = meanFilterAccelSmoothing
.addSamples(mLastAccelerometer);
mLastAccelerometer = medianFilterAccelSmoothing
.addSamples(mLastAccelerometer);
for (int i = 0; i < mLastAccelerometer.length; i++) {
mLastAccelerometer[i] = (float) Math.floor(mLastAccelerometer[i] * 1000) / 1000;
}
mLastAccelerometerSet = true;
}
if (event.sensor == mMagnetometer) {
System.arraycopy(event.values, 0, mLastMagnetometer, 0, event.values.length);
mLastMagnetometer = meanFilterMagneticSmoothing.addSamples(mLastMagnetometer);
mLastMagnetometer = medianFilterMagneticSmoothing.addSamples(mLastMagnetometer);
for (int i = 0; i < mLastMagnetometer.length; i++) {
mLastMagnetometer[i] = (float) Math.floor(mLastMagnetometer[i] * 1000) / 1000;
}
mLastMagnetometerSet = true;
}
if (mLastAccelerometerSet && mLastMagnetometerSet) {
SensorManager.getRotationMatrix(mR, null, mLastAccelerometer, mLastMagnetometer);
SensorManager.getOrientation(mR, mOrientation);
if (angeles.size() > 0) {
for (int i = 0; i < mapObjects.size(); i++) {
compassFunc(i, mOrientation[0], mOrientation[1], mOrientation[2]);
}
}
private void compassFunc(int number, float... values) {
double angularXSpeed = Math.floor(values[0] * 180 / Math.PI * 100) / 100;
double angularYSpeed = Math.floor(values[1] * 180 / Math.PI * 100) / 100;
double angularZSpeed = Math.floor(values[2] * 180 / Math.PI * 100) / 100;
tvOrientation.setText(String.format(
"Screen: lt= %1$.2f : %2$.2f,rt= %3$.2f : %4$.2f,lb= %5$.2f : %6$.2f,rb= %7$.2f : %8$.2f",
xLeftTop, yLeftTop, xRightTop,yRightTop,xLeftBottom,yLeftBottom,xRightBottom,yRightBottom));
}

This sounds like a typical case of Gimbal Lock. Your description of how rotation around one axis acts up when another reaches +-90 degrees suggests that this is indeed the case.
This is a fundamental problem with Euler angles (Yaw/Azimuth, Pitch, Roll), which is why most such computations are done using rotation matrices or quaternions, and Euler angles most often only are used when a particular orientation is to be displayed to a human (humans are generally bad at interpreting rotation matrices and quaternions).
The ROTATION_VECTOR sensor outputs it's data in a quaternion format (source), albeit with rearranged values, and the getRotationMatrixFromVector() method turns this into a rotation matrix. I would suggest using one of these descriptions for your internal calculations.
The answers to this similar question provide some concrete suggestions on how to solve the issue.

How to get smooth orientation data in android

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.

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

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.

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

Compass readings on SGS III

My app needs to show the current bearing of the device using its compass. The code I'm using (below) works perfectly fine on my Galaxy Nexus and Galaxy One, but the compass is spinning around wildly on a Samsung Galaxy S III. I've tried doing a figure-8 to recalibrate the device, but that doesn't change anything. The weird thing is that other compass apps downloaded from Google Play work just fine on the SIII. What could be the issue here?
float[] mGravity;
float[] mGeomagnetic;
public void onSensorChanged( SensorEvent event ) {
float azimuth = 0f;
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
mGravity = event.values;
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
mGeomagnetic = event.values;
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]; // orientation contains: azimut, pitch and roll
}
}
//Discard 0.0-values
if(azimuth == 0.0) { return; }
//Convert the sensor value to degrees
azimuth = (float) Math.toDegrees(azimuth); //same as azimuth = -azimuth*360/(2*3.14159f);
//Smooth the sensor-output
azimuth = smoothValues(azimuth);
}
//From http://stackoverflow.com/questions/4699417/android-compass-orientation-on-unreliable-low-pass-filter
//SmoothFactorCompass: The easing float that defines how smooth the movement will be (1 is no smoothing and 0 is never updating, my default is 0.5).
//SmoothThresholdCompass: The threshold in which the distance is big enough to turn immediately (0 is jump always, 360 is never jumping, my default is 30).
static final float SmoothFactorCompass = 0.5f;
static final float SmoothThresholdCompass = 30.0f;
float oldCompass = 0.0f;
private float smoothValues (float newCompass){
if (Math.abs(newCompass - oldCompass) < 180) {
if (Math.abs(newCompass - oldCompass) > SmoothThresholdCompass) {
oldCompass = newCompass;
}
else {
oldCompass = oldCompass + SmoothFactorCompass * (newCompass - oldCompass);
}
}
else {
if (360.0 - Math.abs(newCompass - oldCompass) > SmoothThresholdCompass) {
oldCompass = newCompass;
}
else {
if (oldCompass > newCompass) {
oldCompass = (oldCompass + SmoothFactorCompass * ((360 + newCompass - oldCompass) % 360) + 360) % 360;
}
else {
oldCompass = (oldCompass - SmoothFactorCompass * ((360 - newCompass + oldCompass) % 360) + 360) % 360;
}
}
}
return oldCompass;
}

Currently I am investigating compass mechanism on Android and I would recommend to start with low-pass filter in your case.
What you need to do - is to apply low-pass filter to both ACCELEROMETER and MAGNETIC_FIELD sensors data.
Here is how I implemented that:
private float[] accel;
private float[] geomagnetic;
float R[] = new float[9];
float I[] = new float[9];
float orientation[] = new float[3];
#Override
public void onSensorChanged(SensorEvent event)
{
synchronized (this)
{
float azimuth = -1f;
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
accel = lowPass( event.values.clone(), accel );
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
geomagnetic = lowPass(event.values.clone(), geomagnetic);
if (accel != null && geomagnetic != null)
{
boolean success = SensorManager.getRotationMatrix(R, I,
accel, geomagnetic);
SensorManager.remapCoordinateSystem(R,
SensorManager.AXIS_X, SensorManager.AXIS_Z, R);
if (success)
{
SensorManager.getOrientation(R, orientation);
azimuth = orientation[0]; // orientation contains:
// azimuth, pitch
// and roll
float newHeading = azimuth * 360 / (2 * 3.14159f);
//do what you need to do with new heading
}
}
}
}
/*
* time smoothing constant for low-pass filter 0 ≤ alpha ≤ 1 ; a smaller
* value basically means more smoothing See:
* http://en.wikipedia.org/wiki/Low-pass_filter#Discrete-time_realization
*/
static final float ALPHA = 0.15f;
/**
* #see http
* ://en.wikipedia.org/wiki/Low-pass_filter#Algorithmic_implementation
* #see http
* ://developer.android.com/reference/android/hardware/SensorEvent.html
* #values
*/
protected float[] lowPass(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;
}

may be you should clone the value from the sensor reading first and then lowpass the filter if this does not work , I know SONY XPeria phone is very sensitive unlike samsung which is quite stable .
In my view , S3 reading is quite ok under android OS 4.3
event.values.clone;
My recent experience couple data feedback force me to apply rotation vector logic on SONY phone loaded with Invesense sensor and it seemed to be working , although I still do not like the choppy response but I have live with that kind of response on SONY android 4.3 OS . It is also appear to me that any phone loaded invensense sensor need to apply the rotation vector for it to work properly

Using Android gyroscope instead of accelerometer. I find lots of bits and pieces, but no complete code

The Sensor Fusion video looks great, but there's no code:
http://www.youtube.com/watch?v=C7JQ7Rpwn2k&feature=player_detailpage#t=1315s
Here is my code which just uses accelerometer and compass. I also use a Kalman filter on the 3 orientation values, but that's too much code to show here. Ultimately, this works ok, but the result is either too jittery or too laggy depending on what I do with the results and how low I make the filtering factors.
/** Just accelerometer and magnetic sensors */
public abstract class SensorsListener2
implements
SensorEventListener
{
/** The lower this is, the greater the preference which is given to previous values. (slows change) */
private static final float accelFilteringFactor = 0.1f;
private static final float magFilteringFactor = 0.01f;
public abstract boolean getIsLandscape();
#Override
public void onSensorChanged(SensorEvent event) {
Sensor sensor = event.sensor;
int type = sensor.getType();
switch (type) {
case Sensor.TYPE_MAGNETIC_FIELD:
mags[0] = event.values[0] * magFilteringFactor + mags[0] * (1.0f - magFilteringFactor);
mags[1] = event.values[1] * magFilteringFactor + mags[1] * (1.0f - magFilteringFactor);
mags[2] = event.values[2] * magFilteringFactor + mags[2] * (1.0f - magFilteringFactor);
isReady = true;
break;
case Sensor.TYPE_ACCELEROMETER:
accels[0] = event.values[0] * accelFilteringFactor + accels[0] * (1.0f - accelFilteringFactor);
accels[1] = event.values[1] * accelFilteringFactor + accels[1] * (1.0f - accelFilteringFactor);
accels[2] = event.values[2] * accelFilteringFactor + accels[2] * (1.0f - accelFilteringFactor);
break;
default:
return;
}
if(mags != null && accels != null && isReady) {
isReady = false;
SensorManager.getRotationMatrix(rot, inclination, accels, mags);
boolean isLandscape = getIsLandscape();
if(isLandscape) {
outR = rot;
} else {
// Remap the coordinates to work in portrait mode.
SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR);
}
SensorManager.getOrientation(outR, values);
double x180pi = 180.0 / Math.PI;
float azimuth = (float)(values[0] * x180pi);
float pitch = (float)(values[1] * x180pi);
float roll = (float)(values[2] * x180pi);
// In landscape mode swap pitch and roll and invert the pitch.
if(isLandscape) {
float tmp = pitch;
pitch = -roll;
roll = -tmp;
azimuth = 180 - azimuth;
} else {
pitch = -pitch - 90;
azimuth = 90 - azimuth;
}
onOrientationChanged(azimuth,pitch,roll);
}
}
private float[] mags = new float[3];
private float[] accels = new float[3];
private boolean isReady;
private float[] rot = new float[9];
private float[] outR = new float[9];
private float[] inclination = new float[9];
private float[] values = new float[3];
/**
Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West
Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis.
Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis.
*/
public abstract void onOrientationChanged(float azimuth, float pitch, float roll);
}
I tried to figure out how to add gyroscope data, but I am just not doing it right. The google doc at http://developer.android.com/reference/android/hardware/SensorEvent.html shows some code to get a delta matrix from the gyroscope data. The idea seems to be that I'd crank down the filters for the accelerometer and magnetic sensors so that they were really stable. That would keep track of the long term orientation.
Then, I'd keep a history of the most recent N delta matrices from the gyroscope. Each time I got a new one I'd drop off the oldest one and multiply them all together to get a final matrix which I would multiply against the stable matrix returned by the accelerometer and magnetic sensors.
This doesn't seem to work. Or, at least, my implementation of it does not work. The result is far more jittery than just the accelerometer. Increasing the size of the gyroscope history actually increases the jitter which makes me think that I'm not calculating the right values from the gyroscope.
public abstract class SensorsListener3
implements
SensorEventListener
{
/** The lower this is, the greater the preference which is given to previous values. (slows change) */
private static final float kFilteringFactor = 0.001f;
private static final float magKFilteringFactor = 0.001f;
public abstract boolean getIsLandscape();
#Override
public void onSensorChanged(SensorEvent event) {
Sensor sensor = event.sensor;
int type = sensor.getType();
switch (type) {
case Sensor.TYPE_MAGNETIC_FIELD:
mags[0] = event.values[0] * magKFilteringFactor + mags[0] * (1.0f - magKFilteringFactor);
mags[1] = event.values[1] * magKFilteringFactor + mags[1] * (1.0f - magKFilteringFactor);
mags[2] = event.values[2] * magKFilteringFactor + mags[2] * (1.0f - magKFilteringFactor);
isReady = true;
break;
case Sensor.TYPE_ACCELEROMETER:
accels[0] = event.values[0] * kFilteringFactor + accels[0] * (1.0f - kFilteringFactor);
accels[1] = event.values[1] * kFilteringFactor + accels[1] * (1.0f - kFilteringFactor);
accels[2] = event.values[2] * kFilteringFactor + accels[2] * (1.0f - kFilteringFactor);
break;
case Sensor.TYPE_GYROSCOPE:
gyroscopeSensorChanged(event);
break;
default:
return;
}
if(mags != null && accels != null && isReady) {
isReady = false;
SensorManager.getRotationMatrix(rot, inclination, accels, mags);
boolean isLandscape = getIsLandscape();
if(isLandscape) {
outR = rot;
} else {
// Remap the coordinates to work in portrait mode.
SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR);
}
if(gyroUpdateTime!=0) {
matrixHistory.mult(matrixTmp,matrixResult);
outR = matrixResult;
}
SensorManager.getOrientation(outR, values);
double x180pi = 180.0 / Math.PI;
float azimuth = (float)(values[0] * x180pi);
float pitch = (float)(values[1] * x180pi);
float roll = (float)(values[2] * x180pi);
// In landscape mode swap pitch and roll and invert the pitch.
if(isLandscape) {
float tmp = pitch;
pitch = -roll;
roll = -tmp;
azimuth = 180 - azimuth;
} else {
pitch = -pitch - 90;
azimuth = 90 - azimuth;
}
onOrientationChanged(azimuth,pitch,roll);
}
}
private void gyroscopeSensorChanged(SensorEvent event) {
// This timestep's delta rotation to be multiplied by the current rotation
// after computing it from the gyro sample data.
if(gyroUpdateTime != 0) {
final float dT = (event.timestamp - gyroUpdateTime) * NS2S;
// Axis of the rotation sample, not normalized yet.
float axisX = event.values[0];
float axisY = event.values[1];
float axisZ = event.values[2];
// Calculate the angular speed of the sample
float omegaMagnitude = (float)Math.sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ);
// Normalize the rotation vector if it's big enough to get the axis
if(omegaMagnitude > EPSILON) {
axisX /= omegaMagnitude;
axisY /= omegaMagnitude;
axisZ /= omegaMagnitude;
}
// Integrate around this axis with the angular speed by the timestep
// in order to get a delta rotation from this sample over the timestep
// We will convert this axis-angle representation of the delta rotation
// into a quaternion before turning it into the rotation matrix.
float thetaOverTwo = omegaMagnitude * dT / 2.0f;
float sinThetaOverTwo = (float)Math.sin(thetaOverTwo);
float cosThetaOverTwo = (float)Math.cos(thetaOverTwo);
deltaRotationVector[0] = sinThetaOverTwo * axisX;
deltaRotationVector[1] = sinThetaOverTwo * axisY;
deltaRotationVector[2] = sinThetaOverTwo * axisZ;
deltaRotationVector[3] = cosThetaOverTwo;
}
gyroUpdateTime = event.timestamp;
SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);
// User code should concatenate the delta rotation we computed with the current rotation
// in order to get the updated rotation.
// rotationCurrent = rotationCurrent * deltaRotationMatrix;
matrixHistory.add(deltaRotationMatrix);
}
private float[] mags = new float[3];
private float[] accels = new float[3];
private boolean isReady;
private float[] rot = new float[9];
private float[] outR = new float[9];
private float[] inclination = new float[9];
private float[] values = new float[3];
// gyroscope stuff
private long gyroUpdateTime = 0;
private static final float NS2S = 1.0f / 1000000000.0f;
private float[] deltaRotationMatrix = new float[9];
private final float[] deltaRotationVector = new float[4];
//TODO: I have no idea how small this value should be.
private static final float EPSILON = 0.000001f;
private float[] matrixMult = new float[9];
private MatrixHistory matrixHistory = new MatrixHistory(100);
private float[] matrixTmp = new float[9];
private float[] matrixResult = new float[9];
/**
Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West
Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis.
Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis.
*/
public abstract void onOrientationChanged(float azimuth, float pitch, float roll);
}
public class MatrixHistory
{
public MatrixHistory(int size) {
vals = new float[size][];
}
public void add(float[] val) {
synchronized(vals) {
vals[ix] = val;
ix = (ix + 1) % vals.length;
if(ix==0)
full = true;
}
}
public void mult(float[] tmp, float[] output) {
synchronized(vals) {
if(full) {
for(int i=0; i<vals.length; ++i) {
if(i==0) {
System.arraycopy(vals[i],0,output,0,vals[i].length);
} else {
MathUtils.multiplyMatrix3x3(output,vals[i],tmp);
System.arraycopy(tmp,0,output,0,tmp.length);
}
}
} else {
if(ix==0)
return;
for(int i=0; i<ix; ++i) {
if(i==0) {
System.arraycopy(vals[i],0,output,0,vals[i].length);
} else {
MathUtils.multiplyMatrix3x3(output,vals[i],tmp);
System.arraycopy(tmp,0,output,0,tmp.length);
}
}
}
}
}
private int ix = 0;
private boolean full = false;
private float[][] vals;
}
The second block of code contains my changes from the first block of code which add the gyroscope to the mix.
Specifically, the filtering factor for accel is made smaller (making the value more stable). The MatrixHistory class keeps track of the last 100 gyroscope deltaRotationMatrix values which are calculated in the gyroscopeSensorChanged method.
I've seen many questions on this site on this topic. They've helped me get to this point, but I cannot figure out what to do next. I really wish the Sensor Fusion guy had just posted some code somewhere. He obviously had it all put together.

Well, +1 to you for even knowing what a Kalman filter is. If you'd like, I'll edit this post and give you the code I wrote a couple years ago to do what you're trying to do.
But first, I'll tell you why you don't need it.
Modern implementations of the Android sensor stack use Sensor Fusion, as Stan mentioned above. This just means that all of the available data -- accel, mag, gyro -- is collected together in one algorithm, and then all the outputs are read back out in the form of Android sensors.
Edit: I just stumbled on this superb Google Tech Talk on the subject: Sensor Fusion on Android Devices: A Revolution in Motion Processing. Well worth the 45 minutes to watch it if you're interested in the topic.
In essence, Sensor Fusion is a black box. I've looked into the source code of the Android implementation, and it's a big Kalman filter written in C++. Some pretty good code in there, and far more sophisticated than any filter I ever wrote, and probably more sophisticated that what you're writing. Remember, these guys are doing this for a living.
I also know that at least one chipset manufacturer has their own sensor fusion implementation. The manufacturer of the device then chooses between the Android and the vendor implementation based on their own criteria.
Finally, as Stan mentioned above, Invensense has their own sensor fusion implementation at the chip level.
Anyway, what it all boils down to is that the built-in sensor fusion in your device is likely to be superior to anything you or I could cobble together. So what you really want to do is to access that.
In Android, there are both physical and virtual sensors. The virtual sensors are the ones that are synthesized from the available physical sensors. The best-known example is TYPE_ORIENTATION which takes accelerometer and magnetometer and creates roll/pitch/heading output. (By the way, you should not use this sensor; it has too many limitations.)
But the important thing is that newer versions of Android contain these two new virtual sensors:
TYPE_GRAVITY is the accelerometer input with the effect of motion filtered out
TYPE_LINEAR_ACCELERATION is the accelerometer with the gravity component filtered out.
These two virtual sensors are synthesized through a combination of accelerometer input and gyro input.
Another notable sensor is TYPE_ROTATION_VECTOR which is a Quaternion synthesized from accelerometer, magnetometer, and gyro. It represents the full 3-d orientation of the device with the effects of linear acceleration filtered out.
However, Quaternions are a little bit abstract for most people, and since you're likely working with 3-d transformations anyway, your best approach is to combine TYPE_GRAVITY and TYPE_MAGNETIC_FIELD via SensorManager.getRotationMatrix().
One more point: if you're working with a device running an older version of Android, you need to detect that you're not receiving TYPE_GRAVITY events and use TYPE_ACCELEROMETER instead. Theoretically, this would be a place to use your own kalman filter, but if your device doesn't have sensor fusion built in, it probably doesn't have gyros either.
Anyway, here's some sample code to show how I do it.
// Requires 1.5 or above
class Foo extends Activity implements SensorEventListener {
SensorManager sensorManager;
float[] gData = new float[3]; // Gravity or accelerometer
float[] mData = new float[3]; // Magnetometer
float[] orientation = new float[3];
float[] Rmat = new float[9];
float[] R2 = new float[9];
float[] Imat = new float[9];
boolean haveGrav = false;
boolean haveAccel = false;
boolean haveMag = false;
onCreate() {
// Get the sensor manager from system services
sensorManager =
(SensorManager)getSystemService(Context.SENSOR_SERVICE);
}
onResume() {
super.onResume();
// Register our listeners
Sensor gsensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
Sensor asensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
Sensor msensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
sensorManager.registerListener(this, gsensor, SensorManager.SENSOR_DELAY_GAME);
sensorManager.registerListener(this, asensor, SensorManager.SENSOR_DELAY_GAME);
sensorManager.registerListener(this, msensor, SensorManager.SENSOR_DELAY_GAME);
}
public void onSensorChanged(SensorEvent event) {
float[] data;
switch( event.sensor.getType() ) {
case Sensor.TYPE_GRAVITY:
gData[0] = event.values[0];
gData[1] = event.values[1];
gData[2] = event.values[2];
haveGrav = true;
break;
case Sensor.TYPE_ACCELEROMETER:
if (haveGrav) break; // don't need it, we have better
gData[0] = event.values[0];
gData[1] = event.values[1];
gData[2] = event.values[2];
haveAccel = true;
break;
case Sensor.TYPE_MAGNETIC_FIELD:
mData[0] = event.values[0];
mData[1] = event.values[1];
mData[2] = event.values[2];
haveMag = true;
break;
default:
return;
}
if ((haveGrav || haveAccel) && haveMag) {
SensorManager.getRotationMatrix(Rmat, Imat, gData, mData);
SensorManager.remapCoordinateSystem(Rmat,
SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X, R2);
// Orientation isn't as useful as a rotation matrix, but
// we'll show it here anyway.
SensorManager.getOrientation(R2, orientation);
float incl = SensorManager.getInclination(Imat);
Log.d(TAG, "mh: " + (int)(orientation[0]*DEG));
Log.d(TAG, "pitch: " + (int)(orientation[1]*DEG));
Log.d(TAG, "roll: " + (int)(orientation[2]*DEG));
Log.d(TAG, "yaw: " + (int)(orientation[0]*DEG));
Log.d(TAG, "inclination: " + (int)(incl*DEG));
}
}
}
Hmmm; if you happen to have a Quaternion library handy, it's probably simpler just to receive TYPE_ROTATION_VECTOR and convert that to an array.

To the question where to find complete code, here's a default implementation on Android jelly bean: https://android.googlesource.com/platform/frameworks/base/+/jb-release/services/sensorservice/
Start by checking the fusion.cpp/h.
It uses Modified Rodrigues Parameters (close to Euler angles) instead of quaternions. In addition to orientation the Kalman filter estimates gyro drift. For measurement updates it uses magnetometer and, a bit incorrectly, acceleration (specific force).
To make use of the code you should either be a wizard or know the basics of INS and KF. Many parameters have to be fine-tuned for the filter to work. As Edward adequately put, these guys are doing this for living.
At least in google's galaxy nexus this default implementation is left unused and is overridden by Invense's proprietary system.