Escolar Documentos
Profissional Documentos
Cultura Documentos
1
INTRODUCCION
El objetivo de esta sesión es presentar la versión no lineal del
filtro de Kalman: Filtro Extendido de Kalman
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.
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
7
LINEALIZACION
● El problema principal es que al usar sistemas no lineales se pierde
la Gausianidad de las distribuciones
● 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 )
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,
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
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
20