[JAVA] Get the acceleration and bearing of the world coordinate system on Android

Introduction

There is a lot of information about the acquisition of the accelerometer value and the slope in the world coordinate system, but there was not much description about the conversion of acceleration to the world coordinate system, so I will summarize it. (Although there was some English)

Commentary

Obtaining gravitational acceleration with a low-pass filter

The geomagnetic sensor is also passed through a low pass filter to remove noise.

switch(event.sensor.getType()) {
    case Sensor.TYPE_ACCELEROMETER:
        rawacc = event.values.clone();
        lowpassFilter(grav, rawacc);
        break;


    case Sensor.TYPE_MAGNETIC_FIELD:
        lowpassFilter(mag, event.values.clone());
        break;


    default:
        return;
}


...


private void lowpassFilter(float[] vecPrev, float[] vecNew) {
    for (int i=0; i<vecNew.length; i++) {
        vecPrev[i] = alpha * vecPrev[i] + (1-alpha) * vecNew[i];
    }
}

Get the rotation matrix, get the slope in the world coordinate system

Find the rotation matrix R from the gravitational acceleration vector grav and the geomagnetic vector mag. The slope can be obtained with SensorManager # getOrientation. The unit is [rad]

float[] R = new float[MATRIX_SIZE];
float[] I = new float[MATRIX_SIZE];
float[] rR = new float[MATRIX_SIZE];
float[] oriRad = new float[3];
SensorManager.getRotationMatrix(R, I, grav, mag);
SensorManager.remapCoordinateSystem(R, SensorManager.AXIS_X, SensorManager.AXIS_Z, rR);
SensorManager.getOrientation(rR, oriRad);
ori = rad2deg(oriRad);

Convert to world coordinate system

First, subtract the gravitational acceleration from the instantaneous value.

float[] accNoGrav = new float[4];
for (int i=0; i<3; i++) accNoGrav[i] = rawacc[i] - grav[i];

Find the inverse of the rotation matrix. It seems that ʻandroid.opengl.Matrix # invertM` can be used.

float[] invertR = new float[16];
Matrix.invertM(invertR, 0, R, 0);

Multiply each. You can also use ʻandroid.opengl.Matrix # multiplyMVhere. However, while the vector obtained fromevent.values of SensorEventListener # onSensorChangedis 3D, the dimension of the rotation matrixR` is 4x4, so care must be taken to match the dimensions.

float[] acc4 = new float[4];
Matrix.multiplyMV(acc4, 0, invertR, 0, accNoGrav, 0);


for (int i=0; i<3; i++) acc[i] = acc4[i];

that's all.

Whole code

SenserMonitorExample.java

package com.example.example;


import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.opengl.Matrix;




public class SensorMonitorExample implements SensorEventListener {


    private Context context;
    private SensorManager mSensorManager;


    private static final float alpha = 0.8f;
    private static final int MATRIX_SIZE = 16;
    private static final int SENSOR_DELAY = SensorManager.SENSOR_DELAY_FASTEST;


    private float[] rawacc = new float[3];
    private float[] acc  = new float[3];
    private float[] grav = new float[3];
    private float[] mag  = new float[3];
    private float[] ori  = new float[3];


    public SensorMonitorExample(Context context, int interval) {
        this.context = context;
        this.mSensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE);
    }




    public void start() {
        mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER),  SENSOR_DELAY);
        mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD), SENSOR_DELAY);
    }


    public void stop() {
        mSensorManager.unregisterListener(this);
    }


    @Override
    public void onAccuracyChanged(Sensor sensor, int accuracy) {}


    @Override
    public void onSensorChanged(SensorEvent event) {
        switch(event.sensor.getType()) {
            case Sensor.TYPE_ACCELEROMETER:
                rawacc = event.values.clone();
                lowpassFilter(grav, rawacc);
                break;


            case Sensor.TYPE_MAGNETIC_FIELD:
                lowpassFilter(mag, event.values.clone());
                break;


            default:
                return;
        }


        if (rawacc != null && mag != null) {
            float[] R  = new float[MATRIX_SIZE];
            float[] I  = new float[MATRIX_SIZE];
            float[] rR = new float[MATRIX_SIZE];
            float[] oriRad = new float[3];
            SensorManager.getRotationMatrix(R, I, grav, mag);
            SensorManager.remapCoordinateSystem(R, SensorManager.AXIS_X, SensorManager.AXIS_Z, rR);
            SensorManager.getOrientation(rR, oriRad);


            ori = rad2deg(oriRad);


            float[] accNoGrav = new float[4];
            for (int i=0; i<3; i++) accNoGrav[i] = rawacc[i] - grav[i];


            float[] invertR = new float[16];
            Matrix.invertM(invertR, 0, R, 0);


            float[] acc4 = new float[4];
            Matrix.multiplyMV(acc4, 0, invertR, 0, accNoGrav, 0);


            for (int i=0; i<3; i++) acc[i] = acc4[i];
        }
    }


    private void lowpassFilter(float[] vecPrev, float[] vecNew) {
        for (int i=0; i<vecNew.length; i++) {
            vecPrev[i] = alpha * vecPrev[i] + (1-alpha) * vecNew[i];
        }
    }


    private float[] rad2deg(float[] vec) {
        int VEC_SIZE = vec.length;
        float[] retvec = new float[VEC_SIZE];
        for (int i=0; i<VEC_SIZE; i++) {
            retvec[i] = vec[i]/(float)Math.PI*180;
        }
        return retvec;
    }
}