Kalman filter does not update position between GPS fixes if GPS updates are too infrequent

[OlofLindber9]

I’m using the library to combine accelerometer data and GPS fixes with a Kalman filter. My intention is to track user movement more frequently than once per minute (the GPS update rate), without draining the battery too much. However, I’m observing that when I only receive a GPS fix once per minute, the calculated Kalman-filtered position never changes from the last GPS fix, even though the user actually moves. If I reduce the GPS interval to every 10 seconds, the Kalman filter does a slightly better job, but its still not great.

Anyone have any ideas what I am missing?

I tested this on a OnePlus Nord 4

This is how I use the library:

protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_accelerometerandgps);

    // 1) Register this class as a listener
    ServicesHelper.addLocationServiceInterface(this);

    // 2) Init preferences
    mSharedPref = PreferenceManager.getDefaultSharedPreferences(this);

    // 5) Set your desired location provider
    Settings.LocationProvider finalProvider = Settings.LocationProvider.FUSED;

    // 6) Connect to the service
    ServicesHelper.getLocationService(this, value -> {
        if (value.IsRunning()) {
            return;
        }
        value.stop();

        // 7) Build your Kalman filter settings
        Settings settings = new Settings(
                Utils.ACCELEROMETER_DEFAULT_DEVIATION,
                Integer.parseInt(mSharedPref.getString("pref_gps_min_distance", "10")),
                Integer.parseInt(mSharedPref.getString("pref_gps_min_time", "60000")),
                Integer.parseInt(mSharedPref.getString("pref_position_min_time", "500")),
                Integer.parseInt(mSharedPref.getString("pref_geohash_precision", "6")),
                Integer.parseInt(mSharedPref.getString("pref_geohash_min_point", "2")),
                Double.parseDouble(mSharedPref.getString("pref_sensor_frequency", "10")),
                this,           //logger
                true,          //filterMockGpsCoordinates
                false,         //onlyGpsSensor
                true,         //useGpsSpeed
                Utils.DEFAULT_VEL_FACTOR,
                Utils.DEFAULT_POS_FACTOR,
                finalProvider
        );

        value.reset(settings);
        value.start();
    });
}

@Override
public void locationChanged(Location location) {
    lastKalmanLocation = location;

    Log.d("debug message", "Method2: lat=" + location.getLatitude() +
            " lon=" + location.getLongitude());

}

[Lezh1k]

@OlofLindber9
Hello there.
Try C++ library in lib_cpp branch.
Java implementation is obsolete and I'm not sure it works correctly.

[OlofLindber9]

@Lezh1k Thanks for the tip! How should I go about using the C++ library? What I can see, there are no other instructions of how to use the library other than the ones I have followed. All help is greatly appreciated :)

[Lezh1k]

@OlofLindber9
https://github.com/maddevsio/mad-location-manager/tree/lib_cpp/lib/android-example

This is not a perfect example, but it's all I have now. There are 2 methods - processGPS and processACC in main activity - they are wrappers around CPP library.

[OlofLindber9]

@Lezh1k
I tried to use the CPP library but got pretty bad results.

I copied all CPP files into my android project and use the given CMakeList.txt file. I believe I build the cpp code with JNI and am about 95% sure that I have incorporated the cpp code correctly into my android project. I can call both processACC and processGPS from one of my activities.

Although my results are pretty bad(It says I walked 200km in 10 minutes).

I call processACC 10 times a second with new data from the accelerometer. I call processGPS once a minute after a GPS request is made.

Does anyone have any idea of what might be the problem? Does other people use this library to achieve more accurate tracking without increasing the number of GPS calls?

Note: I had to add this line of code in the beginning of the predict method in gps_acc_fusion_filter.cpp

if (m_last_predict_sec == 0) {
m_last_predict_sec = time_sec;
return;
}

This prevented a bug that apprears when the first ever predict is made and no prior prediction exist.

Thanks for any help :)

[Lezh1k]

  if (!m_got_start_point) {
    m_fk.reset(x, y, time_sec, speed_x, speed_y, m_acc_sigma_2, m_loc_sigma_2);

And there is a line inside reset function:
m_last_predict_sec = ts;

So maybe you did not need that fix.

It says I walked 200km in 10 minutes

I have received same feedback and can not reproduce on my phone. Maybe I correct this somehow in visualizator application.
Thank you , I'll check

[OlofLindber9]

@Lezh1k I am thankful for all help I get :)

If you want to try to reproduce my errors I can send you my code that results the inaccurate results. It´s not that much code, about 300 lines in total.

[Lezh1k]

That would be helpful too. Some gist or post it right here if it's possible.

[OlofLindber9]

@Lezh1k
This is the activity that collects data from GPS and sensors, calls the processAcc and processGPS methods and logs the positions.

package com.example.gpsapplication;

import android.Manifest;
import android.content.pm.PackageManager;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.location.Location;
import android.os.Bundle;
import android.os.Environment;
import android.os.Handler;
import android.os.Looper;
import android.util.Log;
import android.view.View;
import android.widget.Button;
import android.widget.Toast;

import androidx.appcompat.app.AppCompatActivity;
import androidx.core.app.ActivityCompat;

import com.google.android.gms.location.FusedLocationProviderClient;
import com.google.android.gms.location.LocationServices;

import java.io.File;
import java.io.FileWriter;
import java.io.IOException;

public class LocationTracker2 extends AppCompatActivity implements SensorEventListener {

private static final String TAG = "LocationTracker2";

// For location
private FusedLocationProviderClient fusedLocationClient;

// For sensors
private SensorManager sensorManager;
private Sensor accelSensor;

// For scheduling GPS calls (once per minute)
private Handler gpsHandler = new Handler(Looper.getMainLooper());
private Runnable gpsRunnable;

// Flag to indicate if tracking is active
private boolean isTracking = false;

// -----------------------------------------------------------
// CSV logging fields
// -----------------------------------------------------------
private FileWriter csvWriter = null;
private int accCallCount = 0; // track how many times processAcc() was called

@Override
protected void onCreate(Bundle savedInstanceState) {
    super.onCreate(savedInstanceState);
    setContentView(R.layout.locationtracker2);

    // 1) Initialize FusedLocationProviderClient
    fusedLocationClient = LocationServices.getFusedLocationProviderClient(this);

    // 2) Initialize Sensor Manager & accelerometer
    sensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
    if (sensorManager != null) {
        accelSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
    }

    // Setup Buttons
    Button startButton = findViewById(R.id.button_start_tracking);
    Button stopButton = findViewById(R.id.button_stop_tracking);
    Button testAccButton = findViewById(R.id.processACC);

    startButton.setOnClickListener(new View.OnClickListener() {
        @Override
        public void onClick(View view) {
            startTracking();
        }
    });
    stopButton.setOnClickListener(new View.OnClickListener() {
        @Override
        public void onClick(View view) {
            stopTracking();
        }
    });
    testAccButton.setOnClickListener(new View.OnClickListener() {
        @Override
        public void onClick(View view) {
            // Example: manually test the JNI calls
            onAccelerometerData(1.0, 2.0, 4.0, 231);
        }
    });

    // 3) Initialize or open a CSV file to log data
    initCsvWriter();
}

private void initCsvWriter() {
    // You can store in external files dir to keep data private to your app
    File csvFile = new File(getExternalFilesDir(null), "location_data.csv");
    try {
        // Overwrite each time -> 'false', if you want to append use 'true'.
        csvWriter = new FileWriter(csvFile, false);

        // (No header row needed since we only log lat,lon)
        // e.g., csvWriter.write("latitude,longitude\n");

        csvWriter.flush();
        Log.d(TAG, "CSV Logging to " + csvFile.getAbsolutePath());
    } catch (IOException e) {
        e.printStackTrace();
        Log.e(TAG, "Failed to open CSV file for writing", e);
    }
}

// -----------------------------------------------------------
//  Tracking Start/Stop
// -----------------------------------------------------------
private void startTracking() {
    if (isTracking) {
        Toast.makeText(this, "Already tracking!", Toast.LENGTH_SHORT).show();
        return;
    }
    isTracking = true;

    // Register accelerometer at ~10 Hz
    if (accelSensor != null) {
        // 10 Hz => 100,000 microseconds = 100ms
        sensorManager.registerListener(this, accelSensor, 100_000);
    }

    // Start a runnable to call processGPS once a minute
    startGpsLoop();
    Toast.makeText(this, "Tracking started", Toast.LENGTH_SHORT).show();
}

private void stopTracking() {
    if (!isTracking) {
        Toast.makeText(this, "Not tracking right now!", Toast.LENGTH_SHORT).show();
        return;
    }
    isTracking = false;

    // Unregister accelerometer
    sensorManager.unregisterListener(this);

    // Stop GPS runnable
    gpsHandler.removeCallbacksAndMessages(null);

    Toast.makeText(this, "Tracking stopped", Toast.LENGTH_SHORT).show();
}

// -----------------------------------------------------------
//  SensorEventListener: Acceleration -> processAcc() at 10 Hz
// -----------------------------------------------------------
@Override
public void onSensorChanged(SensorEvent event) {
    if (!isTracking) return;

    if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
        double ax = event.values[0];
        double ay = event.values[1];
        double az = event.values[2];
        double timestampSec = event.timestamp / 1_000_000_000.0;

        accCallCount++;
        // JNI call: 10 times/second
        double[] outputLatLon = MlmFilterJNI.processAcc(ax, ay, az, timestampSec);

        double outputlat = outputLatLon[0];
        double outputlon = outputLatLon[1];

        Log.d(TAG, "processAcc called (#" + accCallCount + "). Output=(lat=" + outputlat + ", lon=" + outputlon + ")");

        // Every 100th call => log just lat/lon to CSV
        if (accCallCount % 100 == 0) {
            writeLatLonCsv(outputlat, outputlon);
        }
    }
}

// Write only lat,lon
private void writeLatLonCsv(double lat, double lon) {
    if (csvWriter == null) return;
    try {
        csvWriter.write(String.format("%.6f,%.6f\n", lat, lon));
        csvWriter.flush();
    } catch (IOException e) {
        e.printStackTrace();
        Log.e(TAG, "Error writing CSV", e);
    }
}

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

// -----------------------------------------------------------
//  Scheduling processGPS once per minute
// -----------------------------------------------------------
private void startGpsLoop() {
    gpsRunnable = new Runnable() {
        @Override
        public void run() {
            if (!isTracking) return; // Safety check

            fetchLocationAndCallProcessGPS();

            // Re-post the runnable in 60 seconds
            gpsHandler.postDelayed(this, 60_000); // 60 seconds
        }
    };
    gpsHandler.post(gpsRunnable); // Start immediately
}

private void fetchLocationAndCallProcessGPS() {
    if (ActivityCompat.checkSelfPermission(
            this,
            Manifest.permission.ACCESS_FINE_LOCATION
    ) != PackageManager.PERMISSION_GRANTED
    ) {
        // You need to request location permission or handle gracefully
        Log.w(TAG, "Location permission not granted!");
        return;
    }
    fusedLocationClient.getLastLocation()
            .addOnSuccessListener(location -> {
                if (location != null) {
                    double lat = location.getLatitude();
                    double lon = location.getLongitude();

                    double[] outputLatLon = MlmFilterJNI.processGPS(lat, lon);
                    double outputlat = outputLatLon[0];
                    double outputlon = outputLatLon[1];

                    Log.d(TAG, "processGPS called. lat=" + outputlat + ", lon=" + outputlon
                            + " (real lat=" + lat + ", real lon=" + lon + ")");

                    // Write only lat/lon to CSV
                    writeLatLonCsv(outputlat, outputlon);
                } else {
                    Log.d(TAG, "Location was null");
                }
            })
            .addOnFailureListener(e -> {
                Log.e(TAG, "Failed to get location", e);
            });
}

// -----------------------------------------------------------
//  Example test method called by your test button
// -----------------------------------------------------------
private void onAccelerometerData(double ax, double ay, double az, double timestampSec) {
    double[] latlon = MlmFilterJNI.processAcc(ax, ay, az, timestampSec);
    double lat = latlon[0];
    double lon = latlon[1];
    Log.d(TAG, "Test button pressed => processAcc lat=" + lat + " lon=" + lon);
}

// -----------------------------------------------------------
//  Lifecycle
// -----------------------------------------------------------
@Override
protected void onPause() {
    super.onPause();
    // If you want to automatically stop sensor updates when paused:
    if (isTracking) {
        sensorManager.unregisterListener(this);
    }
}

@Override
protected void onResume() {
    super.onResume();
    // If still tracking, re-register
    if (isTracking && accelSensor != null) {
        sensorManager.registerListener(this, accelSensor, 100_000);
    }
}

@Override
protected void onDestroy() {
    super.onDestroy();
    // Close CSV if open
    if (csvWriter != null) {
        try {
            csvWriter.close();
        } catch (IOException e) {
            e.printStackTrace();
        }
    }
}

}

To go with this activity I have the following class

package com.example.gpsapplication;
public class MlmFilterJNI{

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


/**
 * A native method that is implemented by the 'mlmexample' native library,
 * which is packaged with this application.
 */
public native String stringFromJNI();
public static native double[] processAcc(double x, double y, double z, double ts);
public static native double[] processGPS(double latitude, double longitude);

}

These are the files I copied and dragged into my project path: RootOfRepo/app/src/main/cpp

In mlmJNI.cpp I changed one line so that it matches my package

extern "C" JNIEXPORT jdoubleArray JNICALL
Java_com_example_gpsapplication_MlmFilterJNI_processAcc( <-- this matches my package
JNIEnv* env,
jobject /* this */,
jdouble acc_x,
jdouble acc_y,
jdouble acc_z,
jdouble ts) {
abs_accelerometer acc(acc_x, acc_y, acc_z);
mlm.process_acc_data(acc, ts);
gps_coordinate pc = mlm.predicted_coordinate();

double lat = pc.location.latitude;
double lon = pc.location.longitude;

double cArray[2];
cArray[0] = lat;
cArray[1] = lon;

jdoubleArray result = env->NewDoubleArray(2);
if (result == nullptr) {
    // Out of memory error thrown
    return nullptr;
}
 env->SetDoubleArrayRegion(result, 0, 2, cArray);
return result;

}

These lines in the mlmJNI.cpp is marked red gives error but I can run the code without receiving any errors so I think its ok
#include <jni.h>
#include "inc/mlm.h"

This can be found in my gradle build file
externalNativeBuild {
cmake {
// Point to your CMakeLists.txt
path = file("src/main/cpp/CMakeLists.txt")
}
}

The phone I use is a Oneplus Nord 4

[Lezh1k]

@OlofLindber9

Ok, you can not use if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) .

Please take a look at the example. You need to convert accelerometer data to NED (north-east-down) vector . And then use it as input of MLM filter. Also consider the screen orientation.