TP2: Filtrage de Kalman

TP2: Filtrage de Kalman
Navigation pour les syst`emes autonomes
ROB312


1 Introduction
Le filtre de Kalman est un filtre `a r´eponse impulsionnelle infinie qui estime
les ´etats d’un syst`eme dynamique `a partir d’une s´erie de mesures incompl`etes
ou bruit´ees. Le filtre a ´et´e nomm´e d’apr`es le math´ematicien et informaticien
am´ericain d’origine hongroise Rudolf Kalman.
Il fait appel `a la dynamique de la cible qui d´efinit son ´evolution dans le temps
pour obtenir de meilleures donn´ees, ´eliminant ainsi l’effet du bruit. Ces donn´ees
peuvent ˆetre calcul´ees pour l’instant pr´esent (filtrage), dans le pass´e (lissage),
ou sur un horizon futur (pr´ediction).
2 Partie 1 : Cas Lin´eaire
2.1 Q1 - La structure du code et les param`etres
La logique de cette partie du code est la suivante :
• X repr´esente le vecteur d’´etat du syst`eme et on l’initialise d’abord.
– L’´etat X = [x, y, z, vx, vy, vz]
– X reel : l’´etat vrai
– X hat : la valeur estim´ee du filtre de Kalman
• On simule la trajectoire physique et le capteur s´epar´ement au temps T.
– Xv sim repr´esente tous les ´etats (x, y, z, vx, vy, vz)
– Y sim repr´esente seulement les 3 premiers ´etat (x, y, z)
1
• Apr`es on fait le boucle de simulation du filtre de Kalman:
– Initialisation des param`etres
∗ P hat : La matrice de covariance
∗ F : La matrice de dynamique
∗ Qf : La matrice de covariance du bruit de dynamique
∗ H : La matrice d’observation
∗ R : La matrice de covariance du bruit de mesure
– On fais des pr´edictions dans la boucle selon la formule de pr´ediction.
– On fais la correction dans la boucle en fonction de la formule d’observation.
• Enfin, on dessine le r´esultat dans la figure
2.2 Q2 - Compl´eter le code et commenter les r´esultats
Tout d’abord, selon l’´equation de pr´ediction et l’´equation de correction dans
le cas lin´eaire (comme Eq. (2.1) et Eq. (2.2)), on compl`ete le code.
L’´equation de pr´ediction:
xˆk|k−1 = Fkxˆk−1 + Gkuk
Pˆ
k|k−1 = FkPˆ
k−1F
T
k + Qk
(2.1)
L’´equation de correction:
xˆk = xˆk|k−1 + Kk(yk − Hkxˆk|k−1)
Pˆ
k = (Id − KkHk)Pˆ
k|k−1
Kk = Pˆ
k|k−1HT
k
(Rk + HkPˆ
k|k−1HT
k
)
−1
(2.2)
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 1: R´esultat
2
Ex´ecutez le code apr`es avoir termin´e le code, et le r´esultat est illustr´e `a la
Figure 1. On constate que mˆeme s’il y a du bruit (erreur de mesure), la valeur
estim´ee dans chaque direction est toujours coh´erente avec la valeur r´eelle. Le
fonctionnement du filtre de Kalman est bien v´erifi´e.
2.3 Q3 - Un trou de mesures entre t = 3s et t = 7s
1 is measurementValid = true;
2 if tk*dt ≥ 3 && tk*dt ≤ 7 % t ≥ 3s et t ≤ 7s
3 is measurementValid = false;
4 end
En utilisant le code ci-dessus, on peut simuler un trou de mesures entre t =
3s et t = 7s et le r´esultat est illustr´e `a la Figure 2.
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 2: R´esultat avec un trou de mesures
Selon la figure 2, on peut constater que la position r´eelle et la position estim´ee
ne sont plus coh´erentes pendant le trou de mesures et l’incertitude augmente
´egalement. Ce ph´enom`ene est dˆu au fait que il n’y a pas de correction entre t
= 3s et t = 7s. Mˆeme s’il y a une grande diff´erence entre la position r´eelle et
la position estim´ee `a t = 7s, la correction parvient `a r´etablir la correspondance.
Le fonctionnement de la correction est bien v´erifi´e.
2.4 Q4 - Faire varier le bruit de dynamique du filtre
Dans cette section, on change Qf `a 0.1 ∗ Qf et 10 ∗ Qf et illustre respectivement les r´esultat dans la Figure 3 et la Figure 4. En le comparant avec la Figure
1, on constate qu’`a mesure que le bruit dynamique augmente, l’incertitude augmente ´egalement. Mais au final, le filtre de Kalman a tout de mˆeme r´eussi `a
obtenir la correspondance entre la position r´eelle et la position estim´ee.
3
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 3: R´esultat avec 0.1 ∗ Qf
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 4: R´esultat avec 10 ∗ Qf
2.5 Q5 - Faire varier le bruit de mesure du filtre
Dans cette section, on change R `a 0.1 ∗ R et 10 ∗ R et illustre respectivement
les r´esultat dans la Figure 5 et la Figure 6. On a simul´e l’estimation d’´etats
du syst`eme en variant le bruit pour avoir une connaissance claire de l’influence
de la qualit´e de capteur sur la qualit´e d’estimation. En le comparant avec la
Figure 1, on peut facilement d´eduire que un petit bruit de mesure nous am`ene
des incertitudes petites. Donc un bon capteur est capable d’am´eliorer la qualit´e
d’estimation.
4
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 5: R´esultat avec 0.1 ∗ R
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 6: R´esultat avec 10 ∗ R
3 Partie 2 : Cas Non-Lin´eaire
Dans cette partie, on ´etudiera l’application du filtre de Kalman dans le cas de
non-lin´eaire : Kalman Etendu ´ . Ce filtre permet de lin´eariser localement le
probl`emes et l’application des ´equations classiques du filtre de Kalman.
3.1 Q1 - Compl´eter le code et commenter les r´esultats
Tout d’abord, selon l’´equation de pr´ediction et l’´equation de correction dans
le cas non-lin´eaire (comme Eq. (3.1) et Eq. (3.2)), on compl`ete le code.
5
L’´equation de pr´ediction:
xˆk|k−1 = f(xˆk−1 + uk)
Pˆ
k|k−1 = FkPˆ
k−1F
T
k + Qk
Fk =
∂f
∂x
 
 
 
 
x=xˆk
∈ R
d×d
(3.1)
L’´equation de correction:
xˆk = xˆk|k−1 + Kk(yk − h(xˆk|k−1))
Pˆ
k = (Id − KkHk)Pˆ
k|k−1
Kk = Pˆ
k|k−1HT
k
(Rk + HkPˆ
k|k−1HT
k
)
−1
Hk =
∂h
∂x
 
 
 
 
x=xˆk
∈ R
dm×d
(3.2)
Ex´ecutez le code apr`es avoir termin´e le code, et le r´esultat est illustr´e `a la
Figure 7. On constate qu’au d´ebut, la position estim´ee est tellement ´eloign´ee
de la position r´eelle que les deux positions mettent plus de temps `a s’accorder.
Mais cette fois, l’incertitude est beaucoup plus faible que dans le cas lin´eaire.
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 7: R´esultat
3.2 Q2 - Modifier la fr´equence de Mesures
On modifie la fr´equence de la mesure (`a 1 Hz) en utilisant la variable
dt mesure=1 et les r´esultats sont pr´esent´es `a la figure 8. On peut constante
qu’au d´ebut la courbe de position estim´ee ne correspond pas `a la position r´eelle
dans le temps et est sujette `a des changements p´eriodiques, car la correction de
la mesure est mise `a jour apr`es que 10 pr´edictions ont d´ej`a ´et´e faites. Mais au
final, on a une bonne estimation de la position r´eelle.
6
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 8: R´esultat avec dt mesure=1
3.3 Q3 - Faire varier le bruit de dynamique du filtre
Dans cette section, on change Qf `a 0.1 ∗ Qf et 10 ∗ Qf dans le cas o`u la
fr´equence de mesure est 10Hz (par d´efaut) et illustre respectivement les r´esultat
dans la Figure 9 et la Figure 10. En le comparant avec la Figure 7, on constate
que: si le bruit dynamique est plus faible, la courbe de la position estim´ee devient
plus lisse, avec moins d’oscillations et moins d’incertitude, mais l’homog´en´eit´e
des deux courbes commence plus tard qu’avant. Si le bruit dynamique est
plus important, l’incertitude devient plus grande, mais l’homog´en´eit´e des deux
courbes commence plus tˆot qu’avant. En d’autres termes, plus le bruit dynamique est important, plus il est probable que la position estim´ee soit proche
de la position r´eelle dans un court laps de temps.
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 9: R´esultat avec 0.1 ∗ Qf
7
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 10: R´esultat avec 10 ∗ Qf
3.4 Q4 - Faire varier le bruit de mesure du filtre
En r´eglant tous les autres param`etres sur la valeur par d´efaut, on modifie le
bruit de la mesure `a une nouvelle valeur(Rfi = diag([3,1*pi/180].ˆ2)) et illustre
respectivement les r´esultat dans la Figure 11. En les comparant avec la Figure
7, on constate que l’incertitude est beaucoup plus faible. Un capteur de bonne
qualit´e peut donc am´eliorer la qualit´e du contrˆole et r´eduire son incertitude.
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 11: R´esultat avec une nouvelle Rf i
3.5 Q5 - Le performance du filtre en foction du nombre
d’amers
Dans cette section on d´efini plus d’amers (de 3 `a 5) et on va ´etudier l’influence
du nombre d’amers, la position d’amer est respectivement [10;0], [00;50], [0;0],
[-10;25] et [-20;40]. Les r´esultat sont illustr´e respectivement dans la Figure 12,
8
Figure 13 et Figure 14. En les comparant avec la Figure 7, on constate que plus
le nombre d’amers ´etait ´elev´e, plus la concordance entre les emplacements r´eels
et estim´es apparaissait rapidement.
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 12: R´esultat avec 3 amers
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 13: R´esultat avec 4 amers
9
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 14: R´esultat avec 5 amers
10