8. Filtro de Particulas

Filtro de Partículas
Luca Martino
Juan Santaella Vallejo
Tratamiento Digital de Señales
Master Interuniversitario en Comunicaciones y Multimedia
[email protected]
[email protected]
INDICE
1.
2.
3.
4.
5.
6.
7.
8.
Introducción .............................................................................................................3
Filtro de Partículas ..................................................................................................4
Descripción Escenario .............................................................................................6
Sistema ......................................................................................................................7
Notas sobre el filtro de partículas utilizado ..........................................................9
Ficheros utilizados .................................................................................................10
Simulaciones ...........................................................................................................10
Conclusiones...........................................................................................................14
1. Introducción
Muchos problemas físicos pueden ser modelados por la pareja de ecuaciones:



 x (t )  Ft ( x (t  1), r1 (t  1))
 

 y (t )  H t ( x (t ), r2 (t ))
(1)


Donde las y(t ) es la observación conocida al tiempo t, y x (t ) es la variable de estado que


se quiere estimar. En general y(t ) y x (t ) son variables continuas , Ft (...) y H t (...)son


dos funciones deterministas (que pueden variar con el tiempo), r1 (t ) y r2 (t ) son dos
componentes aleatorias cuyas densidades de probabilidad son conocidas. De la primera
ecuación se denomina ecuación dinámica, la segunda ecuación de las observaciones.
En principio las funciones Ft y H t pueden ser de cualquier forma (lineales, no lineales
etc…), así como los dos ruidos pueden tener cualquier tipo de distribución. En términos
de cálculo de probabilidad, las ecuaciones (1) pueden ser interpretadas gráficamente
como en figura:
y t 1
yt
yt  2
y t 3
X t 2
X t 3
p(Yt 1 X t 1 )
Xt
p( X t 1 X t )
X t 1
Fig. 1) Interpretación probabilística de las ecuaciones (1).
Es decir, mirando la ecuación dinámica se puede ver claramente como esta supone la
propiedad Markoviana p X t X 0 , X1 ,...X t 1 )  p X t X t 1 ) y por la segunda de las (1) se




ve perfectamente que p Yt X t , Y1 ,...Yt 1 )  p Yt X t ) .
El problema clásico consiste suponiendo conocidos Ft y H t , las propiedades




estadísticas de r1 y r2 y las observaciones y , en estimar el valor del estado x . En el
caso que las dos funciones deterministas Ft y H t sean lineales y los ruidos gausianos
existe una solución optima conocida en literatura como Filtro de Kalman. Pero en
muchas aplicaciones no se cumplen las dos condiciones simultáneamente, así que en
literatura hay muchos métodos subòptimos para resolver el problema. Dentro de
conjunto de método merece particular atención el filtro de partículas.
2. Filtro de Partículas
El filtro de partículas es un método suboptimo muy potente porque no pide ninguna
restricción sobre las funciones Ft y H t y tampoco pide ninguna especifica distribución
para los ruidos. Se supone que hay que conocer las formas analíticas de las Ft y H t y de
 
las propiedades estadísticas de r1 y r2 , cualquiera que sean.
2.1 Idea de fondo
La idea subyacente del algoritmo del filtro de partículas es crear unas hipotéticas
trayectorias del estado (dejando propagar “las partículas”) y a cada una de ellas
atribuir unos pesos según mejor expliquen las observaciones. La estimación final del
estado se hará con una media de las trayectorias ponderada por los pesos.
xt11
xt2 2
xt1
xt2
xt1 2
xt21
t
t+1
t+2
Fig. 2) Dos posibles trayectorias (propagación de 2 partículas).
2.2 Desarrollo Matemático
Antes de hallar las relaciones que soportan el filtro de partículas, hay que recordar un
concepto previo: el importance sampling. Si nosotros quisiéramos calcular el

integral  h( x)  dx en manera no analítica, podríamos utilizar el método de Monte

Carlo. Eligiendo una adecuada densidad de probabilidad q(x) , el integral resultará
numéricamente aproximado por:

h( x )
1
 q( x)  q( x)  dx  M
xi ~ q ( x )
M
 h( x )
i 1
i
(2)
El objetivo final del desarrollo matemático que mostraremos más adelante, es encontrar
una aproximación discreta de la densidad de probabilidad a posteriori
p( x1:t y1:t )  p( x1 , x2 ,....xt y1 , y2 ,....yt ) . La aproximación que hallaremos tendrá en cada
paso un soporte discreto aleatorio definido por las partículas xti con i  1,2,....M ; a cada
partículas estará asociado un peso wti ( 0  wti  1 ) elegido según el principio del
importance sampling. En formula:
M
p( x1:t y1:t )  wti   ( xt  xti )
i 1
(3)
Muestreando las partículas según el importance sampling, a través de una adecuada
importance density:
xti ~ q( x1:t y1:t )
(4)
Lo pesos tendrán que ser proporcionales a:
w 
i
t
w 
i
t
p ( x1i:t y1:t )
q ( x1i:t y1:t )
(5)
wti
M
w
i
t
i
A nosotros nos gustaría también una expresión recursiva para que la actualización de los
pesos sea más simple; para hacer esto, tendremos que desarrollar esta expresión:
p ( x1:t y1:t ) 


p( y t x1:t , y1:t 1 )  p ( x1:t y1:t 1 )
p ( y t y1:t 1 )
p( y t y1:t 1 , x1:t )  p ( xt x1:t 1 , y1:t 1 )  p( x1:t 1 y1:t 1 )
p ( y t y1:t 1 )
(6)
p( y t xt )  p ( xt xt 1 )  p( x1:t 1 y1:t 1 )
p ( y t y1:t 1 )
p ( x1:t y1:t )  p ( y t xt ) p ( xt xt 1 )  p ( x1:t 1 y1:t 1 )
donde en el primer paso hemos tenido en cuenta de la propiedad de Bayes
p( A B, C)  p(B A, C)  p( A, C) p(B, C) , en el segundo paso la siguiente relación
p( A, B C)  p(B A, C)  p( A C) y finalmente hemos utilizados las propiedades de




Markoviana p xt x1:t 1 , y1:t 1 )  p xt x t 1 ) y p yt x1:t , y1:t 1 )  p yt x t ) . La proporción
final puede parecer obvia mirando la figura (1): estando al paso t-1 nos movemos
desde xt 1 al estado x t y desde allí logramos la observación yt .
Para una expresión recurrente de los pesos vamos a elegir una importance density que
pueda ser factorizada así:
q( x1:t y1:t )  q( xt x1:t 1 , y1:t )  q( x1:t 1 y1:t 1 )
(7)
Así que si las partículas hasta el paso t-1 se pueden considerar muestreadas por:
x1i:t 1 ~ q( x1:t 1 y1:t 1 )
y la nueva al paso t:
(8)
xti ~ q( xt x1:t 1 , y1:t )
(9)
Finalmente juntando las tres relaciones (5), (6) y (9), llegamos a:
w 
i
t
p( yt xti )  p( xti xti1 )  p( x1i:t 1 y1:t 1 )
q( xti x1i:t 1 , y1:t ) q( x1i:t 1 y1:t 1 )
wti  wti1 
p( yt xti )  p( xti xti1 )
(10)
q( xti x1i:t 1 , y1:t )
Que es la relación recursiva sobre los pesos que queríamos hallar. Los distintos tipos de
filtro de partículas se distinguen sobre todo para la elección de la función q (...). La
elección óptima que minimiza la varianza de los pesos es q( xt x1:t 1 , y1:t )  p( xt xt 1 , yt ) .
En este caso los pesos se actualizarían:
wti  wti1  p ( y t xti1 )
(11)
Pero en muchos casos no es posible utilizar esta función (además porque no es sencillo
calcular p ( yt xti1 ) ), así que se busca una diferente importance density. Usualmente se
elige q( xt x1:t 1 , y1:t )  p( xt xt 1 ) :
wti  wti1  p ( y t xti )
(12)
Realmente el uso de los filtros de partículas se ha disparado en los últimos años gracias
a la introducción de un nuevo paso en el algoritmo que mejora sensiblemente las
prestaciones. Se trata del Resampling: la idea del resampling es de dejar propagar las
particulas de mayor peso, quitando las que menos información llevan. Haciendo este se
elimina el fenómeno de degeneración de las partículas (todos los pesos van a cero
menos uno).
3. Descripción Escenario
Nuestro escenario es un clásico problema de tracking donde el estado está compuesto
por la posición y la velocidad. Esta ultima está sujeta a una desconocida aceleración.
Utilizaremos 4 sensores fijos; recordamos que en plano para definir unívocamente una
trayectoria, necesitaríamos mínimo 3 sensores no alineados.
Nuestras medidas serán distancias relativas (distancia L1) entre el objeto y nuestros
sensores. Supondremos que los ruidos sean gausianos. Matemáticamente este escenario
se traduce en una ecuación dinámica lineal, y una ecuación de las observaciones no
lineal, así que es necesario utilizar un método suboptimo para estimar la trayectoria.
S2
S1
S3
S4
Fig. 3) Muestra la distribución de los 4 sensores (en rojo) y una trayectoria.
4. Sistema
En Formulas nuestro sistema es:




 x (t )  A  x (t  1)  B  r1 (t )



 y (t )  C  x1, 2 (t )  P 1  D  r2 (t )
(13)
donde:
1
0
A
0

0
0 Ts
1 0
0 1
0 0
Ts2 0 
0
 2 
1

 
2



Ts 
1
0 Ts 
2


,B 
,C 
,D   
 Ts 0 
 2 
1
0


 


1
 2 
 0 Ts 
1
(14)
Es fácil nota que la matriz A que decide la dinámica, siendo triangular superior, tiene
todos los 4 autovalores coincidentes iguales a 1. Nuestro vector de estado será:
 x1 (t )   p x (t ) 
 x (t )  p (t )

y

x (t )   2   
 x3 (t )   v x (t ) 


 
 x 4 (t )  v y (t ) 
(15)
es decir las dos componentes de la posición, y las dos de la velocidad. Entonces con:
 x1 (t )   p x (t ) 

x1, 2 (t )  


 x2 (t )  p y (t )
(16)
consideramos solo las componentes del estado que se refieren a la posición. Por otra
parte, la matriz P contiene las posiciones en el plano de los sensores:
 s x1 s x 2
p
s y1 s y 2
s x3
s y3
s x4 
s y 4 
(17)
Nuestros sensores nos proporcionan la distancia L1 con nuestro objeto, es decir:
 p x (t )  s x1

 p x (t )  s x 2

x1, 2 (t )  P 1  
 p x (t )  s x 3

 p x (t )  s x 4

 p y (t )  s y1 

 p y (t )  s y 2 

 p y (t )  s y 3 

 p y (t )  s y 4 
(18)
Entonces nuestro vector de las observaciones será un vector 4  1 donde cada
componente corresponde a una medida detectada da un sensor (el mínimo numero de
sensores en un plano para detectar en manera univoca una trayectoria es 3):
 y1 (t ) 
 y (t ) 

y (t )   2 
 y 3 (t ) 


 y 4 (t ) 
(19)
Es importante subrayar que nuestras medidas están relacionada solo con la posición de
nuestro objeto; no tenemos ninguna información directa sobre la velocidad (pero
indirectamente si; de hecho, estimaremos todo el vector de estado).
La parte aleatoria de nuestro sistema está compuesta por:

1. ruido dinámico r1 (t ) ~ N (0,  12 ) , que será un vector 2  1 .

2. ruido de medida r2 (t ) ~ N (0, I ) , que será un vector 4  1 (una componente por
cada sensor); nosotros suponemos que el ruido afecta de la misma manera a

cada sensor D  r2 (t ) ~ N (0, 22 ) .
5. Notas sobre el filtro de partículas utilizado
El filtro de partículas que nosotros hemos utilizado utiliza como importance density
p ( xt xti1 ) con M numero de partículas, así que según teoría:
 xti ~ p ( xt xti1 )

wti ~ wti1  p ( y t xti )


i
w i  wt
M
 t
wti


i 1


Para el Resampling evaluamos en cada paso el valor:
N eff 
1
M
 (w )
i 1
i
t
2
Si N eff  M  k (donde 0  k  1 , y en nuestro código corresponde al parámetro “cost”)
hará el Resampling. Realmente si el k es bastante elevado el resampling se hará en
cada paso y el nuestro filtro se convierte en un SIR Filter.
1
Como el Resampling se hace en cada paso, en el filtro SIR, wti1 
(después del
N
Resampling) siempre, así que:
wti ~ p ( y t xti ) (Antes del “Resampling stage” al paso t )
5.1 Resampling Stage
Simplemente muestreamos M veces la distribución discreta con soporte xti
i
t
probabilidades w ; es decir las partículas después del Resampling serán:
M
 k
i
i
ˆ
x
~
 t  wt   ( x  xt )
i 1

wˆ k  1
 t
N
y
Los pesos de las nuevas partículas habrán todas mismo peso.
6. Ficheros utilizados
CreaRandom.m
Muestra una distribución discreta. Recibe como entrada las probabilidades (o mejor
dicho una densidad de probabilidad discreta) y el número N de muestras deseado. Como
salida proporciona las N muestras.
Resampling.m
Calcula las partículas después aplicando el Resampling. Recibe como entradas las
partículas y los pesos al paso t, y restituye como salidas las nuevas muestras y los pesos
todos iguales a 1 N . Utiliza el CreaRandom.
PartFilter2D.m
Calcula una trayectoria y unas observaciones, y sobre ellas luego implementa el filtro de
partículas. Esta función sacará 3 graficas: una con la trayectoria real con los sensores,
otra con la trayectoria estimada y la verdadera junto, y otra grafica del error de
estimación cometido en cada instante. Utiliza las dos funciones anteriores.
7. Simulaciones
Utilizamos siempre trayectorias con T=100 puntos. Para calcular las graficas de Error
(distancia euclidea entre la posición correcta (en blue) y nuestra estimación (en rojo)),
calculamos 20 veces el error medio cometido en estimar una trayectoria por cada valor
de M. Haremos tres distintas pruebas con un nivel alto de ruido que dejaremos igual, y
cambiando el numero de resamplings. Esto lo haremos cambiando la constante k (en el
código seria al variable cost).
100
100
a) con un vector D    (mucho ruido) y k=0.6 (muchos Resamplings).
100
 
100
Fig. 4) Trayectoria real (en blue) y su Reconstrucción (en rojo) con M=50.
Grafica de error con M:
20, 100, 500, 1000, 3000.
(muchas veces resampling )
M
Fig. 5) Error medio por diferentes números de partículas M , haciendo muchas veces resampling.
100
100
b) con un vector mismo que antes D    y k=0.1 (menor que antes, menos
100
 
100
resamplings).
Fig. 6) Trayectoria real (en blue) y su Reconstrucción (en rojo) con M=50.
Grafica de error con M:
20, 100, 500, 1000, 3000.
(muy pocos resamplings)
M
Fig. 7) Error medio por diferentes números de partículas M , con pocos reasamplings.
100
100
c) con un vector D    y k=1 (siempre Resampling, Filtro SIR).
100
 
100
Fig. 8) Trayectoria real (en blue) y su Reconstrucción (en rojo) con M=50.
ERROR
Grafica de error con M:
20, 100, 500, 1000, 3000.
(resampling siempre)
M
Fig. 9) Error medio por diferentes números de partículas M por un filtro SIR (resampling en cada paso).
Comentarios: En las tres simulaciones se puede notar que aunque haya mucho ruido de
medida nuestro filtro de partícula consigue aproximar bien la trayectoria verdadera. Ya
con M=500 se logra casi un error mínimo que se mantiene prácticamente constante
aumentando el numero de partículas. Además el error no se ve sustancialmente afectado
por el número de resampling steps.
De todas formas, se puede notar que con M pequeño se necesita un número de
resampling mayor para lograr un error menor. Esto es lógico porque teniendo pocas
partículas, para estimar bien, es muy conviene dejar propagar las pocas que lleven
información relevante.
8. Conclusiones
En este trabajo se ha realizado una explicación y breve estudio teórico del filtro de
partículas.
Para la practica se ha realizado un escenario de pruebas compuesto de cuatro sensores
que son capaces de detectar la distancia de un objeto. También se ha programado un
filtro de partículas y se han unido los dos programas para poder realizar las
simulaciones.
Hemos visto como se comporta el filtro variando el numero de partículas de nuestro
filtro. Demostrando la eficiencia del filtro de partículas.