MT519 – Robots Autónomos

FILTRO EXTENDIDO DE KALMAN

Ing. Iván A. Calle Flores

1
 INTRODUCCION
El objetivo de esta sesión es presentar la versión no lineal del
filtro de Kalman: Filtro Extendido de Kalman

● La idea básica es aproximar la distribución posterior usando

distribuciones Gaussianas

2
 LOCALIZACION
“Using sensory information to locate the robot
in its environment is the most fundamental
problem to providing a mobile robot with
autonomous capabilities.” [Cox ’91]
 Given
◦ Map of the environment.
◦ Sequence of sensor measurements.
 Wanted
◦ Estimate of the robot’s position.
 Problem classes
◦ Position tracking
◦ Global localization
◦ Kidnapped robot problem (recovery)

3
4
 SISTEMAS NO LINEALES
● En la practica, la mayoría de los modelos(robots, sensores) son
del tipo no lineal. Por lo que no se puede usar el filtro de Kalman.

CINEMATICA DEL ROBOT

 vˆ vˆ  Velocidades reales

 wˆ sin( )  sin(   wˆ t ) 
    
x ' x w ˆ
  vˆ   v    1v2 2 w2 
     ˆ ˆ  ˆ       
ˆ t ) 
v v
y '  y 
     wˆ cos( )  cos(   w  w   w    3v2 4 w2 
wˆ 
 '     
    w ˆ  t  ˆt
  ˆ   v 2
6 w2
  5

No es de la forma xt  At xt 1  Bt ut   t
5
MEDIDAS DE LOS SENSORES

 2 
 rt    m j , x  x    m j , y  y      r2 
2
i

 i   

 ˆt    a tan 2(m j , y  y, m j , x  x)       2 
 sˆi    

    s2 
s
 t  j
 

No es de la forma zt  Ct xt   t

6
 FILTRO EXTENDIDO DE KALMAN
● Es una versión del filtro de Kalman aplicadas a sistemas no lineales
de la forma:

xt  g (ut , xt 1 )   t
zt  h( xt )   t

t Vector(nx1) de ruido gaussiano de media cero, y matriz

de covarianza Rt

t Vector(nx1) de ruido gaussiano de media cero, y matriz

de covarianza Qt

7
 LINEALIZACION
● El problema principal es que al usar sistemas no lineales se pierde
la Gausianidad de las distribuciones

Sistema Lineal Sistema No Lineal

8
● La linealización se hace
alrededor de la posición
mas probable: la media

● De esta manera la
distribución posterior
es una aproximación
gaussiana.

9
 Linealización usando series de
Taylor
ECUACION CINEMATICA

g (ut , t 1 )
g (ut , xt 1 )  g (ut , t 1 )  ( xt 1  t 1 )
xt 1
g (ut , xt 1 )  g (ut , t 1 )  Gt ( xt 1  t 1 )

ECUACION DE LAS MEDIDAS

h( t )
h( xt )  h( t )  ( xt  t )
xt
h( xt )  h( t )  H t ( xt  t )
10
 MATRICES JACOBIANAS
 x' x' x' 

 t 1, x t 1, y t 1, 
 y ' Jacobiano de ‘g’ con
g (ut , t 1 ) y ' y ' 
Gt     respecto al estado
xt 1  t 1, x t 1, y t 1, 
  '  '  ' 
 
 t 1, x t 1, y t 1, 

 rt rt rt 

 
h( t , m)  t , x t , y t ,  Jacobiano de ‘h’ con
Ht  
xt  t t t  respecto al estado
 
 t , x t , y t , 
11
 ALGORITMO FILTRO EXTENDIDO DE KALMAN
1. Extended_Kalman_filter( t-1, St-1, ut, zt):

2. Prediction:
3. t  g (ut , t 1 )  t  At t 1  Bt ut
4. St  Gt St 1GtT  Rt St  At St 1 AtT  Rt

5. Correction:
6. Kt  St H tT ( H t St H tT  Qt ) 1 Kt  St CtT (Ct St CtT  Qt ) 1
7. t  t  Kt ( zt  h(t )) t   t  Kt ( zt  Ct  t )
8. St  ( I  Kt H t )St St  ( I  Kt Ct )St
9. Return t, St

12
CALIDAD DE LA APROXIMACION

Gran incertidumbre Poca incertidumbre

13
 DERIVACION MATEMATICA
1. PASO DE PREDICCION

 v v 
 w sin( )  sin(  w  t ) 
 x'  x   w

     v v   N (0, R)
y '  y
     w cos( )  cos(  w t )
w 
 '     
    w t
xt
 
 
g ( ut , xt 1 )

 1vt2   2 wt2 0 
Mt   2
 0  v
3 t
2
  4 t 
w Rt  Vt M tVt T
g (ut , t 1 )
Vt 
ut 14
 2. PASO DE CORRECCION

 2 
 rt    m j , x  x    m j , y  y      r2 
2
i

 i   

 ˆt    a tan 2(m j , y  y, m j , x  x)       2    r2 0 0 
 sˆi      
sj    Qt   0   0 
2
  t
    s2 
   0 0  2
zt i
h ( xt , m )
 s 

 (m j , x  x) (m j , y  y ) 
  0
 (m j , x  x) 2  (m j , y  y ) 2 (m j , x  x) 2  ( m j , y  y ) 2 
 
 (m j , y  y ) (m j , x  x)
Ht   1 
 (m j , x  x) 2  (m j , y  y ) 2 (m j , x  x) 2  ( m j , y  y ) 2 
 
 0 0 0
 
  15
16
17
18
Sensor impreciso Sensor preciso
19
 RESUMEN
● El filtro Extendido de Kalman es la versión aplicada a sistemas no
lineales del filtro de Kalman

● Para aproximar la distribución posterior una distribución gaussiana

se hace una linealización tanto el modelo cinemático del robot
como el modelo de los sensores

● La calidad de la linealización depende de la incertidumbre del

robot. Mientras mas precisos el robot y los sensores, será menor
el error de aproximación

● El filtro extendido de Kalman solo puede resolver el problema de

tracking.

20