Filtro Kalman

Bases del filtro de Kalman

Rafael Molina Soriano Depto Ciencias de la Computacin e IA Universidad de GranadaRafael Molina Bases del filtro de Kalman 1

Contenidos! !" " "

Introduccin. El filtro de KalmanModelo del sistema Modelo de medida Formulacin de la teora de estimacin lineal ptima del filtro de Kalman Algoritmo de Kalman Ejemplo

" "

!

BibliografaRafael Molina Bases del filtro de Kalman 2

I. IntroduccinEn muchas situaciones se dispone de una secuencia de imgenes y no slo de dos. Esta situacin 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 imgenes.Rafael Molina Bases del filtro de Kalman 3

II. El filtro de KalmanRudolf 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 Ingeniera Elctrica 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 pgina sobre Kalman y su filtro (http://www.cs.unc.edu/~welch/kalman/).Rafael Molina Bases del filtro de Kalman 4

Nuestro objetivo ser la obtencin de un estimador ptimo de un sistema dinmico, basado en observaciones ruidosas y en un modelo de la incertidumbre de la dinmica del sistema. Haremos uso de dos ingredientes fundamentales: los conceptos de modelo del sistema y modelo de medida o de observacin. Para ir entendiendo la formulacin vamos a considerar paralelamente la estimacin de una constante (por ejemplo un voltaje) cuya medida lleva asociado un rudo con una desviacin tpica 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 sistemaEl sistema fsico 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 ecuacin de vectores que describe la evolucin del estado con el tiempo. El tiempo de observacin tiene la forma tk=t0+kT, k=0,1..., T es el intervalo de muestreo y xk el estado x(tk). Vamos a suponer que T es pequeo y que por tanto podemos utilizar un modelo del sistema lineal, es decir,

xk=k-1 xk-1 +k-1Rafael Molina Bases del filtro de Kalman 6

donde k-1 es un vector aleatorio que modeliza el ruido aditivo. El subndice k-1 en indica que la matriz de transicin

es (puede ser) una funcin del tiempo.

Volvamos ahora a nuestro ejemplo. Como el voltaje es una constante podemos utilizar como modelo del sistema

xk=xk-1 +k-1caractersticas de k-1 pero esto lo haremos con posterioridad. Casi siempre el rudo k-1 se supone normal de media cero.Rafael Molina Bases del filtro de Kalman 7

Observemos que cuanto mayor sea k, en principio, ms fiabilidad tendr la estimacin. Es importante analizar las

II.2 Modelo de medidaEl segundo ingrediente en la teora de la estimacin es el modelo de medida. Suponemos que en cada instante tk tenemos una observacin ruidosa del vector de estados o al menos alguna de sus componentes mediante la siguiente relacin zk es el vector de medidas tomadas en el instante tk. Hk es la

zk=Hk xk +k

llamada matriz de medidas y k es un vector aleatorio que modeliza la incertidumbre asociada a las medidas. En nuestro ejemplo tendramos con

k normales independientes de media 0 y desviacin 0.1Bases del filtro de Kalman 8

zk=xk +k

Rafael Molina

II.3 Formulacin de la teora de estimacinlineal ptima del filtro de KalmanEsta seccin es una recopilacin de las dos anteriores. El estado de un sistema dinmico en el instante tk est descrito por un vector n-dimensional xk llamado vector de estados. La evolucin del sistema se modeliza mediante

xk=k-1 xk-1 +k-1donde transicin 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

k-1

es una matriz de tamao nxn llamada matriz de

En cualquier instante tk se obtiene un vector m-dimensional de medidas zk. La relacin entre el estado y las medidas es lineal y se modeliza mediante

zk=Hk xk +kHk es una matriz dependiente del tiempo de tamao mxn y

k es un m-vector aleatorio que modeliza la incertidumbreasociada a las medidas.Rafael Molina Bases del filtro de Kalman

10

Muy importante (hay que recordar la notacin) Los trminos 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, adems 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 seccin hemos supuesto Qk=(0.0001)2.

Rafael Molina


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


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 . Cmo calculamos los nuevos estimadores iterativamente? Los valores iniciales x0 y P0 los proporciona el usuario.

Rafael Molina


13

Tenemos un estimador xk-1 y su matriz de covarianzas Pk-1. Primero calculamos, antes de que llegue la observacin zk,

Pk ' = P t

t k 1 k 1 k 1

+ Qk 1

despus la ganancia

K k = Pk ' H k H k Pk ' H k + Rkt

(

)

1

a continuacin el estimador del estado k ptimo (aqu metemos la observacin)

x k = k 1 x k 1 + Pk ' H k H k Pk ' H k + Rkt t

(

)

1

(zk H kk 1 x k 1 )

y por ltimo la matriz de covarianzas de este estimador

Pk = Pk ' Pk ' H k H k Pk ' H k + Rkt tRafael Molina

= (I K k H k )Pk ' = (I K k )Pk ' (I K k ) K k Rk K ktBases del filtro de Kalman

(

)

1

H k Pk ' = Pk ' K k H k Pk 't14

II.5 EjemploEl siguiente cdigo de Matlab implementa la estimacin del voltaje en el problema que hemos ido describiendo en el apndice. El fichero .m se encuentra en el material complementario de la asignatura% Filtro de Kalman para la estimacin 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 nmero de datos y el valor a estimar % Se introduce un estimador inicial y su desviacin tpica % A continuacin al desviacin tpica en la observacin y estados % Se dibuja el estimador, las observaciones y la varianza de la estimacinRafael 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(desviacin 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 estimacionRafael 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

nmero de datos 100 valor a estimar 3 valor inicial del estimador 2.5 desviacin estimador inicial 0.1 desviacin tpica en la observacin 0.1 desviacin tpica en modelo de estados 0.0001

Rafael Molina


18

IV. BibliografaTrucco, 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


19

Documents

Filtro Kalman