|
|
# Filtrage de Kalman
|
|
|
|
|
|
## Référence
|
|
|
https://stackoverflow.com/questions/1134579/smooth-gps-data/1189790#1189790
|
|
|
|
|
|
Citation:
|
|
|
|
|
|
“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 use.”
|
|
|
|
|
|
## Implémentation
|
|
|
|
|
|
De cela, nous avons donc gardé l’algorithme suivant :
|
|
|
|
|
|
```
|
|
|
Entréee :
|
|
|
position : latitude, longitude, accuracy
|
|
|
temps: timeStampInMs
|
|
|
decayInMetersPerSecond // décrit combien la précision decroit en
|
|
|
// l'absence d'une nouvelle estimation de
|
|
|
// position. Une valeur élevée signifie
|
|
|
// que la précision décroit rapidement.
|
|
|
// Ici, la valeur correspond à la vitesse de
|
|
|
// déplacement puisqu'elle lui est lié,
|
|
|
// ie moins la vitesse de déplacement est
|
|
|
// élevée moins il y a d'incertitude sur la
|
|
|
// position
|
|
|
timeLapse = timeStampInMs - oldTimeStampInMs
|
|
|
// Le temps a avancé et donc l'incertitude sur la position grandit
|
|
|
variance += (timeLapse / 1000) * decayInMetersPerSecond^2
|
|
|
oldTimeStampInMilliseconds = timeStampInMilliseconds
|
|
|
// Matrice de gain = Covariance * Inverse(Covariance + variance de la mesure)
|
|
|
// Ici, la matrice de gain se réduit à un scalaire (voir la justification plus haut)
|
|
|
// NB: parce que K est sans dimension, il importe peu que la variance
|
|
|
// ait une unité différente de la latitude et longitude
|
|
|
K = variance / (variance + accuracy * accuracy)
|
|
|
// On applique la transformation K
|
|
|
latitude += K * (latitude - latitude)
|
|
|
longitude += K * (longitude - longitude)
|
|
|
// nouvelle matrice de covariance : (matrice identite - K) * covarariance, donc ici :
|
|
|
variance = (1 - K) * variance;
|
|
|
``` |
|
|
\ No newline at end of file |