Seguimiento – Filtro de Kalman - UJI

1TAVC. Seguimiento

Seguimiento Seguimiento –– Filtro de Filtro de KalmanKalman

ÍndiceIntroducciónEstimación lineal en sistemas estáticos• Error cuadrático medio (MMSE)• Mínimos cuadrados (LS)• Forma recursiva de los mínimos cuadrados• Ejemplo: ajuste de un polinomio

Estimación lineal en sistemas dinámicos estocásticos• Sistemas dinámicos en el espacio de estados• Filtro de Kalman• Filtro de Kalman en estado estacionario• Inicilización del filtro de Kalman• Ejemplo: movimiento con aceleración constante

2TAVC. Seguimiento

IntroducciónIntroducción

Conceptos:Al seguir un objetivo tenemos• Imprecisión de medida: modelada con ruido aditivo• Incerteza en las medidas: debido al origen de las

medidas

Seguimiento: procesar medidas de un objetivo para mantener una estimación de su estado (posición velocidad, etc)

Aplicaciones: robótica, vigilancia, análisis de video.

3TAVC. Seguimiento

Estimación lineal en sistemas estáticosEstimación lineal en sistemas estáticos

Estimación por mínimo error cuadrático medio(MMSE: Minimum mean-square error)

estado de vector del estimación :ˆodesconocid estado, de vector : medidas, de vector :

,aleatorios vectores:,

xxz

zx

Hallar la estimación lineal: bAzx +=ˆ

Que minimiza el error cuadrático medio: { })ˆ()ˆ(ˆ xxxxxx −′−=−= EJ

La mejor estimación MMSE es aquella en que el error de estimacion es no sesgado:

{ } zxxxxx n observació la a ortogonal es ~ ; 0~ ; ˆ~ =−= E

Resultado, mejor estimación: )(ˆ 1 zzPPxx −+= −zzxz

matriz de covarianza de la estimación: { } zxzzxzxxzxx E PPPPxxP 1/

~~ −−=′=

4TAVC. Seguimiento


Estimación por mínimos cuadrados (LS: Least Squares)

medidas de vector : odesconocid aleatorio, no constante, estado, devector :

zx

Hallar la estimación que minimiza el error cuadrático:

( ) ( ) ( ) ( )xHzxHzxHzxHz ˆ)(ˆˆ)()()(ˆ)()()( 1

1

1 kkkkkk

iRiiiRiikJ −

′−=⎥⎦

⎤⎢⎣⎡ −′−= −

=

−∑donde:

Modelo: kiiii ..1 , )()()( =+= wxHz

diagonal matriz

)(000............0...)2(00...0)1(

)(...

)1( ;

)(...

)1( ;

)(...

)1(

⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜

⎝

⎛

=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=+=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

k

kkk

k

kkkkk

R

RR

R

w

ww

H

HHwxH

z

zz

5TAVC. Seguimiento


El ruido w(i) se considera independiente, de media 0 y covarianza R(i)

Siendo la matriz de covarianza de la estimación:

Minimizando J(k):

[ ] kkkkkkk zRHHRHx 111 )()()()()(ˆ −−− ′′=

0=∇ )(kJx

Se obtiene:

[ ] 11)()()( −−′= kkkk HRHP

Finalmente: kkkkk zRHPx 1)())(()(ˆ −′=

En esta forma del estimador LS la estimación se calcula después de conocer todas las medidas (i=1..k). Pero el estimador LS se puede expresar de forma recursiva, calculándose una mejor estimación acada nueva medida realizada

6TAVC. Seguimiento


Forma recursiva del estimador por mínimos cuadrados

)1()1()(ˆ)]1()1([)1(ˆ)1()1()1()()1(

)1()1()()1()1()1()()1()1(

1

+++++−=++′++−=+

++′=+

+++′+=+−

kkkkkkkkkkk

kkkkkkkkk

zWxHWIxWSWPP

SHPWRHPHS

7TAVC. Seguimiento


Ajuste de un polinomio de grado n a una serie de medidas

n=1 velocidad constante. n=2 aceleración constante. n=3 aceleración lineal

!)(

0 jtatx

jn

jj∑

=

=X(t): posición, t: tiempo

Medidas: kiiii ..1 , )()()( =+= waHz

a(j): parámetros a estimar, derivada j-esima en t=0

⎟⎟⎠

⎞⎜⎜⎝

⎛=

⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜

⎝

⎛

=!

...!2

1)( , ...

21

0

nttti

a

aa

nii

i

n

Ha

8TAVC. Seguimiento


Matriz de medidas apiladas:

IR 2σ=kw(i): ruido no correlacionado, de media cero, varianza

⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜

⎝

⎛

=

)(...

)2()1(

k

k

H

HH

H

estimación: ∑∑=

−

=

′⎥⎦

⎤⎢⎣

⎡ ′=k

i

k

iiiiik

1

1

1)()()()()(ˆ zHHHa

covarianza:1

1

2 )()()(−

=⎥⎦

⎤⎢⎣

⎡ ′= ∑k

iiik HHP σ

donde:

⎟⎟⎟⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜⎜⎜⎜

⎝

⎛

⎟⎟⎠

⎞⎜⎜⎝

⎛

=′

+

+

21

12

!...

!!

............!

...

!...1

)()(

nt

nt

nt

nttt

ntt

ii

ni

ni

ni

ni

ii

ni

i

HH

9TAVC. Seguimiento


Para n=1:

kikiti ..1 , 2

12=∆

−−=Si las muestras están equiespaciadas en el tiempo:

⎟⎟

⎠

⎞

⎜⎜

⎝

⎛

∆+−=

2

2

)1)(1(12001

)(kkk

k σP, )(

)()(

)(ˆ)(ˆ

1

12

1

0

⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜

⎝

⎛

=⎟⎟⎠

⎞⎜⎜⎝

⎛

∑

∑

=

=k

ii

k

i

tiz

izk

kaka

Pσ

Para n=2:

⎟⎟⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜⎜⎜

⎝

⎛

∆−−∆−−

∆−

∆−−

−−

=

42222

22

222

2

)4)(1(7200

)4(30

0)1(

120

)4(300

)4(4)73(3

)(

kkkkk

kk

kkkkk

kP σ,

2)(

)(

)(

)()(ˆ)(ˆ)(ˆ

1

21

1

2

2

1

0

⎟⎟⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜⎜⎜

⎝

⎛

=⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

∑

∑

∑

=

=

=

k

i

i

k

ii

k

i

tiz

tiz

iz

kkakaka

Pσ

10TAVC. Seguimiento

Estimación lineal en sistemas dinámicosEstimación lineal en sistemas dinámicos

Sistema dinámico lineal estocástico de tiempo continuo

{ } { } )()()()( ,0)( blanco. ruido :)(),(

)()()()(

τδτ −=′=⎩⎨⎧

+=++=

tttEtEtttttt

(t)(t)(t)(t)(t)(t)

VvvvwvwxCz

vuBxAx&

La densidad espectral de potencia del ruido (Transformada de Fourier) es constante con la frecuencia.

Sistema dinámico lineal estocástico de tiempo discreto

{ } { }{ }{ } ij

ijijij

ijiE

jijiijiEkEkEkk

kkkk(k)(k)(k)(k)(k))(k

δ

δδδ

)()()(

si 1 , si 0Kroneker de delta ,)()()( ,0)( ,0)( blanco. ruido :)(),(

)()()()(

1

Rww

Qvvwvwv

wxHzvuGxFx

=′

==≠==′==

⎩⎨⎧

+=++=+

Sistemas dinámicos en el espacio de estados

11TAVC. Seguimiento


[ ][ ]

[ ])|(),|(N)(kNkkNk

kkkk(k)(k)(k)(k)(k))(k

0000ˆ0 :inicial Estado)(,0)()(,0)(

)()()()(1

PxxRwQv

wxHzvuGxFx

∈

∈∈

⎭⎬⎫

+=++=+

Filtro de Kalman

Algoritmo

|k)(k)(k|k)(k

(k)(k)k)(k(k)k)(k

1ˆ11ˆmedida nueva la de -

(k)(k)k)|(k(k)k)|1(k :error del covarianza la de -

|ˆ|1ˆ :estado del -

Predicción

++=+

+′=+

+=+

xHz

QFPFP

uGxFx

[ ]

[ ] k)|1(k)1()1(1)k|1(k :error del covarianza la de -

)|1(ˆ)1()1(|1ˆ1|1ˆ :estado del -

1 medida nueva lacon ión Actualizac

+++−=++

+−++++=++

+

PHWP

zzWxx

z

kkI

kkkkk)(k)k(k

)(k

)(k)(k|k)(k)(k

kkkk)(k)(k

1111 :Kalman de ganancia -

)1()1()|1(11 :innovación la de covarianza -

1 ++′+=+

+++′++=+

−SHPW

RHPHS

12TAVC. Seguimiento


La entrada u(t) no tiene efecto en la precisión de la estimación(en la fórmula de la covarianza P(k+1) )

Si el ruido es Gaussiano el filtro de Kalman es el mejor estimador. Si no es Gaussiano, el filtro de Kalman es aún el mejor estimador lineal.

vector de estado(unidimensional)

Estimación

Medidas

13TAVC. Seguimiento


Ejemplo: seguimiento en 1 dimensión. Movimiento uniforme.

( )01,10

10 escalares, )(),(

escalar )( ,)()()1(11

1 ,

velocidadposición

=⎟⎟⎠

⎞⎜⎜⎝

⎛=

∈=

⎭⎬⎫

+++=++=+

⎟⎟⎠

⎞⎜⎜⎝

⎛=

HF

ΓvHx

vFxxx

T,r)N(w(k)kwkz

kvkvkkw)(k)z(k

(k)(k))(k

V(k): error en la aceleración. Ruido blanco:{ }{ } kjvjvkvE

kvEδσ 2)()(

0)(=

=

Inicialización:

{ } 223

342

2

2/2/4/

)()(

2/

vv TTTT

kkE

TT

σσ ⎟⎟⎠

⎞⎜⎜⎝

⎛=′=′=

⎟⎟⎠

⎞⎜⎜⎝

⎛=

ΓΓvvQ

Γ

{ } { }{ } { } ⎟⎟

⎠

⎞⎜⎜⎝

⎛=⎟⎟

⎠

⎞⎜⎜⎝

⎛=

⎟⎟⎠

⎞⎜⎜⎝

⎛−−

=⎟⎟⎠

⎞⎜⎜⎝

⎛=⎟⎟

⎠

⎞⎜⎜⎝

⎛=

21101

1000

1

0

/2//

)0|0(ˆ),0|0(ˆcov)0|0(ˆ),0|0(ˆcov)0|0(ˆ),0|0(ˆcov)0|0(ˆ),0|0(ˆcov

)0|0(

/)]1()0([)0(

)0|0(ˆ)0|0(ˆ

inicial velocidadestimacióninicialposición estimación

)0|0(ˆ

TrTrTrr

xxxxxxxx

Tzzz

xx

P

x

14TAVC. Seguimiento


Ejemplo: 1 dimensión. Movimiento uniformemente acelerado.

( )001,100

102/1

0 escalares, )(),(escalar )( ,)()(

)1(111

,naceleració

velocidadposición

2

=⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

=

∈=

⎪⎭

⎪⎬

⎫

+++=++=+

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

HF

ΓvHx

vFxxx

TTT

,r)N(w(k)kwkzkvkvk

kw)(k)z(k(k)(k))(k

V(k): error en la aceleración. Ruido blanco:{ }{ } kjvjvkvE

kvEδσ 2)()(

0)(=

=

Inicialización:

{ } 2

2

23

234

2

2

12/2/

2/2/4/)()( ,

1

2/

vv

TTTTT

TTTkkET

Tσσ⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

=′=′=⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

= ΓΓvvQΓ

{ } { } { }{ } { } { }{ } { } { } ⎟

⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛−−=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

0000/2/0/

)0|0(ˆ),0|0(ˆcov)0|0(ˆ),0|0(ˆcov)0|0(ˆ),0|0(ˆcov)0|0(ˆ),0|0(ˆcov)0|0(ˆ),0|0(ˆcov)0|0(ˆ),0|0(ˆcov)0|0(ˆ),0|0(ˆcov)0|0(ˆ),0|0(ˆcov)0|0(ˆ),0|0(ˆcov

)0|0(

0/)]1()0([

)0(

)0|0(ˆ)0|0(ˆ)0|0(ˆ

inicialn aceleració estimacióninicial velocidadestimacióninicialposición estimación

)0|0(ˆ

2

221202

211101

201000

2

1

0

TrTrTrr

xxxxxxxxxxxxxxxxxx

Tzzz

xxx

P

x

15TAVC. Seguimiento

Ejemplo resultadosEjemplo resultados

16TAVC. Seguimiento

BibliografiaBibliografia

Y. Bar-Shalom, T.E. Fortman. “Tracking and data association”. Mathematics in Science and Engineering, vol 179. Academic Press.B. Rao. “Data Association methods for tracking systems”. Active vision. MIT Press. Cambridge, MA, EEUU.I.J. Cox. “ A review of statistical data association techniques for motion correspondence”. International Journal of Computer Vision 10:1, 53-66.1993.Z. Zang. “Token tracking in a cluttered scene”. INRIA Sophia-Antipolis. Informe de investigación 2072.1993.R. Deriche, O. Faugueras. “Tracking line segments”. Image and Vision Computing 8:4, 261-270. 1990.G.L.Scott, H.C. Longuet-Higgins. “An algorithm for associating the features of two images”. Proceedings of the Royal Society London B:244,21-26. 1991.I.K. Sethi, R. Jain. “Finding trayectories of feature points in a monocular image sequence”. IEEE Transactions on Pattern Analysis and Machine Intelligence PAMI 9:1. 1987.

Seguimiento – Filtro de Kalman - UJI

Documents

Transcript of Seguimiento – Filtro de Kalman - UJI