Tutoriel sur le filtre de Kalman
(kalmanfilter.net)- Le filtre de Kalman est un algorithme qui utilise à la fois des mesures de capteurs bruitées et un modèle dynamique imparfait pour estimer non seulement l’état actuel et l’état suivant, mais aussi leur incertitude
- Le tutoriel suit numériquement, à partir d’un exemple de suivi radar d’un avion, le processus de combinaison des valeurs prédites et mesurées, en prenant la distance (r) et la vitesse (v) comme vecteur d’état
- En utilisant une mesure initiale de (10,000m), (200m/s) et un intervalle d’échantillonnage de (5s), la position suivante est prédite à (11,000m) dans un modèle à vitesse constante, et le bruit de mesure (R) ainsi que le bruit de processus (Q) sont reflétés dans la covariance
- La deuxième mesure, (11,020m), (202m/s), est plus incertaine, mais le gain de Kalman (K) combine de manière pondérée la prédiction et la mesure pour calculer un état mis à jour à (11,009.37m), (201.43m/s)
- Après l’initialisation, la boucle prédiction-mise à jour se répète, et une implémentation réelle doit aussi prendre en compte des formules de mise à jour de covariance stables comme la forme de Joseph ainsi que le traitement des mesures aberrantes
Le problème d’estimation résolu par le filtre de Kalman
- Le filtre de Kalman est un algorithme qui estime et prédit l’état d’un système dans un environnement incertain
- Données de capteurs avec bruit de mesure
- Facteurs externes inconnus
- Écart entre le modèle dynamique et le mouvement réel
- Il est utilisé pour le suivi d’objets, la navigation, la robotique, le contrôle, l’analyse des marchés financiers, la prévision météo, etc.
- Appliqué à l’estimation de la trajectoire d’une souris d’ordinateur, il peut réduire le bruit et corriger les tremblements de la main pour produire un déplacement plus stable
- Le tutoriel est conçu pour faire comprendre le filtre de Kalman à travers des exemples numériques et des explications intuitives plutôt qu’au moyen d’une présentation mathématique complexe
- Il inclut aussi des exemples où un filtre de Kalman mal conçu ne parvient pas à suivre correctement un objet, ainsi que des méthodes pour y remédier
Parcours d’apprentissage
- Ce projet est structuré pour permettre d’apprendre le filtre de Kalman à trois niveaux de profondeur
- Vue d’ensemble sur une seule page : explique l’idée centrale et les équations essentielles sans dérivation, en supposant des bases en statistiques et en algèbre linéaire
- Tutoriel web gratuit basé sur des exemples : construit l’intuition à partir d’exemples numériques et va progressivement jusqu’à la dérivation des équations du filtre de Kalman, en indiquant qu’aucun prérequis n’est nécessaire
- Kalman Filter from the Ground Up : inclut 14 exemples numériques entièrement résolus, des graphiques et tableaux de performance, l’Extended Kalman Filter, l’Unscented Kalman Filter, la fusion de capteurs et des recommandations d’implémentation
Pourquoi la prédiction est nécessaire dans l’exemple du suivi radar
- Dans un radar qui suit un avion, l’avion est le système et la position à estimer est l’état du système
- Le radar oriente un faisceau étroit vers l’avion ; pour décider où envoyer le faisceau suivant, il faut donc prédire la position future
- En cas d’échec de la prédiction, le faisceau peut pointer dans la mauvaise direction et perdre la cible
- Un modèle dynamique est nécessaire pour représenter le mouvement du système dans le temps
- Dans l’exemple simplifié à une dimension, on suppose que l’avion se déplace en ligne droite vers le radar ou s’en éloigne
- Le radar calcule la distance (r) à partir du temps d’émission-réception des impulsions
- Il peut aussi mesurer la vitesse (v) grâce à l’effet Doppler
- Si, à (t_0), la distance (10,000m) et la vitesse (200m/s) sont mesurées avec une très grande précision, que l’intervalle d’échantillonnage est (\Delta t=5s) et que la vitesse est supposée constante, alors la position suivante est (11,000m)
- (\Delta r = v \cdot \Delta t)
- (r_{t_1}=10,000+200\cdot5=11,000m)
Bruit de mesure et bruit de processus
- Les mesures radar réelles ne sont pas parfaitement précises ; même si plusieurs radars mesurent au même instant, ils peuvent produire des valeurs légèrement différentes
- Cette variabilité est représentée par le bruit de mesure
- Il faut calculer non seulement l’estimation d’état, mais aussi à quel point cette estimation est fiable
- Le modèle dynamique non plus n’est pas parfait
- Même si l’on suppose que l’avion se déplace à vitesse constante, le mouvement réel peut différer à cause de facteurs externes comme le vent
- Ces influences imprévisibles constituent le bruit de processus
- Le filtre de Kalman fournit à la fois l’estimation de l’état actuel, la prédiction de l’état futur et leur incertitude respective
- C’est un algorithme optimal qui minimise l’incertitude de l’estimation d’état, sous l’hypothèse que le système et le bruit suivent les hypothèses du modèle
Vecteur d’état et initialisation
- Dans cet exemple, l’état du système est constitué de la distance (r) et de la vitesse (v) de l’avion
[ \boldsymbol{x}= \begin{bmatrix} r\ v \end{bmatrix} ]
- La première mesure à (t_0) est la suivante
[ \boldsymbol{z}_0= \begin{bmatrix} 10{,}000\ 200 \end{bmatrix} ]
- Comme les mesures comportent une incertitude, chacune est associée à une incertitude de mesure sous forme de variance
- Écart-type de la mesure de distance : (4m)
- Écart-type de la mesure de vitesse : (0.5m/s)
- La variance est le carré de l’écart-type
[ \boldsymbol{R}_0= \begin{bmatrix} 16 & 0\ 0 & 0.25 \end{bmatrix} ]
- Dans cet exemple, on suppose que les erreurs de mesure de distance et de vitesse ne sont pas corrélées ; les éléments hors diagonale de la matrice de covariance sont donc fixés à 0
- À l’étape d’initialisation, comme la mesure et l’état du système représentent les mêmes grandeurs physiques (r) et (v), la première mesure peut être utilisée comme estimation initiale de l’état
[ \hat{\boldsymbol{x}}_{0,0}= \boldsymbol{z}_0= \begin{bmatrix} 10{,}000\ 200 \end{bmatrix} ]
- Cette méthode ne peut être utilisée qu’à l’étape d’initialisation
Étape de prédiction : propagation de l’état et de la covariance
- La prédiction utilise l’état courant et la matrice de transition d’état (\boldsymbol{F}) pour calculer l’état à l’instant suivant
- Dans le modèle à vitesse constante, on utilise les équations suivantes
[ v_1=v_0=v ]
[ r_1=r_0+v_0\Delta t ]
- Sous forme matricielle, l’équation de prédiction d’état est la suivante
[ \hat{\boldsymbol{x}}_{n+1,n}
\boldsymbol{F} \hat{\boldsymbol{x}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n ]
- (\boldsymbol{u}_n) : variable d’entrée
- (\boldsymbol{G}) : matrice de transition d’entrée
- Dans cet exemple simple, il n’y a pas d’entrée, donc (\boldsymbol{u}_n=0)
- Quand (\Delta t=5s), la matrice de transition d’état est la suivante, et le résultat de la prédiction est (11,000m), (200m/s)
[ \boldsymbol{F}= \begin{bmatrix} 1 & 5\ 0 & 1 \end{bmatrix} ]
[ \hat{\boldsymbol{x}}_{1,0}
\begin{bmatrix} 11{,}000\ 200 \end{bmatrix} ]
- La prédiction de la covariance n’utilise pas simplement (\boldsymbol{F}\boldsymbol{P}), mais (\boldsymbol{F}\boldsymbol{P}\boldsymbol{F}^T)
[ \boldsymbol{P}_{n+1,n}
\boldsymbol{F} \boldsymbol{P}_{n,n} \boldsymbol{F}^T + \boldsymbol{Q} ]
- En excluant le bruit de processus, la covariance prédite est la suivante
[ \boldsymbol{P}_{1,0}
\begin{bmatrix} 22.25 & 1.25\ 1.25 & 0.25 \end{bmatrix} ]
- La variance de la vitesse reste à (0.25) à cause du modèle à vitesse constante
- La variance de la distance augmente de (16) à (22.25), car l’incertitude sur la vitesse accroît l’incertitude sur la distance au fil du temps
Prise en compte du bruit de processus
- Comme la vitesse réelle de l’avion peut être affectée par des facteurs externes imprévisibles comme le vent, le bruit de processus (\boldsymbol{Q}) est ajouté à la prédiction de covariance
- Dans l’exemple, on suppose un écart-type d’accélération aléatoire de (\sigma_a=0.2m/s^2)
- La variance vaut (\sigma_a^2=0.04m^2/s^4)
- Quand (\Delta t=5s), la matrice de bruit de processus est la suivante
[ \boldsymbol{Q}
\begin{bmatrix} 6.25 & 2.5\ 2.5 & 1 \end{bmatrix} ]
- La covariance prédite après ajout du bruit de processus est la suivante
[ \boldsymbol{P}_{1,0}
\begin{bmatrix} 28.5 & 3.75\ 3.75 & 1.25 \end{bmatrix} ]
Étape de mise à jour : combinaison pondérée de la prédiction et de la mesure
- À (t_1), la deuxième mesure est la suivante
[ \boldsymbol{z}_1= \begin{bmatrix} 11{,}020\ 202 \end{bmatrix} ]
- On suppose que cette mesure est plus incertaine que la première, en raison d’un fort pic de bruit qui réduit le rapport signal/bruit
- Écart-type de la distance : (6m)
- Écart-type de la vitesse : (1.5m/s)
[ \boldsymbol{R}_1= \begin{bmatrix} 36 & 0\ 0 & 2.25 \end{bmatrix} ]
- Les éléments diagonaux de la covariance prédite (\boldsymbol{P}_{1,0}) sont inférieurs à ceux de la covariance de mesure (\boldsymbol{R}_1), ce qui signifie que l’incertitude est plus faible du côté de la prédiction
- Le filtre de Kalman n’utilise ni uniquement la prédiction, ni uniquement la mesure : il les combine en donnant plus de poids à la source la moins incertaine
- La moyenne pondérée en une dimension s’écrit comme suit
[ \hat{x}_{1,1}
K_1 z_1 + (1-K_1)\hat{x}_{1,0} ]
- (\boldsymbol{K}) est le gain de Kalman ; il détermine le poids de la mesure et de la prédiction de façon à minimiser l’incertitude de l’estimation mise à jour
Innovation, matrice d’observation et gain de Kalman
- L’équation de mise à jour d’état peut s’écrire comme l’ajout d’un terme de correction à la prédiction
[ \hat{\boldsymbol{x}}_{1,1}
\hat{\boldsymbol{x}}_{1,0} + \boldsymbol{K}_1 ( \boldsymbol{z}_1
\boldsymbol{H}\hat{\boldsymbol{x}}_{1,0} ) ]
- (\boldsymbol{z}1-\boldsymbol{H}\hat{\boldsymbol{x}}{1,0}) est l’innovation ou résidu, et représente l’information apportée par la nouvelle mesure
- (\boldsymbol{H}) est la matrice d’observation ou matrice de mesure, qui projette les variables d’état sur les grandeurs physiques effectivement mesurées
- Dans cet exemple, l’état et la mesure sont tous deux la distance et la vitesse, donc (\boldsymbol{H}=\boldsymbol{I})
- De façon générale, la mesure et l’état peuvent appartenir à des domaines physiques différents, comme avec un thermomètre numérique
- Le gain de Kalman multivarié est donné par
[ \boldsymbol{K}_n
\boldsymbol{P}{n,n-1} \boldsymbol{H}^T ( \boldsymbol{H} \boldsymbol{P}{n,n-1} \boldsymbol{H}^T + \boldsymbol{R}_n )^{-1} ]
- Dans l’exemple, le gain de Kalman calculé est le suivant
[ \boldsymbol{K}_1= \begin{bmatrix} 0.4048 & 0.6377\ 0.0399 & 0.3144 \end{bmatrix} ]
- Le calcul d’inverse de matrice peut se faire avec
inv(A)dans MATLAB ounumpy.linalg.inv(A)en Python, mais dans une implémentation réelle, il est en général préférable de résoudre directement le système linéaire avecA\bounumpy.linalg.solve(A, b)plutôt que de calculer explicitement l’inverse
Résultat de la mise à jour et réduction de la covariance
- Dans cet exemple, l’innovation est la suivante
[ \boldsymbol{z}1-\hat{\boldsymbol{x}}{1,0}
\begin{bmatrix} 20\ 2 \end{bmatrix} ]
- En calculant le terme de correction avec le gain de Kalman, on obtient
[ \boldsymbol{K}_1 \begin{bmatrix} 20\ 2 \end{bmatrix}
\begin{bmatrix} 9.37\ 1.43 \end{bmatrix} ]
- L’estimation d’état mise à jour est la suivante
[ \hat{\boldsymbol{x}}_{1,1}
\begin{bmatrix} 11{,}009.37\ 201.43 \end{bmatrix} ]
- Pour la mise à jour de covariance multivariée, on utilise souvent la forme de Joseph, numériquement stable
[ \boldsymbol{P}_{n,n}
(\boldsymbol{I}-\boldsymbol{K}n\boldsymbol{H}) \boldsymbol{P}{n,n-1} (\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H})^T + \boldsymbol{K}_n \boldsymbol{R}_n \boldsymbol{K}_n^T ]
- Une formule simplifiée de mise à jour de la covariance apparaît aussi fréquemment dans la littérature
[ \boldsymbol{P}_{n,n}
(\boldsymbol{I}-\boldsymbol{K}n\boldsymbol{H}) \boldsymbol{P}{n,n-1} ]
- En arithmétique exacte, les deux formes donnent le même résultat, mais dans une implémentation informatique, la forme de Joseph est en général plus stable numériquement
- Dans l’exemple, la covariance mise à jour calculée avec la formule simplifiée est la suivante
[ \boldsymbol{P}_{1,1}
\begin{bmatrix} 14.57 & 1.43\ 1.43 & 0.71 \end{bmatrix} ]
- Les éléments diagonaux de la covariance mise à jour sont inférieurs à ceux de la covariance prédite ((28.5, 1.25)) et de la covariance de mesure ((36, 2.25))
- Même si une nouvelle information est très incertaine, elle réduit l’incertitude de l’estimation ; en théorie, une nouvelle mesure ne devrait donc pas être ignorée
- En pratique, il peut être nécessaire de rejeter des mesures peu fiables ; les méthodes de traitement des valeurs aberrantes sont abordées dans le chapitre Outlier Treatment du livre
Prédiction suivante et boucle répétitive
- L’étape de prédiction de l’itération 1 est identique à celle de l’itération 0, mais le point de départ devient (\hat{\boldsymbol{x}}{1,1}) et (\boldsymbol{P}{1,1})
- Le résultat de la prédiction d’état est le suivant
[ \hat{\boldsymbol{x}}_{2,1}
\boldsymbol{F} \hat{\boldsymbol{x}}_{1,1}
\begin{bmatrix} 12{,}016.5\ 201.43 \end{bmatrix} ]
- Le résultat de la prédiction de covariance est le suivant
[ \boldsymbol{P}_{2,1}
\begin{bmatrix} 52.86 & 7.47\ 7.47 & 1.71 \end{bmatrix} ]
- Quand le temps passe sans nouvelle mesure, l’incertitude augmente naturellement, donc la variance réaugmente à l’étape de prédiction
- L’incertitude sur la vitesse accroît encore l’incertitude sur la distance
- C’est pourquoi la variance de la distance augmente plus vite que celle de la vitesse
- L’exemple montre les trois étapes du filtre de Kalman
- Initialisation : effectuée une seule fois au départ
- Prédiction : propagation de l’état suivant et de l’incertitude à l’aide du modèle dynamique
- Mise à jour : combinaison de la nouvelle mesure et de la prédiction via le gain de Kalman
- Après l’initialisation, le filtre de Kalman fonctionne en continu dans une boucle prédiction-mise à jour
1 commentaires
Avis sur Hacker News
J’ai toujours tendance à dire qu’apprendre le filtre de Kalman isolément, c’est prendre les choses à l’envers : on risque facilement de manquer les grandes intuitions qu’apportent les théories autour
Pour bien le comprendre, il vaut mieux passer successivement par la méthode des moindres carrés (régression linéaire), les moindres carrés récursifs, puis le filtre d’information (une autre formulation du KF)
On voit alors que le KF n’est rien d’autre qu’une reformulation des moindres carrés récursifs qui privilégie l’efficacité de l’étape de mise à jour
Ce PDF en donne un aperçu concis : http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/...
J’ai quand même une curiosité intellectuelle, et j’ai besoin d’un chemin qui me permette de garder cette curiosité tout en avançant progressivement vers la compréhension
Relire The Six (Not So) Easy Pieces ne me permet toujours pas de comprendre, mais cela reste précieux ; et en jouant avec le chat d’Arnold, on peut faire l’expérience, avec une curiosité brute de primate, sans procédure scientifique rigoureuse, de concepts qui étaient à l’origine derrière la porte du contexte
http://gerdbreitenbach.de/arnold_cat/cat.html
En une dimension, on obtient la distribution a priori à partir de la prédiction linéaire X'1 = X0*a + b ; mean(X'1) = mean(X0)*a + b et var(X'1) = var(X0)*a^2, où a et b représentent la dynamique supposée
La distribution a posteriori gaussienne étant une moyenne pondérée par la précision entre la distribution a priori et l’observation, on a X1 = (1 - K)X'1 + YK, avec K = (1/var(X'1))/(1/var(X'1) + 1/var(Y)), où Y est une observation gaussienne
En répétant cela, on obtient le filtre de Kalman ; et si l’on comprend la linéarité des gaussiennes multidimensionnelles, la généralisation à plusieurs dimensions devient intuitive
Cela dit, la linéarité des gaussiennes multidimensionnelles et la distribution a posteriori gaussienne elles-mêmes peuvent ne pas être des notions faciles
Chaque fois que ce sujet revient, cette ressource revient aussi, et inversement : https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pyt...
Le fait d’utiliser des notebooks Jupyter est aussi un très gros plus
Il ne semble pas encore exister d’outil de calcul symbolique pour les distributions de probabilité
Par exemple, un outil qui multiplierait deux densités de probabilité gaussiennes multivariées pour obtenir la matrice de covariance, ou qui, une fois tous les composants d’un filtre de Kalman définis (modèle de prédiction et processus d’observation), générerait les formules nécessaires à la manière de lambdify dans sympy
Je ne sais toutefois pas si Sympy peut traiter les distributions conditionnelles nécessaires au filtre de Kalman, c’est-à-dire la distribution a posteriori bayésienne
Quoi qu’il en soit, si l’on veut expérimenter avec un filtre de Kalman dans Sympy, mieux vaut manipuler directement les moyennes et variances, ou les matrices de covariance
Référence : https://reference.wolfram.com/language/howto/WorkWithStatist...
Et aussi : https://reference.wolfram.com/language/ref/MultinormalDistri...
https://www.squiggle-language.com/docs
Si Q et R sont constants, comme c’est généralement le cas, le gain converge rapidement et le filtre de Kalman devient presque équivalent à un filtre exponentiel avec une étape de prédiction
Pour beaucoup de gens, cette explication est beaucoup plus facile à comprendre et correspond aussi mieux à la manière dont il est utilisé en pratique
En général, on règle Q et R à la main jusqu’à ce que le résultat “ait l’air correct”, puis on n’y touche plus
En plus, au lieu d’ajuster plusieurs valeurs comme Q et R, il suffit alors de régler manuellement un seul gain
Est-ce qu’on se contente de les ajuster jusqu’à ce que le résultat ait l’air plausible ? Dans ce cas, je ne comprends pas comment il peut fonctionner correctement même quand on n’est pas en plein surapprentissage
Par exemple, si l’on suit un oiseau dans une vidéo, on peut choisir un certain Q, mais les statistiques du bruit peuvent changer selon le moment de la journée. Que faire dans ce cas ?
Article connexe : Kalman filter from the ground up - https://news.ycombinator.com/item?id=37879715 - octobre 2023, 150 commentaires
Je me demande aussi quelle serait la meilleure année à mettre dans le titre ci-dessus
Le filtre de Kalman figure dans le sujet plus général traité par David G. Luenberger dans Optimization by Vector Space Methods, John Wiley and Sons, Inc., New York, 1969
Une idée m’est venue. Pourrait-on encoder d’une manière ou d’une autre sous forme de vecteurs des affaires ne reposant que sur des témoignages, puis les traiter avec un filtre de Kalman afin de renforcer la valeur probante des observations ?
L’idée serait de traiter à la fois les mensonges et les imprécisions comme des “erreurs”
Je pense aux Phoenix lights ou aux ovnis en général, aux fantômes, aux expériences de mort imminente, et plus banalement à des accusations de viol
La meilleure ressource est presque toujours celle-ci : https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pyt...
Elle est excellente même pour les personnes qui n’utilisent pas Python, et elle couvre vraiment très bien l’ensemble du sujet
Quelqu’un d’autre a-t-il regardé les cours sur le filtre de Kalman de Michael van Biezem, avec son nœud papillon, en apprenant ce sujet ?
https://www.youtube.com/watch?v=CaCcOwJPytQ&list=PLX2gX-ftPV...
La seule phrase qu’il faut vraiment connaître est celle-ci : “Ce filtre porte le nom de Rudolf E. Kálmán (19 mai 1930 – 2 juillet 2016). En 1960, Kálmán a publié un article célèbre décrivant une solution récursive au problème du filtrage linéaire de données discrètes”