Related
I am trying to figure out how to point the device marker where the user is heading. I tried the code below to get the bearing angle between the true north and where the user is heading. Once the bearing is found, I would apply it to the device marker and rotate it based on that bearing, but it only works when the device is not moving.
I tried out the code I found from the link below.
Code Source Link
// 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 = m_NormMagFieldValues = null;; // 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 = new float[3]; // normalised cross product of raw gravity vector with magnetic field values, points east
float[] m_NormNorthVector = new float[3]; // Normalised vector pointing to magnetic north
boolean m_OrientationOK = false; // 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
#Override
public void onSensorChanged(SensorEvent sensorEvent) {
int SensorType = sensorEvent.sensor.getType();
switch(SensorType) {
case Sensor.TYPE_GRAVITY:
if (m_NormGravityVector == null) m_NormGravityVector = new float[3];
System.arraycopy(sensorEvent.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(sensorEvent.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 = getActivity().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;
currentAzimuth = (float) Math.toDegrees(m_azimuth_radians);
}
}
}
#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;
}
}
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 :)
I'm implementing a kind of a "compass arrow" that follows destination depending on physical orientation of the device using magnetic field sensor. Suddenly I faced with a little problem.
Obtaining bearing and azimuth is OK, but performing a realistic animation turned into a really hard task. I tried to use different interpolators to make animation more "physical" (i. e. as in real compass, which arrow oscillate after hairpin rotation, accelerate and decelerate during movement etc).
Now I'm using interpolator.accelerate_decelerate and everything is quite good until updates start arriving quickly. That makes animations overlap each other and the arrow becomes twitchy and nervous. I want to avoid this. I tried to implement a queue to make every next animation wait until previous ends, or drop updates that come very quickly. That made animation look smooth, but arrow's behavior turned into absolutely illogical.
So I have 2 questions:
1) is there some way to make animated transitions more smooth in the cases when animations overlap each other?
2) is there a way to stop animation that is currently processing and get intermediate position of an object?
My code is below. An UpdateRotation() method handles orientation and bearing updates and executes animation of external viewArrow view.
public class DirectionArrow {
// View that represents the arrow
final View viewArrow;
// speed of rotation of the arrow, degrees/sec
final double rotationSpeed;
// current values of bearing and azimuth
float bearingCurrent = 0;
float azimuthCurrent = 0;
/*******************************************************************************/
/**
* Basic constructor
*
* #param view View representing an arrow that should be rotated
* #param rotationSpeed Speed of rotation in deg/sec. Recommended from 50 (slow) to 500 (fast)
*/
public DirectionArrow(View view, double rotationSpeed) {
this.viewArrow = view;
this.rotationSpeed = rotationSpeed;
}
/**
* Extended constructor
*
* #param viewArrow View representing an arrow that should be rotated
* #param rotationSpeed Speed of rotation in deg/sec. Recommended from 50 (slow) to 500 (fast)
* #param bearing Initial bearing
* #param azimuth Initial azimuth
*/
public DirectionArrow(View viewArrow, double rotationSpeed, float bearing, float azimuth){
this.viewArrow = viewArrow;
this.rotationSpeed = rotationSpeed;
UpdateRotation(bearing, azimuth);
}
/**
* Invoke this to update orientation and animate the arrow
*
* #param bearingNew New bearing value, set >180 or <-180 if you don't need to update it
* #param azimuthNew New azimuth value, set >360 or <0 if you don't need to update it
*/
public void UpdateRotation(float bearingNew, float azimuthNew){
// look if any parameter shouldn't be updated
if (bearingNew < -180 || bearingNew > 180){
bearingNew = bearingCurrent;
}
if (azimuthNew < 0 || azimuthNew > 360){
azimuthNew = azimuthCurrent;
}
// log
Log.println(Log.DEBUG, "compass", "Setting rotation: B=" + bearingNew + " A=" + azimuthNew);
// calculate rotation value
float rotationFrom = bearingCurrent - azimuthCurrent;
float rotationTo = bearingNew - azimuthNew;
// correct rotation angles
if (rotationFrom < -180) {
rotationFrom += 360;
}
while (rotationTo - rotationFrom < -180) {
rotationTo += 360;
}
while (rotationTo - rotationFrom > 180) {
rotationTo -= 360;
}
// log again
Log.println(Log.DEBUG, "compass", "Start Rotation to " + rotationTo);
// create an animation object
RotateAnimation rotateAnimation = new RotateAnimation(rotationFrom, rotationTo,
Animation.RELATIVE_TO_SELF, (float) 0.5, Animation.RELATIVE_TO_SELF, (float) 0.5);
// set up an interpolator
rotateAnimation.setInterpolator(viewArrow.getContext(), interpolator.accelerate_decelerate);
// force view to remember its position after animation
rotateAnimation.setFillAfter(true);
// set duration depending on speed
rotateAnimation.setDuration((long) (Math.abs(rotationFrom - rotationTo) / rotationSpeed * 1000));
// start animation
viewArrow.startAnimation(rotateAnimation);
// update cureent rotation
bearingCurrent = bearingNew;
azimuthCurrent = azimuthNew;
}
}
Here is my custom ImageDraw class where I implemted physical behavior of the pointing arrow based on equation of circular motion of dipole in magnetic field.
It don't uses any animators nor interpolators--on every iteration angular position is recalculated based on physical parameters. These parameters can be widely adjusted via setPhysical method. For example, to make rotations more smooth and slow, increase alpha (damping coefficient), to make arrow more responsitive, increase mB (coefficient of magnetic field), to make arrow oscillate on rotations, increase inertiaMoment.
Animation and redraw is performed implicitly by invoke of invalidate() on every iteration. There is no need to handle it explicitly.
To update the angle at which the arrow should rotate, just call rotationUpdate (by user's choice or using device orientation sensor callback).
/**
* Class CompassView extends Android ImageView to perform cool, real-life animation of objects
* such compass needle in magnetic field. Rotation is performed relative to the center of image.
*
* It uses angular motion equation of magnetic dipole in magnetic field to implement such animation.
* To vary behaviour (damping, oscillation, responsiveness and so on) set various physical properties.
*
* Use `setPhysical()` to vary physical properties.
* Use `rotationUpdate()` to change angle of "magnetic field" at which image should rotate.
*
*/
public class CompassView extends ImageView {
static final public float TIME_DELTA_THRESHOLD = 0.25f; // maximum time difference between iterations, s
static final public float ANGLE_DELTA_THRESHOLD = 0.1f; // minimum rotation change to be redrawn, deg
static final public float INERTIA_MOMENT_DEFAULT = 0.1f; // default physical properties
static final public float ALPHA_DEFAULT = 10;
static final public float MB_DEFAULT = 1000;
long time1, time2; // timestamps of previous iterations--used in numerical integration
float angle1, angle2, angle0; // angles of previous iterations
float angleLastDrawn; // last drawn anglular position
boolean animationOn = false; // if animation should be performed
float inertiaMoment = INERTIA_MOMENT_DEFAULT; // moment of inertia
float alpha = ALPHA_DEFAULT; // damping coefficient
float mB = MB_DEFAULT; // magnetic field coefficient
/**
* Constructor inherited from ImageView
*
* #param context
*/
public CompassView(Context context) {
super(context);
}
/**
* Constructor inherited from ImageView
*
* #param context
* #param attrs
*/
public CompassView(Context context, AttributeSet attrs) {
super(context, attrs);
}
/**
* Constructor inherited from ImageView
*
* #param context
* #param attrs
* #param defStyle
*/
public CompassView(Context context, AttributeSet attrs, int defStyle) {
super(context, attrs, defStyle);
}
/**
* onDraw override.
* If animation is "on", view is invalidated after each redraw,
* to perform recalculation on every loop of UI redraw
*/
#Override
public void onDraw(Canvas canvas){
if (animationOn){
if (angleRecalculate(new Date().getTime())){
this.setRotation(angle1);
}
} else {
this.setRotation(angle1);
}
super.onDraw(canvas);
if (animationOn){
this.invalidate();
}
}
/**
* Use this to set physical properties.
* Negative values will be replaced by default values
*
* #param inertiaMoment Moment of inertia (default 0.1)
* #param alpha Damping coefficient (default 10)
* #param mB Magnetic field coefficient (default 1000)
*/
public void setPhysical(float inertiaMoment, float alpha, float mB){
this.inertiaMoment = inertiaMoment >= 0 ? inertiaMoment : this.INERTIA_MOMENT_DEFAULT;
this.alpha = alpha >= 0 ? alpha : ALPHA_DEFAULT;
this.mB = mB >= 0 ? mB : MB_DEFAULT;
}
/**
* Use this to set new "magnetic field" angle at which image should rotate
*
* #param angleNew new magnetic field angle, deg., relative to vertical axis.
* #param animate true, if image shoud rotate using animation, false to set new rotation instantly
*/
public void rotationUpdate(final float angleNew, final boolean animate){
if (animate){
if (Math.abs(angle0 - angleNew) > ANGLE_DELTA_THRESHOLD){
angle0 = angleNew;
this.invalidate();
}
animationOn = true;
} else {
angle1 = angleNew;
angle2 = angleNew;
angle0 = angleNew;
angleLastDrawn = angleNew;
this.invalidate();
animationOn = false;
}
}
/**
* Recalculate angles using equation of dipole circular motion
*
* #param timeNew timestamp of method invoke
* #return if there is a need to redraw rotation
*/
protected boolean angleRecalculate(final long timeNew){
// recalculate angle using simple numerical integration of motion equation
float deltaT1 = (timeNew - time1)/1000f;
if (deltaT1 > TIME_DELTA_THRESHOLD){
deltaT1 = TIME_DELTA_THRESHOLD;
time1 = timeNew + Math.round(TIME_DELTA_THRESHOLD * 1000);
}
float deltaT2 = (time1 - time2)/1000f;
if (deltaT2 > TIME_DELTA_THRESHOLD){
deltaT2 = TIME_DELTA_THRESHOLD;
}
// circular acceleration coefficient
float koefI = inertiaMoment / deltaT1 / deltaT2;
// circular velocity coefficient
float koefAlpha = alpha / deltaT1;
// angular momentum coefficient
float koefk = mB * (float)(Math.sin(Math.toRadians(angle0))*Math.cos(Math.toRadians(angle1)) -
(Math.sin(Math.toRadians(angle1))*Math.cos(Math.toRadians(angle0))));
float angleNew = ( koefI*(angle1 * 2f - angle2) + koefAlpha*angle1 + koefk) / (koefI + koefAlpha);
// reassign previous iteration variables
angle2 = angle1;
angle1 = angleNew;
time2 = time1;
time1 = timeNew;
// if angles changed less then threshold, return false - no need to redraw the view
if (Math.abs(angleLastDrawn - angle1) < ANGLE_DELTA_THRESHOLD){
return false;
} else {
angleLastDrawn = angle1;
return true;
}
}
Are you filtering your sensor data? The Magnetometer is a pain low pass filtering isn't really enough. You could use weighted-smoothing or maybe rounding data would be helpful:
Math.round( xyz * 10) / 10; ?
You could also reduce the frequency at which you get sensor updates. That might help.
mSensorManager.registerListener(this, mMagnetometer, 10000);
Espessially for gilonm, nice implementation of fixed size queue and getting its mean value:
float queue[ARRAY_LENGTH] = {0};
int queueFront = queue.length - 1 // position of front element
float meanValue = 0; // calculated mean value
float pushNewAndGetMean(float newValue){
// recalculate mean value
meanValue = meanValue + (newValue - queue[queueFront]) / queue.length;
// overwrite value in front pointer position
queue[queueFront] = newValue;
// shift front pointer 1 step right or to '0' if end of array reached
queueStart = (queueFront + 1) % array.length;
return meanValue
};
Here, not dependent on array length, you make just 2 reassignments of variables (instead of N) and use only 3 elements in mean calculation (instead of N). This makes algorithm O(1) complexity instead of O(N).
What you could do is where you get your data from the sensors - you can just use and array to do an average of say last 5 readings - that should smooth things down.
something like this:
Declare an array private float azimArray[] = {0,0,0,0,0};
Now where you get sensor data, use:
azimArray[0] = azimArray[1];
azimArray[1] = azimArray[2];
azimArray[2] = azimArray[3];
azimArray[3] = azimArray[4];
azimArray[4] = event.values[0]; //get actual sensor data into last array cell
currentAzimuth = Math.round(azimArray[0]+azimArray[1]+azimArray[2]+azimArray[3]+azimArray[4]/5);
Now currentAzimuth holds the rounded average of last 5 readings, which should smooth things down for you.
Hope this helped!
When you use TYPE_MAGNETOMETER sensor, you get X, Y, Z values of magnetic field strength in relation to the device orientation. What I want to get is to convert these values into global reference frame, clarifying: user takes the device, measure these values, than rotate the device for some degrees around any axis and gets ~the same values.
Please, find similar questions below:
Getting magnetic field values in global coordinates
How can I get the magnetic field vector, independent of the device rotation?
In this answer sample solution is described (it is for linear acceleration, but I think it doesn't matter): https://stackoverflow.com/a/11614404/2152255
I used it and I got 3 values, X is always very small (don't think that it is correct), Y and Z are OK, but they still changed a bit when I rotate the device. How could it be adjusted? And could it be solved all? I use simple Kalman filter to approximate measurement values, because w/o it I get quiet different values even if the device is not moving/rotating at all. Please, find my code below:
import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.opengl.Matrix;
import android.os.Bundle;
import android.view.View;
import android.widget.CheckBox;
import android.widget.TextView;
import com.test.statistics.filter.kalman.KalmanState;
import com.example.R;
/**
* Activity for gathering magnetic field statistics.
*/
public class MagneticFieldStatisticsGatheringActivity extends Activity implements SensorEventListener {
public static final int KALMAN_STATE_MAX_SIZE = 80;
public static final double MEASUREMENT_NOISE = 5;
/** Sensor manager. */
private SensorManager mSensorManager;
/** Magnetometer spec. */
private TextView vendor;
private TextView resolution;
private TextView maximumRange;
/** Magnetic field coordinates measurements. */
private TextView magneticXTextView;
private TextView magneticYTextView;
private TextView magneticZTextView;
/** Sensors. */
private Sensor mAccelerometer;
private Sensor mGeomagnetic;
private float[] accelerometerValues;
private float[] geomagneticValues;
/** Flags. */
private boolean specDefined = false;
private boolean kalmanFiletring = false;
/** Rates. */
private float nanoTtoGRate = 0.00001f;
private final int gToCountRate = 1000000;
/** Kalman vars. */
private KalmanState previousKalmanStateX;
private KalmanState previousKalmanStateY;
private KalmanState previousKalmanStateZ;
private int previousKalmanStateCounter = 0;
#Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.main2);
mSensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
mAccelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
mGeomagnetic = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
vendor = (TextView) findViewById(R.id.vendor);
resolution = (TextView) findViewById(R.id.resolution);
maximumRange = (TextView) findViewById(R.id.maximumRange);
magneticXTextView = (TextView) findViewById(R.id.magneticX);
magneticYTextView = (TextView) findViewById(R.id.magneticY);
magneticZTextView = (TextView) findViewById(R.id.magneticZ);
mSensorManager.registerListener(this, mAccelerometer, SensorManager.SENSOR_DELAY_FASTEST);
mSensorManager.registerListener(this, mGeomagnetic, SensorManager.SENSOR_DELAY_FASTEST);
}
/**
* Refresh statistics.
*
* #param view - refresh button view.
*/
public void onClickRefreshMagneticButton(View view) {
resetKalmanFilter();
}
/**
* Switch Kalman filtering on/off
*
* #param view - Klaman filetring switcher (checkbox)
*/
public void onClickKalmanFilteringCheckBox(View view) {
CheckBox kalmanFiltering = (CheckBox) view;
this.kalmanFiletring = kalmanFiltering.isChecked();
}
#Override
public void onSensorChanged(SensorEvent sensorEvent) {
if (sensorEvent.accuracy == SensorManager.SENSOR_STATUS_UNRELIABLE) {
return;
}
synchronized (this) {
switch(sensorEvent.sensor.getType()){
case Sensor.TYPE_ACCELEROMETER:
accelerometerValues = sensorEvent.values.clone();
break;
case Sensor.TYPE_MAGNETIC_FIELD:
if (!specDefined) {
vendor.setText("Vendor: " + sensorEvent.sensor.getVendor() + " " + sensorEvent.sensor.getName());
float resolutionValue = sensorEvent.sensor.getResolution() * nanoTtoGRate;
resolution.setText("Resolution: " + resolutionValue);
float maximumRangeValue = sensorEvent.sensor.getMaximumRange() * nanoTtoGRate;
maximumRange.setText("Maximum range: " + maximumRangeValue);
}
geomagneticValues = sensorEvent.values.clone();
break;
}
if (accelerometerValues != null && geomagneticValues != null) {
float[] Rs = new float[16];
float[] I = new float[16];
if (SensorManager.getRotationMatrix(Rs, I, accelerometerValues, geomagneticValues)) {
float[] RsInv = new float[16];
Matrix.invertM(RsInv, 0, Rs, 0);
float resultVec[] = new float[4];
float[] geomagneticValuesAdjusted = new float[4];
geomagneticValuesAdjusted[0] = geomagneticValues[0];
geomagneticValuesAdjusted[1] = geomagneticValues[1];
geomagneticValuesAdjusted[2] = geomagneticValues[2];
geomagneticValuesAdjusted[3] = 0;
Matrix.multiplyMV(resultVec, 0, RsInv, 0, geomagneticValuesAdjusted, 0);
for (int i = 0; i < resultVec.length; i++) {
resultVec[i] = resultVec[i] * nanoTtoGRate * gToCountRate;
}
if (kalmanFiletring) {
KalmanState currentKalmanStateX = new KalmanState(MEASUREMENT_NOISE, accelerometerValues[0], (double)resultVec[0], previousKalmanStateX);
previousKalmanStateX = currentKalmanStateX;
KalmanState currentKalmanStateY = new KalmanState(MEASUREMENT_NOISE, accelerometerValues[1], (double)resultVec[1], previousKalmanStateY);
previousKalmanStateY = currentKalmanStateY;
KalmanState currentKalmanStateZ = new KalmanState(MEASUREMENT_NOISE, accelerometerValues[2], (double)resultVec[2], previousKalmanStateZ);
previousKalmanStateZ = currentKalmanStateZ;
if (previousKalmanStateCounter == KALMAN_STATE_MAX_SIZE) {
magneticXTextView.setText("x: " + previousKalmanStateX.getX_estimate());
magneticYTextView.setText("y: " + previousKalmanStateY.getX_estimate());
magneticZTextView.setText("z: " + previousKalmanStateZ.getX_estimate());
resetKalmanFilter();
} else {
previousKalmanStateCounter++;
}
} else {
magneticXTextView.setText("x: " + resultVec[0]);
magneticYTextView.setText("y: " + resultVec[1]);
magneticZTextView.setText("z: " + resultVec[2]);
}
}
}
}
}
private void resetKalmanFilter() {
previousKalmanStateX = null;
previousKalmanStateY = null;
previousKalmanStateZ = null;
previousKalmanStateCounter = 0;
}
#Override
public void onAccuracyChanged(Sensor sensor, int i) {
}
}
Thanks everybody who read this post and who post some thoughts about the problem in advance.
In my comment on the checked answer on the link you provided above, I referred to my simple answer at calculate acceleration in reference to true north
Let me answer here again with more clarification. The answer is the product of the rotation matrix and the magnetic field values. If you read further on the "X is always very small" is the correct value.
The accelerometer and magnetic field sensors measure the acceleration of the device and the magnetic field of the earth at the device location respectively. They are vectors in 3 dimentional space, let call them a and m respectively.
If you stand still and rotate your device, theoretically m does not change assuming there are no magnetic interference from surrounding objects (actually m should change little, if you move around since the magnetic field of the earth should change little in a short distance). But a does change, even though it should not be drastic in most situation.
Now a vector v in 3 dimensional space can be represented by a 3-tuples (v_1, v_2, v_3) with respect to some basis (e_1, e_2, e_3), i.e v = v_1 e_1 + v_2 e_2 + v_3 e_3. (v_1, v_2, v_3) are called the coordinates of v with respect to the basis (e_1, e_2, e_3).
In Android devices, the basis is (x, y, z) where, for most phone, x is along the shorter side and pointing right, y is along the longer side and pointing up and z is perpendicular to the screen and pointing out.
Now this basis changes as the position of the device changes. One can think these bases as a function of time (x(t), y(t), z(t)), in mathematics term it is a moving coordinate system.
Thus even though m does not change, but the event.values returns by the sensors are different because the basis is different (I will talk about fluctuation later). As is, the event.values are useless because it gives us the coordinates but we do not know what the basis is, i.e with respect to some basis we know.
Now the question is: is it possible to find the coordinates of a and m with respect to the fixed world basis (w_1, w_2, w_3) where w_1 points toward East, w_2 points toward magnetic North and w_3 points up toward the sky?
The answer is yes provided 2 important assumptions are satisfied.
With these 2 assumptions it is simple to calculate (just a few cross products) the change of basis matrix R from the basis (x, y, z) to the basis (w_1, w_2, w_3), which in Android is called the Rotation matrix. Then the coordinates of a vector v with respect to the basis (w_1, w_2, w_3) is obtained by multiply R with the coordinates of v with respect to (x, y, z). Thus the coordinates of m with respect to the world coordinates system is just the product of the rotation matrix and the event.values returned by the TYPE_MAGNETIC_FIELD sensor and similarly for a.
In android the rotation matrix is obtained by calling getRotationMatrix (float[] R, float[] I, float[] gravity, float[] geomagnetic) and we normally pass in the returned accelerometer values for the gravity parameter and the magnetic field values for the geomagnetic.
The 2 important assumptions are:
1- The gravity parameter represents a vector lying in w_3, more particular it is the minus of the vector influenced by gravity alone.
Thus if you pass in the accelerometer values without filtering, the rotation matrix will be slightly off. That is why you need to filter the accelerometer so that the filter values are approximately just the minus gravity vector. Since the gravitational acceleration is the dominant factor in the accelerometer vector, normally low pass filter is sufficient.
2- The geomagnetic parameter represents a vector lying in the plane spanned by the w_2 and the w_3 vectors. That is it lies in the North-Sky plane. Thus in term of the (w_1, w_2, w_3) basis, the first coordinate should be 0. Therefore, the "X is always very small" as you stated it above is the correct value, ideally it should be 0. Now the magnetic field values will fluctuate quite a bit. This is kind of expected, just as a regular compass needle will not stand still if you keep it in your hand and your hand shakes a little. Also, you may get interference from objects surround you and in this case the magnetic field values are unpredictable. I once test my compass app sitting near a "stone" table and my compass was off by more than 90 degrees, only by using a real compass that I found out that there is nothing wrong with my app and the "stone" table produces a real strong magnetic field.
With gravity as a dominant factor you can filter accelerometer values, but without any other knowledge, how do you fitler magnetic values? How do you know if there is or isn't any interference from surrounding objects?
You can do a lot more like complete knowledge of your device spatial position etc with the understanding of rotation matrix.
As per the above explanation, do this
private static final int TEST_GRAV = Sensor.TYPE_ACCELEROMETER;
private static final int TEST_MAG = Sensor.TYPE_MAGNETIC_FIELD;
private final float alpha = (float) 0.8;
private float gravity[] = new float[3];
private float magnetic[] = new float[3];
public void onSensorChanged(SensorEvent event) {
Sensor sensor = event.sensor;
if (sensor.getType() == TEST_GRAV) {
// Isolate the force of gravity with the low-pass filter.
gravity[0] = alpha * gravity[0] + (1 - alpha) * event.values[0];
gravity[1] = alpha * gravity[1] + (1 - alpha) * event.values[1];
gravity[2] = alpha * gravity[2] + (1 - alpha) * event.values[2];
} else if (sensor.getType() == TEST_MAG) {
magnetic[0] = event.values[0];
magnetic[1] = event.values[1];
magnetic[2] = event.values[2];
float[] R = new float[9];
float[] I = new float[9];
SensorManager.getRotationMatrix(R, I, gravity, magnetic);
float [] A_D = event.values.clone();
float [] A_W = new float[3];
A_W[0] = R[0] * A_D[0] + R[1] * A_D[1] + R[2] * A_D[2];
A_W[1] = R[3] * A_D[0] + R[4] * A_D[1] + R[5] * A_D[2];
A_W[2] = R[6] * A_D[0] + R[7] * A_D[1] + R[8] * A_D[2];
Log.d("Field","\nX :"+A_W[0]+"\nY :"+A_W[1]+"\nZ :"+A_W[2]);
}
}
Stochastically has put an answer together that I find superior to what's being posted here.
https://stackoverflow.com/a/16418016/4033525
It seems SensorManager.getOrientation() fails to translate to the world frame correctly.
The correct code would be:
SensorManager.getRotationMatrix(gravityCompassRotationMatrix, inclinationValues, gravityValues, magnitudeValues);
SensorManager.remapCoordinateSystem(currentOrientationRotationMatrix.matrix, worldAxisX, worldAxisY, adjustedRotationMatrix);
float sin = adjustedRotationMatrix[1] - adjustedRotationMatrix[4];
float cos = adjustedRotationMatrix[0] + adjustedRotationMatrix[5];
float m_azimuth_radians = (float) (sin != 0 && cos != 0 ? Math.atan2(sin, cos) : 0);
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.