Hier ist ein einfacher Kalman-Filter, der genau für diese Situation verwendet werden kann. Es kam von einer Arbeit, die ich auf Android-Geräten gemacht habe.
In der allgemeinen Kalman-Filtertheorie dreht sich alles um Schätzungen für Vektoren, wobei die Genauigkeit der Schätzungen durch Kovarianzmatrizen dargestellt wird. Für die Standortschätzung auf Android-Geräten reduziert sich die allgemeine Theorie jedoch auf einen sehr einfachen Fall. Android-Standortanbieter geben den Standort als Breiten- und Längengrad sowie eine Genauigkeit an, die als einzelne Zahl in Metern angegeben wird. Dies bedeutet, dass anstelle einer Kovarianzmatrix die Genauigkeit im Kalman-Filter durch eine einzelne Zahl gemessen werden kann, obwohl die Position im Kalman-Filter durch zwei Zahlen gemessen wird. Auch die Tatsache, dass Breite, Länge und Meter effektiv alle unterschiedliche Einheiten sind, kann ignoriert werden, denn wenn Sie Skalierungsfaktoren in den Kalman-Filter einfügen, um sie alle in dieselben Einheiten umzuwandeln,
Der Code könnte verbessert werden, da davon ausgegangen wird, dass die beste Schätzung des aktuellen Standorts der letzte bekannte Standort ist. Wenn sich jemand bewegt, sollte es möglich sein, die Sensoren von Android zu verwenden, um eine bessere Schätzung zu erstellen. Der Code hat einen einzelnen freien Parameter Q, ausgedrückt in Metern pro Sekunde, der beschreibt, wie schnell die Genauigkeit abnimmt, wenn keine neuen Standortschätzungen vorliegen. Ein höherer Q-Parameter bedeutet, dass die Genauigkeit schneller abnimmt. Kalman-Filter funktionieren im Allgemeinen besser, wenn die Genauigkeit etwas schneller abnimmt als erwartet. Wenn ich also mit einem Android-Telefon herumlaufe, funktioniert Q = 3 Meter pro Sekunde einwandfrei, obwohl ich im Allgemeinen langsamer gehe. Aber wenn Sie in einem schnellen Auto fahren, sollte natürlich eine viel größere Anzahl verwendet werden.
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;
}
}
}