Ecco un semplice filtro Kalman che potrebbe essere utilizzato proprio per questa situazione. È venuto da un lavoro che ho fatto su dispositivi Android.
La teoria generale del filtro di Kalman riguarda le stime per i vettori, con l'accuratezza delle stime rappresentate dalle matrici di covarianza. Tuttavia, per stimare la posizione sui dispositivi Android la teoria generale si riduce a un caso molto semplice. I fornitori di servizi di localizzazione Android forniscono la posizione come latitudine e longitudine, insieme a una precisione specificata come un singolo numero misurato in metri. Ciò significa che invece di una matrice di covarianza, l'accuratezza nel filtro Kalman può essere misurata da un singolo numero, anche se la posizione nel filtro Kalman è misurata da due numeri. Inoltre, il fatto che latitudine, longitudine e metri siano effettivamente tutte le diverse unità può essere ignorato, perché se si inseriscono i fattori di ridimensionamento nel filtro Kalman per convertirli tutti nelle stesse unità,
Il codice potrebbe essere migliorato, poiché si presume che la migliore stima della posizione corrente sia l'ultima posizione nota e se qualcuno si sta spostando dovrebbe essere possibile utilizzare i sensori di Android per produrre una stima migliore. Il codice ha un singolo parametro libero Q, espresso in metri al secondo, che descrive la velocità con cui la precisione diminuisce in assenza di nuove stime di posizione. Un parametro Q più elevato indica che l'accuratezza decade più rapidamente. I filtri Kalman funzionano generalmente meglio quando l'accuratezza diminuisce un po 'più rapidamente di quanto ci si potrebbe aspettare, quindi per andare in giro con un telefono Android trovo che Q = 3 metri al secondo funzioni bene, anche se generalmente cammino più lentamente di così. Ma se si viaggia in un'auto veloce dovrebbe ovviamente essere usato un numero molto più grande.
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;
}
}
}