1
Diego Rodríguez-Losada
Robots Móviles.
UPM
El Filtro de Kalman
CA
R. U
PM
-C
SIC
2 Diego Rodriguez-Losada
Indice
Probabilidad
Derivación caso 1D
KF lineal
EKF
Aplicaciones:
Estimación de un voltaje
Estimación de un misil
Localización 1D
Localización 2D
2
CA
R. U
PM
-C
SIC
3 Diego Rodriguez-Losada
Probabilidad
CA
R. U
PM
-C
SIC
4 Diego Rodriguez-Losada
Distribución uniforme
3
CA
R. U
PM
-C
SIC
5 Diego Rodriguez-Losada
Distribución normal
CA
R. U
PM
-C
SIC
6 Diego Rodriguez-Losada
Propiedades Normal
Linealidad
Intervalos
4
CA
R. U
PM
-C
SIC
7 Diego Rodriguez-Losada
Normal multivariable
CA
R. U
PM
-C
SIC
8 Diego Rodriguez-Losada
Indice
Probabilidad
Derivación caso 1D
KF lineal
EKF
Aplicaciones:
Estimación de un voltaje
Estimación de un misil
Localización 1D
Localización 2D
5
CA
R. U
PM
-C
SIC
9 Diego Rodriguez-Losada
Derivación 1D (var min)
CA
R. U
PM
-C
SIC
10 Diego Rodriguez-Losada
Derivación 1D
Reorganizando:
Podemos llamar:
Entonces:
6
CA
R. U
PM
-C
SIC
11 Diego Rodriguez-Losada
Indice
Probabilidad
Derivación caso 1D
KF lineal
EKF
Aplicaciones:
Estimación de un voltaje
Estimación de un misil
Localización 1D
Localización 2D
CA
R. U
PM
-C
SIC
12 Diego Rodriguez-Losada
Filtro Kalman Lineal (KF)
Modelo del proceso
Modelo de observación
Ruidos
7
CA
R. U
PM
-C
SIC
13 Diego Rodriguez-Losada
Filtro Lineal: Predicción
CA
R. U
PM
-C
SIC
14 Diego Rodriguez-Losada
Filtro Lineal: Corrección
8
CA
R. U
PM
-C
SIC
15 Diego Rodriguez-Losada
Indice
Probabilidad
Derivación caso 1D
KF lineal
EKF
Aplicaciones:
Estimación de un voltaje
Estimación de un misil
Localización 1D
Localización 2D
CA
R. U
PM
-C
SIC
16 Diego Rodriguez-Losada
Filtro Extendido (EKF)
Modelo del proceso
Modelo de observación
Ruidos
( 1) ( ( ), ( 1))k k k x f x u
ˆ( 1) ( 1), ( 1)
1 0, ( 1)
k N k k
v k N k
u u Q
R
( 1) ( ( 1)) 1k k v k z h x
9
CA
R. U
PM
-C
SIC
17 Diego Rodriguez-Losada
Predicción: Linealización
modelo del proceso
2
( 1) ( ( ), ( 1))
ˆ ˆ ( ( | ), ( 1))
ˆ ( 1) ( ) ( | )
ˆ ( 1) ( 1) ( 1)
x
u
k k k
k k k
k k k k
k k k O
x f x u
f x u
F x x
F u u
ˆ ( | )ˆ ( 1)
ˆ ( | )ˆ ( 1)
( 1)
( 1)
xk kk
uk kk
k
k
xu
xu
fF
x
fF
u
CA
R. U
PM
-C
SIC
18 Diego Rodriguez-Losada
Filtro Extendido: Predicción
T
T
ˆ ˆ ˆ( 1| ) ( ( | ), ( 1))
( 1| ) = ( 1) ( | ) ( 1)
( 1) ( 1) ( 1)
x x
u u
k k k k k
k k k k k k
k k k
x f x u
P F P F
F Q F
ˆ( ) ( | ), ( | )k N k k k kx x P
ˆ( 1) ( 1), ( 1)k N k k u u Q
10
CA
R. U
PM
-C
SIC
19 Diego Rodriguez-Losada
Corrección: Linealización
modelo de observación
2
( 1) ( ( 1)) 1
ˆ ( ( 1| ))
ˆ ( 1) ( 1) ( 1| )x
k k v k
k k
k k k k O
z h x
h x
H x x
ˆ
( 1)x k
x
z
hH
x
CA
R. U
PM
-C
SIC
20 Diego Rodriguez-Losada
Filtro Extendido: Corrección
ˆ( 1| ), ( 1| )k k k k x P
1
Ganancia del filtro
(k + 1) = ( 1| ) (k + 1) (k + 1)
Correccion del estado
ˆˆ ˆ( 1| 1) ( 1| ) (k + 1) ( 1)
( 1| 1) = (k + 1) (k + 1) ( 1| )
( 1| ) (k + 1) (k
T
x
x
k k
k k k k k
k k k k
k k
K P H S
x x K h
P I K H P
P K S + 1) (k + 1)TK
ˆ( 1| 1), ( 1| 1)k k k k x P
11
CA
R. U
PM
-C
SIC
21 Diego Rodriguez-Losada
Corrección (Joseph)
T
( 1| 1) = ( 1| ) (k + 1) (k + 1)
(k + 1) (k + 1)
Tk k k k
P PI K H I K H
K RK
CA
R. U
PM
-C
SIC
22 Diego Rodriguez-Losada
Ciclo
12
CA
R. U
PM
-C
SIC
23 Diego Rodriguez-Losada
Indice
Probabilidad
Derivación caso 1D
KF lineal
EKF
Aplicaciones:
Estimación de un voltaje
Estimación de un misil
Localización 1D
Localización 2D
CA
R. U
PM
-C
SIC
24 Diego Rodriguez-Losada
Estimación de un voltaje
13
CA
R. U
PM
-C
SIC
25 Diego Rodriguez-Losada
Estado y modelo del proceso Estado
Modelo del proceso
Estimación inicial
Vx
( 1) ( )V t V t
ˆ(0) 4.5 , (0) 0.5P x
CA
R. U
PM
-C
SIC
26 Diego Rodriguez-Losada
Observación y corrección
= +
(0, )
V
N
z
R
S=P+cov_obs;
K=P*inv(S);
inn=z-x;
x=x+K*inn;
P=P-K*S*K';
14
CA
R. U
PM
-C
SIC
27 Diego Rodriguez-Losada
Demo
CA
R. U
PM
-C
SIC
28 Diego Rodriguez-Losada
Trayectoria proyectil
15
CA
R. U
PM
-C
SIC
29 Diego Rodriguez-Losada
Trayectoria proyectil
Estado
x
y
x
y
v
v
x
CA
R. U
PM
-C
SIC
30 Diego Rodriguez-Losada
Modelo del proceso
function x=ProcessModel(x,t)
x(1)=x(1)+x(3)*t;
x(2)=x(2)+x(4)*t-4.9*t*t;
x(3)=x(3);
x(4)=x(4)-9.8*t;
end
2
1
2
x
y
x x
y y
x x v t
y y v t g t
v v
v v g t
1 0 0
0 1 0( )
0 0 1 0
0 0 0 1
t
tf
x x Fx x
16
CA
R. U
PM
-C
SIC
31 Diego Rodriguez-Losada
Observación
z= [ x_r(1)+sigma_obs*randn();
x_r(2)+sigma_obs*randn()];
%Jacobian
H=[1 0 0 0;
0 1 0 0];
=h + +
(0, )
x
y
N
z x
R
1 0 0 0
0 1 0 0
ya que x
y
x
y
v
v
H
x
CA
R. U
PM
-C
SIC
32 Diego Rodriguez-Losada
Corrección
%Next kalman updated position (k|k)
S=H*P*H'+R;
K=P*H'*inv(S);
inn=z-x_e(1:2);
x_e=x_e+K*inn;
P=P-K*S*K';
17
CA
R. U
PM
-C
SIC
33 Diego Rodriguez-Losada
Inicialización
(1|1) (1)
(1|1) (1)
(2 | 2) (2)
(2 | 2) (2)
(2 | 2) (1|1)(2 | 2)
(2 | 2) (1|1)(2 | 2)
x
y
x
y
y
y
x
y
x
y
x xv
t
y yv
t
z
z
z
z
0 0 0 0
0 0 0 0ˆ (0) , (0)
0 0 0 0
0 0 0 0
P
x
CA
R. U
PM
-C
SIC
34 Diego Rodriguez-Losada
Demo
18
CA
R. U
PM
-C
SIC
35 Diego Rodriguez-Losada
Localización 1D
s
p(s)
CA
R. U
PM
-C
SIC
36 Diego Rodriguez-Losada
Predicción
19
CA
R. U
PM
-C
SIC
37 Diego Rodriguez-Losada
Observación
Modelo inverso de
observación
1 1
6 posicion conocida baliza
k kz y x
y
CA
R. U
PM
-C
SIC
38 Diego Rodriguez-Losada
Derivación: Información
20
CA
R. U
PM
-C
SIC
39 Diego Rodriguez-Losada
Derivación II
CA
R. U
PM
-C
SIC
40
Demo
Robot se mueve 1
m
sigma_u=0.25
sigma_z=0.35
Una baliza a 6
metros
21
CA
R. U
PM
-C
SIC
41
Localización robot
CA
R. U
PM
-C
SIC
42
Localización
ˆN( ( | ), ( | ))
T
R
R R
x y
k k k k
x
x x P
1 2
1 2
n
n
x x xM
y y y
22
CA
R. U
PM
-C
SIC
43 Diego Rodriguez-Losada
Transformaciones relativas
Composición e inversión
cos sin
sin cos
ik ij jk ij jk ij
ik ij jk ik ij jk ij jk ij
ik ij jk
x x x y
y y x y
x x x
cos sin
sin cos
ji ij ij ij ij
ji ij ji ij ij ij ij
ji ij
x x y
y x y
x x
CA
R. U
PM
-C
SIC
44 Diego Rodriguez-Losada
Jacobianas
ˆ ˆ,
2 2
ˆ ˆ,
cos sin
sin cos
cos sin
sin cos
ˆ ˆ( ) ( , )
ˆ ˆ( ) ( , )
a b
a b
c a b a b a
c a b c a b a b a
c a b
c a b a b a
c a b
c a b a b a
c
c a b
a
c
c a b
b
x x x y
y y x y
x x x y
y y x y
1 1
x x
x x
x x x
x x x
xJ x J x x
x
xJ x J x x
x
23
CA
R. U
PM
-C
SIC
45 Diego Rodriguez-Losada
Movimiento del robot I
ˆ( 1) N( ( 1), ( 1))k k k u u Q
( 1) ( ( ), ( 1)) ( ) ( 1)k k k k k x f x u x u
Medida odométrica
Ecuación de predicción
CA
R. U
PM
-C
SIC
46
Movimiento del robot II
Predicción del estado en k+1|k
ˆ ( | )
( | )
k k
k k
x
P
T T( 1| ) = ( 1) ( | ) ( 1) ( 1) ( 1) ( 1)x x u uk k k k k k k k k P F P F F Q F
ˆ ( | )ˆ ( 1)
2ˆ ( | )ˆ ( 1)
ˆ ˆ( 1) ( ( | ), ( 1))
ˆ ˆ( 1) ( ( | ), ( 1))
R
R
x Rk k
k
u Rk k
k
k k k k
k k k k
1xu
xu
fF J x u
x
fF J x u
u
( 1) ( ( ), ( 1)) ( ) ( 1)k k k k k x f x u x u
ˆ( 1), ( 1)k k u Q
ˆ ˆ ˆ( 1| ) ( | ) ( 1)R Rk k k k k x x u
coste
computacional
O(1)
24
CA
R. U
PM
-C
SIC
47 Diego Rodriguez-Losada
Observaciones En el instante k+1 el robot realiza la
observación de ‘m’ objetos.
Asociacion de datos conocida
Asociación de datos desconocida:
Decidir a cual objeto corresponde cada observación, o si
es un objeto nuevo.
Test de compatibilidad individual
Test de compatibilidad conjunta
1 2
1 2
1 2
m
m
x x x
y y y
m
z z z
z z z
i i i i
z
CA
R. U
PM
-C
SIC
48 Diego Rodriguez-Losada
Corrección I
Ecuación implícita de medida
( ( 1), ( 1)) 0t
Rk k h z x
( ( 1), ( 1)) ( 1) ( 1) ( 1) 0i
t t
R F Rk k k k k h z x x x z
25
CA
R. U
PM
-C
SIC
49
For point landmarks
index
index=indices(i)
( 1) ( ( 1), ( 1), )
( 1) ( 1) 0
i R
R i
k k k
k k
h h z x M
M x z
CA
R. U
PM
-C
SIC
50
Corrección II
ˆ ( 1| )
( 1| )
R k k
k k
x
P
ˆ ˆ ˆ( 1) ( ( 1), ( 1| )) 0Rk k k k h h z x
T TS(k + 1) = H ( 1)P( 1| )H ( 1) H ( 1)R( 1)H ( 1)x x z zk k k k k k k
( 1) ( 1), ( 1)t k N k k z z R ˆ
2ˆ
ˆ( 1) = ( , ) 2x3
ˆ( 1) = ( , ) 2x2
R
i
R
i
x R i
R
z R i
i
k
k
1xz
xz
hH J x z
x
hH J x z
z
( ( 1), ( 1)) ( 1) ( 1) ( 1) 0i
t t
R F Rk k k k k h z x x x z
26
CA
R. U
PM
-C
SIC
51
Corrección EKF
1
Ganancia del filtro
(k + 1) = ( 1| ) (k + 1) (k + 1)
Correccion del estado
ˆˆ ˆ( 1| 1) ( 1| ) (k + 1) ( 1)
( 1| 1) = (k + 1) (k + 1) ( 1| )
T
x
R R
x
k k
k k k k k
k k k k
K P H S
x x K h
P I K H P
ˆ ( 1)k h
(k + 1)S
ˆ( 1| 1)
P( 1| 1)
x k k
k k
ˆ
2ˆ
ˆ( 1) = ( , )
ˆ( 1) = ( , )
R
i
R
i
x R i
R
z R i
i
k
k
1xz
xz
hH J x z
x
hH J x z
z
ˆ ( 1| )
( 1| )
R k k
k k
x
P
CA
R. U
PM
-C
SIC
52
Asociación datos desconocida
ˆ d=dist( ( 1),0);
if(d<min)
min
j
i
F
foreach
foreach M
k
d
end
end
end
z z
x
h
27
CA
R. U
PM
-C
SIC
53 Diego Rodriguez-Losada
Test de Mahalanobis
individual
1 2
dim( ( 1)),ˆ ˆ( 1) (k + 1) ( 1)T T
kk k
hh S h
ˆ ( 1)k h S(k + 1)
NO SI Emparejamiento
no válido Emparejamiento
válido
Estrategias:
-Nearest Neighbour NN: vecino más cercano
-Todos los emparejamientos válidos.
CA
R. U
PM
-C
SIC
54 Diego Rodriguez-Losada
Test Compatibilidad
Conjunta
Robustez en la asociación.
T T -1 2
dim( ( 1)),ˆ ˆ
acumacum acum acum k hh S h
ˆ ( 1)acum k h S (k + 1)acum
28
CA
R. U
PM
-C
SIC
55 Diego Rodriguez-Losada
Demo
CA
R. U
PM
-C
SIC
56 Diego Rodriguez-Losada
Derivación bayesiana
1
1
1
1 1
ˆ ˆ( | , ) exp Estimation
2
( ) ( )( | ) exp Measurement model
2
( | , ) ( | , , ) ( | , )
( | ) ( |
T
t t t t tt t
t
T
t t t t t
t t
Bayest t t t t t
t t t t
Markov
t t t
p
p
p p p
p p
x x P x xx z u
z h x R z h xz x
x z u z x z u x z u
z x x
1
1 1
, )
ˆ ˆ ( ) ( ) exp
2 2
t t
T TGauss
t t t t t t t t t t
z u
x x P x x z h x R z h x
29
CA
R. U
PM
-C
SIC
57 Diego Rodriguez-Losada
Derivación bayesiana
2
1 1
1 1
1 1 1
arg max( ( | , ))
ˆ ˆ ˆ ˆ ˆ( ) ( ) ( )
ˆ ˆ ˆ ˆ ˆ ˆ( ) ( )
ˆ ˆ ˆ2 2 ( ) 0
ˆ
t
t t
t t
t t t t t t t t t
T T
t t t t t t t t t t t t t t
T
t t t t t t t t
t
T
t t t t t t
p
O
x
μ x z u
h x h x H x x x x h x Hx Hx
f x x P x x z h x Hx Hx R z h x Hx Hx
fP x x H R z h x Hx Hx
x
P x P x H R z h
1 1
1 1 1 1 1
11 1 1
1
ˆ ˆ( ) 0
ˆ ˆ( )
ˆ ˆ( )
ˆ ˆ ˆ ˆ( ) ( )
T T
t t t t t
T T T
t t t t t t t t t
T T
t t t t t t t
T T
t t t t t t t t t t
x
x H R H H R Hx
P H R H x P H R H x H R z h x
μ x P H R H H R z h x
μ x PH HPH R z h x x K z h x
CA
R. U
PM
-C
SIC
58 Diego Rodriguez-Losada
Derivación bayesiana
' ' '
'
T T
t t t t t t t t
T T T T
t t t t t t t t
T T T T T
t t t t t t t t
T
P E x x E x W x W
E x x W x x W W W
E x x E W x E x W E W W
P P WSW