Introduzione sui Particle Filters nella robotica mobile Brugiotti.pdf · Dopo alcune iterazioni,...
Transcript of Introduzione sui Particle Filters nella robotica mobile Brugiotti.pdf · Dopo alcune iterazioni,...
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.