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;
    }
}

Recommended Posts

Get the acceleration and bearing of the world coordinate system on Android
[Android] Get the date on Monday
[Android] Get the tapped position (coordinates) on the screen
The world of clara-rules (2)
The world of clara-rules (4)
[Android] Display images and characters on the ViewPager tab
The world of clara-rules (1)
The world of clara-rules (3)
[Android] How to get the setting language of the terminal
The world of clara-rules (5)
[Introduction] Display Android Studio Hello World on the emulator
Access Web API on Android with Get and process Json (Java for the time being)
About truncation by the number of bytes of String on Android
[Rails] How to get the URL of the transition source and redirect
[Java] Get the dates of the past Monday and Sunday in order
Android --Is the order of serial processing and parallel processing of AsyncTask guaranteed? ??
[Swift5] How to get an array and the complement of arrays
I opened the menu bar (option menu) on Android and saw it.
Method to add the number of years and get the end of the month
Released the No Todo app instead of Todo. .. (And notes on ridgepole)
About the basics of Android development
Get JUnit code coverage on Android.
Note on the path of request.getRequestDispatcher
This and that of the JDK
Get the ID of automatic numbering
Android OS (7.1.2) build and Hello World
[Android] How to turn the Notification panel on and off using StatusBarManager
Read the IC balance of your student ID card (Felica) on Android
Get a rough idea of the differences between protocols, classes and structs!
What wasn't fair use in the diversion of Java APIs on Android
The story of installing raspi-config on ubuntu 18.04 and changing the initial settings of GPIO
Implementation comparison of production that makes images shine on iOS and Android