El Filtro de Kalman - Marco Montagni · El Filtro de Kalman UPM-CSIC Diego Rodriguez-Losada 2...

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


Probabilidad

CA

R. U

PM

-C

SIC


Distribución uniforme

3

CA

R. U

PM

-C

SIC


Distribución normal

CA

R. U

PM

-C

SIC


Propiedades Normal

Linealidad

Intervalos

http://upload.wikimedia.org/wikipedia/commons/7/74/Normal_Distribution_PDF.svg

http://es.wikipedia.org/wiki/Archivo:Standard_deviation_diagram_(decimal_comma).svg

4

CA

R. U

PM

-C

SIC


Normal multivariable

CA

R. U

PM

-C

SIC


Indice

Probabilidad

Derivación caso 1D

KF lineal

EKF

Aplicaciones:



Localización 1D

Localización 2D

5

CA

R. U

PM

-C

SIC


Derivación 1D (var min)

CA

R. U

PM

-C

SIC


Derivación 1D

Reorganizando:

Podemos llamar:

Entonces:

6

CA

R. U

PM

-C

SIC


Indice

Probabilidad

Derivación caso 1D

KF lineal

EKF

Aplicaciones:



Localización 1D

Localización 2D

CA

R. U

PM

-C

SIC


Filtro Kalman Lineal (KF)

Modelo del proceso

Modelo de observación

Ruidos

7

CA

R. U

PM

-C

SIC


Filtro Lineal: Predicción

CA

R. U

PM

-C

SIC


Filtro Lineal: Corrección

8

CA

R. U

PM

-C

SIC


Indice

Probabilidad

Derivación caso 1D

KF lineal

EKF

Aplicaciones:



Localización 1D

Localización 2D

CA

R. U

PM

-C

SIC


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


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


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


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


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


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


Ciclo

12

CA

R. U

PM

-C

SIC


Indice

Probabilidad

Derivación caso 1D

KF lineal

EKF

Aplicaciones:



Localización 1D

Localización 2D

CA

R. U

PM

-C

SIC



13

CA

R. U

PM

-C

SIC


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


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


Demo

CA

R. U

PM

-C

SIC


Trayectoria proyectil

15

CA

R. U

PM

-C

SIC


Trayectoria proyectil

Estado

x

y

x

y

v

v

x

CA

R. U

PM

-C

SIC


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


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


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


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


Demo

18

CA

R. U

PM

-C

SIC


Localización 1D

s

p(s)

CA

R. U

PM

-C

SIC


Predicción

19

CA

R. U

PM

-C

SIC


Observación

Modelo inverso de

observación

1 1

6 posicion conocida baliza

k kz y x

y

CA

R. U

PM

-C

SIC


Derivación: Información

20

CA

R. U

PM

-C

SIC


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


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


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


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


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


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


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


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


Demo

CA

R. U

PM

-C

SIC


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



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



' ' '

'

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

El Filtro de Kalman - Marco Montagni · El Filtro de Kalman UPM-CSIC Diego Rodriguez-Losada 2...

Documents

Transcript of El Filtro de Kalman - Marco Montagni · El Filtro de Kalman UPM-CSIC Diego Rodriguez-Losada 2...