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. Seguimiento de rasgos usando el 1

Alvarez filtro de Kalman
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 0 1 0   xk −1   ξ1 
      
 yk   0 1 0 1   yk −1   ξ 2 
 vx  = 0 · x  +  
0 1 0 vk −1 ξ3
 ky    y   
v  0 0 0 1   vk −1   ξ 4 
 k 

φ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 
 
 0 σQ 2
0 0 
Qk =  
 0 0 σ Q
2
0 
 0 0 0 σ Q 
2


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 
 
 z k  1 0 0 0   yk   µ1 
x
 y  = 
 z  0 1 0 0 · v x  +  µ 
 k   k   2
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)
pmr=2 (nº entero dado como parámetro)
zxk r*(pmr+1)*2 = r*3*2 Є (0, 6.0)
[0, 6.0] – (pmr+1) = (0, 6.0) – 3 Є (-3.0, 3.0)

zyk 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; Hk
! 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;
Ak-1= φk-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
! % Pseudovariables (R de k-1 // Q de k-1)
!
! R_k=eye(2)*varR; % Matriz de covarianza R (2x2) Rk : µk
! Q_k1=eye(4)*varQ; % Matriz de covarianza Q (4x4) 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]'; sk
! P_k=[1 0 0 0;
! 0 1 0 0;
! 0 0 1 0;
Pk
! 0 0 0 1];
!
! % 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

Pk ' = φk −1 Pk −1φk −1 + Qk −1
t
! P_pk = ( A_k1 * P_k1 * A_k1' ) + Q_k1;
!

!
!
% 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]';
!

! % 3ª ecuacion del filtro

s k = φ k −1 s k −1 + K k ( z k − H k φ k −1 s k −1 )
! S_pk = (A_k1 * S_pk1) + (K_k * (z_k - (H_k * A_k1 * S_pk1)));
!

!
!
% 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
Punto en
dirección de las
seguimiento
x
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

+m1
simple es el llamado de agrupaciones •p1

pequeñas y objetivos que no se •p2

solapan. +m2

Figura a. En la imagen siguiente

seleccionamos la medida a
correspondiente al objeto detectado
que se encuentra en la zona de
predicción. •p1
•p2
+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.