Filtre de Kalman

Il existe de nombreux articles différents sur le filtre de Kalman, mais il est difficile de trouver celui qui contient une explication, d'où proviennent toutes les formules de filtrage. Je pense que sans comprendre cela, cette science devient complètement incompréhensible. Dans cet article, je vais essayer de tout expliquer de manière simple.

Le filtre de Kalman est un outil très puissant pour filtrer différents types de données. L'idée principale derrière cela est que l'on devrait utiliser une information sur le processus physique. Par exemple, si vous filtrez les données du compteur de vitesse d'une voiture, son inertie vous donne le droit de traiter un grand écart de vitesse comme une erreur de mesure. Le filtre de Kalman est également intéressant du fait qu'il est en quelque sorte le meilleur filtre. Nous discuterons précisément de ce que cela signifie. À la fin de l'article, je montrerai comment il est possible de simplifier les formules.

Préliminaires

Tout d'abord, mémorisons quelques définitions et faits de la théorie des probabilités.

Variable aléatoire

Quand on dit qu'on lui donne une variable aléatoire $$$\ xi$ , cela signifie qu'il peut prendre des valeurs aléatoires. Différentes valeurs viennent avec des probabilités différentes. Par exemple, si quelqu'un lance un dé, alors l'ensemble de valeurs est discret $$$\ {1,2,3,4,5,6 \}$ . Lorsque vous traitez avec une vitesse de déplacement des particules, vous devez évidemment travailler avec un ensemble continu de valeurs. Les valeurs qui sortent après chaque expérience (mesure) que nous dénoterions $$$x_1, x_2, ...$ , mais parfois nous utilisons la même lettre que celle que nous utilisons pour une variable aléatoire $$$\ xi$ . Dans le cas d'un ensemble continu de valeurs, une variable aléatoire est caractérisée par sa fonction de densité de probabilité $$$\ rho (x)$ . Cette fonction montre une probabilité que la variable aléatoire tombe dans un petit voisinage $$$dx$ du point $$$x$ . Comme nous pouvons le voir sur l'image, cette probabilité est égale à l'aire du rectangle hachuré sous le graphique $$$\ rho (x) dx$ .

Très souvent dans notre vie, les variables aléatoires ont la distribution de Gauss, lorsque la densité de probabilité est $$$\ rho (x) \ sim e ^ {- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}}$ .

Nous pouvons voir que la fonction en forme de cloche $$$\ rho (x)$ est centré au point $$$\ mu$ et sa largeur caractéristique est d'environ $$$\ sigma$ .
Puisque nous parlons de la distribution gaussienne, alors ce serait un péché de ne pas mentionner d'où il vient. Ainsi que le nombre $$$e$ et $$$\ pi$ sont fermement pénétrés dans les mathématiques et peuvent être trouvés dans les endroits les plus inattendus, donc la distribution gaussienne a des racines profondes dans la théorie des probabilités. L'énoncé remarquable suivant explique en partie la présence de la distribution de Gauss dans de nombreux processus:
Soit une variable aléatoire $$$\ xi$ a une distribution arbitraire (en fait, il existe certaines restrictions à l'arbitraire, mais elles ne le sont pas du tout). Jouons $$$n$ expériences et calculer une somme $$$\ xi_1 + ... + \ xi_n$ , des valeurs tombées. Faisons beaucoup d'expériences. Il est clair que nous obtiendrons à chaque fois une valeur différente de la somme. En d'autres termes, cette somme est une variable aléatoire avec sa propre loi de distribution. Il s'avère que pour suffisamment grand $$$n$ , la loi de distribution de cette somme tend à une distribution gaussienne (au fait, la largeur caractéristique d'une cloche croît comme $$$\ sqrt n$ . En savoir plus sur Wikipedia: Théorème central limite . Dans la vie réelle, il y a beaucoup de valeurs qui sont une somme d'un grand nombre de variables aléatoires indépendantes et identiquement distribuées. Ces valeurs ont donc la distribution de Gauss.

Signification valeur

Par définition, une valeur moyenne d'une variable aléatoire est une valeur que nous obtenons dans une limite si nous effectuons de plus en plus d'expériences et calculons une moyenne de valeurs tombées. Une valeur moyenne est notée de différentes manières: les mathématiciens désignent par $$$E \ xi$ (attente), les physiciens le désignent par $$$\ overline {\ xi}$ ou $$$<\ xi>$ . Nous le désignerons comme le font les mathématiciens.

Par exemple, une valeur moyenne de la distribution gaussienne $$$\ rho (x) \ sim e ^ {- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}}$ est égal à $$$\ mu$ .

Écart

Pour la distribution gaussienne, nous voyons clairement que la variable aléatoire a tendance à tomber dans une certaine région de sa valeur moyenne $$$\ mu$ . Profitons à nouveau de la distribution gaussienne:

Sur l'image, on peut voir qu'une largeur caractéristique d'une région où les valeurs chutent le plus est $$$\ sigma$ . Comment pouvons-nous estimer cette largeur pour une variable aléatoire arbitraire? Nous pouvons dessiner un graphique de sa fonction de densité de probabilité et évaluer simplement visuellement la plage caractéristique. Cependant, il serait préférable de choisir une méthode algébrique précise pour cette évaluation. Nous pouvons trouver une longueur moyenne d'écart par rapport à la valeur moyenne: $$$E | \ xi-E \ xi |$ . Cette valeur est une bonne estimation d'un écart caractéristique de $$$\ xi$ . Cependant, nous savons très bien à quel point il est problématique d'utiliser des valeurs absolues dans les formules, cette formule est donc rarement utilisée dans la pratique.

Une approche plus simple (simple du point de vue du calcul) consiste à calculer $$$E (\ xi-E \ xi) ^ 2$ .

Cette valeur appelée variance et notée par $$$\ sigma_ \ xi ^ 2$ . La racine quadratique de la variance est une bonne estimation de l'écart caractéristique de la variable aléatoire. C'est ce qu'on appelle l'écart type.

Par exemple, on peut calculer cela pour la distribution gaussienne $$$\ rho (x) \ sim e ^ {- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}}$ la variance est égale à $$$\ sigma ^ 2$ ainsi l'écart type est $$$\ sigma$ . Ce résultat correspond vraiment à notre intuition géométrique. En fait, une petite tricherie est cachée ici. En fait, dans une définition de la distribution de Gauss, vous voyez le nombre $$$2$ dans un dénominateur d'expression $$$- \ frac {(x- \ mu) ^ 2} {2 \ sigma ^ 2}$ . Ce $$$2$ se tient là dans le but, pour l'écart type $$$\ sigma_ \ xi$ être exactement égal à $$$\ sigma$ . La formule de la distribution de Gauss est donc écrite d'une manière qui tient compte du fait que l'on calculerait son écart type.

Variables aléatoires indépendantes

Les variables aléatoires peuvent dépendre les unes des autres ou non. Imaginez que vous jetez une aiguille sur le sol et mesurez les coordonnées de ses deux extrémités. Ces deux coordonnées sont des variables aléatoires, mais elles dépendent l'une de l'autre, car une distance entre elles doit toujours être égale à la longueur de l'aiguille. Les variables aléatoires sont indépendantes les unes des autres si la baisse des résultats de la première ne dépend pas des résultats de la seconde. Pour deux variables indépendantes $$$\ xi_1$ et $$$\ xi_2$ la moyenne de leur produit est égale au produit de leur moyenne: $$$E (\ xi_1 \ cdot \ xi_2) = E \ xi_1 \ cdot \ xi_2$

Filtre de Kalman

Énoncé du problème

Soit dénoté par $$$x_k$ une valeur que nous comptons mesurer puis filtrer. Il peut s'agir d'une coordonnée, de la vitesse, de l'accélération, de l'humidité, de la température, de la pression, etc.

Commençons par un exemple simple qui nous conduira à la formulation du problème général. Imaginez que vous ayez une petite voiture radiocommandée qui ne peut fonctionner que vers l'avant et vers l'arrière. Connaissant sa masse, sa forme et d'autres paramètres du système, nous avons calculé comment la façon dont un joystick de contrôle agit sur la vitesse d'une voiture $$$v_k$ .


La coordonnée de la voiture serait par la formule suivante

$$

$$x_ {k + 1} = x_k + v_kdt$$

Dans la vraie vie, nous ne pouvons pas, nous ne pouvons pas avoir une formule précise pour les coordonnées, car de petites perturbations agissent sur la voiture comme le vent, les bosses, les pierres sur la route, donc la vitesse réelle de la voiture sera différente de celle calculée. . Nous ajoutons donc une variable aléatoire $$$\ xi_k$ à droite de la dernière équation:

$$

$$x_ {k + 1} = x_k + v_kdt + \ xi_k$$

Nous avons également un capteur GPS sur la voiture qui essaie de mesurer les coordonnées de la voiture $$$x_k$ . Bien sûr, il y a une erreur dans cette mesure, qui est une variable aléatoire $$$\ eta_k$ . Donc, à partir du capteur, nous obtiendrions des données erronées:

$$

$$z_k = x_k + \ eta_k$$

Notre objectif est de trouver une bonne estimation de la vraie coordonnée $$$x_k$ , connaître les données d'un mauvais capteur $$$z_k$ . Cette bonne estimation sera notée $$$x ^ {opt}$ .
En général, les coordonnées $$$x_k$ peut représenter n'importe quelle valeur (température, humidité, ...) et l'élément de contrôle que nous désignons par $$$u_k$ (dans l'exemple avec une voiture $$$u_k = v_k \ cdot dt$ ) Les équations pour les coordonnées et les mesures des capteurs seraient les suivantes:

(1)

Voyons ce que nous savons dans ces équations.

• $$$u_k$ est une valeur connue qui contrôle une évolution du système. Nous le savons grâce au modèle du système.
• La variable aléatoire $$$\ xi$ représente l'erreur dans le modèle du système et la variable aléatoire $$$\ eta$ est une erreur de capteur. Leurs lois de distribution ne dépendent pas du temps (sur l'indice d'itération $$$k$ )
• Les moyennes des erreurs sont égales à zéro: $$$E \ eta_k = E \ xi_k = 0$ .
• Nous ne connaissons peut-être pas la loi de distribution des variables aléatoires, mais nous connaissons leurs variances $$$\ sigma_ \ xi$ et $$$\ sigma_ \ eta$ . Notez que les écarts ne dépendent pas du temps (sur $$$k$ ), puisque les lois de distribution correspondantes non plus.
• Nous supposons que toutes les erreurs aléatoires sont indépendantes les unes des autres: l'erreur au moment $$$k$ ne dépend pas de l'erreur à l'époque $$$k ’$ .

Notez qu'un problème de filtration n'est pas un problème de lissage. Notre objectif n'est pas de lisser les données d'un capteur, nous voulons juste obtenir la valeur la plus proche possible de la coordonnée réelle $$$x_k$ .

Algorithme de Kalman

Nous utiliserions une induction. Imaginez qu'à l'étape $$$k$ nous avons déjà trouvé la valeur du capteur filtré $$$x ^ {opt}$ , qui est une bonne estimation de la coordonnée réelle $$$x_k$ . Rappelons que nous connaissons l'équation qui contrôle la coordonnée réelle:

$$

$$x_ {k + 1} = x_k + u_k + \ xi_k$$

Par conséquent, avant d'obtenir la valeur du capteur, nous pouvons déclarer qu'il affichera la valeur qui est proche de $$$x ^ {opt} + u_k$ . Malheureusement, jusqu'à présent, nous ne pouvons pas dire quelque chose de plus précis. Mais au pas $$$k + 1$ nous aurions une lecture non précise du capteur $$$z_ {k + 1}$ .
L'idée de Kalman est la suivante. Pour obtenir la meilleure estimation de la coordonnée réelle $$$x_ {k + 1}$ nous devrions obtenir un milieu doré entre la lecture du capteur non précis $$$z_ {k + 1}$ et $$$x ^ {opt} + u_k$ - notre prédiction, ce que nous nous attendions à voir sur le capteur. Nous donnerons un poids $$$K$ à la valeur du capteur et $$$(1-K)$ à la valeur prédite:

$$

$$x ^ {opt} _ {k + 1} = K \ cdot z_ {k + 1} + (1-K) \ cdot (x_k ^ {opt} + u_k)$$

Le coefficient $$$K$ est appelé coefficient de Kalman. Cela dépend de l'indice d'itération, et à proprement parler, nous devrions plutôt écrire $$$K_ {k + 1}$ . Mais pour garder les formules dans une belle forme, nous omettions l'indice de $$$K$ .
Nous devons choisir le coefficient de Kalman de manière à ce que la coordonnée estimée $$$x ^ {opt} _ {k + 1}$ serait aussi proche que possible des coordonnées réelles $$$x_ {k + 1}$ .
Par exemple, si nous savons que notre capteur est très super précis, nous ferons confiance à sa lecture et lui donnerons un poids important ( $$$K$ est proche de 1). Si le capteur à l'inverse n'est pas précis du tout, alors nous nous baserions sur notre valeur théoriquement prédite $$$x ^ {opt} _k + u_k$ .
En situation générale, il faut minimiser l'erreur de notre estimation:

$$

$$e_ {k + 1} = x_ {k + 1} -x ^ {opt} _ {k + 1}$$

Nous utilisons les équations (1) (celles qui sont sur un cadre bleu), pour réécrire l'équation de l'erreur:

$$

$$e_ {k + 1} = (1-K) (e_k + \ xi_k) - K \ eta_ {k + 1}$$

Maintenant vient le moment de discuter, que signifie l'expression «minimiser l'erreur»? Nous savons que l'erreur est une variable aléatoire, donc à chaque fois qu'elle prend des valeurs différentes. En fait, il n'y a pas de réponse unique à cette question. De même, c'était dans le cas de la variance d'une variable aléatoire, lorsque nous essayions d'estimer la largeur caractéristique de sa fonction de densité de probabilité. Nous choisirions donc un critère simple. Nous minimiserions une moyenne du carré:

$$

$$E (e ^ 2_ {k + 1}) \ rightarrow min$$

Réécrivons la dernière expression:

$$

$$E (e ^ 2_ {k + 1}) = (1-K) ^ 2 (E_k ^ 2 + \ sigma ^ 2_ \ xi) + K ^ 2 \ sigma ^ 2_ \ eta$$

La dernière expression prend sa valeur minimale, lorsque sa dérivation est nulle. Alors quand:

$$

$$\ displaystyle K_ {k + 1} = \ frac {Ee ^ 2_k + \ sigma ^ 2_ \ xi} {Ee ^ 2_k + \ sigma ^ 2_ \ xi + \ sigma ^ 2_ \ eta}$$

Ici, nous écrivons le coefficient de Kalman avec son indice, nous insistons donc sur le fait qu'il dépend de l'étape d'itération. Nous remplaçons l'équation de l'erreur quadratique moyenne $$$E (e ^ 2_ {k + 1})$ le coefficient de Kalman $$$K_ {k + 1}$ qui minimisent sa valeur:

$$

$$\ displaystyle E (e ^ 2_ {k + 1}) = \ frac {\ sigma ^ 2_ \ eta (Ee ^ 2_k + \ sigma ^ 2_ \ xi)} {Ee ^ 2_k + \ sigma ^ 2_ \ xi + \ sigma ^ 2_ \ eta}$$

Nous avons donc résolu notre problème. Nous avons obtenu la formule itérative pour calculer le coefficient de Kalman.

Exemple

Sur l'intrigue du début de cet article, il y a des données filtrées du capteur GPS fictif, installées sur la voiture fictive, qui se déplace avec l'accélération constante $$$a$ .

$$

$$x_ {t + 1} = x_t + à \ cdot dt + \ xi_t$$

Analyse

Si l'on regarde comment le coefficient de Kalman $$$K_k$ changements depuis l'itération $$$k$ , il est possible de voir qu'il se stabilise à une certaine valeur $$$K_ {stab}$ . Par exemple, si les erreurs quadratiques moyennes du capteur et du modèle se respectent comme dix à un, le tracé de la dépendance du coefficient de Kalman à partir de l'étape d'itération serait le suivant:


Dans l'exemple suivant, nous discuterons de la façon dont cela peut simplifier notre vie.

Deuxième exemple

En pratique, il arrive que nous ne sachions presque rien du modèle physique ce que nous filtrons. Imaginez que vous avez décidé de filtrer ces mesures à partir de votre accéléromètre préféré. En fait, vous ne savez pas en avant comment l'accéléromètre serait déplacé. La seule chose que vous savez peut-être est la variance de l'erreur du capteur $$$\ sigma ^ 2_ \ eta$ . Dans ce problème difficile, nous pourrions mettre toutes les informations inconnues du modèle physique à la variable aléatoire $$$\ xi_k$ :

$$

$$x_ {k + 1} = x_k + \ xi_k$$

À proprement parler, ce type de système ne satisfait pas la condition que nous avons imposée à la variable aléatoire $$$\ xi_k$ . Puisqu'il contient les informations inconnues pour nous la physique du mouvement. Nous ne pouvons pas dire que ce moment de temps différent les erreurs sont indépendantes les unes des autres et leurs moyennes sont égales à zéro. En d'autres termes, cela signifie que pour ce type de situations, la théorie de Kalman n'est pas appliquée. Mais de toute façon, nous pouvons essayer d'utiliser la machinerie de la théorie de Kalman simplement en choisissant des valeurs raisonnables pour $$$\ sigma_ \ xi ^ 2$ et $$$\ sigma_ \ eta ^ 2$ pour obtenir juste un joli graphique de données filtrées.
Mais il existe un moyen beaucoup plus simple. Nous avons vu qu'avec l'augmentation de l'étape $$$k$ le coefficient de Kalman se stabilise toujours à une certaine valeur $$$K_ {stab}$ . Donc au lieu de deviner les valeurs des coefficients $$$\ sigma ^ 2_ \ xi$ et $$$\ sigma ^ 2_ \ eta$ et calculer le coefficient de Kalman $$$K_k$ par des formules difficiles, on peut supposer que ce coefficient est constant et ne sélectionner que cette constante. Cette hypothèse n'affecterait pas beaucoup les résultats de filtrage. Au début, de toute façon, la machinerie de Kalman n'est pas exactement applicable à notre problème et deuxièmement, le coefficient de Kalman se stabilise rapidement à la constante. Au final, tout devient très simple. Nous n'avons pas besoin de presque n'importe quelle formule de la théorie de Kalman, nous avons juste besoin de sélectionner une valeur raisonnable $$$K_ {stab}$ et l'insérer dans la formule itérative

$$

$$x ^ {opt} _ {k + 1} = K_ {stab} \ cdot z_ {k + 1} + (1-K_ {stab}) \ cdot x_k ^ {opt}$$

Sur le graphique suivant, vous pouvez voir les mesures filtrées de deux manières différentes à partir d'un capteur imaginaire. La première méthode est honnête, avec toutes les formules de la théorie de Kalman. La deuxième méthode est la méthode simplifiée.


Nous voyons qu'il n'y a pas de grande différence entre deux méthodes. Il y a une petite variation au début, lorsque le coefficient de Kalman n'est toujours pas stabilisé.

Discussionion

Nous avons vu que l'idée principale du filtre de Kalman est de choisir le coefficient $$$K$ d'une manière que la valeur filtrée

$$

$$x ^ {opt} _ {k + 1} = Kz_ {k + 1} + (1-K) (x ^ {opt} _k + u_k)$$

en moyenne serait aussi proche que possible des coordonnées réelles $$$x_ {k + 1}$ . On voit que la valeur filtrée $$$x ^ {opt} _ {k + 1}$ est une fonction linéaire de la mesure du capteur $$$z_ {k + 1}$ et la valeur filtrée précédente $$$x ^ {opt} _k$ . Mais la valeur filtrée précédente $$$x ^ {opt} _k$ lui-même est une fonction linéaire de la mesure du capteur $$$z_k$ et la valeur filtrée pré-précédente $$$x ^ {opt} _ {k-1}$ . Et ainsi de suite jusqu'à la fin de la chaîne. La valeur filtrée dépend donc linéairement de toutes les lectures précédentes du capteur:

$$

$$x ^ {opt} _ {k + 1} = \ lambda + \ lambda_0z_0 + \ ldots + \ lambda_ {k + 1} z_ {k + 1}$$

C'est une raison pour laquelle le filtre de Kalman est appelé filtre linéaire. Il est possible de prouver que le filtre de Kalman est le meilleur de tous les filtres linéaires. Le meilleur dans le sens où il minimise une moyenne carrée de l'erreur.

Cas multidimensionnel

Il est possible de généraliser toute la théorie de Kalman au cas multidimensionnel. Les formules y semblent un peu plus élaborées, mais l'idée de leur dérivation reste la même que dans une seule dimension. Par exemple, dans cette belle vidéo, vous pouvez voir l'exemple.

Littérature

L'article original écrit par Kalman, vous pouvez le télécharger ici:
http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf