Appunti di filtraggio non-lineare - Dipartimento di Ingegneria dell

Appunti di filtraggio non-lineare
MAGGIO 2007
Indice
1 Filtraggio ricorsivo non lineare
1.1 Filtraggio ricorsivo Bayesiano . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.2 Filtro di Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.3 Filtro di Kalman Esteso (EKF) . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1
1
6
7
2 Filtro di Kalman Unscented (UKF)
2.1 Trasformazione Unscented . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2 L’algoritmo UKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
9
13
3 Particle Filter
3.1 Integrazione Monte Carlo (MCI) . . . . . . . . . . . . . . . . . . . . . .
3.2 Campionamento della funzione di importanza (IS) . . . . . . . . . . . .
3.3 Applicazione di MCI al filtraggio ricorsivo . . . . . . . . . . . . . . . . .
3.4 Campionamento sequenziale della funzione di importanza (SIS) . . . . .
3.5 Scelta della funzione di importanza . . . . . . . . . . . . . . . . . . . . .
3.6 Ricampionamento e algoritmo SIR (Sequential Importance Resampling)
16
16
18
19
21
24
25
II
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
Capitolo 1
Filtraggio ricorsivo non lineare
Il problema del filtraggio consiste nello stimare ricorsivamente lo stato di un sistema dinamico
utilizzando un insieme di misure rumorose. Un sistema dinamico tempo discreto è modellato
attraverso un’equazione di stato e un’equazione di uscita:
x(t + 1) = f (t, x(t), w(t))
(1.0.1)
y(t) = h(t, x(t), v(t))
dove x(t) è il vettore di stato, w(t) è il rumore di processo, y(t) è il vettore delle misure e v(t)
il rumore di misura. Nel caso in cui f (·) e h(·) siano funzioni lineari e i rumori di processo e
misura siano Gaussiani, la soluzione ottima del problema di filtraggio ricorsivo è data dal filtro di
Kalman (KF=Kalman Filter ). Nel caso non lineare non esiste in generale una soluzione ottima
in forma chiusa. Tuttavia, assumendo che i rumori di processo e di misura siano Gaussiani
e additivi, uno dei metodi classici per risolvere il problema di filtraggio non lineare è quello di
linearizzare il sistema, nell’intorno della stima corrente, ed applicare il filtro di Kalman al sistema
linearizzato; il filtro così ottenuto prende il nome di Filtro di Kalman Esteso (EKF=Extended
Kalman Filter ). Se le non linearità sono significative e/o i rumori non sono Gaussiani, EKF
può fornire prestazioni inadeguate e quindi si deve ricorrere ad altre soluzioni. Due di queste,
oggigiorno comunemente utilizzate, sono unscented Kalman filter ed il particle filter.
1.1
Filtraggio ricorsivo Bayesiano
Definendo con xt = {x(0), x(1), . . . , x(t)} la sequenza degli stati fino all’istante t, con yt =
{y(1), y(2), . . . , y(t)} la sequenza delle misure e osservando che, a causa dei rumori di processo
e misura, stato e misure sono variabili aleatorie, l’obiettivo è stimare l’evolvere dello stato x(t)
usando tutte le misure yt raccolte fino al tempo corrente t. Poiché x(t) è una variabile aleatoria,
tutte le informazioni fornite da yt sullo stato x(t) sono raccolte nella PDF a-posteriori
p(x(t)|yt )1
Quindi una tecnica di filtraggio ricorsivo deve fornire una stima di p(x(t)|yt ). La conoscenza
della PDF a-posteriori permette di determinare qualsiasi statistica di x(t), come ad esempio la
1
Si osserva che p(x(0)|y(0)) = p(x(0)) è la PDF a-priori dello stato iniziale.
1
CAPITOLO 1. FILTRAGGIO RICORSIVO NON LINEARE
media condizionata, che corrisponde alla stima MMSE:
Z
x̂(t) = x(t) p(x(t)|yt ) dx(t).
2
(1.1.1)
La differenza rispetto alla stima batch (non ricorsiva) è che si impone che la stima di p(x(t)|yt )
sia generata solo sulla base della stima precedente p(x(t − 1)|yt−1 ) e della misura corrente y(t),
cioè ricorsivamente.
La ricorsione consiste di due passi: aggiornamento temporale (predizione) e aggiornamento della
misura (correzione).
Predizione:
Data p(x(t − 1)|yt−1 ) si calcola p(x(t)|yt−1 ) tramite
Z
t−1
p(x(t)|x(t − 1), yt−1 ) p(x(t − 1)|yt−1 ) dx(t − 1)
p(x(t)|y ) =
(1.1.2)
x(t−1)
Aggiornamento della misura
Dati p(x(t)|yt−1 ) e y(t) si calcola p(x(t)|yt ) tramite
p(x(t)|yt ) =
p(y(t)|x(t), yt−1 ) p(x(t)|yt−1 )
p(y(t)|yt−1 )
(1.1.3)
Esaminando (1.1.2) e (1.1.3) si nota come non sia possibile in generale ottenere una stima ricorsiva, a causa dei termini p(x(t)|x(t−1), yt−1 ) e p(y(t)|x(t), yt−1 ) che richiedono la memorizzazione
di tutta la sequenza di misure yt−1 . Si osserva che anche il termine p(y(t)|yt−1 ) dipende dalle
misure passate, tuttavia tale termine può essere espresso come:
Z
t−1
p(y(t)|y ) =
p(y(t)|x(t), yt−1 ) p(x(t)|yt−1 ) dx(t)
(1.1.4)
cioè in funzione di p(y(t)|x(t), yt−1 ) e p(x(t)|yt−1 ). Quindi per poter realizzare uno stimatore
ricorsivo è necessario fare delle assunzioni sul sistema in modo da eliminare, in p(x(t)|x(t −
1), yt−1 ) e p(y(t)|x(t), yt−1 ), il condizionamento rispetto a yt−1 . In primo luogo si assume che
la sequenza di stato sia un processo di Markov, cioè:
p(x(t)|xt−1 ) = p(x(t)|x(t − 1)).
(1.1.5)
In secondo luogo si assume che le misure yt siano condizionatamente indipendenti dalla sequenza
degli stati, vale a dire:
t
Y
p(y(i)|x(i))
(1.1.6)
p(yt |xt ) =
i=1
Dalle due assunzioni (1.1.5)-(1.1.6) si deduce il seguente passo di aggiornamento temporale (predizione).
CAPITOLO 1. FILTRAGGIO RICORSIVO NON LINEARE
Predizione sotto le ipotesi (1.1.5) e (1.1.6)
Z
p(x(t)|x(t − 1)) p(x(t − 1)|yt−1 ) dx(t − 1)
p(x(t)|yt−1 ) =
3
(1.1.7)
x(t−1)
Dimostrazione - Per la definizione di probabilità marginale si ha
Z
p(x(t), xt−1 |yt−1 ) dxt−1
p(x(t)|yt−1 ) =
t−1
x
Z
p(yt−1 |xt ) p(xt )
=
dxt−1
p(yt−1 )
xt−1
(1.1.8)
Dall’assunzione di sistema Markoviano p(xt ) = p(x(t)|x(t − 1)) p(xt−1 ) e dall’ipotesi di misure
condizionatamente indipendenti è possibile eliminare il condizionamento delle misure rispetto a
x(t); infatti
Z
t−1
t
p yt |xt dy(t)
p y |x
=
y(t)
=
=
=
Z
t
Y
p (y(i)|x(i)) dy(t)
y(t) i=1
t−1
Y
p (y(i)|x(i))
i=1
t−1
Y
Z
p (y(t)|x(t)) dy(t)
(1.1.9)
y(t)
p (y(i)|x(i))
i=1
= p yt−1 |xt−1
Sostituendo (1.1.9) in (1.1.8), si ha
Z
p(yt−1 |xt−1 ) p(xt−1 ) p(x(t)|x(t − 1))
dxt−1
p(x(t)|yt−1 ) =
t−1 )
p(y
t−1
x
Z
Z
p(yt−1 |xt−1 ) p(xt−1 ) p(x(t)|x(t − 1))
=
dx(t − 1) dxt−2
p(yt−1 )
x(t−1) xt−2
Z
Z
p(xt−1 |yt−1 ) p(x(t)|x(t − 1)) dx(t − 1) dxt−2
=
x(t−1) xt−2
Z
Z
p(x(t − 1), xt−2 |yt−1 ) p(x(t)|x(t − 1)) dxt−2 dx(t − 1)
=
t−2
x(t−1) x
Z
p(x(t)|x(t − 1)) p(x(t − 1)|yt−1 ) dx(t − 1)
=
x(t−1)
(1.1.10)
Aggiornamento della misura sotto le ipotesi (1.1.5) e 1.1.6)
CAPITOLO 1. FILTRAGGIO RICORSIVO NON LINEARE
p(x(t)|yt ) =
p(y(t)|x(t)) p(x(t)|yt−1 )
p(y(t)|yt−1 )
Dimostrazione - Applicando la definizione di probabilità marginale e la regola di Bayes
Z
t
p(x(t), xt−1 |yt ) dxt−1
p(x(t)|y ) =
xt−1
Z
p(yt |xt ) p(xt )
dxt−1
=
t)
p(y
t−1
x
Z
p(y(t)|x(t)) p(yt−1 |xt−1 ) p(xt )
=
dxt−1
t)
p(y
t−1
x
Z
p(y(t)|x(t)) p(yt−1 |xt−1 ) p(x(t)|xt−1 ) p(xt−1 )
=
dxt−1
p(yt )
xt−1
Z
p(x(t)|xt−1 ) p(xt−1 |yt−1 )p(yt−1 ) dxt−1
xt−1
= p(y(t)|x(t))
p(y(t)|yt−1 ) p(yt−1 )
Z
p(x(t)|xt−1 ) p(xt−1 |yt−1 ) dxt−1
xt−1
= p(y(t)|x(t))
p(y(t)|yt−1 )
Z
p(x(t)|xt−1 , yt−1 ) p(xt−1 |yt−1 ) dxt−1
t−1
= p(y(t)|x(t)) x
p(y(t)|yt−1 )
p(y(t)|x(t)) p(x(t)|yt−1 )
=
p(y(t)|yt−1 )
4
(1.1.11)
si ha
(1.1.12)
(1.1.13)
(1.1.14)
(1.1.15)
(1.1.16)
(1.1.17)
(1.1.18)
dove (1.1.13) segue dall’ipotesi di misure condizionatamente indipendenti dalla sequenza degli
stati e in (1.1.16) si è sfruttato il fatto che
p x(t)|xt−1 , yt−1 p yt−1 |xt−1
t−1
= p x(t)|xt−1 , yt−1
p x(t)|x
=
t−1
t
p (y |x )
avendo precedentemente dimostrato in (1.1.9) che p(yt−1 |xt ) = p(yt−1 |xt−1 ).
Confrontando le formule (1.1.2) e (1.1.3) con (1.1.7) e (1.1.11) si derivano, nell’ipotesi di
sistema Markoviano e misure condizionatamente indipendenti, le uguaglianze seguenti:
p(x(t)|x(t − 1), yt−1 ) = p(x(t)|x(t − 1))
p(y(t)|x(t), y
t−1
) = p(y(t)|x(t))
(1.1.19)
(1.1.20)
Riassumendo, quindi, il filtraggio Bayesiano è esprimibile in forma ricorsiva se il sistema è Markoviano e le misure sono condizionatamente indipendenti. Si può verificare che il sistema dinamico
(1.0.1) è Markoviano e ha misure condizionatamente indipendenti se le seguenti condizioni sono
soddisfatte:
CAPITOLO 1. FILTRAGGIO RICORSIVO NON LINEARE
5
wt è una sequenza di variabili aleatorie indipendenti;
• vt è una sequenza di variabili aleatorie indipendenti;
• wt , vt e x(0) sono mutuamente indipendenti.
•
Tuttavia, le equazioni (1.1.7) e (1.1.11) forniscono solo una soluzione concettuale al problema del
filtraggio ricorsivo; infatti, l’integrale in (1.1.7) e l’integrazione (1.1.4) necessaria per calcolare
il denominatore di (1.1.11) non hanno in generale soluzione in forma chiusa. In generale non è
neanche possibile esprimere p(x(t)|x(t−1)) e p(y(t)|x(t)) in forma chiusa, a meno che le equazioni
di stato e di misura del sistema (1.0.1) soddisfino le condizioni riportate nel seguente teorema [3].
Teorema - Siano x e y vettori aleatori della stessa dimensione tali che y = f (x). Supponendo
che f −1 (·) esiste, allora:
−1
∂f
(y)
−1
py (y) = px f (y) det
(1.1.21)
∂y
dove f −1 (·) è la funzione inversa di f (·).
Tramite (1.1.21) è possibile calcolare p(x(t)|x(t − 1)) e p(y(t)|x(t)) per il sistema (1.0.1), se
le funzioni f (t, x(t), ·) ed h(t, x(t), ·) sono invertibili rispetto a w(t) e v(t) rispettivamente. Sotto
queste ipotesi, da (1.1.21) segue che:
−1 ∂fw (·) −1
p(x(t + 1)|x(t)) = pw(t) fw (t, x(t), x(t + 1)) det
(1.1.22)
∂x(t)
−1 ∂hv (·) −1
(1.1.23)
p(y(t)|x(t)) = pv(t) hv (t, x(t), y(t)) det
∂y(t)
Esempio - La probabilità di transizione è esprimibile nella forma (1.1.22) nel caso in cui in
(1.0.1) i rumori di processo e misura siano additivi, cioè il sistema dinamico sia descritto dalle
equazioni seguenti:
x(t + 1) = f (t, x(t)) + w(t)
(1.1.24)
y(t) = h(t, x(t)) + v(t)
In tal caso
p(x(t)|x(t − 1)) = pw(t−1) (x(t) − f (t, x(t − 1)))
p(y(t)|x(t)) = pv(t) (y(t) − h(t, x(t)))
(1.1.25)
(1.1.26)
Nel filtraggio ricorsivo non solo si vuole calcolare una stima dello stato del sistema dinamico, ma
si vuole anche quantificare l’incertezza associata a tale stima tramite la matrice di covarianza
P(t) = E[x̂(t) − x(t)][x̂(t) − x(t)]′
Dato che non si conosce lo stato vero x(t), per calcolare questa incertezza si deve stimare anche
la matrice di covarianza.
Calcolate le stime x̂(t) e P̂(t) si richiede che queste soddisfino le seguenti proprietà.
CAPITOLO 1. FILTRAGGIO RICORSIVO NON LINEARE
6
Assenza di polarizzazione: una stima è non polarizzata se il suo valore atteso è uguale al
valore vero.
Efficienza: uno stimatore è efficiente se possiede la più piccola matrice di covarianza tra tutti
gli stimatori non polarizzati.
Consistenza: una stima è consistente se converge al valore vero al crescere del numero di
osservazioni disponibili.
Robusto: uno stimatore è robusto se è poco sensibile agli errori di misura e alle incertezze di
modellizzazione.
La stima della matrice di covarianza è consistente se, oltre a convergere alla covarianza vera,
soddisfa la disuguaglianza matriciale P̂(t) ≥ P(t), cioè la covarianza non deve essere mai sottostimata. Nei capitoli seguenti vengono presentati due metodi per risolvere il problema della
stima ricorsiva, che seguono rispettivamente un approccio analitico e un approccio numerico.
1.2
Filtro di Kalman
Il problema del filtraggio si riduce alla stima della PDF condizionata dello stato rispetto alle
misure. Nell’ipotesi di sistema (1.0.1) lineare e Gaussiano, la PDF condizionata è una Gaussiana
e quindi completamente caratterizzata dai suoi primi due momenti: media e covarianza. In
questo caso la stima della PDF condizionata equivale alla stima della sua media e covarianza e
il filtro di Kalman fornisce proprio le equazioni che permettono di stimare queste due quantità
e propagare le stime nel tempo. Inoltre il filtro di Kalman è ottimale, in quanto, nell’ipotesi di
sistema lineare e Gaussiano, le stime coincidono con media e covarianza condizionata e quindi
sono ottimali nel senso del minimo errore quadratico medio (MMSE). Si consideri, quindi, il caso
in cui il sistema (1.0.1) si riduce ad un sistema lineare tempo discreto:
x(t + 1) = A(t) x(t) + w(t)
(1.2.1)
con w(t) rumore di processo, bianco, gaussiano, a media nulla e di covarianza:
E[w(t)w′ (t)] = Q(t).
(1.2.2)
y(t) = C(t) x(t) + v(t)
(1.2.3)
L’equazione di misura è
dove v(t) è il rumore di misura, bianco, gaussiano, a media nulla e di covarianza
E[v(t)v′ (t)] = R(t)
(1.2.4)
Si suppone che le matrici A(t), C(t), Q(t), R(t) siano note e che lo stato iniziale x(0), non
noto, sia modellabile come una variabile aleatoria gaussiana, con media e covarianza note. Si
assume inoltre che i due disturbi (di processo e di misura) e lo stato iniziale siano mutuamente
CAPITOLO 1. FILTRAGGIO RICORSIVO NON LINEARE
7
indipendenti. Sotto queste ipotesi esiste una soluzione ricorsiva al problema di filtraggio e tale
soluzione è data dal filtro di Kalman. Le ben note equazioni del filtro di Kalman sono:

ỹ(t) = y(t) − C(t)x̂(t|t − 1)
innovazione



′

S(t) = C(t)P(t|t − 1)C (t) + R(t)
covarianza dell’innovazione



′ (t)S−1 (t)

L(t)
=
P(t|t
−
1)C
guadagno



(1.2.5)
x̂(t|t) = x̂(t|t − 1) + L(t)ỹ(t)
stima filtrata





P(t|t) = P(t|t − 1) − L(t)S(t)L′ (t) covarianza della stima filtrata




x̂(t + 1|t) = A(t)x̂(t|t)
stima predittiva


P(t + 1|t) = A(t)P(t|t)A′ (t) + Q(t) covarianza della stima predittiva
Le equazioni vengono inizializzate con x̂(0|0) e P(0|0), media e covarianza a-priori di x(0). Si
noti che nel caso in cui le matrici A(t) = A, C(t) = C, Q(t) = Q e R(t) = R sono tempo
invarianti, il filtro ammette (sotto certe ipotesi) una soluzione di regime K(t) = K e P(t) = P.
Si osserva anche che, nel caso in cui non sia valida l’ipotesi di gaussianità per lo stato iniziale e/o
i disturbi, la stima fornita dal filtro di Kalman non è quella ottimale a minimo errore quadratico
medio (MMSE), ma è solo la migliore stima lineare non polarizzata MMSE. Per maggiori
dettagli si consulti [?].
1.3
Filtro di Kalman Esteso (EKF)
Nel caso generale (1.0.1), la soluzione del problema del filtraggio è infinito-dimensionale, cioè si
devono conoscere tutti i momenti della PDF condizionata per poterla caratterizzare completamente. Quindi, escluso alcuni casi particolari (fra i quali è compreso anche il filtro di Kalman),
non è possibile ottenere una soluzione esatta al problema del filtraggio e si deve ricorrere a soluzioni approssimate. Il metodo di linearizzazione, sul quale si basa il filtro di Kalman Esteso
(EKF), è la tecnica più usata per trovare una soluzione approssimata al problema del filtraggio. L’idea è quella di approssimare le funzioni nonlineari f (·) e h(·) in (1.0.1) mediante uno
sviluppo in serie di Taylor al primo ordine (o ad ordini superiori se si vuole ottenere una migliore
approssimazione) nell’intorno della stima corrente. A seguito della linearizzazione, il sistema
(1.0.1) diventa lineare e ipotizzando che la PDF condizionata sia Gaussiana, si può applicare
il filtro di Kalman al sistema linearizzato, calcolando media e covarianza della PDF. La stima
fornita dal filtro EKF, in generale, è polarizzata e inconsistente, e ha buone prestazioni solo se
le approssimazioni fatte (linearizzazione e Gaussianità) sono ragionevoli.
Si consideri, quindi, il sistema
x(t + 1) = f (t, x(t)) + w(t)
(1.3.1)
y(t) = h(t, x(t)) + v(t)
con f (t, x) e h(t, x) funzioni non lineari rispetto a x. Per i disturbi si suppongono valide le
assunzioni precedenti. Per risolvere il problema in questo caso, si linearizzano le due funzioni
f (t, x) ed h(t, x) rispettivamente nell’intorno dei punti x = x̂(t|t) e x = x̂(t|t − 1). Si definiscono
CAPITOLO 1. FILTRAGGIO RICORSIVO NON LINEARE
quindi le matrici Jacobiane

∂f (·)


A(t) =


∂x x=x̂(t|t)

∂h(·)


 C(t) =
∂x x=x̂(t|t−1)
e si approssimano al primo ordine le suddette funzioni nel seguente modo:
f (t, x(t)) ∼
= f (t, x̂(t|t)) + A(t) [x(t) − x̂(t|t)]
h(t, x(t)) ∼
= h(t, x̂(t|t − 1)) + C(t) [x(t) − x̂(t|t − 1)]
8
(1.3.2)
(1.3.3)
Utilizzando tali approssimazioni, il sistema è diventato lineare per cui si può applicare il filtro di
Kalman, ottenendo le seguenti ricorsioni:

S(t) = C(t)P(t|t − 1)C′ (t) + R(t)



 L(t) = P(t|t − 1)C′ (t)S−1 (t)




ỹ(t) = y(t) − h(t, x̂(t|t − 1))



(1.3.4)
x̂(t|t) = x̂(t|t − 1) + L(t)ỹ(t)





P(t|t) = P(t|t − 1) − L(t)S(t)L′ (t)




x̂(t + 1|t) = f (t, x̂(t|t))


P(t + 1|t) = A(t)P(t|t)A′ (t) + Q(t)
Si osserva che nel filtro di Kalman esteso, oltre agli errori di processo e di misura, sono presenti
errori di linearizzazione, che non possono essere quantificati e di cui non si tiene conto nel valutare
la covarianza dell’errore di stima. Pertanto non vi sono garanzie sulla qualità delle stime ottenute
ed il filtro di Kalman esteso risulta molto sensibile all’accuratezza delle stime iniziali.
Capitolo 2
Filtro di Kalman Unscented (UKF)
Il filtro EKF assume che la PDF p(x(t−1)|yt−1 ) sia Gaussiana e propaga media e covarianza della
distribuzione attraverso il sistema non lineare semplicemente linearizzandolo e quindi applicando
le equazioni del filtro di Kalman al sistema linearizzato per ottenere p(x(t)|yt ). Al contrario di
EKF, UKF non approssima il sistema non lineare, ma approssima la distribuzione. UKF assume
sempre che p(x(t − 1)|yt−1 ) sia Gaussiana tuttavia, anzichè ricorrere alla linearizzazione, per
propagare la media e la covarianza della distribuzione, utilizza la trasfomazione Unscented.
2.1
Trasformazione Unscented
Sia x una variabile aleatoria con media x e covarianza Px , l’obiettivo è calcolare media e covarianza della variabile aleatoria y = f (x), con f (·) arbitraria funzione non lineare (ad esempio nel
filtraggio x = x̂(t − 1|t − 1) e y = x̂(t|t − 1)). Sviluppando in serie di Taylor la funzione f (·)
rispetto a x, considerando per semplicità x e y scalari, si ottiene:
1
1
y = f (x) = f (x + δx) = f (x) + ∇f δx + ∇2 f δx2 + ∇3 f δx3 + · · ·
2
6
(2.1.1)
dove ∇i f è la derivata i-esima di f calcolata in x, δx è una variabile aleatoria gaussiana con
media nulla e covarianza Px . Calcolando media e covarianza di (2.1.1) si ha:
1
1
y = f (x) + ∇2 f Px + ∇4 f E[δx4 ] + . . .
2
4!
2 1
1
2
2
∇ f
E[δx4 ] − Px2 +
∇f ∇3 f E δx4 + · · ·
Py = (∇f ) Px +
4
3
(2.1.2)
Dalle equazioni precedenti si può notare che l’n-esimo termine dello sviluppo in serie di y è
funzione dell’n-esimo momento della variabile aleatoria x moltiplicato per la derivata n-esima di
f (·) calcolata in x. Pertanto se si calcolano i momenti di x fino all’ordine n si può determinare
esattamente y fino allo stesso ordine dello sviluppo di Taylor. Un discorso analogo vale per Py . I
metodi di linearizzazione (tipo EKF) assumono che i contributi dei termini di secondo ordine e di
9
CAPITOLO 2. FILTRO DI KALMAN UNSCENTED (UKF)
10
ordine superiore in (2.1.1) siano trascurabili; sotto questa ipotesi le equazioni (2.1.2) diventano:
y = f (x)
Py = ∇f 2 Px
(2.1.3)
Tuttavia, se i termini di ordine superiore al primo sono significativi, la trasformazione (2.1.3)
può essere polarizzata e inconsistente. In questo caso si deve ricorrere ad un metodo di approssimazione migliore della linearizzazione.
La trasformazione Unscented si basa sull’idea che è più facile approssimare una distribuzione
di probabilità piuttosto che approssimare un’arbitraria funzione non lineare. In pratica vengono
generati dei campioni (punti sigma) xi nello spazio degli stati in modo da catturare certe proprietà
della distribuzione di x. Questi campioni, a cui sono associati dei pesi wi , vengono poi propagati
attraverso la funzione non lineare f (·), ottenendo in tal modo dei campioni trasformati dei quali
si calcolano le statistiche per ottenere y e Py .
I passi della trasformazione Unscented sono quindi:
1. Generazione dei punti sigma, σ = {xi , wi : i = 0, 1, . . . , l − 1};
2. Propagazione dei punti sigma attraverso la funzione non lineare
yi = f (xi )
3. Calcolo delle statistiche dei campioni trasformati:
y =
Py =
l−1
X
i=0
l−1
X
i=0
wi y i
wi (yi − y)(yi − y)′
(2.1.4)
Quello che distingue la trasformazione Unscented dai metodi Monte Carlo è che il numero l
dei punti sigma, la loro locazione nello spazio degli stati nonché i pesi associati sono scelti
deterministicamente in modo da catturare le proprietà statistiche più importanti della variabile
aleatoria x. In particolare, come mostrato in (2.1.2), se si riescono a catturare i momenti di x
fino all’ordine n, allora y e Py sono accurati fino al termine n-esimo dello sviluppo di Taylor. A
tale proposito, si osserva che non è facile catturare i momenti di x per distribuzioni generiche.
Tuttavia, se si assume per x una distribuzione Gaussiana, è possibile determinare i punti sigma
garantendo un’accuratezza fino ad almeno il secondo ordine. La distribuzione Gaussiana soddisfa
le seguenti due proprietà:
1. simmetria ⇒ i punti xi devono essere simmetrici (rispetto al valor medio);
CAPITOLO 2. FILTRO DI KALMAN UNSCENTED (UKF)
11
2. una variabile aleatoria x con media x e covarianza Px , può essere sempre scritta nella
forma
x = x + Cz
dove z è una variabile aleatoria Gaussiana standard (con media nulla e covarianza unitaria)
e CC′ = Px . La matrice C può essere ottenuta ad esempio tramite una decomposizione
di Cholesky di Px .
I punti sigma sono quindi {(xi = x + Czi , wi ) : i = 0, 1, . . . , l − 1}. L’obiettivo adesso è generare
i campioni zi della variabile aleatoria z in modo da catturarne i primi due momenti, cioè imporre
E [z] =
l−1
X
wi z i
= 0
i=0
Cov (z) =
l−1
X
(2.1.5)
wi zi z′i = I
i=0
Esempio 2.1 - Nel caso bi-dimensionale sono sufficienti 5 punti sigma per catturare i primi due
momenti di z, come è illustrato in figura 2.1. I punti sono collocati sugli assi e sono simmetrici
Figura 2.1: Disposizione dei punti sigma
rispetto all’origine. Con una scelta opportuna dei parametri w0 , w1 , s1 si riescono ad assegnare
CAPITOLO 2. FILTRO DI KALMAN UNSCENTED (UKF)
12
media e covarianza di z. Per la media si osserva che, indipendentemente dalla scelta dei pesi, si
ha
w0 z 0 + w1 z 1 + w1 z 2 + w1 z 3 + w1 z 4 = 0
P
data la simmetria dei punti rispetto all’origine.PImponendo la condizione sulla covarianza wi zi z′i =
I e la condizione di normalizzazione dei pesi
wi = 1, si ottengono le relazioni:
2 w1 s21 = 1
w0 + 2n w1 = 1
(2.1.6)
dove n = 2 è la dimensione di x. Questo è un sistema di due equazioni in tre incognite ed è
quindi indeterminato. Fissando una delle tre incognite w1 = 0.5(n + k)−1 , dove k è un parametro
il cui significato sarà spiegato successivamente, la soluzione del sistema è fornita da
n
k
w0 = 1 −
m =
n+k
n+k
√
n+k
s1 =
(2.1.7)
(2.1.8)
Il risultato ottenuto nell’esempio è generalizzabile al caso n-dimensionale. In particolare si può
verificare che la trasformazione Unscented, utilizzando 2n + 1 punti di cui uno collocato sull’origine degli assi e gli altri 2n disposti simmetricamente lungo gli assi coordinati, riesce ad
approssimare i momenti di una variabile aleatoria Gaussiana in forma standard sino al secondo
ordine. Dalla relazione x = x + Cz si ottengono i punti sigma associati alla variabile aleatoria x:
x0
= x
xi
= x +
xi+n = x −
w0
=
=
√
n + k Ci
wi
√
n + k Ci
wi+n =
k
n+k
1
2(n + k)
1
2(n + k)
(2.1.9)
per i = 1, 2, . . . , n, dove Ci indica la i-esima colonna della matrice C. La scelta del parametro
k è un grado di libertà della trasformazione Unscented. Scegliendo k + n = 3 si minimizza la
differenza tra il quarto momento di x e quello calcolato tramite i punti sigma.
La trasformazione Unscented riesce a catturare correttamente i momenti di x fino al secondo
ordine, senza dover calcolare né lo Jacobiano né l’Hessiano della funzione f (·). L’approssimazione
è quindi migliore di quella ottenibile tramite linearizzazione.
Esempio 2.2 - Si consideri la funzione non lineare y = x2 con x ∼ N (x, σx2 ). Effettuando
uno sviluppo in serie della funzione rispetto a x si ha:
y = x2 = (x + δx)2 = x2 + 2 x δx + δx2
con δx ∼ N (0, σx2 ). Calcolando il valore atteso e la varianza di y si ottiene:
y = E[y]
= x2 + σx2
2
2
σy = E[(y − y) ] = 2 σx4 + 4 x2 σx2
CAPITOLO 2. FILTRO DI KALMAN UNSCENTED (UKF)
13
Questi sono i valori esatti della media e della varianza di y. Ricorrendo alla linearizzazione, la
relazione y = x2 può essere approssimata nel modo seguente:
ŷ ≈ x2 + 2 x δx
da cui, calcolando media e varianza, si ha:
ŷ = E[ŷ]
= x2
2
2
σ̂y = E[(ŷ − ŷ) ] = 4 x2 σx2
Confrontando i valori approssimati con quelli esatti si nota che la stima ottenuta tramite linearizzazione è polarizzata (y 6= ŷ) e inconsistente (σ̂y < σy ). La trasformazione Unscented opera
invece nel seguente modo:
• genera 2n + 1 = 3 punti sigma
x0 = x
con σ =
p
x1 = x + σ
x2 = x − σ
(1 + k)σx2 ;
• propaga i punti attraverso la non linearità
y0 = x2 ,
w0 =
y1 = (x + σ)2 ,
w1 =
y2 = (x − σ)2 ,
w2 =
k
1+k
1
2(1 + k)
1
;
2(1 + k)
• calcola media e covarianza dei punti sigma trasformati
P
= x2 + σx2
ŷ = Pi yi wi
2
4
2 2
2
σ̂y =
i (yi − y) wi = k σx + 4 x σx
In questo caso la stima non è polarizzata (y = ŷ) e, scegliendo k = 3 − n = 2, è anche consistente
ed efficiente (σ̂y = σy ).
2.2
L’algoritmo UKF
Il filtro UKF è la diretta applicazione della trasformazione Unscented al problema della stima
ricorsiva a minimo errore quadratico medio. Nel sistema non lineare (1.0.1) le due funzioni f (·) e
h(·) dipendono, oltre che dallo stato, anche dai rumori di processo e, rispettivamente, di misura;
quindi, per poter applicare la trasformazione Unscented, è necessario definire lo stato esteso
xa (t) = [x′ (t), w′ (t), v′ (t)]′ ∈ IRna . I passi del filtro UKF sono:
CAPITOLO 2. FILTRO DI KALMAN UNSCENTED (UKF)
14
1. Inizializzazione:
Dati:
x0
=
E[x(0)]
P0
=
E[(x(0) − x0 )(x(0) − x0 )′ ]
xa (0)
=
E[xa (0)] = [x′0 , 0, 0]′
Pa (0)
=
si pone:

P0 0 0
E[(xa (0) − xa (0))(xa (0) − xa (0))′ ] =  0 Q 0 
0 0 R

Ca (0) t.c. Ca (0)Ca′ (0) = Pa (0)
2. Per t = 1, 2, . . . ,
(a) Calcola i punti sigma:
χa0 (t − 1) = xa (t − 1)
p
na + k Cai (t − 1) per i = 1, 2, . . . , na
p
χai+na (t − 1) = xa (t − 1) − na + k Cai (t − 1) per i = 1, 2, . . . , na
χai (t − 1) = xa (t − 1) +
(b) Predizione:
χxi (t|t − 1) = f (χxi (t − 1), χw
i (t − 1)) per i = 0, 1, . . . , 2na
2na
X
x(t|t − 1) =
wi χxi (t|t − 1)
P(t|t − 1) =
i=0
2n
a
X
i=0
wi [χxi (t|t − 1) − x(t|t − 1)] [χxi (t|t − 1) − x(t|t − 1)]′
yi (t|t − 1) = h(χxi (t|t − 1), χvi (t|t − 1)) per i = 0, 1, . . . , 2na
2na
X
y(t|t − 1) =
wi yi (t|t − 1)
i=0
CAPITOLO 2. FILTRO DI KALMAN UNSCENTED (UKF)
15
(c) Aggiornamento della misura:
S(t)
Pxy (t)
=
=
2na
X
i=0
2n
a
X
i=0
L(t)
=
x(t)
=
P(t)
=
Pa (t)
=
wi [yi (t|t − 1) − y(t|t − 1)] [yi (t|t − 1) − y(t|t − 1)]′
wi [χxi (t|t − 1) − x(t|t − 1)] [yi (t|t − 1) − y(t|t − 1)]′
Pxy (t) S−1 (t)
x(t|t − 1) + L(t) (y(t) − y(t|t − 1))
P(t|t − 1) − L(t) S(t) L′ (t)




x(t)
P(t) 0 0
 0
Q 0  , xa (t)  0 
0
0
0 R
Calcola Ca (t) t.c. Ca (t)Ca′ (t) = Pa (t)
dove χa = [χx′ , χw′ , χv′ ]′ , Q è la covarianza del rumore di processo, R è la covarianza del
rumore di misura, na = nx + nw + nv è la dimensione dello stato esteso, ed i pesi sono dati da
w0 =
wi =
k
na + k
1
2(na + k)
i = 1, 2, . . . , 2na
Si osserva, infine, che nell’ipotesi di rumori di processo e misura additivi (1.1.24), le equazioni del
filtro UKF si semplificano in quanto non è necessario introdurre lo stato esteso (in questo caso la
covarianza del rumore di processo/misura si somma, come nel filtro di Kalman, alla covarianza di
stato/misura). In questo caso l’algoritmo precedentemente esposto, oltre ad operare nello stato
x di dimensione nx anziché nello stato esteso xa di dimensione na , si modifica nelle seguenti
equazioni di aggiornamento di covarianze:
P(t|t − 1) =
S(t) =
2nx
X
i=0
2nx
X
i=0
wi [χi (t|t − 1) − x(t|t − 1)] [χi (t|t − 1) − x(t|t − 1)]′ + Q(t − 1)
wi [yi (t|t − 1) − y(t|t − 1)] [yi (t|t − 1) − y(t|t − 1)]′ + R(t)
Riassumendo, le principali caratteristiche del filtro UKF sono le seguenti:
1. per implementare il filtro non è necessario calcolare lo Jacobiano (o l’Hessiano) delle
funzioni f (·) e h(·);
2. il numero dei punti sigma dipende linearmente dalla dimensione dello stato, quindi il carico
computazionale risulta contenuto.
Capitolo 3
Particle Filter
Il filtro UKF usa la trasformazione Unscented per costruire, in modo deterministico, 2n + 1
campioni con i quali approssimare numericamente i momenti della PDF a-posteriori, che è assunta
Gaussiana. Un approccio alternativo è quello di utilizzare metodi di integrazione numerica Monte
Carlo per il calcolo di integrali multi-dimensionali e quindi dei momenti della PDF a-posteriori.
Confrontando questi due metodi numerici si possono notare delle somiglianze anche se vi sono
sostanziali differenze. Infatti, mentre nella trasformazione Unscented il numero e la posizione
dei campioni sono scelti in modo deterministico, per catturare i primi due momenti della PDF
(assunta Gaussiana), nei metodi Monte Carlo i campioni sono posizionati casualmente ed il loro
numero M è un parametro di progetto. Nell’ipotesi di PDF a-posteriori Gaussiana e a parità di
numero di campioni M = 2n + 1, è logico aspettarsi che la trasformazione Unscented fornisca
una approssimazione migliore dei metodi Monte Carlo, in quanto i campioni sono posizionati
“ad hoc”, in modo deterministico, per catturare i primi due momenti della PDF. Tuttavia, per
i Metodi Monte Carlo la legge forte dei grandi numeri assicura che per M → ∞ la PDF può
essere approssimata con arbitrario grado di accuratezza. L’aspetto più interessante dei metodi
Monte Carlo, rispetto alla trasformazione Unscented, è la possibilità di rimuovere l’ipotesi di
Gaussianità della PDF a-posteriori. Pertanto i metodi Monte Carlo sono applicabili anche a
sistemi non lineari e rumori non Gaussiani. Di seguito si richiamano gli elementi esenziali dei
metodi Monte Carlo e, successivamente, la loro applicazione al filtraggio ricorsivo, che dà luogo
al cosiddetto Particle Filter (PF).
3.1
Integrazione Monte Carlo (MCI)
La maggior parte degli integrali non possono essere calcolati analiticamente, per questo si cerca
di approssimarne la soluzione utilizzando metodi numerici. Per calcolare numericamente gli
integrali esistono molti metodi, fra i quali il più noto è quello della somma di Riemann. Si
supponga di voler approssimare l’integrale seguente (caso scalare):
I[a,b] =
Z
b
g(x) p(x) dx
a
16
(3.1.1)
CAPITOLO 3. PARTICLE FILTER
17
R
dove p(x) è tale che p(x) dx = 1.1 La soluzione di Riemann è quella di troncare la funzione
integranda nell’intervallo [a, b], dividere tale intervallo in M sotto-intervalli
[x0 , x1 ], [x1 , x2 ], [x2 , x3 ], ..., [xM −1 , xM ] e approssimare l’integrale attraverso la somma:
IM =
M
X
i=1
g(xi ) p(xi ) (xi − xi−1 )
(3.1.2)
Per M → ∞ risulta che IM → I[a,b] . Questo metodo può essere esteso al caso multi-dimensionale
anche se la sua efficienza peggiora, in quanto la convergenza ha ordine O(M −1/n ), dove n è la
dimensione di x, per cui all’aumentare di n la convergenza è più lenta.
Il metodo MCM opera in modo diverso approssimando l’integrale tramite la somma:
IM
M
1 X
=
g(x(i) )
M
(3.1.3)
i=1
dove x(i) : i = 1, ..., M sono dei punti campionati in modo indipendente dalla PDF p(x). Per
esempio l’integrale
Z
Z
1
x2
2
I = x N (x; 0, σ ) dx = x √
(3.1.4)
exp − 2 dx
2σ
2πσ 2
che corrisponde alla media di una distribuzione Gaussiana, è approssimato tramite
IM
M
1 X (i)
x
=
M
(3.1.5)
i=1
dove x(i) : i = 1, ..., M sono realizzazioni della variabile aleatoria gaussiana x avente distribuzione
x2
1
2 △
exp − 2 .
N (x; 0, σ ) = √
2σ
2πσ 2
In pratica utilizzando MCM, in questo caso, si è approssimata la media statistica (valore atteso)
con la media campionaria.
La validità dell’approssimazione è garantita dalla legge forte dei grandi numeri, la quale afferma
che la media di molte variabili aleatorie indipendenti con media comune e varianza finita converge
alla loro media comune:
lim IM = I con probabilità 1
(3.1.6)
M →∞
Inoltre, se la varianza
σg2
della funzione g(x) rispetto alla PDF p(x) è finita, vale il limite:
√
1
M (IM − I) → N (0, σg2 )
(3.1.7)
La funzione p(x) ha il significato di densità di probabilità. La scelta di questa particolare funzione integranda
g(x) p(x) consegue dal fatto che l’obiettivo, in questo capitolo, è quello di usare i metodi di integrazione numerica
per calcolare la stima MMSE; ovviamente i metodi esposti sono applicabili ad una generica funzione integranda
f (x).
CAPITOLO 3. PARTICLE FILTER
18
cioè il tasso di convergenza dell’errore di approssimazione è O(M −1/2 ) ed è quindi indipendente
dalla dimensione n dell’integrale. L’approccio MCM è, quindi, preferibile rispetto alla somma
di Riemann nel caso di integrali multi-dimensionali; si fa notare comunque che la convergenza
dell’MCM dipende, oltre che da M −1/2 , anche da σg2 che influenza l’errore di approssimazione.
Il punto critico del metodo MCM è il campionamento dalla distribuzione p(x), che in generale
sarà multi-dimensionale e non-standard. Per superare il problema del campionamento dalla
PDF p(x) vi sono vari metodi, che si differenziano a seconda delle informazioni disponibili su
p(x) [5]. Nel paragrafo seguente verrà presentato il metodo di campionamento della funzione di
importanza (IS= Importance Sampling).
3.2
Campionamento della funzione di importanza (IS)
Si supponga di riuscire a scegliere una funzione di importanza q(x) in modo che il suo supporto
contenga quello della PDF p(x), allora si può esprimere l’integrale nella forma seguente [4]:
Z
Z
p(x)
I = g(x) p(x) dx = g(x)
q(x) dx
(3.2.1)
q(x)
Applicando la (3.1.2), questo integrale può essere calcolato numericamente attraverso la somma:
IM
M
1 X
=
g(x(i) ) w∗ (x(i) )
M
(3.2.2)
i=1
dove i campioni x(i) sono ottenuti campionando la distribuzione q(x) e i pesi di importanza w∗
(importance weights) sono per definizione:
w∗ (x(i) ) =
p(x(i) )
q(x(i) )
(3.2.3)
In questo modo, utilizzando una PDF q(x) facilmente campionabile, si riesce a calcolare l’integrale. Si osserva che l’ipotesi che il supporto di q(x) contenga quello di p(x) è necessaria affinchè
l’intero spazio dello stato sia raggiunto dai campioni prodotti da q(x); questa è una condizione
necessaria e sufficiente per la convergenza dell’approssimazione MCM al valore vero dell’integrale.
Utilizzando il metodo Monte Carlo per calcolare la stima MMSE si deve risolvere l’integrale:
Z +∞
x p(x|z) dx
(3.2.4)
x̂MMSE =
−∞
In questo caso i pesi w∗ sono uguali a:
w∗ (x(i) ) =
p(x(i) |z)
p(z|x(i) ) p(x(i) )
=
q(x(i) )
p(z) q(x(i) )
dove
p(z) =
Z
p(z|x) p(x) dx
(3.2.5)
CAPITOLO 3. PARTICLE FILTER
19
Il problema è che p(z) non è noto quindi non si possono calcolare i pesi w∗ (x(i) ). Comunque si
osserva che l’integrale I può essere riscritto nella forma
Z
1
p(z|x) p(x)
I=
g(x)
q(x) dx
(3.2.6)
p(z)
q(x)
in quanto p(z) non dipende da x e quindi può essere portato fuori dall’integrale. Esplicitando
l’espressione per p(z) si ha:
Z
p(z|x) p(x)
q(x) dx
g(x)
q(x)
Z
(3.2.7)
I=
p(z|x) p(x) dx
L’idea è quella di applicare MCM anche all’integrale a denominatore. Si ottiene così :
Z
p(z|x) p(x)
g(x)
q(x) dx
q(x)
Z
I=
p(z|x) p(x)
q(x) dx
q(x)
(3.2.8)
e, quindi, passando dall’integrale alla sua approssimazione MCM, si ottiene
IM
dove
1 PM
(i)
(i)
M
X
i=1 g(x ) w(x )
M
=
g(x(i) ) w̄(x(i) )
=
1 PM
i=1
w(x(i) )
M i=1
w(x(i) ) =
e
sono i pesi normalizzati.
3.3
p(z|x(i) ) p(x(i) )
q(x(i) )
w(x(i) )
w̄(x(i) ) = PM
(i)
i=1 w(x )
(3.2.9)
(3.2.10)
(3.2.11)
Applicazione di MCI al filtraggio ricorsivo
Sia p(xt |yt ) la PDF a-posteriori della sequenza degli stati xt = {x(0), x(1), . . . , x(t)} condizionata alla sequenza delle misure yt = {y(1), y(2), . . . , y(t)}. L’obiettivo è stimare ricorsivamente la
probabilità condizionata p(xt |yt ), la distribuzione marginale del filtraggio p(x(t)|yt ) ed i relativi
momenti ottenuti mediante il valore atteso
Z
I(g) = g(xt ) p(xt |yt ) dxt
(3.3.1)
CAPITOLO 3. PARTICLE FILTER
20
per opportune funzioni integrabili g(·). Nel paragrafo 3.1 è stato mostrato che, tramite il metodo
IS, è possibile approssimare l’integrale precedente tramite la sommatoria:
b
I(g)
=
M
X
g(xti ) w(xti )
dove xti sono i campioni di xt generati rispetto alla funzione di importanza q(xt |yt ),
t |yt p yt
t |xt ) p(xt )
p
x
p(y
i
i
i
w xti =
=
q(xti |yt )
q (xti |yt )
e
(3.3.2)
i=1
w(xt )
w(xti ) = PM i
t
j=1 w(xj )
(3.3.3)
(3.3.4)
Se le seguenti ipotesi sono valide:
• xti ; i = 1, . . . , M è un insieme di vettori i.i.d. (indipendenti ed identicamente distribuiti)
in accordo alla PDF q(xti |yt );
• il supporto Sq = xt : q(xt |yt ) > 0 di q(xt |yt ) contiene il supporto Sp = xt : p(xt |yt ) > 0
di p(xt |yt );
• I(g) esiste finito;
• varp(xt |yt ) [g(xt )] < ∞ e w(xt ) < ∞ per ogni xt ∈ Sq ,
b
allora si dimostra [4] che, per M finito, la stima I(g)
è polarizzata ma, grazie alla
legge forte dei grandi numeri, è asintoticamente non polarizzata cioè
b
= I(g) con probabilità 1
lim I(g)
M →∞
(3.3.5)
L’equazione (3.3.2) può essere riscritta equivalentemente nella forma
b
I(g)
=
Z
g(xt )
M
X
i=1
w(xti ) δ(xt − xti ) dxt
(3.3.6)
dove δ(·) indica la delta di Dirac. Dal confronto tra (3.3.6) e (3.3.1) risulta che
p(xt |yt ) ≈
M
X
i=1
w(xti ) δ(xt − xti )
(3.3.7)
cioè è possibile interpretare i metodi Monte Carlo come metodi di campionamento e non solo
come metodi di integrazione. Quindi i metodi Monte Carlo risolvono completamente il problema
della stima fornendo una approssimazione della PDF a-posteriori e dei suoi momenti tramite una
distribuzione di probabilità discreta.
CAPITOLO 3. PARTICLE FILTER
3.4
21
Campionamento sequenziale della funzione di importanza (SIS)
Lo scopo della stima ricorsiva è quello di ottenere una stima della PDF p(xt |yt ) a partire dalla
PDF p(xt−1 |yt−1 ) e sulla base della misura corrente y(t). Questo obiettivo, dal punto di vista
dei metodi Monte Carlo,
si traduce nell’approssimare p(xt |yt ) aggiungendo ai campioni passati
t−1
xi ; i = 1, . . . , M , relativi a p(xt−1 |yt−1 ), nuovi campioni ottenuti propagando quelli passati
dopo averli aggiornati sulla base della misura recente y(t). Si può notare che, scegliendo la
funzione di importanza q(xt |yt ) in modo che sia fattorizzabile nella forma
q(xt |yt ) = q(xt−1 |yt−1 ) q(x(t)|xt−1 , yt ),
(3.4.1)
si possono ottenere campioni xti ∼ q(xt |yt ) aggiungendo ai campioni esistenti xt−1
∼ q(xt−1 |yt−1 )
i
t−1
t
quelli ottenuti tramite xi (t) ∼ q(x(t)|x , y ). Di seguito vengono mostrati i passaggi per
rendere ricorsiva anche l’equazione di aggiornamento dei pesi. Sfruttando le relazioni sviluppate
nel paragrafo 1.1, si osserva che
p(xt |yt ) =
p(yt |xt ) p(xt )
p(yt )
=
p(y(t), yt−1 |xt ) p(xt )
p(y(t)|yt−1 ) p(yt−1 )
=
p(y(t)|xt , yt−1 ) p(yt−1 |xt ) p(xt )
p(y(t)|yt−1 ) p(yt−1 )
=
p(y(t)|xt , yt−1 ) p(xt |yt−1 ) p(yt−1 )
p(y(t)|yt−1 ) p(yt−1 )
=
p(y(t)|xt , yt−1 ) p(xt |yt−1 )
p(y(t)|yt−1 )
=
p(y(t)|xt , yt−1 ) p(x(t)|xt−1 , yt−1 )
p(xt−1 |yt−1 )
p(y(t)|yt−1 )
=
p(y(t)|x(t)) p(x(t)|x(t − 1))
p(xt−1 |yt−1 )
p(y(t)|yt−1 )
(3.4.2)
L’ultima uguaglianza è conseguenza delle relazioni (1.1.19) e (1.1.20). Infine, sostituendo (3.4.1)
e (3.4.2) in (3.3.3) si ha:
t−1 t−1
p xti |yt
)
t
t
t = p(y(t)|xi (t)) p(xi (t)|xi (t − 1)) p(xi |y
p
y
w(xi ) =
p
y
t
t−1
t−1
t
q (xi |y )
q(xi |yt−1 ) q(xi (t)|xi , yt ) p(y(t)|yt−1 )
(3.4.3)
t−1 p(y(t)|xi (t)) p(xi (t)|xi (t − 1))
= w(xi )
t
q(xi (t)|xt−1
i ,y )
Le espressioni precedenti risolvono il problema della stima ricorsiva di p(xt |yt ); nel filtraggio
l’obiettivo è stimare p(x(t)|yt ). In questo caso è possibile scegliere delle funzioni di importanza
tali che
q(x(t)|xt−1 , yt ) = q(x(t)|x(t − 1), y(t))
CAPITOLO 3. PARTICLE FILTER
22
eliminando quindi la dipendenza dalla sequenza di stati passati. L’assunzione precedente semplifica l’equazione di aggiornamento dei pesi che diventa:
wi (t) = wi (t − 1)
p(y(t)|xi (t)) p(xi (t)|xi (t − 1))
q(xi (t)|xi (t − 1), y(t))
(3.4.4)
e la corrispondente approssimazione discreta della PDF è:
t
p(x(t)|y ) ≈
M
X
i=1
wi (t) δ(x(t) − xi (t))
(3.4.5)
dove per semplicità si adotta la notazione wi (t) = w(xi (t)).
Riassumendo, i passi dell’algoritmo SIS sono:
1. Inizializzazione: xi (0) ∼ q(x(0)|y(0)) e wi (0) =
1
per i = 1, . . . , M ;
M
2. Per t > 1 e per i = 1, 2, . . . , M :
• Campionamento xi (t) ∼ q(x(t)|xi (t − 1), y(t));
• Aggiornamento dei pesi wi (t) sulla base di (3.4.4);
3. Normalizzazione dei pesi
4. Calcolo della stima: x̂(t) =
PM
wi (t)
wi (t) = PM
j=1 wj (t)
(3.4.6)
wi (t) xi (t)
P
′
b
5. Calcolo della covarianza: P(t)
= M
i=1 w i (t) (xi (t) − x̂(t)) (xi (t) − x̂(t))
i=1
Si nota che nell’algoritmo SIS vi sono due gradi di libertà; il numero dei campioni M e la scelta
della funzione di importanza q(x(t)|x(t − 1), y(t)). L’unico vincolo richiesto sulla scelta della
funzione di importanza è che il suo supporto contenga il supporto di p(x(t)|yt ); tuttavia, per M
finito, questo non è sufficiente per ottenere una stima accurata.
Esempio - Si consideri il seguente sistema dinamico non lineare (scalare)

25 x(t)

 x(t + 1) = 0.5 x(t) +
+ 8 cos(1.2 t) + w(t) = f (t, x(t)) + w(t)
1 + x2 (t)

x2 (t)

+ v(t) = h(x(t)) + v(t)
y(t) =
20
(3.4.7)
con x(0) ∼ N (0, p0 ), w(t) ∼ N (0, q), v(t) ∼ N (0, r), p0 = 10, q = 10 e r = 1. I rumori sono additivi e quindi si possono applicare (1.1.22) e (1.1.23). Fissando q(x(t)|x(t − 1), y t ) =
p(x(t)|x(t − 1), cioè scegliendo come funzione di importanza la probabilità di transizione, e
applicando l’algoritmo SIS si ottiene:
CAPITOLO 3. PARTICLE FILTER
23
1. Inizializzazione: xi (0) ∼ N (0, p0 ) e wi (0) =
1
per i = 1, 2, . . . , M ;
M
2. Per t > 1 e per i = 1, 2, . . . , M :
• Campionamento: ξi (t − 1) ∼ N (0, q),
• Aggiornamento dei pesi:
xi (t) = f (t, xi (t − 1)) + ξi (t − 1);
wi (t) ∝ p(y(t)|xi (t)) wi (t − 1)
(
)
[y(t) − h(xi (t)]2
= wi (t − 1) exp −
2r
(3.4.8)
e normalizzazione mediante (3.4.6).
All’istante t, la stima MMSE di x(t) fornita dall’algoritmo è:
x̂(t) =
M
X
wi (t) xi (t)
i=1
Applicando l’algoritmo al sistema (3.4.7) si è riscontrato che, al crescere del tempo t, il valore
dei pesi tende a zero con l’eccezione di un solo peso cioè, tenuto conto della normalizzazione,
wj (t) → 1 e wi (t) → 0 per ogni i 6= j e t → ∞. Questo fenomeno di degenerazione dei pesi
causa la divergenza della stima x̂(t), in quanto asintoticamente un solo campione è utilizzato per
calcolare tale stima. Intuitivamente si può capire il fenomeno della degenerazione dei pesi osservando (3.4.8); infatti, a causa della presenza dell’esponenziale negativo, i campioni con errori
y(t) − h(xi (t)) grandi tendono ad avere pesi piccoli e questo, per la struttura ricorsiva dell’equazione, produce nel tempo la degenerazione.
Il fenomeno della degenerazione dei pesi, che è causato dalla fattorizzazione della funzione di
importanza (3.4.1), è quantificabile valutando il numero dei campioni effettivi [4], cioè i campioni
i cui pesi sono significativi
1
△
Mef f = PM
(3.4.9)
2
i=1 w i
Si noti che Mef f ≤ M e piccoli valori di Mef f indicano una consistente degenerazione. La
degenerazione degrada l’accuratezza della stima e non utilizza in modo efficiente le risorse di
calcolo. Si devono, infatti, sprecare risorse di calcolo per propagare campioni con pesi nulli che,
quindi, non contribuiscono alla stima. Tale fenomeno può essere contrastato in due modi:
1. scegliendo accuratamente la funzione di importanza;
2. effettuando un ricampionamento.
CAPITOLO 3. PARTICLE FILTER
3.5
24
Scelta della funzione di importanza
La funzione di importanza ottimale è quella che massimizza Mef f . Si dimostra [4] che tale
funzione è:
qopt (x(t)|x(t − 1), y(t)) = p(x(t)|x(t − 1), y(t))
p(y(t)|x(t), x(t − 1)) p(x(t)|x(t − 1))
=
p(y(t)|x(t − 1))
(3.5.1)
Sostituendo (3.5.1) in (3.4.4) si ottiene
wi (t) = wi (t − 1) p(y(t)|xi (t − 1))
Z
= wi (t − 1)
p(y(t)|x(t)) p(x(t)|xi (t − 1)) dx(t)
(3.5.2)
La scelta ottimale della funzione di importanza comporta due difficoltà:
1. si deve riuscire a campionare p(x(t)|x(t − 1), y(t));
2. si deve calcolare l’integrale in (3.5.2).
L’uso della funzione di importanza ottimale risulta, quindi, possibile soltanto in due casi. Il
primo caso è quando lo stato x(t) può assumere solo un numero finito di valori, ad esempio
quando con lo stato si descrivono i tipi di manovre di un bersaglio (e.g. moto rettilineo, curva
a destra, curva a sinistra etc.). In questo caso l’integrale in (3.5.2) si riduce ad una sommatoria
ed è possibile campionare p(x(t)|x(t − 1), y(t)). Il secondo caso si ha nell’ipotesi di Gaussianità
della PDF p(x(t)|x(t − 1), y(t)). Tale PDF è Gaussiana se il sistema (1.0.1) è della forma:
x(t + 1) = f (t, x(t)) + w(t)
(3.5.3)
y(t) = C(t) x(t) + v(t)
dove:
w(t) ∼ N (0, Q(t)) ,
v(t) ∼ N (0, R(t)) ;
(3.5.4)
f (·) è una funzione nonlineare; C(t) è una matrice; i campioni delle sequenze di rumore {w(t)}
e {v(t)} sono variabili aleatorie i.i.d. . Definendo
Σ−1 (t) = Q−1 (t− 1) + C′ (t) R−1 (t) C(t)
m(t) = Σ(t) Q−1 (t − 1) f (t − 1, x(t − 1)) + C′ (t) R−1 (t) y(t)
(3.5.5)
si ottiene la relazione
p(x(t)|x(t − 1), y(t)) = N (x(t); m(t), Σ(t))
p(y(t)|x(t − 1)) = N (y(t); C(t)f (t − 1, x(t − 1)), R(t) + C(t) Q(t − 1) C′ (t))
(3.5.6)
Quando il sistema non è nella forma (3.5.3) si può comunque ricorrere a tecniche di approssimazione per ricondursi a tale forma. Ad esempio, si può utilizzare EKF linearizzando l’equazione
di uscita. Naturalmente, in questo modo si ottiene solo una approssimazione della funzione di
CAPITOLO 3. PARTICLE FILTER
25
importanza ottimale. Nell’esempio precedente è stata scelta la densità a-priori p(x(t)|x(t − 1))
come funzione di importanza. Questa è un’altra possibile scelta che ha il vantaggio di rendere il
campionamento semplice da implementare. Lo svantaggio è che tale PDF non tiene conto della
misura y(t) nel campionamento e tende quindi a basarsi esclusivamente sull’equazione di stato
trascurando l’equazione di uscita. Pertanto se la covarianza del rumore di misura è molto più
piccola della covarianza del rumore di processo, tale scelta non è efficiente. Vi sono comunque
altre possibili scelte della funzione di importanza [4].
3.6
Ricampionamento e algoritmo SIR (Sequential Importance
Resampling)
Il secondo metodo per contrastare il fenomeno della degenerazione dei pesi è il ricampionamento.
L’idea è eliminare i campioni con pesi trascurabili e moltiplicare i campioni con pesi significativi.
Le tecniche di ricampionamento sono molto simili alle tecniche di riproduzione negli algoritmi
genetici e si basano sul seguente principio: la probabilità di duplicazione di un campione è proporzionale al valore del peso associato al campione. L’obiettivo delle tecniche di ricampionamento
è quindi quello di generare un nuovo insieme di campioni (x∗j , w∗j ) per j = 1, . . . , M a partire da
(xi , wi ) per i = 1, . . . , M , in modo che
P r(x∗j = xi ) = wi
Il valore dei nuovi pesi è re-inizializzato a w∗j = 1/M .
Il ricampionamento può essere effettuato ad ogni passo del particle filter o solo quando è necessario, cioè quando il numero efficace dei campioni Mef f scende sotto una certa soglia.
Infine, si fa notare che il ricampionamento introduce due problemi: la perdita di diversità dei
campioni (causata dalla duplicazione) e impedisce la parallelizzazione del particle filter, in quanto
per implementare il campionamento si devono combinare i campioni. Esistono delle tecniche per
ridurre la perdita di diversità, ad esempio aggiungere del rumore ai campioni duplicati in modo
che non siano esattamente uguali. Applicando le tecniche di ricampionamento all’algoritmo SIS
si ottiene l’algoritmo SIR (Sequential Importance Resampling):
1. Inizializzazione: xi (0) ∼ q(x(0)|y(0)) e wi (0) =
1
per i = 1, . . . , M ;
M
2. Per t > 1
(a) per i = 1, . . . , M :
• campionamento: xi (t) ∼ q(x(t)|xi (t − 1), y(t))
• aggiornamento dei pesi wi (t) sulla base di (3.4.4);
(b) normalizzazione dei pesi wi (t) sulla base di (3.4.6);
P
(c) Calcolo della stima: x̂(t) = M
i=1 w i (t) xi (t)
P
b
(d) Calcolo della covarianza: P(t) = M wi (t) (xi (t) − x̂(t))(xi (t) − x̂(t))′
(e) Ricampionamento: genera (x∗i , w∗i =
i=1
1
M)
ricampionando (xi , wi ) per i = 1, . . . , M
Bibliografia
[1] Y. Bar-Shalom, X. Rong Li, T. Kirubarajan, Estimation with applications to tracking and
navigation, John Wiley & Sons, 2001.
[2] S.M. Kay, Fundamentals of statistical signal processing estimation theory, Prentice Hall,
1993.
[3] H. Jazwinski, Stochastic processes and filtering theory, Academic Press, New York, 1974.
[4] A. Doucet, “On Sequential Simulation-Based Methods for Bayesian Filtering”, Available
as Technical Report CUED/F-INFENG/TR. 310, Cambridge University Department of
Engineerin, http://citeseer.ist.psu.edu/doucet00sequential.html, 1998.
[5] S. Arulampalam, S. Maskell, N. Gordon, T. Clapp, “A Tutorial on Particle Filters for Online Non-linear/Non-Gaussian Bayesian Tracking”, IEEE Trans. on Signal Processing, vol.
50, pp. 174-188, 2002.
[6] B. Ristic, S. Arulampalam, and N. Gordon, Beyond the Kalman filter: particle filters for
tracking applications, Artech House, 2004.
[7] S. M. Herman, A particle filtering approach to joint passive radar tracking. PhD thesis,
University of Illinois, 2002
[8] S. Challa, S. Arulampalam and N. Bergman, “Target Tracking Incorporating Flight Envelope
Information”, Proc. 3rd International Conference on Information Fusion, CD-ROM, ThC24, Paris, France, 2000.
26