Android

How to get acceleration in ENU (east-north-up) coordinate system

1. Collect data from LINEAR_ACCELERATION and ROTATION_VECTOR sensors

package com.example.mlm_example.sensors;

import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.util.Log;

import java.util.ArrayList;
import java.util.List;

public class RawENUSensor extends ISensor implements SensorEventListener {
    private static final String TAG = "RawENUSensor";
    private final List<Sensor> m_lst_sensors = new ArrayList<>();
    private final SensorManager m_sensor_manager;
    private static final int[] sensor_types = {Sensor.TYPE_LINEAR_ACCELERATION, Sensor.TYPE_ROTATION_VECTOR};
    public RawENUSensor(SensorManager sensor_manager) {
        m_sensor_manager = sensor_manager;
        for (Integer st : sensor_types) {
            Sensor sensor = m_sensor_manager.getDefaultSensor(st);
            if (sensor == null) {
                Log.e(TAG, String.format("Couldn't get sensor %d", st));
                continue;
            }
            m_lst_sensors.add(sensor);
        }
    }

    @Override
    protected boolean onStart() {
        for (Sensor sensor : m_lst_sensors) {
            if (!m_sensor_manager.registerListener(this, sensor, SensorManager.SENSOR_DELAY_GAME)) {
                Log.e(TAG, String.format("Couldn't registerListener %d", sensor.getType()));
                return false;
            }
        }
        return true;
    }

    @Override
    protected boolean onStop() {
        for (Sensor sensor : m_lst_sensors) {
            m_sensor_manager.unregisterListener(this, sensor);
        }
        return true;
    }
    protected final float[] qDev2World = new float[4];
    protected final float[] lin_acc = {0.f, 0.f, 0.f};
    // For sensor timing synchronization
    private long m_last_rotation_ts = 0;
    private volatile boolean m_is_rotation_valid = false;
    private static final long MAX_SENSOR_TIMESTAMP_DELTA_NS = 4 * 1000_000; // 5ms in nanoseconds
    protected void onRawENUReceived(double ts, float[] acc, float[] q) {
        float w = q[0], x = q[1], y = q[2], z = q[3];
        String fmt = "5 %f:::%f %f %f %f %f %f %f";
        String msg = String.format(java.util.Locale.US, fmt, ts, acc[0], acc[1], acc[2], w, x, y, z);
        System.out.println(msg);
    }

    private void handleLinearAcceleration(SensorEvent event) {
        if (!m_is_rotation_valid) {
            return; // ignore this
        }

        if (Math.abs(event.timestamp - m_last_rotation_ts) > MAX_SENSOR_TIMESTAMP_DELTA_NS) {
            return; // Skip this acceleration update as rotation might be outdated
        }

        double ts = android.os.SystemClock.elapsedRealtime() / 1000.;
        System.arraycopy(event.values, 0, lin_acc, 0, 3);
        onRawENUReceived(ts, lin_acc, qDev2World);
    }

    private void handleRotationVector(SensorEvent event) {
        long ts = event.timestamp;
        SensorManager.getQuaternionFromVector(qDev2World, event.values);
        // Update timestamp and validity
        m_last_rotation_ts = ts;
        m_is_rotation_valid = true;
    }

    @Override
    public void onSensorChanged(SensorEvent event) {
        switch (event.sensor.getType()) {
            case Sensor.TYPE_LINEAR_ACCELERATION:
                handleLinearAcceleration(event);
                break;
            case Sensor.TYPE_ROTATION_VECTOR:
                handleRotationVector(event);
                break;
        }
    }

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

2. Rotate the acceleration vector with a quaternion

    private static void quatRotateVec(float[] q, float[] v, float[] out) {
        // q = (w, U); qV = (0, v)
        // out = qvq(-1)
        // qv=(−u⋅v,wv+u×v)  - see quaternion multiplication formula
        // q(−1)=(w,−u)
        final float w = q[0], x = q[1], y = q[2], z = q[3];

        // t = 2 * cross(q.xyz, v)
        final float t0 = 2f * (y * v[2] - z * v[1]);
        final float t1 = 2f * (z * v[0] - x * v[2]);
        final float t2 = 2f * (x * v[1] - y * v[0]);

        // out = v + w * t + cross(q.xyz, t)
        out[0] = v[0] + w * t0 + (y * t2 - z * t1);
        out[1] = v[1] + w * t1 + (z * t0 - x * t2);
        out[2] = v[2] + w * t2 + (x * t1 - y * t0);
    }

    @Override
    protected void onRawENUReceived(double ts, float[] acc, float[] q) {
        quatRotateVec(q, acc, enu_acc);
        onENUReceived(ts, enu_acc[0], enu_acc[1], enu_acc[2]);
    }

3. Feed these values to MLM (see mlm.h file)

How to build and use the filter in an Android application

1. Create a new Native C++ Android application
2. The app/src folder will look something like this:

├── androidTest
│   └── java
│       └── com
├── main
│   ├── AndroidManifest.xml
│   ├── cpp
│   │   ├── CMakeLists.txt│   │   
│   │   └── native-lib.cpp
│   ├── java
│   │   └── com
│   └── res
└── test
    └── java
        └── com

1. Go to main/cpp folder and add mlm as git submodule

Example: cd main/cpp && git submodule add https://github.com/maddevsio/mad-location-manager-lib.git

1. Change the CMakeLists.txt file in main/cpp. Probably there is a better way to build the filter, but here is an example file which works on my machine:

cmake_minimum_required(VERSION 3.22.1)

project("mlm_filter_android")

set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

if(CMAKE_BUILD_TYPE MATCHES Debug)
    message("Debug build.")
elseif(CMAKE_BUILD_TYPE MATCHES Release)
    message("Release build.")
else()
    message("Some other build type. Setting up default = Debug")
    set(CMAKE_BUILD_TYPE Debug)
endif()

find_package(Git REQUIRED)

message("CMAKE_CURRENT_SOURCE_DIR:" ${CMAKE_CURRENT_SOURCE_DIR})
find_path(MLM_LIB "mad-location-manager-lib" ${CMAKE_CURRENT_SOURCE_DIR}/external)
if (NOT MLM_LIB)
    message("unable to find mad-location-manager-lib in ${CMAKE_CURRENT_SOURCE_DIR}/external")
    execute_process(COMMAND
            git submodule update --init -- external/mad-location-manager-lib
            WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
    set(MLM_LIB ${CMAKE_CURRENT_SOURCE_DIR}/external)
endif()
set(MLM_LIB ${MLM_LIB}/mad-location-manager-lib)

file(GLOB_RECURSE sources_lib ${MLM_LIB}/filter/inc/*.h ${MLM_LIB}/filter/src/*.cpp native-lib.cpp)

# HACK! For some reason, the build launched from Android Studio fails
# with error  error: instantiation of variable
# 'GeographicLib::Geodesic30<double>::tiny_' required here,
# but no definition is available [-Werror,-Wundefined-var-template]
# Can not suppress it by doing something like this:
# target_compile_options(GeographicLib PUBLIC -Wno-undefined-var-template)
# Any better solution is more than welcome here
set(CMAKE_CXX_FLAGS_BACK ${CMAKE_CXX_FLAGS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -Wno-undefined-var-template")
add_subdirectory(${MLM_LIB}/filter)
set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS_BACK})

add_library(mlm_filter_android SHARED ${sources_lib})

target_include_directories(mlm_filter_android PUBLIC
        ${MLM_LIB}/filter/inc
)
target_include_directories(mlm_filter_android PRIVATE
        ${MLM_LIB}/filter/external/eigen/
        ${MLM_LIB}/filter/external/geographiclib/include
        ${CMAKE_CURRENT_BINARY_DIR}/external/mad-location-manager-lib/filter/inc
        ${CMAKE_CURRENT_BINARY_DIR}/external/mad-location-manager-lib/filter/external/eigen/
        ${CMAKE_CURRENT_BINARY_DIR}/external/mad-location-manager-lib/filter/external/geographiclib/include
)

target_compile_options(mlm_filter_android PUBLIC ${warning_level} -fPIC)
target_link_libraries(mlm_filter_android
        mlm_filter
        android
        log)

1. Change the native-lib.cpp file:

#include <jni.h>
#include "mlm.h"

static const double acc_sigma_2 = 0.3;
static const double loc_sigma_2 = 8.0;
static const double vel_sigma_2 = 0.1;
static MLM mlm(acc_sigma_2, loc_sigma_2, vel_sigma_2);

extern "C" JNIEXPORT jstring JNICALL
Java_com_example_mlm_1example_MainActivity_stringFromJNI(
        JNIEnv* env,
        jobject /* this */) {
    std::string hello = "MLM example";
    return env->NewStringUTF(hello.c_str());
}

extern "C" JNIEXPORT jboolean JNICALL
Java_com_example_mlm_1example_MainActivity_processENUAcc(
        JNIEnv* env,
        jobject /* this */,
        jdouble acc_east,
        jdouble acc_north,
        jdouble acc_up,
        jdouble ts) {
    enu_accelerometer acc(acc_east, acc_north, acc_up);
    bool pad = mlm.process_acc_data(acc, ts);
    return pad ? JNI_TRUE : JNI_FALSE; // probably there is better way to do that

}

extern "C" JNIEXPORT jdoubleArray JNICALL
Java_com_example_mlm_1example_MainActivity_processGPS(
        JNIEnv* env,
        jobject /* this */,
        jdouble loc_latitude,
        jdouble loc_longitude,
        jdouble loc_altitude,
        jdouble loc_error,
        jdouble speed_value,
        jdouble speed_azimuth,
        jdouble speed_error,
        jdouble ts) {
    gps_coordinate gps(loc_latitude, loc_longitude, loc_altitude, loc_error,
            speed_value, speed_azimuth, speed_error);

    mlm.process_gps_data(gps, ts);
    gps_coordinate pc = mlm.predicted_coordinate();

    // Create a double array with [latitude, longitude]
    jdoubleArray result = env->NewDoubleArray(2);
    if (result == nullptr)
        return nullptr;

    jdouble coords[2];
    coords[0] = pc.location.latitude;
    coords[1] = pc.location.longitude;
    env->SetDoubleArrayRegion(result, 0, 2, coords);
    return result;
}

1. Somewhere in Android define these functions (for example in MainActivity.java):

    static {
        System.loadLibrary("mlm_filter_android");
    }

    public native Boolean processENUAcc(double acc_east, double acc_north, double acc_up, double ts) ;
    public native Double[] processGPS(
            double loc_latitude,
            double loc_longitude,
            double loc_altitude,
            double loc_error,
            double speed_value,
            double speed_azimuth,
            double speed_error,
            double ts
    );

Then use the processENUAcc and processGPS functions to get filtered GPS coordinates.