Hey everyone,
I'm trying to get the heading of the phone without using the magnetic compass of the android, I read up a lot on quaternions and that seemed to be the solution. I used a sensor fusion tutorial I found online to write this:
*/
public class MainActivity extends Activity implements SensorEventListener {
private SensorManager mSensorManager = null;
private float[] gyro = new float[3];
private float[] gyroMatrix = new float[9];
private float[] gyroOrientation = new float[3];
private float[] magnet = new float[3];
private float[] accel = new float[3];
private float[] accMagOrientation = new float[3];
private float[] fusedOrientation = new float[3];
private float[] rotationMatrix = new float[9];
private TextView tvOrientation = null;
String TAG = "Main";
private Timer fuseTimer = new Timer();
public static final int TIME_CONSTANT = 30;
@Overridepublic void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
Log.v(TAG, "in create");
setContentView(R.layout.activity_main);
TextView tvOrientation = (TextView) findViewById(R.id.tvOrientation);
gyroOrientation[0] = 0.0f;
gyroOrientation[1] = 0.0f;
gyroOrientation[2] = 0.0f;
gyroMatrix[0] = 1.0f;
gyroMatrix[1] = 0.0f;
gyroMatrix[2] = 0.0f;
gyroMatrix[3] = 0.0f;
gyroMatrix[4] = 1.0f;
gyroMatrix[5] = 0.0f;
gyroMatrix[6] = 0.0f;
gyroMatrix[7] = 0.0f;
gyroMatrix[8] = 1.0f;
mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
initListeners();
fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(), 1000, TIME_CONSTANT);
}
private void initListeners() {
mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE), SensorManager.SENSOR_DELAY_NORMAL);
mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_NORMAL);
mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_NORMAL);
}
@Overridepublic boolean onCreateOptionsMenu(Menu menu) {
// Inflate the menu; this adds items to the action bar if it is present.getMenuInflater().inflate(R.menu.menu_main, menu);
return true;
}
@Overridepublic boolean onOptionsItemSelected(MenuItem item) {
// Handle action bar item clicks here. The action bar will // automatically handle clicks on the Home/Up button, so long // as you specify a parent activity in AndroidManifest.xml.int id = item.getItemId();
//noinspection SimplifiableIfStatementif (id == R.id.action_settings) {
return true;
}
return super.onOptionsItemSelected(item);
}
@Overridepublic void onSensorChanged(SensorEvent event) {
switch (event.sensor.getType()) {
case Sensor.TYPE_ACCELEROMETER:
System.arraycopy(event.values, 0, accel, 0, 3);
calculateAccOrientation();
break;
case Sensor.TYPE_GYROSCOPE:
gyroFunction(event);
break;
case Sensor.TYPE_MAGNETIC_FIELD:
System.arraycopy(event.values, 0, magnet, 0, 3);
}
}
private void calculateAccOrientation() {
if (SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet))
SensorManager.getOrientation(rotationMatrix, accMagOrientation);
}
@Overridepublic void onAccuracyChanged(Sensor sensor, int accuracy) {
}
public static final float EPSILON = 0.000000001f;
private void getRotationVectorFromGyro(float[] gyroValues, float[] deltaRotationVector, float timeFactor) {
float[] normValues = new float[3];
float omegaMagnitude = (float) Math.sqrt(gyroValues[0] * gyroValues[0] + gyroValues[1] * gyroValues[1] + gyroValues[2] * gyroValues[2]);
if (!Float.isNaN(omegaMagnitude) && omegaMagnitude > EPSILON) {
normValues[0] = gyroValues[0] / omegaMagnitude;
normValues[1] = gyroValues[1] / omegaMagnitude;
normValues[2] = gyroValues[2] / omegaMagnitude;
}
float thetaOverTwo = omegaMagnitude * timeFactor;
float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);
deltaRotationVector[0] = sinThetaOverTwo * normValues[0];
deltaRotationVector[1] = sinThetaOverTwo * normValues[1];
deltaRotationVector[2] = sinThetaOverTwo * normValues[2];
deltaRotationVector[3] = cosThetaOverTwo;
}
private static final float NS2S = 1.0f / 1000000000.0f;
private float timestamp;
private boolean initState = true;
public void gyroFunction(SensorEvent event) {
if (accMagOrientation == null)
return;
//initialize gyroscopeif (initState) {
float[] initMatrix = new float[9];
initMatrix = getRotationMatrixFromOrientation(accMagOrientation);
float[] test = new float[9];
SensorManager.getOrientation(initMatrix, test);
gyroMatrix = matrixMultiplication(gyroMatrix, initMatrix);
initState = false;
}
//copy new gyro values //convert raw datafloat[] deltaVector = new float[4];
if (timestamp != 0) {
final float dT = (event.timestamp - timestamp) * NS2S;
System.arraycopy(event.values, 0, gyro, 0, 3);
getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
}
timestamp = event.timestamp;
float[] deltaMatrix = new float[9];
SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);
float[] tempMatrix = new float[9];
tempMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);
if (Float.isNaN(gyroMatrix[0]) || Float.isNaN(gyroMatrix[1]) || Float.isNaN(gyroMatrix[2])
|| Float.isNaN(gyroMatrix[3]) || Float.isNaN(gyroMatrix[4]) || Float.isNaN(gyroMatrix[5])
|| Float.isNaN(gyroMatrix[6]) || Float.isNaN(gyroMatrix[7]) || Float.isNaN(gyroMatrix[8])) {
Log.v(TAG, gyroMatrix.toString());
}
gyroMatrix = tempMatrix;
SensorManager.getOrientation(gyroMatrix, gyroOrientation);
}
private float[] getRotationMatrixFromOrientation(float[] o) {
float[] xM = new float[9];
float[] yM = new float[9];
float[] zM = new float[9];
float sinX = (float) Math.sin(o[1]);
float cosX = (float) Math.sin(o[1]);
float sinY = (float) Math.sin(o[2]);
float cosY = (float) Math.sin(o[2]);
float sinZ = (float) Math.sin(o[0]);
float cosZ = (float) Math.sin(o[0]);
xM[0] = 1.0f;
xM[1] = 0.0f;
xM[2] = 0.0f;
xM[3] = 0.0f;
xM[4] = cosX;
xM[5] = sinX;
xM[6] = 0.0f;
xM[7] = -sinX;
xM[8] = cosX;
yM[0] = cosY;
yM[1] = 0.0f;
yM[2] = sinY;
yM[3] = 0.0f;
yM[4] = 1.0f;
yM[5] = 0.0f;
yM[6] = -sinY;
yM[7] = 0.0f;
yM[8] = cosY;
zM[0] = cosZ;
zM[1] = sinZ;
zM[2] = 0.0f;
zM[3] = -sinZ;
zM[4] = cosX;
zM[5] = 0.0f;
zM[6] = 0.0f;
zM[7] = 0.0f;
zM[8] = 1.0f;
float[] resultMatrix = matrixMultiplication(xM, yM);
resultMatrix = matrixMultiplication(zM, resultMatrix);
return resultMatrix;
}
private float[] matrixMultiplication(float[] A, float[] B) {
float[] result = new float[9];
result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];
result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];
result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];
return result;
}
class calculateFusedOrientationTask extends TimerTask {
public static final float FILTER_COEFFICIENT = 0.98f;
@Overridepublic void run() {
float oneMinusCoeff = 1.0f - FILTER_COEFFICIENT;
fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * accMagOrientation[0];
fusedOrientation[1] = FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * accMagOrientation[1];
fusedOrientation[2] = FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * accMagOrientation[2];
if (Float.isNaN(fusedOrientation[0])) {
boolean abc = true;
}
Log.v(TAG, "" + fusedOrientation[0] + " " + fusedOrientation[1] + " " + fusedOrientation[2]);
gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
}
}
}
/*
The fusedOrientation matrix contains the fused values. If I tilt the phone in a certain direction I start getting NaN values. Also how can I head the heading from the values?
Any help would be appreciated!
Thank you,
Wasp
I'm trying to get the heading of the phone without using the magnetic compass of the android, I read up a lot on quaternions and that seemed to be the solution. I used a sensor fusion tutorial I found online to write this:
*/
public class MainActivity extends Activity implements SensorEventListener {
private SensorManager mSensorManager = null;
private float[] gyro = new float[3];
private float[] gyroMatrix = new float[9];
private float[] gyroOrientation = new float[3];
private float[] magnet = new float[3];
private float[] accel = new float[3];
private float[] accMagOrientation = new float[3];
private float[] fusedOrientation = new float[3];
private float[] rotationMatrix = new float[9];
private TextView tvOrientation = null;
String TAG = "Main";
private Timer fuseTimer = new Timer();
public static final int TIME_CONSTANT = 30;
@Overridepublic void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
Log.v(TAG, "in create");
setContentView(R.layout.activity_main);
TextView tvOrientation = (TextView) findViewById(R.id.tvOrientation);
gyroOrientation[0] = 0.0f;
gyroOrientation[1] = 0.0f;
gyroOrientation[2] = 0.0f;
gyroMatrix[0] = 1.0f;
gyroMatrix[1] = 0.0f;
gyroMatrix[2] = 0.0f;
gyroMatrix[3] = 0.0f;
gyroMatrix[4] = 1.0f;
gyroMatrix[5] = 0.0f;
gyroMatrix[6] = 0.0f;
gyroMatrix[7] = 0.0f;
gyroMatrix[8] = 1.0f;
mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
initListeners();
fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(), 1000, TIME_CONSTANT);
}
private void initListeners() {
mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE), SensorManager.SENSOR_DELAY_NORMAL);
mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER), SensorManager.SENSOR_DELAY_NORMAL);
mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SensorManager.SENSOR_DELAY_NORMAL);
}
@Overridepublic boolean onCreateOptionsMenu(Menu menu) {
// Inflate the menu; this adds items to the action bar if it is present.getMenuInflater().inflate(R.menu.menu_main, menu);
return true;
}
@Overridepublic boolean onOptionsItemSelected(MenuItem item) {
// Handle action bar item clicks here. The action bar will // automatically handle clicks on the Home/Up button, so long // as you specify a parent activity in AndroidManifest.xml.int id = item.getItemId();
//noinspection SimplifiableIfStatementif (id == R.id.action_settings) {
return true;
}
return super.onOptionsItemSelected(item);
}
@Overridepublic void onSensorChanged(SensorEvent event) {
switch (event.sensor.getType()) {
case Sensor.TYPE_ACCELEROMETER:
System.arraycopy(event.values, 0, accel, 0, 3);
calculateAccOrientation();
break;
case Sensor.TYPE_GYROSCOPE:
gyroFunction(event);
break;
case Sensor.TYPE_MAGNETIC_FIELD:
System.arraycopy(event.values, 0, magnet, 0, 3);
}
}
private void calculateAccOrientation() {
if (SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet))
SensorManager.getOrientation(rotationMatrix, accMagOrientation);
}
@Overridepublic void onAccuracyChanged(Sensor sensor, int accuracy) {
}
public static final float EPSILON = 0.000000001f;
private void getRotationVectorFromGyro(float[] gyroValues, float[] deltaRotationVector, float timeFactor) {
float[] normValues = new float[3];
float omegaMagnitude = (float) Math.sqrt(gyroValues[0] * gyroValues[0] + gyroValues[1] * gyroValues[1] + gyroValues[2] * gyroValues[2]);
if (!Float.isNaN(omegaMagnitude) && omegaMagnitude > EPSILON) {
normValues[0] = gyroValues[0] / omegaMagnitude;
normValues[1] = gyroValues[1] / omegaMagnitude;
normValues[2] = gyroValues[2] / omegaMagnitude;
}
float thetaOverTwo = omegaMagnitude * timeFactor;
float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);
deltaRotationVector[0] = sinThetaOverTwo * normValues[0];
deltaRotationVector[1] = sinThetaOverTwo * normValues[1];
deltaRotationVector[2] = sinThetaOverTwo * normValues[2];
deltaRotationVector[3] = cosThetaOverTwo;
}
private static final float NS2S = 1.0f / 1000000000.0f;
private float timestamp;
private boolean initState = true;
public void gyroFunction(SensorEvent event) {
if (accMagOrientation == null)
return;
//initialize gyroscopeif (initState) {
float[] initMatrix = new float[9];
initMatrix = getRotationMatrixFromOrientation(accMagOrientation);
float[] test = new float[9];
SensorManager.getOrientation(initMatrix, test);
gyroMatrix = matrixMultiplication(gyroMatrix, initMatrix);
initState = false;
}
//copy new gyro values //convert raw datafloat[] deltaVector = new float[4];
if (timestamp != 0) {
final float dT = (event.timestamp - timestamp) * NS2S;
System.arraycopy(event.values, 0, gyro, 0, 3);
getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
}
timestamp = event.timestamp;
float[] deltaMatrix = new float[9];
SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);
float[] tempMatrix = new float[9];
tempMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);
if (Float.isNaN(gyroMatrix[0]) || Float.isNaN(gyroMatrix[1]) || Float.isNaN(gyroMatrix[2])
|| Float.isNaN(gyroMatrix[3]) || Float.isNaN(gyroMatrix[4]) || Float.isNaN(gyroMatrix[5])
|| Float.isNaN(gyroMatrix[6]) || Float.isNaN(gyroMatrix[7]) || Float.isNaN(gyroMatrix[8])) {
Log.v(TAG, gyroMatrix.toString());
}
gyroMatrix = tempMatrix;
SensorManager.getOrientation(gyroMatrix, gyroOrientation);
}
private float[] getRotationMatrixFromOrientation(float[] o) {
float[] xM = new float[9];
float[] yM = new float[9];
float[] zM = new float[9];
float sinX = (float) Math.sin(o[1]);
float cosX = (float) Math.sin(o[1]);
float sinY = (float) Math.sin(o[2]);
float cosY = (float) Math.sin(o[2]);
float sinZ = (float) Math.sin(o[0]);
float cosZ = (float) Math.sin(o[0]);
xM[0] = 1.0f;
xM[1] = 0.0f;
xM[2] = 0.0f;
xM[3] = 0.0f;
xM[4] = cosX;
xM[5] = sinX;
xM[6] = 0.0f;
xM[7] = -sinX;
xM[8] = cosX;
yM[0] = cosY;
yM[1] = 0.0f;
yM[2] = sinY;
yM[3] = 0.0f;
yM[4] = 1.0f;
yM[5] = 0.0f;
yM[6] = -sinY;
yM[7] = 0.0f;
yM[8] = cosY;
zM[0] = cosZ;
zM[1] = sinZ;
zM[2] = 0.0f;
zM[3] = -sinZ;
zM[4] = cosX;
zM[5] = 0.0f;
zM[6] = 0.0f;
zM[7] = 0.0f;
zM[8] = 1.0f;
float[] resultMatrix = matrixMultiplication(xM, yM);
resultMatrix = matrixMultiplication(zM, resultMatrix);
return resultMatrix;
}
private float[] matrixMultiplication(float[] A, float[] B) {
float[] result = new float[9];
result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];
result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];
result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];
return result;
}
class calculateFusedOrientationTask extends TimerTask {
public static final float FILTER_COEFFICIENT = 0.98f;
@Overridepublic void run() {
float oneMinusCoeff = 1.0f - FILTER_COEFFICIENT;
fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * accMagOrientation[0];
fusedOrientation[1] = FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * accMagOrientation[1];
fusedOrientation[2] = FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * accMagOrientation[2];
if (Float.isNaN(fusedOrientation[0])) {
boolean abc = true;
}
Log.v(TAG, "" + fusedOrientation[0] + " " + fusedOrientation[1] + " " + fusedOrientation[2]);
gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
}
}
}
/*
The fusedOrientation matrix contains the fused values. If I tilt the phone in a certain direction I start getting NaN values. Also how can I head the heading from the values?
Any help would be appreciated!
Thank you,
Wasp