TP4: Filtrage Particulaire

TP4: Filtrage Particulaire
Navigation pour les syst`emes autonomes
ROB312

1 Introduction
Un filtre `a particules est un filtre r´ecursif qui utilise la m´ethode de Monte
Carlo pour estimer l’´etat d’un syst`eme dynamique `a partir d’une s´equence
d’observations bruyantes ou incompl`etes en repr´esentant la probabilit´e post´erieure
d’un ´ev´enement al´eatoire par un ensemble d’´echantillons al´eatoires (appel´es particules) avec des poids. l’espace du mod`ele.
Le filtre `a particules est une g´en´eralisation du filtre de Kalman, qui est
bas´e sur un espace d’´etat lin´eaire et une distribution gaussienne du bruit, alors
que le mod`ele d’espace d’´etat du filtre `a particules peut ˆetre non lin´eaire et la
distribution du bruit peut ˆetre de n’importe quel type. Par rapport aux mod`eles
gaussiens, il peut exprimer une plus large gamme de distributions et poss`ede
une plus grande capacit´e `a mod´eliser des caract´eristiques non lin´eaires.
Malgr´e les nombreux avantages du filtrage particulaire, le grand nombre
d’´echantillons requis ne peut ˆetre ignor´e : plus le nombre d’´echantillons est important, plus l’algorithme est complexe. En outre, la phase de r´e-´echantillonnage
entraˆıne une perte de validit´e et de diversit´e de l’´echantillon.
2 Q1 - La structure de code et les param`etres
Cette section d´ecrit la structure principale du code et la signification des
param`etres. Afin de montrer plus clairement la structure du code, nous allons
d’abord pr´esenter la signification de quelques param`etres importants.
• X reel : l’´etat r´eel du syst`eme, puisqu’elle est inconnue, c’est aussi la
valeur que nous voulons estimer. Il est initialis´e par la valeur estim´ee et
l’erreur d’initialisation
1
• R : la matrice de covariance du bruit de mesure r´eelle
• T : La dur´ee de la simulation
• N : Le nombre de particules
• P hat : La matrice de covariance
• X hat : L’estimation initial (x, y, z, θ)
• Qf : La matrice de covariance de bruit de dynamique
• Rf : La covariance du bruit de mesure du filtre
• thresholdresampling : Le seuil de r´e-´echantillonnage (θef f ). Quand
Nef f = P
1
i
(wi
k
)
2 < θef fN, on va faire le r´e-´echantillonnage.
• Xp : Le tirage des particules autour de X hat initial
• wp :Les poids initiaux associ´es aux particules
• Y : la simulation physique des mesures
Ayant compris la signification des param`etres ci-dessus, nous pouvons facilement comprendre la structure de l’ensemble du code (illustr´e `a la Figure 1).
Une fois que le temps de simulation a atteint T, nous d´eroulons la simulation
et pr´esentons ensuite les r´esultats.
Figure 1: La structure de code
2
3 Q2 - Compl´eter le code et commenter le r´esultat
Sur la base de l’´equation du filtre PF (pr´ediction, correction, r´e-´echantillonnage
diapo 24) et le mod`ele dynamique et le mod`ele de mesure (diapo 33), on peut
compl´eter le code. Les r´esultats sont pr´esent´es dans la Figure 2.
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 2: R´esultat
On peut constater que l’incertitude entre les ´etats r´eels et estim´es est ´egalement
importante au d´ebut. Cependant, le filtrage particulaire a permis d’estimer la
position r´eelle et de faire converger les incertitudes. Finalement, l’erreur entre
les ´etats r´eel et estim´e a ´et´e r´eduite `a 458m et l’incertitude est beaucoup plus
faible qu’au d´ebut.
4 Q3 - Faire varier le bruit de dynamique du
filtre
Dans cette section, on fait varier le bruit de dynamique (matrice Qf) du
filtre pour ´etudier l’effet du bruit dynamique du filtre sur son comportement.
On a utilis´e 2 valeurs : 0.1Qf et 10Qf.
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 3: R´esultat avec 0.1Qf
3
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 4: R´esultat avec 10Qf
Le bruit dynamique (Qf) interf`ere avec la proc´edure de pr´ediction du filtre
particulaire. Un bruit dynamique trop important peut empˆecher la pr´ediction
correcte des ´etats, ce qui nous conduit alors `a trop d’erreurs et d’incertitudes.
Sur la base des observations des Figure 2, Figure 3 et Figure 4, on constate que
plus le bruit dynamique est faible, plus l’incertitude de l’estimation est faible et
plus l’erreur d’estimation finale est faible. Plus important encore, dans le cas de
0.1Qf, l’erreur finale est de 32m, ce qui est moins que dans le cas de la figure 2,
mais dans le cas de 10Qf, l’erreur est de 2430m, ce qui est un ´ecart trop grand.
5 Q4 - Faire varier le bruit de mesure du filtre
Dans cette section, on fait varier le bruit de mesure (matrice Rf) afin de
d´eterminer l’effet du bruit de mesure du filtre sur son comportement. On a
essay´e diff´erentes valeurs de Rf entre 102
et 1002
: 102
, , 602
, 1002
(par d´efaut
Rf = 202
). Tous les autres param`etres ´etaient d´efinis par d´efaut. Rf interf`ere
avec la correction des poids des particules et affecte directement l’incertitude.
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 5: R´esultat avec Rf = 102
4
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 6: R´esultat avec Rf = 602
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 7: R´esultat avec Rf = 1002
Sur la base de Figure 5, Figure 6, Figure 7 et Figure 2, on constate que
plus le bruit de mesure est petit, plus la convergence de l’incertitude se produit
tˆot, c’est-`a-dire que il faut moins de temps pour que l’incertitude devienne plus
petite. En outre, lorsque Rf est trop grand, l’incertitude devient importante
tout au long de la simulation.
6 Q5 - Faire varier le nombre de particules
Dans cette section, on va ´etudier l’effet du nombre de particules N sur le
comportement du filtre en essayant diff´erentes valeurs de N : 300, 30000 (par
d´efaut N = 300). Tous les autres param`etres ´etaient d´efinis par d´efaut.
5
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 8: R´esultat avec N = 300
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 9: R´esultat avec N = 300
D’apr`es Figure 8, Figure 9 et Figure 2, on constate que l’incertitude devient
plus petite lorsqu’il y a moins de particules, mais l’erreur associ´ee est plus grande
et le filtre ne fonctionne pas bien. On peut en d´eduire que plus le nombre de
particules est ´elev´e, plus le calcul est lent et plus la convergence est bonne (plus
petite l’erreur), mais plus le temps n´ecessaire pour r´eduire l’incertitude est long.
7 Q6 - Faire varier le seuil de r´e-´echantillonnage
Dans cette section, on va ´etudier l’effet des seuils de r´e-´echantillonnage sur
le comportement du filtre en essayant diff´erents seuils : 0, 0.5 et 1. Un histogramme des poids sera ´egalement trac´e. Tous les autres param`etres ´etaient
d´efinis par d´efaut.
6
(a) La position vraie et estim´ee
(b) Incertitude d’estimation (c) Histogramme des poids
Figure 10: R´esultat avec θef f = 0
(a) La position vraie et estim´ee
(b) Incertitude d’estimation (c) Histogramme des poids
Figure 11: R´esultat avec θef f = 0.5
D’apr`es Figure 10, Figure 11 et Figure 12, on constante que lorsque θef f = 0,
on ne fait jamais de r´e-´echantillonnage. L’histogramme bas´e sur le poids a
des poids est 0. Lorsque θef f = 0.5 (par d´efaut), on ne fait jamais de r´e-
´echantillonnage. Le r´e-´echantillonnage a eu lieu et le poids de chaque particule
a chang´e, certaines particules ayant vu leur poids augmenter et d’autres ayant
vu leur poids diminuer. Lorsque θef f = 1, un plus grand r´e-´echantillonnage a
´et´e effectu´e (`a chaque fois) et le probl`eme de d´eg´en´erescence appariˆat, car tous
les poids des particules avaient des valeurs mˆeme (0.3333).
8 Q7 - Un trou de mesures entre t = 50s et t =
75s
Dans cette section, on va simuler un trou entre t = 50s et t = 75s en utilisant
le code ci-dessous. Tous les autres param`etres ´etaient d´efinis par d´efaut.
1 is measurementValid = true;
2 if tk*dt ≥ 50 && tk*dt ≤ 75 % t ≥ 50s et t ≤ 75s
3 is measurementValid = false;
4 end
7
(a) La position vraie et estim´ee
(b) Incertitude d’estimation (c) Histogramme des poids
Figure 12: R´esultat avec θef f = 1
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 13: R´esultat avec un trou de mesures entre t = 50s et t = 75s
Selon la Figure 13, on constante que l’incertitude augmente dans le trou
`a cause de manque de mesure et la correction des poids. Malgr´e le manque
de mesures, le filtre final a tout de mˆeme ´et´e corrig´e en temps apr`es 75s et a
finalement estim´e l’´etat correctement, montrant une petite erreur de 405m.
9 Q8 - Modifier la fr´equence des mesures
Dans cette section, on va simuler avec la fr´equence de mesure (0.1Hz) en
utilisant le code ci-dessous. Tous les autres param`etres ´etaient d´efinis par d´efaut.
1 is measurementValid = true;
2 if mod(tk*dt, 10) ̸= 0
3 is measurementValid = false;
4 end
Selon la Figure 14, on constante que l’incertitude diminue toutes les 10
secondes en utilisant la fr´equence de mesure = 0.1Hz,. Par rapport `a la Figure
2 o`u les mesures sont toujours disponibles, cela nous donne une image plus claire
de la fa¸con dont les mesures nous aident `a r´eduire l’incertitude et de la fa¸con
dont l’incertitude augmente lorsqu’il n’y a pas de mesures.
8
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 14: R´esultat avec la fr´equence de mesure = 0.1Hz
10 Q9 - Une autre fa¸con de r´e-´echantillonner les
poids Li et al. (2015)
Dans cette section, on va utiliser une m´ethode alternative de r´e-´echantillonnage
des poids. L’algorithme de r´e-´echantillonnage polynomial ayant ´et´e utilis´e dans
le code, on choisi donc d’utiliser la m´ethode de r´e-´echantillonnage stratifi´e, dont
l’algorithme est illustr´e `a la Figure 15.
Figure 15: Algorithme de r´e-´echantillonner les poids
9
1 function [index] = stratified(wp)
2 N = length(wp);
3 index = zeros(1,N);
4 Q = cumsum(wp);
5 n = 1;
6 m = 1;
7 while (n ≤ N && m ≤ N)
8 u0 = rand/N;
9 u = u0 + n/N;
10 while (Q(m) < u && m < N)
11 m = m+1;
12 end
13 index(n) = m;
14 n = n+1;
15 end
16 end
(a) La position vraie et estim´ee (b) Incertitude d’estimation
Figure 16: R´esultat avec la m´ethode de r´e-´echantillonnage stratifi´e
Les r´esultats de la m´ethode de r´e-´echantillonnage stratifi´e sont pr´esent´es `a
la Figure 16, o`u le filtre particulaire converge avec succ`es pour nous en termes
d’erreur et d’incertitude. Son erreur finale n’est pas tr`es diff´erente de celle de
la m´ethode polynomiale.
References
Li, T., Bolic, M., and Djuric, P. M. (2015). Resampling methods for particle
filtering: Classification, implementation, and strategies. IEEE Signal Processing Magazine, 32(3):70–86.
10