Les différents filtres de Kalman

Spraakforskaren
11 août 2017

Table des matières

1 Introduction 1

2 Problématique 1

3 Modélisation de la réalité 2
3.1 De l’art du dosage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
3.2 Cas linéaire . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
3.3 Et si on faisait des trucs bizarres ? Le cas des balises à ultrason . . . . . . . . . . . . . . . . . . . 3
3.3.1 Système de balisage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
3.3.2 Repérage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
3.3.3 Modélisation et Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

4 Filtrage 4
4.1 Filtre de Kalman classique : cas linéaire et gaussien . . . . . . . . . . . . . . . . . . . . . . . . . 5
4.2 Filtre de Kalman étendu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
4.3 Filtrage de Kalman sans parfum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

5 Conclusion 6

1 Introduction
Ce petit cours sur le filtrage de Kalman a pour but de présenter quelques filtres de Kalman : le classique,
l’étendu (extended) et le sans parfum (unscented, c’est moche). Il s’adresse aux membres d’Intech, club de
robotique de Télécom SudParis, à ceux qui en auront besoin pour filtrer les signaux venant de capteurs, mais
aussi à d’autres curieux.
Le bagage nécessaire pour appréhender ce machin est assez "léger" (pour un ingénieur) mais quand même
assez large. Alors, il faut savoir ce qu’est une probabilité, une variable aléatoire, il faut connaître toutes les
règles de manipulation des probabilités (marginalisation, formule de probabilité totale, conditionnement), il
faut connaître la formule de Bayes (très important ! ! !), il faut aussi connaître ce qu’est une gaussienne et savoir
que cette densité de probabilité a plein de propriétés intéressantes. Il faut connaître les règles de calcul matriciel.
L’objectif ici est de montrer comment modéliser le problème auquel on fait face, pour ensuite pouvoir
utiliser correctement le filtre de Kalman adapté, de plus, ce cours peut être une introduction au domaine et
vous permettra de comprendre les cours et les papiers disponibles sur les différents filtres.
On s’intéressera au filtrage de la mesure d’une position en deux dimensions (oui, les robots d’Intech ne
sautent pas). Mais sachez que ça peut s’appliquer à beaucoup de problèmes !

2 Problématique
Pourquoi avoir besoin de trucs probabilistes pour passer d’une suite de mesures faite avec un capteur à une
connaissance précise (avec la plus grande certitude suivant une règle qu’on se donne) ? À compléter, c’est chiant
à convaincre les gens sur le sujet.

1
3 Modélisation de la réalité
3.1 De l’art du dosage
La réalité est compliquée ! Enfin, imaginez un instant qu’il fallait donner des valeurs à toutes les grandeurs
physiques mesurables de l’univers ou même d’une petite table ou d’un petit robot, combien de téraoctet faudrait-
il ? Beaucoup sans doute [je divague là] ! Bref, faire des calculs dessus est impossible en l’état actuel des choses,
surtout quand on a une petite Raspberry Pi sur le robot (même la 3). Il faut donc simplifier la réalité pour
pouvoir faire des calculs dessus. Bien sûr, dans cette simplification, il ne faut pas jeter ce qui pourrait être utile.
Enfin bref, tout est affaire de dosage dans la modélisation. Si on modélise trop simplement, les calculs seront
faciles, mais la précision des résultats sera très faible, la réalité ne sera même pas représentée, et donc rien ne
sera exploitable ; et si on modélise de manière trop complexe, alors on peut s’y perdre et surtout les résultats
des calculs seront trop complexes et inatteignable dans une vie humaine.

3.2 Cas linéaire

Supposons que l’on ait un robot qui roule tout seul comme un grand sur une table plane. Il ne peut avancer
que tout droit ou alors tourner autour de lui-même. On veut garder un œil sur sa position sur la table, donc on
lui met deux capteurs (capteur 1 ou CA1 et capteur 2 ou CA2 ) à infrarouge qui lui permettent de mesurer la
distance entre lui et les bords de la table de telle sorte que les capteurs soient perpendiculaires entre eux. En
fait, la mesure venant de CA1 est proportionnelle à l’abscisse du robot R et celle de CA2 est proportionnelle à
l’ordonnée de R.
[Faire un beau dessin] Si on passe à une représentation mathématique avec des grandeurs réelles, et un indice
k, un entier positif qui permet de se placer n’importe où dans la série de mesure, ça donne :

M ES_XR (k) = αX ∗ XR (k)

(1)
M ES_YR (k) = αY ∗ YR (k)

Si on met ça sous forme de vecteur, ça devient :

M ESR (k) = α ∗ ET ATR (k) (2)

On observe d’abord qu’entre l’état et la mesure, il y une relation linéaire. Ensuite, avec l’équation (2), on a
le passage de la mesure à l’état (du robot). Pour tous les filtres de Kalman, on a besoin de cette relation dans
ce sens, toujours de l’état qu’on veut obtenir vers la mesure, c’est bizarre, puisqu’en pratique, on part toujours
de la mesure pour comprendre l’état (réel), mais en fait, en posant les choses comme ça, on peut voir ce qui
peut perturber les mesures, c’est ce qu’on appelle l’équation d’observation.
Entre la réalité inatteignable et ce qu’on observe, il se passe souvent plein de choses qu’on n’a du mal à
quantifier et donc on a souvent des erreurs de mesures (léger décalage par rapport à ce qu’on croit), des erreurs
d’arrondis, etc. C’est pour cela qu’on garde cette équation (2) en rajoutant une variable aléatoire gaussienne, de
2
moyenne nulle et de variance σobservation . Pourquoi rajouter ça ? Tout d’abord une variable aléatoire gaussienne
est pratique pour plusieurs raisons. La première est au niveau de l’information qu’on a du "bruit" ajouté : on
ne le connaît pas ! Donc on va supposer que le désordre est le plus grand "possible". On peut montrer [à donner
le lien] que pour une variance donnée, la gaussienne est celle qui a l’entropie la plus grande (l’entropie est une
grandeur qui correspond au désordre d’une variable aléatoire). Deuxièmement, la gaussienne est entièrement
définie par sa moyenne et sa variance, ça facilite donc les calculs ! Ensuite, pourquoi une moyenne nulle ? On ne
sait pas de "quel côté" penche le bruit donc comme on ne veut rien privilégier, on met la moyenne à zéro. Il
reste donc à mettre une dispersion "élevée", ou du moins, de l’ordre de celle qu’on peut observer en faisant des
mesures.
2
L’équation (2) devient donc en posant U qui suit N(0,σobservation ) et en supposant que le bruit n’évolue pas
au cours des mesures :

M ESR (k) = α ∗ ET ATR (k) + U (3)

Maintenant que l’on a l’équation d’observation, il nous reste à établir l’équation d’évolution. Pourquoi cette
équation ? Parce qu’on filtre une suite de mesures et donc pour pouvoir corriger les erreurs dues aux mesures, il
faut pouvoir comparer la nouvelle valeur reçues avec la valeur que l’on aurait du avoir (ou plutôt quelle valeur
était prévisible).
L’équation d’évolution concerne seulement l’état réel que l’on veut suivre. Dans notre exemple, le robot que
l’on veut suivre ne dit pas où il va, mais comme il va souvent tout droit, on sait qu’entre l’instant k et l’instant

2
k+1, il avancera d’une certaine distance mais de combien ? Il faut connaître la vitesse du robot... Et si on suivait
aussi sa vitesse ? Soient V XR (k) et V YR (k) les composantes de la vitesse du robot à l’instant k. La vitesse fait
partie de l’état du robot, par conséquent on peut le rajouter au vecteur d’état du robot. On a donc :

 
XR (k)
 YR (k) 
ET ATR (k) = 
V XR (k)
 (4)
V YR (k)

Par conséquent on peut réécrire l’équation d’observation :

M ESR (k) = α ∗ ET ATR (k) (5)

avec
 
αX 0 0 0
α= (6)
0 αY 0 0

Et l’équation d’évolution est :

ET ATR (k + 1) = G ∗ ET ATR (k) + V (7)

2
avec V qui suit N (0, σvolution )
 
1 0 t(k, k + 1) 0
0 1 0 t(k, k + 1)
G=
0 0
 (8)
1 0 
0 0 0 1

(Si vous comprenez pas ça, développez et dites "Ahhhhh".) t(k,k+1) est le temps entre l’instant k et l’instant
k+1 (on peut dire que c’est la durée qui sépare deux mesures), on fera en sorte que ça soit une constante (période
de mesure constante). G est écrit de telle sorte que le robot va tout droit à vitesse constante quelle que soit sa
direction. Un bruit V a été rajouté pour prendre en compte les moment où il ralentit et où il tourne. Bien sûr,
ce choix est mauvais si le robot va dans tous les sens rapidement, si le robot accélère et décélère souvent, etc.
Mais dans les cas où le robot avance à vitesse faible, ça peut le faire.
On a vu dans cette partie la modélisation de l’observation et de l’évolution de l’état d’un robot quand tout
est linéaire. Malheureusement, la réalité est souvent moins linéaire (en fait pas du tout).
Quant à l’équation d’évolution, elle est la même que pour le cas précédent.

3.3 Et si on faisait des trucs bizarres ? Le cas des balises à ultrason

y
3
2000
(x,y)

x
1
-1500 1500
3.3.1 Système de balisage
Le système de balisage est composé d’une partie émettrice et d’une partie réceptrice. La partie émettrice
(PE) est placée sur le robot adverse et émet des ultrasons. La partie réceptrice (PR) est composée de trois
balises qui se trouvent autour de la table (Voir figure 1). Les trois balises sont synchronisées entre elles mais ne
le sont pas avec la partie émettrice.

3
3.3.2 Repérage
PE émet à intervalle régulier un signal particulier. Soient t1 , t2 , t3 respectivement les dates de réception du
même signal par PR. Comme PE et PR ne sont pas synchronisées il est impossible de calculer la durée entre
l’émission du signal et sa réception et donc on ne peut pas trianguler le signal pour en retrouver l’origine.
Cependant, on est certain des trois valeurs suivantes :

t1 − t2
t1 − t3 (9)
t2 − t3

, ce qui est proportionnel à

m1 = d((x, y), P R1) − d((x, y), P R2)

m2 = d((x, y), P R1) − d((x, y), P R3) (10)
m3 = d((x, y), P R2) − d((x, y), P R3)

, il faut juste multiplier les différences de dates par la vitesse du son dans l’air dans des conditions normales.

3.3.3 Modélisation et Simulation

Soit Xt , la position du robot à l’instant t.
 
xt
Xt = (11)
yt

Soit Yt la mesure prise à l’instant t.

 
m1
Yt = m2  (12)
m3

Soit H la fonction qui à une position (x, y) associe la différence de distance entre cette position etles balises
suivant le schéma suivant :
 
d((x, y), P R1) − d((x, y), P R2)
H(x, y) = d((x, y), P R1) − d((x, y), P R3) (13)
d((x, y), P R2) − d((x, y), P R3)

On considère les Yt ainsi parce que c’est plus clair de raisonner avec des distances qu’avec des différences de
temps. On notera les valeurs estimées avec un accent circonflexe sur la lettre, les valeurs simulées avec un tilde.
Soit E([t1 , t2 ], PAS) une liste dont les éléments dont les multiples de t1 compris entre t1 et t2 . On considèrera
dans la suite que l’unité de ses éléments est en seconde. [t1 , t2 ] sera [0, 90] parce qu’un match dure 90 s. Le pas
n’est pas encore décidé, cela dépendra des ressources disponibles du robot. On prendra pour l’exemple PAS =
0.025 s.
Pour le filtrage, on considérera ces équations avec Q, matrice de covariance de l’équation d’évolution et R,
matrice de covariance de l’équation d’observation
L’équation d’observation est :
Xt = H(Yt ) + N (0, R) (14)
L’équation d’évolution est :
Xt+1 = G(Xt ) + N (0, Q) (15)
[Le truc le plus compliqué ! ! !]

4 Filtrage
[Faudra me dire si vous pouvez utiliser ce qu’il y a écrit ici pour réellement programmer ces filtres. Si
c’est oui, le but est atteint, sinon, je détaillerai les dimensions des différentes éléments présents] L’idée derrière
tous ces filtres, c’est qu’on estime récursivement la moyenne de p(ET ATR (k)|M ESR (1 : k)) Pour initialiser les
algorithmes, on donne l’état initial du robot avec une certitude quasi totale (moyenne donnée et norme de la
matrice de covariance faible). On suppose que le p(ET ATR (1)) = N (ET ATR (1)|µ1 , Σ1 ) On pose aussi U suit
N(0,Q) et V suit N(0,R).

4
4.1 Filtre de Kalman classique : cas linéaire et gaussien
Dans ce cas-ci, l’inférence se fait de manière exacte, c’est-à-dire que la valeur calculée par l’algorithme qui
va suivre est optimale (au sens de l’erreur quadratique, norme de la différence au carré).
— Étape de prédiction : on calcule la moyenne et la variance de p(ET ATR (k + 1)|M ESR (1 : k)) que l’on
peut espérer en fonction de l’équation d’évolution du robot.

µk+1|k = G ∗ µk
(16)
Σk+1|k = G ∗ Σk ∗ GT + Q

— Étape de mesure : on calcule la moyenne et la variance de p(ET ATR (k + 1()|M ESR (1 : k + 1))

µk+1 = µk+1|k + Kk+1 ∗ rk+1

(17)
Σk+1 = (I − Kk+1 ∗ α) ∗ Σk+1|k

avec rk+1 = M ESR (k + 1) − α ∗ µk+1|k (l’innovation, l’information qu’apporte la nouvelle mesure) et

Kk+1 = Σk+1|k ∗αT ∗(α∗Σk+1|k ∗αT +R)−1 . On considère alors qu’à chaque itération ET ATR (k+1) = µk+1 .

4.2 Filtre de Kalman étendu

Ce filtre existe car, malheureusement, beaucoup de relations entre la réalité et les mesures ne sont pas
linéaires. On peut même filtrer des relations non linéaires dans l’équation d’évolution, mais dans le cas du
robot, c’est moyennement utile.
L’idée ici développée par ce filtre, c’est qu’on va linéariser ces relations (en prenant le premier terme du
développement limité) et utiliser le filtre de Kalman classique.
On réécrit alors les équations d’évolution et d’observation :

ET ATR (k) = g(ET ATR (k − 1)) + U

(18)
M ESR (k) = h(ET ATR (k)) + V

On traduit les équations (11) en termes de probabilités et en approximant à l’ordre 1 :

p(ET ATR (k)) = N (ET ATR (k)|g(µk−1 ) + Gk ∗ (ET ATR (k − 1) − µk−1 ), Q)

(19)
p(M ESR (k)|ET ATR (k)) = N (M ESR (k)|h(µk|k−1 ) + Ht ∗ (M ESR (k) − µk|k−1 ), R)

avec Hk la Jacobienne de h au vecteur µk|k−1 et Gk la jacobienne de g au vecteur µk−1

On retrouve alors maintenant le filtre de Kalman classique légèrement remanié :
— Étape de prédiction : on calcule la moyenne et la variance de p(ET ATR (k + 1)|M ESR (1 : k)) que l’on
peut espérer en fonction de l’équation d’évolution du robot.

µk+1|k = g(µk )
(20)
Σk+1|k = G ∗ Σk ∗ GT + Q.

— Étape de mesure : on calcule la moyenne et la variance de p(ET ATR (k + 1)|M ESR (1 : k + 1))

µk+1 = µk+1|k + Kk+1 ∗ rk+1

(21)
Σk+1 = (I − Kk+1 ∗ α) ∗ Σk+1|k

avec rk+1 = µk+1|k − h(µk+1|k ) (l’innovation, l’information qu’apporte la nouvelle mesure) et Kk+1 =
Σk+1|k ∗ H T ∗ (H ∗ Σk+1|k ∗ H T + R)−1 .
On considère encore une fois qu’à chaque itération ET ATR (k + 1) = µk+1 .

4.3 Filtrage de Kalman sans parfum

[1] Ici, au lieu de chercher à linéariser l’équation de mesure, on va estimer la mesure après le calcul non
linéaire en faisant passer de manière déterministe une suite de points nommée les points sigmas dans la fonction
de mesure pour finalement obtenir la moyenne et la variance de l’état qu’on cherche. Cette méthode ressemble

5
à la méthode de Monte-Carlo sauf que notre méthode est déterministe et n’a qu’un nombre limité de points
(2*dimension du vecteur d’état +1)
Données: d = 2 : dimension des mesures, α = 1, β = 0, κ = 2, µ0 : position initiale, Σ0 matrice de
covariance initiale,
λ = α2 ∗ (d + κ) - d, γ = d + λ,
Q : matrice de covariance de l’équation d’évolution, R : matrice de covariance de l’équation d’observation,
H est décrite telle que dans la partie 4 et G est la fonction identité.
0 λ 1
wm = d+λ , wc0 = wm 0
+ (1 − α2 + β), wci = wm i
= 2∗(d+λ)
répéter
Partie prédiction :
début
√
Σ = decompositionDeCholesky(γ√ ∗ Σ) ; √
σ̇t−1 = [µt−1 ; µt−1 − Σ:,i (i ∈ {1, ..., d}); µt−1 + Σ:,i (i ∈ {1, ..., d})] ;
pour i ∈ {0, ..., 2 ∗ d} faire
z̄t∗ i = G(σ̇t−1
i
);
fin
P2∗d i
µ̄t = i=0 wm ∗ z̄ ∗i ;
P2∗d i ∗i t
Σ̄t = i=0 wc (z̄t − µ̄t )(z̄t∗i − µ̄t )T + Qt ;
fin
Partie mise-à-jour grâce à l’observation :
début
√
Σ̄ =decompositionDeCholesky(γ ∗ Σ̄)p ;
¯
p
σ̇t = [µ ; µ − Σ̄:,i (i ∈ {1, ..., d}); µ + Σ̄:,i (i ∈ {1, ..., d})] ;
pour i ∈ {0, ..., 2 ∗ d} faire
ȳt∗ i = H(σ̇¯ti ) ;
fin
P2∗d i
ŷt∗i = i=0 wm ∗ ȳ ∗i ;
P2∗d i ∗i t ∗i
St = i=0 wc (ȳt − ŷt ) ∗ (ȳt∗i − ŷt∗i )T + Rt ;
fin
Partie estimation de l’état actuel :
début
P2∗d
Σ̄z,y
t = i=0 wci (z̄t∗i − µ̄t ) ∗ (ȳt∗i − ŷt∗i )T ;
Kt = Σ̄z,y −1
t ∗ St ;
µt = µ̄ + Kt ∗ (yt − ŷt ) ;
Σt = Σ̄ − Kt ∗ St ∗ KtT ;
fin
jusqu’à plus aucune mesure;
Algorithm 1: Algorithme de Kalman Unscented

5 Conclusion
Références
[1] Simon J Julier and Jeffrey K Uhlmann. New extension of the kalman filter to nonlinear systems. In
AeroSense’97, pages 182–193. International Society for Optics and Photonics, 1997.