Comment utiliser le filtre de Kalman en Python pour les données de localisation ?

Question

[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 de Kalman sur mes données. J'ai donc modifié l'exemple de données ci-dessous pour qu'il contienne des bruits de variations subtiles qui ne sont pas si extrêmes (ce que je vois aussi souvent). Si quelqu'un d'autre peut me donner des indications sur la façon d'utiliser PyKalman sur mes données, ce serait formidable. [/EDIT]

Dans le cadre d'un projet de robotique, j'essaie de suivre un cerf-volant dans les airs à l'aide d'une caméra. Je programme en Python et j'ai collé quelques résultats de localisation bruyante ci-dessous (chaque élément contient également un objet datetime, mais je l'ai laissé de côté 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 et à les supprimer simplement des données en temps réel. Puis j'ai lu des articles sur les filtres de Kalman et sur la façon dont ils sont spécifiquement conçus pour lisser les données bruitées. Après quelques recherches, j'ai trouvé le logiciel Bibliothèque PyKalman qui semble parfait pour cela. Comme j'étais un peu perdu dans la terminologie du filtre de Kalman, j'ai lu le wiki et d'autres pages sur les filtres de Kalman. Je comprends l'idée générale d'un filtre de Kalman, mais je ne sais pas comment l'appliquer à mon code.

Dans le cadre de la PyKalman docs 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 remplacé les observations par 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 pas de données significatives. 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 conseils ou des exemples dans la bonne direction ? Tous les conseils sont les bienvenus !

Answer 1

TL;DR, voir le code et l'image en bas.

Je pense qu'un filtre de Kalman pourrait très bien fonctionner dans votre cas, mais il faudra réfléchir un peu plus à la dynamique/physique du cerf-volant.

Je recommande vivement la lecture de cette page web . Je n'ai aucun lien avec l'auteur, ni ne le connais, mais j'ai passé une journée à essayer de comprendre les filtres de Kalman, et cette page m'a vraiment fait comprendre ce qu'il en était.

En bref, pour un système qui est linéaire et dont la dynamique est connue (c'est-à-dire que si vous connaissez l'état et les entrées, vous pouvez prédire l'état futur), il s'agit d'un moyen optimal de combiner ce que vous savez d'un système pour estimer son état réel. La partie la plus intelligente (qui est prise en charge par toute l'algèbre matricielle que vous voyez sur les pages qui la décrivent) est la façon dont elle combine de manière optimale les deux éléments d'information dont vous disposez :

1. les mesures (qui sont sujettes au "bruit de mesure", c'est-à-dire que les capteurs ne sont pas parfaits)

2. La dynamique (c'est-à-dire la manière dont vous pensez que les états évoluent en fonction des entrées, qui sont soumises à un "bruit de processus", ce qui est juste une manière de dire que votre modèle ne correspond pas parfaitement à la réalité).

Vous indiquez votre degré de certitude pour chacun d'entre eux (via les matrices de covariances R y Q respectivement), et le Gain de Kalman détermine dans quelle mesure vous devez croire votre modèle (c'est-à-dire votre estimation actuelle de votre état) et dans quelle mesure vous devez croire vos mesures.

Sans plus attendre, 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 cerf-volant et pouvez donc en créer un meilleur.

Considérons le cerf-volant comme une particule (il s'agit évidemment d'une simplification, un vrai cerf-volant est un corps étendu, il a donc une orientation en trois dimensions), qui a quatre états que, pour des raisons de commodité, nous pouvons écrire sous la forme d'un vecteur d'état :

x \= [x, x_dot, y, y_dot],

Où x et y sont les positions, et les _points 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 déprécier la matrice de mesure ( H discuté aquí y observation_matrices en 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. Je supposerai ici qu'aucune force extérieure n'agit et que le mouvement du cerf-volant n'est pas amorti (avec plus de connaissances, vous pourrez peut-être faire mieux, mais cela revient à traiter les forces extérieures et l'amortissement comme une perturbation inconnue/non modélisée).

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 suit :

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 que 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" dans les équations ci-dessus, c'est-à-dire en unités de position_unités/intervalle_échantillon (je suppose que vos échantillons mesurés sont à un intervalle constant). Nous pouvons résumer ces quatre équations en une matrice de dynamique ( F dont il est question ici, et transition_matrices en 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 donné le résultat suivant, montrant qu'il fait un travail raisonnable pour rejeter le bruit (le bleu est la position x, le rouge la position y, et l'axe x est juste le numéro de l'échantillon).

Supposons que vous regardiez le graphique ci-dessus et que vous pensiez qu'il est trop irrégulier. Comment pouvez-vous y remédier ? Comme nous l'avons vu plus haut, un filtre de Kalman agit sur deux éléments d'information :

1. Mesures (dans ce cas de deux de nos états, x et y)
2. Dynamique du système (et estimation actuelle de l'état)

La dynamique décrite dans le modèle ci-dessus est très simple. Pris au pied de la lettre, ils indiquent que les positions seront mises à jour en fonction des vitesses actuelles (d'une manière évidente et physiquement raisonnable) et que les vitesses restent constantes (ce qui n'est manifestement pas vrai physiquement, mais correspond à notre intuition selon laquelle les vitesses devraient changer lentement).

Si nous pensons que l'état estimé devrait être plus lisse, une façon d'y parvenir est de dire que nous avons moins confiance dans nos mesures que dans notre dynamique (c'est-à-dire que nous avons un niveau de confiance plus élevé dans nos mesures que dans notre dynamique). observation_covariance , par rapport à notre state_covariance ).

En commençant par la fin du code ci-dessus, fixez le observation covariance à 10x la valeur estimée précédemment, en fixant em_vars comme indiqué est nécessaire pour éviter la réestimation de la covariance des observations (voir aquí )

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 donne le graphique ci-dessous (les mesures sont représentées par des points, les estimations de l'État sont représentées par une ligne en pointillés). La différence est assez subtile, mais j'espère que vous pouvez voir qu'elle est plus lisse.

Enfin, si vous souhaitez utiliser ce filtre ajusté en ligne, vous pouvez le faire en utilisant la commande filter_update méthode. Notez que cette méthode utilise la méthode filter plutôt que la méthode smooth car la méthode smooth ne peut être appliquée qu'à des mesures par lots. Plus d'informations aquí :

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 la performance de la méthode de filtrage, y compris les 3 points trouvés en utilisant la fonction filter_update méthode. Les points représentent les mesures, la ligne en pointillé les estimations d'état pour la période d'apprentissage du filtre, la ligne continue les estimations d'état pour la période "en ligne".

Et les informations sur le chronométrage (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

Answer 2

D'après ce que je vois, l'utilisation du filtrage de Kalman n'est peut-être pas le bon outil dans votre cas.

Et si on le faisait 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 le résultat :

      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

Véase aquí pour en savoir plus et connaître la source d'où provient le code ci-dessus.