Dpto.

 Ingeniería y Tecnología de Computadores*
Dpto. Informática y Sistemas**

University of Murcia (Spain)
Artificial Perception and Pattern Recognition Research Group (PARP)

Bayesian tracking: Una introducción a 
los filtros probabilísticos aplicada al 
problema del SLAM 

Authors:
Pedro E. López­de­Teruel*   
pedroe@ditec.um.es

PARP
1
 Introducción

● SLAM: Simultáneamente construir un mapa y determinar la 
posición de un “vehículo” en él, manejando adecuadamente 
la incertidumbre tanto en el propio movimiento como en las 
medidas tomadas del entorno.
● Notación:
(SLAM 
Summer School 
documentation)

PARP
2
 Definición del problema

● Problema SLAM:
– A partir de las medidas  Zk={z1,...,zk}, ...
– ... hacer inferencia sobre las posiciones (e incertidumbres 
asociadas) Xk={x1,...,xk}, ...
– ... y las posiciones de los landmarks m={m1,...,mM},
● Necesitamos modelos que manejen incertidumbre:
– Modelo del sensor (verosimilitud):  P(zk|xk,m) 
– Modelo del movimiento: P(xk|xk-1,uk) 
● En  estas  condiciones,  la  solución  es  una  estimación 
recursiva de la siguiente probabilidad conjunta a posteriori:
PARP
P(xk,m|Zk,Uk,xo) 3
 Suposiciones

● Es razonable asumir que las medidas de distintos landmarks 
son  independientes  entre  sí,  e  independientes  entre 
distintos  instantes  de  tiempo  (condicionado  a  conocer  la 
posición xi):

P(Zk|Xk,m) = Pi=1..k P(zi|xi,m)
● Supondremos también que el modelo del movimiento es un 
proceso de Markov:

(Markov: “El futuro no depende del pasado, dado el presente”)

PARP
4
 Algoritmo básico recursivo (I)

● PASO  0:  Proveer  una  buena  estimación  inicial  de  x0  (quizá  con 
incertidumbre asociada).
● WHILE TRUE:
– PREDICCIÓN:
Utilizar  la posición estimada actual ,  xk­1, y la última medida 
de  movimiento  (control)  tomada,  uk,  para  hacer  una  primera 
predicción del siguiente, xk.
– ACTUALIZACIÓN:
Utilizar  la  posición  estimada  actual  ,  xk,  y  la  última  medida 
del entorno (sensor) tomada, zk, para refinar el valor de xk.

PARP
5
 Algoritmo básico recursivo (II)

● PREDICCIÓN: Prob. total

P(A,B)=
P(A/B)P(B)

Markov
Estimación en el paso anterior
● ACTUALIZACIÓN:
Si expandimos la distribución conjunta (xk,m,zk) en términos del 
estado, por un lado (I), y de la observación, por otro (II):

(I)

(II)
Despejando:
PARP
6
 Algoritmo básico recursivo (III)

● En resumen:
CÁLCULO SLAM RECURSIVO COMPLETO:
Estimación posición en el paso k

Modelo del  Modelo del  Estimación posición en 

sensor movimiento el paso k­1 (recursión)
Interpretación: Si pensamos en la integral como una suma discreta, 
lo que se hace es, para cada posible posición anterior, ponderada por su
probabilidad, se lleva a su posición predicha tras el movimiento, y finalmente 
PARP se ponderan las posiciones resultado con sus respectivas verosimilitudes
7
 Filtro de Kalman (lineal)

● Entrada Wikipedia (Kalman Filter):
Modelo del sistema Modelo de medida:

– Paso 1 (Predicción):
Estado actual

– Paso 2 (Actualización):
Innovación (o residuo)
Varianza de la medida
Ganancia Kalman (óptima)
Correcciones 
del estado
PARP
8
 Filtro de Kalman Extendido (no lineal)

● Entrada Wikipedia (Kalman Filter):
Modelo del sistema Modelo de medida:

– Paso 1 (Predicción):

Lo único que cambia
– Paso 2 (Actualización):

Donde:

PARP
9
 Unscented Kalman Filter

● En  lugar  de  “linealizar”  el  problema,  hacemos  un  muestreo 

determinista del estado actual, lo proyectamos al estado futuro, 
y allí volvemos a suponer “Gaussianidad”.
● Así, no sólo la media, sino también la covarianza, es proyectada 
al futuro usando la no linealidad.

PARP
10
 Filtros de partículas
● En  lugar  de  intentar  trabajar  con  distribuciones  parametrizadas, 
trabajamos con muestras (sampling) y métodos de Montecarlo.
● El estado es ahora una población de N partículas xi, cada una con un peso 
wi (la suma de los wi es 1).
● Paso 1: Predicción
– Para  cada  partícula,  simulamos  un  movimiento  basándonos  en  el 
modelo  de  movimiento  (el resultado será aleatorio, dependiendo de 
la incertidumbre del mismo).
● Paso 2: Actualización
– Para  cada  partícula  resultado,  estimamos  su  nuevo  peso  wi  , 
dependiendo de su verosimilitud, usando el modelo del sensor.
● Paso 3: Remuestreo
– (Sólo si el número de partículas efectivas                   baja de un 
determinado nivel,  remuestrear el  conjunto  de  partículas  de  forma 
PARP
que todos los wi vuelvan a ser 1/N). 11
 FastSLAM
● Aprovechamos  el  hecho  de  la  independencia  condicional  entre  las 
estimaciones de los landmarks, una vez que no hay incertidumbre en la 
posición del vehículo.
● Concepto de D­separabilidad en Redes Bayesianas

● En cada partícula (= una posición concreta), podremos llevar una matriz 
de covarianza independiente por landmark [O(N*M*3²), con M número 
de  landmarks)]  en  lugar  de,  como  en  el  EKF,  una  gran  matriz  de 
covarianza [O(~(3*N)²)]  FastSLAM útil cuando crece mucho el # de 
PARP
landmarks. 12
 Visual SLAM

● Vídeo kitchen.avi, A. Davison (fail, success, unused)
● Transparencias 12­40 monoslamtutorial.pdf
● Problemas Davison:
– Número de landmarks limitado (EKF)
– Sólo puntos como features.

● Otras aproximaciones más modernas:
– Filtros de incertidumbre más potentes (partículas, UKF)
– Otras  features  (segmentos,  bordes,  ...),  incluso 
combinados.
– Distintas  parametrizaciones  del  espacio  de  estados  (Lie­
Algebras).
Drummond & Eade, Gee & Mayol­Cuevas, Rosten & Drummond, ... 
PARP (Univ. Cambridge & Univ. Bristol) 13
 Referencias
● "A  Tutorial  on  Particle  Filters  for  On­line  Non­Linear/Non­
Gaussian  Bayesian  Tracking",    S.  Arulampalam,  S.  Maskel,  N. 
Gordon & T. Clapp. IEEE Trans. on Signal Processing, 2001.
● http://en.wikipedia.org/wiki/Kalman_filter    
    (y mirar referencias ahí para KF, EKF, UKF).
● “FastSLAM: A Factored Solution to the Simultaneous Localization 
and  Mapping  Problem”,  M.  Montemerlo,  S.  Thrun,  D.  Koller  & 
B.  Wegbreit”,Proceedings  of  the  AAAI  National  Conference  on 
Artificial Intelligence, 2002.
● "MonoSLAM:  Real­Time  Single  Camera  SLAM",  A.  Davison,  I. 
Reid, N. Molton and Ol. Stasse, IEEE Trans. PAMI (accepted) , 
2007. (EKF)
● "Real­Time Model­Based SLAM Using Line Segments”, A.P. Gee 
&  W.  Mayol­Cuevas,    2nd  International  Symposium  on  Visual 
Computing, November 2006. (UKF)
PARP
● “Scalable  Monocular  SLAM”,  E.  Eade  &  T.  Drummond,  IEEE 
CVPR 2006. (FASTSLAM) 14