Bases del filtro de Kalman - Departamento de Ciencias de la

Bases del filtro de Kalman
Rafael Molina Soriano
Depto Ciencias de la Computación e IA
Universidad de Granada
Rafael Molina
Bases del filtro de Kalman
1
Contenidos
Introducción.
El filtro de Kalman
!
!
"
"
"
"
"
!
Modelo del sistema
Modelo de medida
Formulación de la teoría de estimación lineal
óptima del filtro de Kalman
Algoritmo de Kalman
Ejemplo
Bibliografía
Rafael Molina
Bases del filtro de Kalman
2
I. Introducción
En muchas situaciones se dispone de una secuencia de
imágenes y no sólo de dos. Esta situación permite mejorar
el acoplamiento (matching) de los rasgos.
Si el movimiento (de un objeto) en una escena es continuo
es, obviamente, posible hacer predicciones sobre él en
cada instante basado en las trayectorias previas.
Si tenemos las disparidades entre Ii-1 e Ii-2 e Ii-2 e Ii-3 etc,
es obvio que podemos predecir Ik - Ik-1.
El seguimiento de rasgos consiste en acoplar rasgos de
imagen en imagen en largas secuencias de imágenes.
Rafael Molina
Bases del filtro de Kalman
3
II. El filtro de Kalman
Rudolf E. Kalman nació en Budapest en
1930, emigró a Estados Unidos durante
la Segunda Guerra Mundial y se
doctoró en el M.I.T. en Ingeniería
Eléctrica en 1954.
En 1958 cuando viajaba en tren de Princeton a Baltimore el
tren se detuvo durante una hora a las 11 pm en las afueras
de Baltimore, entonces se le ocurrió aplicar el concepto de
variables de estado al filtro de Wiener.
Greg Welch y Gary Bishop, miembros del Department of
Computer Science de University of North Carolina at Chapel
Hill mantienen una excelente página sobre Kalman y su
filtro (http://www.cs.unc.edu/~welch/kalman/).
Rafael Molina
Bases del filtro de Kalman
4
Nuestro objetivo será la obtención de un estimador óptimo
de un sistema dinámico, basado en observaciones ruidosas y
en un modelo de la incertidumbre de la dinámica del
sistema.
Haremos uso de dos ingredientes fundamentales: los
conceptos de modelo del sistema y modelo de medida o de
observación.
Para ir entendiendo la formulación vamos a considerar
paralelamente la estimación de una constante (por ejemplo un
voltaje) cuya medida lleva asociado un ruído con una desviación
típica de 0.1 voltios. El voltaje será una constante y tenemos
observaciones secuenciales ruidosas.
Ver "An Introduction to the Kalman Filter" de Greg Welch y Gary
Bishop. Una copia local de este trabajo se encuentra en el
material del curso.
Rafael Molina
Bases del filtro de Kalman
5
II.1 Modelo del sistema
El sistema físico se modeliza por un vector de estados x,
llamado simplemente el estado, y un conjunto de
ecuaciones llamado el modelo del sistema.
El modelo del sistema es una ecuación de vectores que
describe la evolución del estado con el tiempo.
El tiempo de observación tiene la forma tk=t0+k∆T, k=0,1...,
∆T es el intervalo de muestreo y xk el estado x(tk).
Vamos a suponer que ∆T es pequeño y que por tanto
podemos utilizar un modelo del sistema lineal, es decir,
xk=φk-1 xk-1 +ξk-1
Rafael Molina
Bases del filtro de Kalman
6
donde ξk-1 es un vector aleatorio que modeliza el ruido
aditivo. El subíndice k-1 en φ indica que la matriz de
transición
φ
es (puede ser) una función del tiempo.
Volvamos ahora a nuestro ejemplo. Como el voltaje es una
constante podemos utilizar como modelo del sistema
xk=xk-1 +ξk-1
Observemos que cuanto mayor sea k, en principio, más
fiabilidad tendrá la estimación. Es importante analizar las
características de ξk-1 pero esto lo haremos con
posterioridad. Casi siempre el ruído ξk-1 se supone normal
de media cero.
Rafael Molina
Bases del filtro de Kalman
7
II.2 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 o al menos alguna de sus
componentes mediante la siguiente relación
zk=Hk xk +µk
zk es el vector de medidas tomadas en el instante tk. Hk es la
llamada matriz de medidas y µk es un vector aleatorio que
modeliza la incertidumbre asociada a las medidas.
En nuestro ejemplo tendríamos
zk=xk +µk
con
µk normales independientes de media 0 y desviación 0.1
Rafael Molina
Bases del filtro de Kalman
8
II.3 Formulación de la teoría de estimación
lineal óptima del filtro de Kalman
Esta sección es una recopilación de las dos anteriores.
El estado de un sistema dinámico en el instante tk está
descrito por un vector n-dimensional xk llamado vector de
estados. La evolución del sistema se modeliza mediante
xk=φk-1 xk-1 +ξk-1
donde
φk-1
es una matriz de tamaño nxn llamada matriz de
transición de estados y ξk es un n-vector que modeliza el
ruido asociado al modelo del sistema.
Rafael Molina
Bases del filtro de Kalman
9
En cualquier instante tk se obtiene un vector m-dimensional
de medidas zk. La relación entre el estado y las medidas es
lineal y se modeliza mediante
zk=Hk xk +µk
Hk es una matriz dependiente del tiempo de tamaño mxn y
µk es un m-vector aleatorio que modeliza la incertidumbre
asociada a las medidas.
Rafael Molina
Bases del filtro de Kalman
10
Muy importante (hay que recordar la notación)
Los términos ξk y µk son vectores aleatorios gaussianos
blancos de media cero y matrices de covarianza Qk y Rk
respectivamente.
En los casos que se comentan en clase dichas matrices de
covarianza son diagonales aunque en general pueden no
serlo, además el modelo de ruido puede no ser gaussiano.
Para el problema del voltaje Rk=(0.1)2 y la varianza del
modelo de estados la fijamos nosotros. En el ejemplo al final
de esta sección hemos supuesto Qk=(0.0001)2.
Rafael Molina
Bases del filtro de Kalman
11
Objetivo
Calcular el mejor estimador del sistema en el
instante tk que notaremos xk teniendo en cuenta el
estimador del estado predicho por el sistema en el
instante tk-1, que obviamente notamos xk-1, y la
medida realizada en el instante tk, que como
sabemos hemos notado zk.
Volviendo a nuestro problema de medida de voltaje es
importante tener claro que m=n=1.
Rafael Molina
Bases del filtro de Kalman
12
II.4 Algoritmo del filtro de Kalman
Supongamos que tenemos xk-1 y Pk-1 . En el
primer instante hemos de tener x0 y P0 .
¿Cómo calculamos los nuevos estimadores
iterativamente?
Los valores iniciales x0 y P0 los proporciona el
usuario.
Rafael Molina
Bases del filtro de Kalman
13
Tenemos un estimador xk-1 y su matriz de covarianzas Pk-1.
Primero calculamos, antes de que llegue la observación zk,
Pk ' = φ P φ
+ Qk −1
t
k −1 k −1 k −1
después la ganancia
(
K k = Pk ' H k H k Pk ' H k + Rk
t
t
)
−1
a continuación el estimador del estado k óptimo (aquí
metemos la observación)
(
x k = φ k −1 x k −1 + Pk ' H k H k Pk ' H k + Rk
t
t
)
−1
(zk − H kφk −1 x k −1 )
y por último la matriz de covarianzas de este estimador
(
Pk = Pk '− Pk ' H k H k Pk ' H k + Rk
t
t
)
−1
H k Pk ' = Pk '− K k H k Pk '
= (I − K k H k )Pk ' = (I − K k )Pk ' (I − K k ) − K k Rk K k
t
Rafael Molina
Bases del filtro de Kalman
t
14
II.5 Ejemplo
El siguiente código de Matlab implementa la estimación del
voltaje en el problema que hemos ido describiendo en el
apéndice.
El fichero .m se encuentra en el material complementario
de la asignatura
% Filtro de Kalman para la estimación de una constante
% El modelo de estados es x_k=x_(k-1) +N(0,var_estados)
% El de observaciones es y_k=x_k+N(0,var_obs)
% Se introduce el número de datos y el valor a estimar
% Se introduce un estimador inicial y su desviación típica
% A continuación al desviación típica en la observación y estados
% Se dibuja el estimador, las observaciones y la varianza de la
estimación
Rafael Molina
Bases del filtro de Kalman
15
N=input('numero de datos ');
x0=input('valor a estimar\ ');
est_inicial=input('valor inicial del estimador ');
incertidumbre_inicial=input(‘desviación estimador inicial ');
desv_observacion=input('desviacion tipica en la observacion ');
desv_estados=input('desviacion tipica en modelo de estados ');
var_obs=desv_observacion*desv_observacion;
var_estados=desv_estados*desv_estados;
X=zeros(N+1,1); % contendra las estimaciones
X_antes=zeros(N+1,1);
Ganancia=zeros(N+1,1);
Y=zeros(N+1,1); % contendra las observaciones
P_antes=zeros(N+1,1); % contendra la varianza de estimadore sin observacion
P_despues=zeros(N+1,1); % contendra la varianza de la estimacion
Rafael Molina
Bases del filtro de Kalman
16
X(1)=estado_inicial;
P_despues(1)=incertidumbre_inicial*incertidumbre_inicial;
for k=2:N+1
X_antes(k)=X(k-1);
P_antes(k)=P_despues(k-1)+var_estados;
Ganancia(k)=P_antes(k)/(P_antes(k)+var_obs);
Y(k)=x0+desv_observacion*randn(1);
X(k)=X_antes(k)+Ganancia(k)*(Y(k)-X_antes(k));
P_despues(k)=(1-Ganancia(k))*P_antes(k);
end
close all
plot([1:N+1],X), title('Estimador de Kalman');
figure
plot([2:N+1],Y(2:N+1)), title('Observaciones');
figure
plot([2:N+1],P_despues(2:N+1)), title('Varianza del estimador de Kalman');
Rafael Molina
Bases del filtro de Kalman
17
número de datos 100
valor a estimar 3
valor inicial del estimador 2.5
desviación estimador inicial 0.1
desviación típica en la
observación 0.1
desviación típica en modelo de
estados 0.0001
Rafael Molina
Bases del filtro de Kalman
18
IV. Bibliografía
•Trucco, E. y Verri, A., (1998), ‘Introductory
Techniques for 3-D Computer Vision’, Prentice
Hall.
Complementario
•Material adicional del tema
•Greg Welch y Gary Bishop, "An Introduction to the
Kalman Filter“, 2002.
Rafael Molina
Bases del filtro de Kalman
19