Comment utiliser le filtre Kalman dans Python pour les données de localisation?
[EDIT] La réponse de @Claudio me donne un très bon conseil sur la façon de filtrer les valeurs aberrantes. Je veux cependant commencer à utiliser un filtre Kalman sur mes données. J'ai donc changé les données d'exemple ci-dessous pour qu'il y ait un bruit de variation subtile qui n'est pas si extrême (que je vois aussi beaucoup). Si quelqu'un d'autre pouvait me donner des indications sur la façon d'utiliser PyKalman sur mes données, ce serait formidable. [/MODIFIER]
Pour un projet de robotique, j'essaie de suivre un cerf-volant dans les airs avec une caméra. Je programme en Python et j'ai collé quelques résultats de localisation bruyants ci-dessous (chaque élément a également un objet datetime inclus, mais je les ai laissés pour plus de clarté).
[ # X Y
{'loc': (399, 293)},
{'loc': (403, 299)},
{'loc': (409, 308)},
{'loc': (416, 315)},
{'loc': (418, 318)},
{'loc': (420, 323)},
{'loc': (429, 326)}, # <== Noise in X
{'loc': (423, 328)},
{'loc': (429, 334)},
{'loc': (431, 337)},
{'loc': (433, 342)},
{'loc': (434, 352)}, # <== Noise in Y
{'loc': (434, 349)},
{'loc': (433, 350)},
{'loc': (431, 350)},
{'loc': (430, 349)},
{'loc': (428, 347)},
{'loc': (427, 345)},
{'loc': (425, 341)},
{'loc': (429, 338)}, # <== Noise in X
{'loc': (431, 328)}, # <== Noise in X
{'loc': (410, 313)},
{'loc': (406, 306)},
{'loc': (402, 299)},
{'loc': (397, 291)},
{'loc': (391, 294)}, # <== Noise in Y
{'loc': (376, 270)},
{'loc': (372, 272)},
{'loc': (351, 248)},
{'loc': (336, 244)},
{'loc': (327, 236)},
{'loc': (307, 220)}
]
J'ai d'abord pensé à calculer manuellement les valeurs aberrantes, puis à les supprimer simplement des données en temps réel. Ensuite, j'ai lu sur les filtres Kalman et comment ils sont spécifiquement destinés à atténuer les données bruyantes. Donc, après quelques recherches, j'ai trouvé la bibliothèque PyKalman qui semble parfaite pour cela. Depuis que j'étais un peu perdu dans toute la terminologie du filtre Kalman, j'ai lu le wiki et quelques autres pages sur les filtres Kalman. J'ai une idée générale d'un filtre de Kalman, mais je suis vraiment perdu dans la façon de l'appliquer à mon code.
Dans les documents PyKalman j'ai trouvé l'exemple suivant:
>>> from pykalman import KalmanFilter
>>> import numpy as np
>>> kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]])
>>> measurements = np.asarray([[1,0], [0,0], [0,1]]) # 3 observations
>>> kf = kf.em(measurements, n_iter=5)
>>> (filtered_state_means, filtered_state_covariances) = kf.filter(measurements)
>>> (smoothed_state_means, smoothed_state_covariances) = kf.smooth(measurements)
J'ai simplement substitué les observations à mes propres observations comme suit:
from pykalman import KalmanFilter
import numpy as np
kf = KalmanFilter(transition_matrices = [[1, 1], [0, 1]], observation_matrices = [[0.1, 0.5], [-0.3, 0.0]])
measurements = np.asarray([(399,293),(403,299),(409,308),(416,315),(418,318),(420,323),(429,326),(423,328),(429,334),(431,337),(433,342),(434,352),(434,349),(433,350),(431,350),(430,349),(428,347),(427,345),(425,341),(429,338),(431,328),(410,313),(406,306),(402,299),(397,291),(391,294),(376,270),(372,272),(351,248),(336,244),(327,236),(307,220)])
kf = kf.em(measurements, n_iter=5)
(filtered_state_means, filtered_state_covariances) = kf.filter(measurements)
(smoothed_state_means, smoothed_state_covariances) = kf.smooth(measurements)
mais cela ne me donne aucune donnée significative. Par exemple, le smoothed_state_means devient le suivant:
>>> smoothed_state_means
array([[-235.47463353, 36.95271449],
[-354.8712597 , 27.70011485],
[-402.19985301, 21.75847069],
[-423.24073418, 17.54604304],
[-433.96622233, 14.36072376],
[-443.05275258, 11.94368163],
[-446.89521434, 9.97960296],
[-456.19359012, 8.54765215],
[-465.79317394, 7.6133633 ],
[-474.84869079, 7.10419182],
[-487.66174033, 7.1211321 ],
[-504.6528746 , 7.81715451],
[-506.76051587, 8.68135952],
[-510.13247696, 9.7280697 ],
[-512.39637431, 10.9610031 ],
[-511.94189431, 12.32378146],
[-509.32990832, 13.77980587],
[-504.39389762, 15.29418648],
[-495.15439769, 16.762472 ],
[-480.31085928, 18.02633612],
[-456.80082586, 18.80355017],
[-437.35977492, 19.24869224],
[-420.7706184 , 19.52147918],
[-405.59500937, 19.70357845],
[-392.62770281, 19.8936389 ],
[-388.8656724 , 20.44525168],
[-361.95411607, 20.57651509],
[-352.32671579, 20.84174084],
[-327.46028214, 20.77224385],
[-319.75994982, 20.9443245 ],
[-306.69948771, 21.24618955],
[-287.03222693, 21.43135098]])
Une âme plus brillante que moi pourrait-elle me donner des indices ou des exemples dans la bonne direction? Tous les conseils sont les bienvenus!
TL; DR, voir le code et l'image en bas.
Je pense qu'un filtre de Kalman pourrait très bien fonctionner dans votre application, mais cela nécessitera un peu plus de réflexion sur la dynamique/physique du cerf-volant.
Je recommanderais fortement de lire cette page Web . Je n'ai aucun lien avec l'auteur ou je ne le connais pas, mais j'ai passé environ une journée à essayer de contourner les filtres Kalman, et cette page l'a vraiment fait cliquer pour moi.
Brièvement; pour un système qui est linéaire et qui a une dynamique connue (c'est-à-dire si vous connaissez l'état et les entrées, vous pouvez prédire l'état futur), il fournit un moyen optimal de combiner ce que vous savez sur un système pour estimer son véritable état. La partie intelligente (qui est prise en charge par toute l'algèbre matricielle que vous voyez dans les pages qui la décrivent) est de savoir comment elle combine de manière optimale les deux informations dont vous disposez:
Mesures (qui sont sujettes au "bruit de mesure", c'est-à-dire que les capteurs ne sont pas parfaits)
Dynamique (c'est-à-dire comment vous pensez que les états évoluent en fonction des entrées, qui sont soumises au "bruit de processus", ce qui est juste une façon de dire que votre modèle ne correspond pas parfaitement à la réalité).
Vous spécifiez à quel point vous êtes sûr de chacun de ces éléments (via les matrices de co-variances [~ # ~] r [~ # ~] et [~ # ~] q [~ # ~] respectivement), et le Kalman Gain détermine combien vous devez croire votre modèle (c.-à-d. votre estimation actuelle de votre état), et combien vous devriez croire vos mesures.
Sans plus tarder, construisons un modèle simple de votre cerf-volant. Ce que je propose ci-dessous est un modèle possible très simple. Vous en savez peut-être plus sur la dynamique du Kite, vous pouvez donc en créer un meilleur.
Considérons le cerf-volant comme une particule (évidemment une simplification, un vrai cerf-volant est un corps étendu, donc a une orientation en 3 dimensions), qui a quatre états que pour plus de commodité nous pouvons écrire dans un vecteur d'état:
x = [x, x_dot, y, y_dot],
Où x et y sont les positions, et les _dot sont les vitesses dans chacune de ces directions. D'après votre question, je suppose qu'il y a deux mesures (potentiellement bruyantes), que nous pouvons écrire dans un vecteur de mesure:
z = [x, y],
Nous pouvons noter la matrice de mesure ( [~ # ~] h [~ # ~] discutée ici , et observation_matrices dans pykalman bibliothèque):
z = [~ # ~] h [~ # ~] x => [~ # ~] h [~ # ~] = [[1, 0, 0, 0], [0, 0, 1, 0]]
Nous devons ensuite décrire la dynamique du système. Ici, je supposerai qu'aucune force externe n'agit et qu'il n'y a pas d'amortissement sur le mouvement du cerf-volant (avec plus de connaissances, vous pourrez peut-être faire mieux, cela traite efficacement les forces externes et l'amortissement comme une perturbation inconnue/sans modèle).
Dans ce cas, la dynamique de chacun de nos états dans l'échantillon actuel "k" en fonction des états dans les échantillons précédents "k-1" est donnée comme:
x(k) = x(k-1) + dt*x_dot(k-1)
x_dot (k) = x_dot (k-1)
y(k) = y(k-1) + dt*y_dot(k-1)
y_dot (k) = y_dot (k-1)
Où "dt" est le pas de temps. Nous supposons que la position (x, y) est mise à jour en fonction de la position et de la vitesse actuelles, et la vitesse reste inchangée. Étant donné qu'aucune unité n'est donnée, nous pouvons simplement dire que les unités de vitesse sont telles que nous pouvons omettre "dt" des équations ci-dessus, c'est-à-dire en unités de position_units/sample_interval (je suppose que vos échantillons mesurés sont à un intervalle constant). Nous pouvons résumer ces quatre équations dans une matrice dynamique comme ( [~ # ~] f [~ # ~] discutée ici, et transition_matrices dans pykalman bibliothèque):
x (k) = Fx (k-1) => [~ # ~] f [~ # ~] = [[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]].
Nous pouvons maintenant essayer d'utiliser le filtre de Kalman en python. Modifié à partir de votre code:
from pykalman import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import time
measurements = np.asarray([(399,293),(403,299),(409,308),(416,315),(418,318),(420,323),(429,326),(423,328),(429,334),(431,337),(433,342),(434,352),(434,349),(433,350),(431,350),(430,349),(428,347),(427,345),(425,341),(429,338),(431,328),(410,313),(406,306),(402,299),(397,291),(391,294),(376,270),(372,272),(351,248),(336,244),(327,236),(307,220)])
initial_state_mean = [measurements[0, 0],
0,
measurements[0, 1],
0]
transition_matrix = [[1, 1, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 1],
[0, 0, 0, 1]]
observation_matrix = [[1, 0, 0, 0],
[0, 0, 1, 0]]
kf1 = KalmanFilter(transition_matrices = transition_matrix,
observation_matrices = observation_matrix,
initial_state_mean = initial_state_mean)
kf1 = kf1.em(measurements, n_iter=5)
(smoothed_state_means, smoothed_state_covariances) = kf1.smooth(measurements)
plt.figure(1)
times = range(measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo',
times, measurements[:, 1], 'ro',
times, smoothed_state_means[:, 0], 'b--',
times, smoothed_state_means[:, 2], 'r--',)
plt.show()
Ce qui a produit ce qui suit, montrant qu'il fait un travail raisonnable de rejet du bruit (le bleu est la position x, le rouge est la position y et l'axe x n'est que le numéro d'échantillon).
Supposons que vous regardiez l'intrigue ci-dessus et que vous pensiez qu'elle semble trop cahoteuse. Comment pouvez-vous résoudre ce problème? Comme indiqué ci-dessus, un filtre de Kalman agit sur deux informations:
- Mesures (dans ce cas de deux de nos états, x et y)
- Dynamique du système (et estimation actuelle de l'état)
La dynamique capturée dans le modèle ci-dessus est très simple. Pris littéralement, ils disent que les positions seront mises à jour par les vitesses actuelles (d'une manière évidente et physiquement raisonnable), et que les vitesses restent constantes (ce n'est clairement pas physiquement vrai, mais capture notre intuition que les vitesses devraient changer lentement).
Si nous pensons que l'état estimé devrait être plus fluide, une façon d'y parvenir est de dire que nous avons moins confiance en nos mesures que notre dynamique (c'est-à-dire que nous avons un observation_covariance, par rapport à notre state_covariance).
À partir de la fin du code ci-dessus, corrigez le observation covariance à 10 fois la valeur estimée précédemment, en définissant em_vars comme indiqué est nécessaire pour éviter une réestimation de la covariance d'observation (voir ici )
kf2 = KalmanFilter(transition_matrices = transition_matrix,
observation_matrices = observation_matrix,
initial_state_mean = initial_state_mean,
observation_covariance = 10*kf1.observation_covariance,
em_vars=['transition_covariance', 'initial_state_covariance'])
kf2 = kf2.em(measurements, n_iter=5)
(smoothed_state_means, smoothed_state_covariances) = kf2.smooth(measurements)
plt.figure(2)
times = range(measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo',
times, measurements[:, 1], 'ro',
times, smoothed_state_means[:, 0], 'b--',
times, smoothed_state_means[:, 2], 'r--',)
plt.show()
Ce qui produit le graphique ci-dessous (mesures sous forme de points, estimations d'état sous forme de ligne pointillée). La différence est plutôt subtile, mais j'espère que vous pouvez voir qu'elle est plus fluide.
Enfin, si vous souhaitez utiliser ce filtre ajusté en ligne, vous pouvez le faire en utilisant le filter_update méthode. Notez que cela utilise la méthode filter plutôt que la méthode smooth, car la méthode smooth ne peut être appliquée qu'aux mesures par lots. Plus ici :
time_before = time.time()
n_real_time = 3
kf3 = KalmanFilter(transition_matrices = transition_matrix,
observation_matrices = observation_matrix,
initial_state_mean = initial_state_mean,
observation_covariance = 10*kf1.observation_covariance,
em_vars=['transition_covariance', 'initial_state_covariance'])
kf3 = kf3.em(measurements[:-n_real_time, :], n_iter=5)
(filtered_state_means, filtered_state_covariances) = kf3.filter(measurements[:-n_real_time,:])
print("Time to build and train kf3: %s seconds" % (time.time() - time_before))
x_now = filtered_state_means[-1, :]
P_now = filtered_state_covariances[-1, :]
x_new = np.zeros((n_real_time, filtered_state_means.shape[1]))
i = 0
for measurement in measurements[-n_real_time:, :]:
time_before = time.time()
(x_now, P_now) = kf3.filter_update(filtered_state_mean = x_now,
filtered_state_covariance = P_now,
observation = measurement)
print("Time to update kf3: %s seconds" % (time.time() - time_before))
x_new[i, :] = x_now
i = i + 1
plt.figure(3)
old_times = range(measurements.shape[0] - n_real_time)
new_times = range(measurements.shape[0]-n_real_time, measurements.shape[0])
plt.plot(times, measurements[:, 0], 'bo',
times, measurements[:, 1], 'ro',
old_times, filtered_state_means[:, 0], 'b--',
old_times, filtered_state_means[:, 2], 'r--',
new_times, x_new[:, 0], 'b-',
new_times, x_new[:, 2], 'r-')
plt.show()
Le graphique ci-dessous montre les performances de la méthode de filtrage, y compris 3 points trouvés en utilisant le filter_update méthode. Les points sont des mesures, la ligne pointillée est une estimation d'état pour la période de formation du filtre, la ligne continue est une estimation d'état pour la période "en ligne".
Et les informations de synchronisation (sur mon ordinateur portable).
Time to build and train kf3: 0.0677888393402 seconds
Time to update kf3: 0.00038480758667 seconds
Time to update kf3: 0.000465154647827 seconds
Time to update kf3: 0.000463008880615 seconds
D'après ce que je peux voir, l'utilisation du filtrage de Kalman n'est peut-être pas le bon outil dans votre cas.
Que diriez-vous de le faire de cette façon? :
lstInputData = [
[346, 226 ],
[346, 211 ],
[347, 196 ],
[347, 180 ],
[350, 2165], ## noise
[355, 154 ],
[359, 138 ],
[368, 120 ],
[374, -830], ## noise
[346, 90 ],
[349, 75 ],
[1420, 67 ], ## noise
[357, 64 ],
[358, 62 ]
]
import pandas as pd
import numpy as np
df = pd.DataFrame(lstInputData)
print( df )
from scipy import stats
print ( df[(np.abs(stats.zscore(df)) < 1).all(axis=1)] )
Voici la sortie:
0 1
0 346 226
1 346 211
2 347 196
3 347 180
4 350 2165
5 355 154
6 359 138
7 368 120
8 374 -830
9 346 90
10 349 75
11 1420 67
12 357 64
13 358 62
0 1
0 346 226
1 346 211
2 347 196
3 347 180
5 355 154
6 359 138
7 368 120
9 346 90
10 349 75
12 357 64
13 358 62
Voir ici pour en savoir plus et la source dont j'ai obtenu le code ci-dessus.