Seguimiento – Filtro de Kalman - UJI
Transcript of 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 lineal en sistemas estáticosEstimación lineal en sistemas estáticos
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
Estimación lineal en sistemas estáticosEstimación lineal en sistemas estáticos
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
Estimación lineal en sistemas estáticosEstimación lineal en sistemas estáticos
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
Estimación lineal en sistemas estáticosEstimación lineal en sistemas estáticos
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
Estimación lineal en sistemas estáticosEstimación lineal en sistemas estáticos
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
Estimación lineal en sistemas estáticosEstimación lineal en sistemas estáticos
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
Estimación lineal en sistemas dinámicosEstimación lineal en sistemas dinámicos
[ ][ ]
[ ])|(),|(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
Estimación lineal en sistemas dinámicosEstimación lineal en sistemas dinámicos
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
Estimación lineal en sistemas dinámicosEstimación lineal en sistemas dinámicos
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
Estimación lineal en sistemas dinámicosEstimación lineal en sistemas dinámicos
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.