Here's a simple Kalman filter that could be used for exactly this situation. It came from some work I did on Android devices.
General Kalman filter theory is all about estimates for vectors, with the accuracy of the estimates represented by covariance matrices. However, for estimating location on Android devices the general theory reduces to a very simple case. Android location providers give the location as a latitude and longitude, together with an accuracy which is specified as a single number measured in metres. This means that instead of a covariance matrix, the accuracy in the Kalman filter can be measured by a single number, even though the location in the Kalman filter is a measured by two numbers. Also the fact that the latitude, longitude and metres are effectively all different units can be ignored, because if you put scaling factors into the Kalman filter to convert them all into the same units, then those scaling factors end up cancelling out when converting the results back into the original units.
The code could be improved, because it assumes that the best estimate of current location is the last known location, and if someone is moving it should be possible to use Android's sensors to produce a better estimate. The code has a single free parameter Q, expressed in metres per second, which describes how quickly the accuracy decays in the absence of any new location estimates. A higher Q parameter means that the accuracy decays faster. Kalman filters generally work better when the accuracy decays a bit quicker than one might expect, so for walking around with an Android phone I find that Q=3 metres per second works fine, even though I generally walk slower than that. But if travelling in a fast car a much larger number should obviously be used.
public class KalmanLatLong {
private final float MinAccuracy = 1;
private float Q_metres_per_second;
private long TimeStamp_milliseconds;
private double lat;
private double lng;
private float variance; // P matrix. Negative means object uninitialised. NB: units irrelevant, as long as same units used throughout
public KalmanLatLong(float Q_metres_per_second) { this.Q_metres_per_second = Q_metres_per_second; variance = -1; }
public long get_TimeStamp() { return TimeStamp_milliseconds; }
public double get_lat() { return lat; }
public double get_lng() { return lng; }
public float get_accuracy() { return (float)Math.sqrt(variance); }
public void SetState(double lat, double lng, float accuracy, long TimeStamp_milliseconds) {
this.lat=lat; this.lng=lng; variance = accuracy * accuracy; this.TimeStamp_milliseconds=TimeStamp_milliseconds;
}
/// <summary>
/// Kalman filter processing for lattitude and longitude
/// </summary>
/// <param name="lat_measurement_degrees">new measurement of lattidude</param>
/// <param name="lng_measurement">new measurement of longitude</param>
/// <param name="accuracy">measurement of 1 standard deviation error in metres</param>
/// <param name="TimeStamp_milliseconds">time of measurement</param>
/// <returns>new state</returns>
public void Process(double lat_measurement, double lng_measurement, float accuracy, long TimeStamp_milliseconds) {
if (accuracy < MinAccuracy) accuracy = MinAccuracy;
if (variance < 0) {
// if variance < 0, object is unitialised, so initialise with current values
this.TimeStamp_milliseconds = TimeStamp_milliseconds;
lat=lat_measurement; lng = lng_measurement; variance = accuracy*accuracy;
} else {
// else apply Kalman filter methodology
long TimeInc_milliseconds = TimeStamp_milliseconds - this.TimeStamp_milliseconds;
if (TimeInc_milliseconds > 0) {
// time has moved on, so the uncertainty in the current position increases
variance += TimeInc_milliseconds * Q_metres_per_second * Q_metres_per_second / 1000;
this.TimeStamp_milliseconds = TimeStamp_milliseconds;
// TO DO: USE VELOCITY INFORMATION HERE TO GET A BETTER ESTIMATE OF CURRENT POSITION
}
// Kalman gain matrix K = Covarariance * Inverse(Covariance + MeasurementVariance)
// NB: because K is dimensionless, it doesn't matter that variance has different units to lat and lng
float K = variance / (variance + accuracy * accuracy);
// apply K
lat += K * (lat_measurement - lat);
lng += K * (lng_measurement - lng);
// new Covarariance matrix is (IdentityMatrix - K) * Covarariance
variance = (1 - K) * variance;
}
}
}
