Seguimiento de rasgos usando el filtro de Kalman. Ejemplo.

Seguimiento de rasgos usando el filtro de Kalman. Ejemplo.

Seguimiento de rasgos usando el
filtro de Kalman. Ejemplo.
Rafael Molina y Luis David Alvarez Corral
Dpto. Ciencias de la Computación e IA
Universidad de Granada
Rafael Molina y Luis D.
Alvarez
Seguimiento de rasgos usando el
filtro de Kalman
1
Contenidos
! Seguimiento basado en el filtro de Kalman
"
"
"
Filtro de Kalman aplicado al seguimiento
Parámetros a estimar
Asociación de rasgos
! Bibliografía
I. Seguimiento basado en el filtro de Kalman
I.1 Filtro de Kalman aplicado al siguimiento
El filtro de Kalman es, como ya sabemos, un algoritmo recursivo
que estima la posición y la incertidumbre de un rasgo en
movimiento en la imagen siguiente.
Es importante tener en cuenta que vamos a determinar: dónde
buscar el rasgo y su incertidumbre. Supondremos que en cada
imagen somos capaces de detectar rápidamente los rasgos de
interés.
Tenemos imágenes en los instantes tk=t0+k∆T, donde ∆T se
supone pequeño.
Modelo del sistema
El sistema físico se modeliza por un vector de estados s, llamado
simplemente el estado, y un conjunto de ecuaciones llamado el
modelo del sistema.
El tiempo de observación tiene la forma tk=t0+k∆T, k=0,1..., ∆T es
el intervalo de muestreo y sk el estado s(tk).
Vamos a suponer que ∆T es pequeño y que por tanto podemos
utilizar un modelo del sistema lineal.
Estado:
sk=(xk yk vxk vyk )t
posición en el instante k : (xk yk)
velocidad en el instante k : (vxk vyk)
Modelo del sistema:
sk=φk-1 sk-1 + ξk-1
 xk   1
  
 yk   0
 vx  = 0
 ky  
v  0
 k 
0 1 0   xk −1   ξ1 
  

1 0 1   yk −1   ξ 2 
· x  +  

ξ3
0 1 0 vk −1
 y   
0 0 1   vk −1   ξ 4 
φk-1 : El subíndice k-1 en φ indica que la matriz de transición φ es
(puede ser) una función del tiempo. En este caso no depende del
tiempo, la consideramos constante.
¿Cuál es la interpretación de sk=φk-1 sk-1?
xk= xk-1+vxk-1
yk= yk-1+vyk-1
vxk = vxk-1
vyk = vyk-1
En un instante k la posición es posición en el instante k-1 más
undesplazamiento (velocidad por tiempo, con t=1).
ξk-1 : Es un vector aleatorio gaussiano blanco de media cero y
matriz de covarianza Qk , y modeliza el ruido aditivo.
σ Q 2
0
0
0 


2
 0 σQ
0
0 
Qk = 

2
0
0
σ
0
Q


2
 0
0
0 σ Q 

donde σQ2 es un parámetro
Modelo de medida
El segundo ingrediente en la teoría de la estimación es el modelo
de medida.
Suponemos que en cada instante tk tenemos una observación
ruidosa del vector de estados.
Medida: zk=(zxk zyk) , donde zxk y zyk son las medidas que nos da
un sensor.
Modelo de medida: zk = Hk sk +µk
 xk 
 
x
 z k  1 0 0 0   yk   µ1 
 y  = 
 z  0 1 0 0 · v x  +  µ 
 k   2
 k 
vy 
 k
Hk : La matriz H controla el cambio de la estimación y es (puede
ser) una función del tiempo dependiente. En este caso no
depende del tiempo, la consideramos constante.
¿Cuál es la interpretación de zk=Hk sk?
zxk= xk
zyk= yk
µk-1 : Es un vector aleatorio gaussiano blanco de media cero y
matriz de covarianza Rk , y modeliza el ruido aditivo.
σ R 2
0 

Rk = 
2
 0 σR 
donde σR2 es un parámetro
Tenemos un punto que se mueve y se realizan observaciones
ruidosas de su posición en cada instante. Notemos que en casos
reales dichas observaciones ruidosas tienen que ser localizadas
utilizando, por ejemplo un detector de rasgos
Queremos estimar las posiciones reales usando el filtro de Kalman
Ecuaciones del filtro de Kalman
Tenemos un estimador sk-1 y su matriz de covarianzas Pk-1.
Primero calculamos, antes de que llegue la observación zk,
Pk ' = φ k −1 Pk −1φ k −1 + Q k −1
t
después la ganancia
(
K k = Pk ' H k H k Pk ' H k + R k
t
t
)
−1
a continuación el estimador del estado k óptimo (aquí metemos la
observación)
s k = φ k −1 s k −1 + K k ( z k − H k φ k −1 s k −1 )
y por último la matriz de covarianzas de este estimador
(
Pk = Pk '− Pk ' H k H k Pk ' H k + R k
t
t
)
−1
H k Pk ' = Pk '− K k H k Pk '
= (I − K k )Pk ' (I − K k ) − K k R k K k = (I − K k H k )Pk '
t
t
Tenemos un objeto que deseamos seguir.
El problema es que el sensor (estimador, etc) que nos da la
posición del objeto en cada instante no es muy preciso.
Este proceso lo simulamos en el ejemplo
detectando la posición del punto en cada
instante, y añadiéndole un error aleatorio
basado en un máximo dado como
parámetro.
r = Rand(1) Є (0, 1.0)
zxk
zyk
pmr=2 (nº entero dado como parámetro)
r*(pmr+1)*2 = r*3*2 Є (0, 6.0)
[0, 6.0] – (pmr+1) = (0, 6.0) – 3 Є (-3.0, 3.0)
fix redondea al entero más cercano hacia 0
fix(-3.0,3.0) = -2, -1, 0, 1, 2
ruido_x=fix(((rand(1)*(pmr+1))*2)-(pmr+1))
ruido_y=fix(((rand(1)*(pmr+1))*2)-(pmr+1))
Implementación
!
% Implementacion del FILTRO DE KALMAN ***************************
!
!
!
!
!
!
!
!
!
!
!
%
% Parametros:
% vx0
% vy0
% varR
% varQ
% pixMaxRuido
% rutaSec -> directorio en el que esta contenida la secuencia (sin \ al final) [ruta absoluta]
% nombreSec -> nombre que tienen los ficheros de la secuencia (hasta justo antes del numero y sin .bmp)
% nFrames
%
!
function [ ] = FiltroKalman(vx_0,vy_0,varR,varQ,pixMaxRuido,rutaSec,nombreSec,nFrames)
!
!
!
!
!
!
!
!
!
% Constantes prefijadas (H de k // A de k-1)
H_k=[1 0 0 0;
0 1 0 0]; % Matriz H que controla el cambio de la estimacion z_k
A_k1=[1 0 1 0;
0 1 0 1;
0 0 1 0;
0 0 0 1]; % Matriz phi que relaciona un estado con el siguiente
I=eye(4); % Matriz identidad de tamaño 4x4
Hk
Ak-1= φk-1
% Pseudovariables (R de k-1 // Q de k-1)
!
!
!
R_k=eye(2)*varR; % Matriz de covarianza R (2x2)
!
Q_k1=eye(4)*varQ; % Matriz de covarianza Q (4x4)
!
!
!
!
!
!
!
!
!
!
!
Rk : µk
Qk-1 : ξk-1
% Valores iniciales ( P_0 // S_0)
vx_k=vx_0; % suponemos velocidad en x constante
vy_k=vy_0; % suponemos velocidad en y constante
[x_k y_k zx_k zy_k]=ObtencionObservaciones03(rutaSec,nombreSec,1,pixMaxRuido);
% medida real en el frame k=1 (inicialmente)
S_pk=[x_k y_k vx_k vy_k]';
P_k=[1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];
sk
Pk
!
!
% Iteramos <==> KALMAN
!
!
k=2; % iteracion
while(k<nFrames)
!
!
!
% Actualizamos el estado k con el k-1
S_pk1=S_pk;
P_k1=P_k;
!
!
!
% 1ª ecuacion del filtro
P_pk = ( A_k1 * P_k1 * A_k1' ) + Q_k1;
Pk ' = φk −1 Pk −1φk −1 + Qk −1
t
!
!
!
% 2ª ecuacion del filtro
K_k = P_pk * H_k' * inv( (H_k * P_pk * H_k') + R_k );
(
K k = Pk ' H k H k Pk ' H k + Rk
t
t
)
−1
!
!
!
!
!
!
!
!
% Llega la observacion z_k
[x_k y_k zx_k zy_k]=
ObtencionObservaciones(rutaSec,nombreSec,k,pixMaxRuido);
z_k=[zx_k zy_k]';
s k = φ k −1 s k −1
% 3ª ecuacion del filtro
S_pk = (A_k1 * S_pk1) + (K_k * (z_k - (H_k * A_k1 * S_pk1)));
+ K k ( z k − H k φ k −1 s k −1 )
% 4ª ecuacion del filtro
P_k = (I - (K_k * H_k)) * P_pk;
Pk = (I − K k H k )Pk '
!
!
!
!
!
% Pasamos a calcular el siguiente estado
k=k+1;
!
!
end % while(i<nFrames)
Secuencia: obstáculos
Frames: 58
vx≈5 pix/frame
vy≈5 pix/frame
Estrella roja = real
Cruz verde = sensor
Círculo azul = Kalman
Secuencia: círculos
Frames: 105
vx≈5 pix/frame
vy≈5 pix/frame
Estrella roja = real
Cruz verde = sensor
Círculo azul = Kalman
¿Qué hacemos para representar la incertidumbre?.
Por simplicidad, representaremos los elementos diagonales de la
matriz de covarianzas Pk como semiejes sobre el punto que
estamos siguiendo.
Varianza
dirección de las
y
Un ejemplo bidimensional
Varianza
dirección de las
x
Punto en
seguimiento
I.2 Parámetros a estimar
Es necesario tener estimadores, tanto de la matriz de
covarianza del modelo como del sistema, así como un estimador
inicial de la matriz de covarianzas de estados.
Sin embargo, lo más importante es tener un estimador de la
matriz de ganancia Kk. Por la forma de esta matriz, si Rk es
mucho menor que Qk el estimador será más dependiente del
modelo de las observaciones que del sistema. Mientras que en
caso contrario, la estimación depende más del sistema que de
la observación.
Es, por tanto, el peso relativo de estas matrices lo que
condiciona la estimación.
I.3 Asociación de rasgos
No es una tarea fácil. El método más
simple es el llamado de agrupaciones
pequeñas y objetivos que no se
solapan.
Figura a. En la imagen siguiente
seleccionamos la medida
correspondiente al objeto detectado
que se encuentra en la zona de
predicción.
+m1
•p1
•p2
+m2
a
•p2
•p1
+m1
+m2
El método falla en la situación de la
figura b.
b
II. Bibliografía
•Trucco, E. y Verri, A., (1998), ‘Introductory
Techniques for 3-D Computer Vision’, Prentice Hall.