... | ... | @@ -22,17 +22,17 @@ Entréee : |
|
|
// d'incertitude sur la position
|
|
|
Algorithme :
|
|
|
timeLapseInMs = timeStampInMs - oldTimeStampInMs
|
|
|
// Le temps a avancé et donc l'incertitude sur la position grandit
|
|
|
// Le temps a avancé et donc l'incertitude sur la position grandit
|
|
|
variance += (timeLapseInMs / 1000) * decayInMetersPerSecond^2
|
|
|
oldTimeStampInMilliseconds = timeStampInMs
|
|
|
// 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
|
|
|
// 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
|
|
|
// On applique la transformation K
|
|
|
latitude += K * (latitude - latitude)
|
|
|
longitude += K * (longitude - longitude)
|
|
|
// Nouvelle matrice de covariance : (matrice identité - K) * covarariance, donc ici :
|
|
|
// Nouvelle matrice de covariance : (matrice identité - K) * covarariance, donc ici :
|
|
|
variance = (1 - K) * variance;
|
|
|
``` |
|
|
\ No newline at end of file |