Introduzione suiParticle Filters

nella robotica mobile

Autore : Paolo BrugiottiDipartimento Informatica

e AutomazioneUniversità degli studi Roma TRE

Argomenti Trattati

Filtri bayesianiFiltri a soluzione ottima: KFFiltri a soluzione sub-ottima: EKFParticle Filters :

SISSIRGPF

Particle Filters : possibili applicazioni in robotica

Approccio Spazio di Stato

Formulazione tempo-discreta del problema

Sistemi modellati con Spazio di stato:Equazione dell’evoluzione dello stato (system model)Equazione della misura dello stato (measurament model)

Vantaggi : modella processi Non-Lineari / Non-Gaussiani

Modello Matematico

Filtri Bayesiani : IntroduzioneProblema di “stima stocastica” del processo, utilizzando :

l’equazione dell’evoluzione dello statol’equazione delle osservazioni tutte le misure z1:k disponibili dall’istante iniziale fino a k

Problema:Nasce l’esigenza di aggiornare la stima ogni volta che è disponibile una nuova misura

Idea:Utilizzare filtri di natura ricorsiva

Filtri Bayesiani : RicorsivitàASSUNZIONI : p(x0|z0) è disponibile

La pdf a posteriori p(xk|z1:k) si calcolaricorsivamente in due fasi:

PREDIZIONE : p(xk-1|z1:k-1) p(xk|z1:k-1) AGGIORNAMENTO : p(xk|z1:k-1), zk p(xk|z1:k)

Filtri Bayesiani : Predizione

ASSUNZIONE : p(xk-1|z1:k-1) al tempo k è disponibile

Usa il modello dell’evoluzione (1) dello stato del sistemaTrasla, deforma e allarga la pdf per via del rumoreDetermina la densità a priori con l’equazione di Chapman-Kolmogorov :

)3()|()|()|( 11:1111:1 −−−−− ∫= kkkkkkk dxzxpxxpzxp

Nella (3) si sfruttato il fatto che il processo è MarkovianoLo stato xk dipende solamente dallo stato xk-1 :

)|(),|( 11:11 −−− = kkkkk xxpzxxp

Filtri Bayesiani : AggiornamentoASSUNZIONI : la misura zk al tempo k è disponibile

Usa il modello della misura (2) dello stato del sistemaMigliora e corregge la pdf dello stato corrente xkCalcola la densità a posteriori con il Teorema di il Teorema di BayesBayes ::

)4()|(

)|()|()|(1:1

1:1:1

−

−=kk

kkkkkk zzp

zxpxzpzxp

dove la costante di normalizzazione vale:dove la costante di normalizzazione vale:

kkkkkkk dxzxpxzpzzp )|()|()|( 1:11:1 −− ∫==η

Considerazioni Generali

La ricorsiva propagazione della densitricorsiva propagazione della densitàà a posteriori a posteriori èèsolamente una soluzione concettuale:solamente una soluzione concettuale:

difficile da determinare analiticamente!!!difficile da determinare analiticamente!!!coinvolge integrali difficili coinvolge integrali difficili

Solo in pochi casi, la Soluzione Ottima Solo in pochi casi, la Soluzione Ottima èè calcolabile :calcolabile :Filtro di KalmanFiltro di KalmanGridGrid--Based Based FilterFilter

Negli altri casi ci dobbiamo accontentare di Negli altri casi ci dobbiamo accontentare di unun’’approssimazione della Soluzione Ottima :approssimazione della Soluzione Ottima :

Filtro di Kalman EstesoFiltro di Kalman EstesoGridGrid--Based Based FilterFilter approssimatoapprossimatoParticle Particle FilterFilter

KF : Assunzioni

⎩⎨⎧

+=+= −−

kkkk

kkkk

nxHzvxFx 11

La La densitdensitàà a posterioria posteriori p(xp(xkk--11 |z|z1:k1:k--1 1 ) ) èè Gaussiana Gaussiana I rumori su processo e misura sono :I rumori su processo e misura sono :

),(~),,0(~),,0(~ 00011 PxNxRNnQNv kkkk

∧

−−

Le equazioni del sistema sono :Le equazioni del sistema sono :

FFkk, matrice del sistema

HHkk, matrice delle misure

dove dove ffkk(x(xkk--11, v, vkk--11) e ) e hhkk ((xxkk,,nnkk) sono funzioni lineari) sono funzioni lineari

Con tali restrizioni, si può dimostrare che Con tali restrizioni, si può dimostrare che anche la pdf a posteriori p(anche la pdf a posteriori p(xxkk |z|z1:k 1:k ) ) èè GaussianaGaussiana

KF : Predizione

Disponiamo della pdf a posteriori al tempo k-1:

),()|( 1|11|11:11 −−−−−− = kkkkkk PmNzxpUtilizzando la (1) e la (3) si ha la pdf a priori:

),()|( 1|1|1:1 −−− = kkkkkk PmNzxp

NOTA : Con la notazione N(m,P) abbiamo indicato una pdf gaussiana con Valor Atteso mm e Covarianza P

KF : Correzione

Utilizzando le equazioni (2), (4) e la pdf a priori, possiamo Utilizzando le equazioni (2), (4) e la pdf a priori, possiamo calcolare la pdf a posteriori :calcolare la pdf a posteriori :

)10(),()|( ||:1 kkkkkk PmNzxp =

e dove

KF: Considerazioni

VantaggiForniscono la Soluzione Ottima Bayesiana

Svantaggi Assunzioni molto restrittiveSe l’equazioni sono Non Lineari e\o i rumori non sono bianchi?

Non c’è soluzione analitica : come risolviamo il problema?Operiamo delle approssimazioni…….

EKF : Assunzioni

La La densitdensitàà a posterioria posteriori p(xp(xkk--11 |z|z1:k1:k--1 1 ) ) èè approssimata ad approssimata ad una Gaussiana una Gaussiana I rumori sul processo e sulla misura sono :I rumori sul processo e sulla misura sono :

),0(~),,0(~ 11 kkkk RNnQNv −−

⎩⎨⎧

+=+= −−

kkkk

kkkk

nxhzvxfx

)()( 11

Le equazioni del modello e delle misure sono Non Lineari :ffkk(.) è non lineare

hhkk(.) è non lineare

Ad ogni istante k, le funzioni f(.) ed h(.) vengono sviluppate in serie di Taylor fino al primo ordine, operando una linearizzazione intorno al punto di predizione

EKF : Predizione

All’istante k, si linearizza intorno al punto di predizione, ottenendo un’approssimazione della pdf a priori:

),()|( 1|1|1:1 −−− ≈ kkkkkk PmNzxp

∧

kFdove è una linearizzazione locale di fk(.)

1|1

)(

−−=

∧

=kkmx

kk

dxxdfF

EKF : Correzione

dove

La pdf a posteriori p(xLa pdf a posteriori p(xkk|z|z1:k1:k)) è approssimata ad una Gaussiana :

),()|( ||:1 kkkkkk PmNzxp ≈

EKF: ConsiderazioniVantaggi

Trova una soluzione nel caso di dinamiche non lineari

Svantaggi

Fallisce nel caso in cui:La pdf a posteriori è bimodale / multimodaleLa pdf a posteriori risulta fortemente Non-Lineare

Non fornisce la soluzione ottima a causa di:Linearizzazione : fa perdere informazioni sul processoNon Gaussianità dei fenomeni aleatori presenti nel sistema, che rende necessaria la stima delle statistiche superiori a quelle del secondo ordine

Particle Filters : Algoritmi

SIS: Sequential Importance Sampling

PF: Generic Particle Filter

SIR: Sequential Importance Re-Sampling

GPF : Gaussian Particle Filter

Approssimano la pdf a posteriori degli stati mediante misure casuali, rappresentate da:

Particles: campioni random dello spazio di statoPesi associati ai particles: rappresentano la massa di probabilitàassociata al campione

Particle Filters : Idea

I particles ed i loro pesi si aggiornano in tre fasi:

Campionamento (sampling)Calcolo dei pesi (weight computation )Ricampionamento (resampling)

Aumenta NS Convergenza (quasi certa) della pdf a posteriori vera

∑Ν

=

−≈S

i

ikk

ikkk xxwzxp

1:1 )()|( δ

SIS : Definizioni

SIS : Funzione di ImportanzaNon possiamo estrarre direttamente da p(.) maestraiamo i campioni dalla funzione di importanza(importance density): xi ~ q(x)

Sia la pdf dalla quale è difficile estrarre i campioni ma per cui si può calcolare π(x).

ikx

)()( xxp π∝

)|(),|()|( 1:11:11:1 −−−= kkkkkkk zxqzxxqzxqè possibile ottenere i campioni

)|(~ :1 kkik zxqx

)|(~ 1:111 −−− kkik zxqx

),|(~ :11 kkkik zxxqx −

.

Si può dimostrare che scegliendo q(.) fattorizzata in :

aggiungendo ad ogni campione stimato

il nuovo stato

SIS : Predizione

)48(),|(

)|()|(

1

11

kik

ik

ik

ik

ikki

kik zxxq

xxpxzpww−

−−∝

La pdf a posteriori è approssimata a :

),|()|()|(

:11:0

11

ki

kik

ik

ik

ikki

kik zxxq

xxpxzpww−

−−=

Scegliendo la q(.) in questo modo, l’equazione di aggiornamento dei pesi si può dimostrare essere :

L’equazione dei pesi modificata diventerà dunque :

SIS : Aggiornamento

ASSUNZIONE : q(xk|x0:k-1, z1:k) = q(xk|xk-1, zk)

SIS : Pseudo - Codice

FOR i =1:NS

Estrai

Assegna al Particle il peso in accordo alla (48)

END FOR

Normalizza i pesi :

),|(~ 1 kikk

ik zxxqx −

∑ == SN

iik

ik

ik www

1~/~

ikw~

Dopo alcune iterazioni, solamente pochi particles hanno un peso rilevante mentre tutti gli altri hanno un peso trascurabileSforzo computazionale per aggiornare particles il cui contributo nell’approssimazione della pdf è quasi nullo.Tale fenomeno non si può evitare e si può stimare sulla base della varianza dei pesi :

∑=

∧

=SN

i

ik

Seff

w

NN

1

2)()51(

Soluzioni al problema ?Approccio Brute Force : NS ∞ (Irrealizzabile)Buona scelta della funzione di importanza Ricampionamento

SIS : Degenerazione

Scelta della q(.)

)|(),|( 11ikkk

ikk xxpzxxq −− =

Funzione d’importanza a priori :

Le traiettorie dei campioni si calcolano applicando l’equazione del modello del sistema (1).L’equazione di aggiornamento dei pesi diventa :

Risulta indipendente dalle misure : Forte sensibilità del filtro agli outlier.

Tale procedimento avviene generando un nuovo insieme di particles, ottenuti ricampionando e rimuovendo tra i vecchi particles

ottenuti dalla relazione :

SNi

ikx 1*}{ =

SNi

ikx 1}{ =

∑Ν

=

−≈S

i

ikk

ikkk xxwzxp

1:1 )()|( δ

quelli a cui è associato un peso minore, e replicando invece quelli a cui è associato un peso maggiore.

.

A ciascun particle ricampionato viene assegnato lo stesso peso: S

ik Nw \1=

Ricampionamento

IF

),|(~ 1 kikk

ik zxxqx −

ikw~

∑ == SN

iik

ik

ik www

1~/~

∧

effN

teff NN <∧

]},[{ 1SN

iik

ik wx =

FOR i =1:NS

Estrai :Assegna al Particle il peso in accordo alla (48)

END FOR

Normalizza i pesi :

Calcola usando la(51)

END IF

Ricampiona :

PF : Pseudo Codice

SIR : Assunzioni

Deriva dal SIS con le seguenti scelte:La funzione d’importanza è quella a priori :

il ricampionamento è effettuato ad ogni passo

Assunzioni :1) le funzioni fk(.) e hk(.) devono essere note;2) bisogna essere in grado di campionare

la funzione di distribuzione dei rumori sul processo vk-1la densità di probabilità a priori all’istante k:

3) deve essere nota in forma funzionale

)|(),|( 11ikkk

ikk xxpzxxq −− =

)|( kk xzp)|( 1−kk xxp

SIR : PredizioneLa scelta della funzione d’importanza a priori implica che ogni campione :

)|(~ 1ikk

ik xxpx −

viene generato :

prima : )(~ 11 −− kvik vpv

),( 11ik

ikk

ik vxfx −−= , sfruttando la (1).poi,

Nota : pv(.) è la pdf di vk-1

)|(1ikk

ik

ik xzpww −∝

L’equazione di aggiornamento dei pesi diventa :

Poiché si campiona ad ogni istante si ottiene : iNw S

ik ∀=− ,/11

l’equazione dei pesi diventa :

La pdf a posteriori è approssimata a :

SIR : Aggiornamento

)|(~ 1ikk

ik xxqx −

∑ == SN

iik

ik

ik www

1~/~

]},[{ 1SN

iik

ik wx =

FOR i =1:NS

Estrai :

Calcola :

END FOR

Normalizza i pesi :

Ricampiona:

)|(~~ ikk

ik xzpw

SIR : Pseudo Codice

SIR : ConsiderazioniVANTAGGI

I pesi associati ai particle sono valutati facilmente La funzione d’importanza è facilmente campionata

SVANTAGGI

La funzione d’importanza è indipendente dalla misura:tale filtro può risultare particolarmente inefficiente e fortemente sensibile agli outliers

Applicare il ricampionamento ad ogni istante può causare una rapida perdita di diversità nei particles.

GPF : IntroduzioneI GPFs approssimano :

la pdf a priori e la pdf a posteriori

mediante distribuzioni gaussiane che indicheremo con :

Per calcolare i valori attesi e e le covarianze e viene usata la tecnica di filtraggio con Particles

kµ kµkΣ

),()|( 1:1 kkkk Nzxp Σ≈− µ

),()|( :1 kkkk Nzxp Σ≈ µ

kΣ

GPF : PredizioneEstraiamo i particles dalla pdf a posteriori a k-1:

),(~ 111 −−− Σkkik Nx µ

)|(~ 1ikk

ik xxpx −

A questo punto siamo in grado di estrarre :

applicando l’equazione del modello (1)

kµ∑ =

= SN

iikSk xN

1)/1(µ

∑ =−−=Σ SN

iTi

kkikkSk xxN

1))(()/1( µµ

Essendo nota , è possibile calcolare il valore atteso e la covarianza :

ikx

kΣ

GPF : AggiornamentoOra calcola i pesi dei Particles, utilizzando l’equazione della misura (2):

ikw~

)|(~ ikk

ik xzpw =

∑ == SN

iik

ik

ik www

1~/~

∑ == SN

iik

ikk xw

1µ

∑ =−−=Σ SN

iTi

kkikk

ikk xxw

1))(( µµ

Normalizza i pesi secondo la relazione :

Calcola il valor atteso e la covarianza :

che sono i parametri che approssimano la pdf a posteriori al tempo k

kΣkµ

GPF : Pseudo Codice I

GPF : Pseudo Codice II

GPF : Considerazioni

Simile al filtro SIR poiché i campioni vengono estratti dalla funzione d’importanza

A differenza del SIR, i GPF non necessitano del passo di ricampionamento :

l’onere computazionale viene ridotto notevolmente

A differenza del EKF, per i GPF possono essere rimosse le assunzioni fatte sul rumore additivo :

può anche essere non gaussiano e non additivo.

PF in robotica : Definizioni

In robotica : Bel (xk) = p(xk | z1:k)In robotica, distinguiamo due tipi di dati:

dati percepiti (misure), indicati con ykdati del controllo, che portano informazioni sul movimento del robot, indicati con uk

Otteniamo dunque :Bel (xk) = p(xk | yk ,uk-1, yk-1 ,uk-2,…, u0, y0)

Da notare che:la misura più recente è indicata con ykil controllo più recente è indicata con uk-1

PF in robotica : PredizioneLa pdf a priori viene calcolata usando la (1):

1011011 ),...,|(),...,,|()( −−−−−− ∫= kkkkkkk dxyuxpyuxxpxBel

),|(),...,,|( 11011 −−−− = kkkkkk uxxpyuxxp

),|()( 1:11 −−− = kkkk zuxpxBeldove :

lo stato xk dipende solamente dallo stato xk-1

Essendo il processo Markoviano risulta :

101111 ),...,|(),|()( −−−−−− ∫= kkkkkkk dxyuxpuxxpxBel

L’espressione finale della pdf a priori diventa:

PF in robotica : AggiornamentoApplichiamo il teorema di Bayes :

),|(),...,|(),...,,|()(

1:11

0101

−−

−−=kkk

kkkkkk zuyp

yuxpyuxypxBel

Processo Markoviano : ),|(),...,,|( 11011 −−−− = kkkkkk uxxpyuxxp

PF e applicazioni in robotica

POSITION TRACKINGPosizione iniziale del robot è notaLa localizzazione avviene correggendo errori delle misure

ALTRI PROBLEMI:Global Localization Problem

Posizione iniziale del robot non è notaIl robot deve determinarla

Kidnapped Robot ProblemUn robot, la cui posizione iniziale è nota, è trasportato in un’altra posizione non nota

Multy-Robot Localization ProblemPiù robot collaborano per localizzare se stessi

RIFERIMENTI

Peter S. Maybeck, “Stochastic models, estimation, and control”, Vol. I, edited by Richard Bellman, University of Southern California,1979A.Gelb,F. Kasper,R.A. Nash, C.F.Price, A.A. Sutherland, “Applied optimalestimation”, The analytic sciences corporation, edited by Arthur Gelb, Massachusetts Institute of Technology,1996 M.Muhlich, T.Buçaresti “Particle Filters: an overview”, University of FrankfurtM.S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking,” IEEE Trans.Signal Processing, vol. 50, pp. 174-188, Feb. 2002.P.M. Djuric,J.H. Kotecha,J. Zhang,Y. Huang,T. Ghirmai, M.F. Bugallo e J.Miguez, “Particle filtering”E. Bolviken, G. Storvik, “Deterministic and Stochastic Particle Filters in State-Space Models”, Perceptual Interfaces and Reality LabUniversity of MarylandD. Fox, S.Thrun, W. Burgard,F. Dellaert “Particle Filters for Mobile RobotLocalization”

RIFERIMENTI

K. Copsey,“Tutorial on Particle Filters”, DERA MalvernT. Chen, J. Morris and E. Martin,“Particle Filters for the Estimation of a State Space Model”, Centre for Process Analytics and Control Technology School of ChemicalEngineering and Advanced Materials,University of Newcastle, NE1 7RU, UKS.Thrun,“Particle Filters in Robotics”, Computer Science Department, Carnegie MellonUniversity, Pittsburgh, PA 15213J. H. Kotecha and P. M. Djuric´, “Gaussian particle filtering,” IEEE Trans. SignalProcessing, vol. 51, pp. 2593-2602, Ott. 2003.R. Karlsson,“Various Topics on Angle-Only Tracking using Particle Filters” division of Communication Systems department of ElectricalEngineeringN.J. Gordon,D.J. Salmond,A.F.M. Smith,“Novel approach to nonlinear/non-Gaussian Bayesian state estimation”F. Dellaert, D. Fox, W. Burgard, and S. Thrun,“Monte carlo localization for mobile robots,” in Proc.IEEE Int. Conf. on Robotics and Autoniation, Detroit,Michigan, May 1999.