filt rode kalman

7/29/2019 Filt Rode Kalman

1/16

- 1 -

EL FILTRO DE KALMAN

Introduccin

En el siguiente documento se explicar un mtodo para estimar los estados de unsistema estocstico. El mtodo fue descrito por Rudolf E. Kalman en 1958.

En un sistema determinstico trabajaramos de la siguiente forma: En primer lugar,construiramos un modelo del sistema a partir de las leyes fsicas que definen lasdinmicas. En segundo lugar, estudiaramos su estructura y su respuesta. Por ltimo, ysi fuera necesario, se disearan compensadores para alterar las caractersticas delsistema o bien reguladores. Para ello tendramos adems un amplio conjunto deexperiencias que han sido aplicadas con anterioridad sobre estos sistemas. En la realidadocurre que los sistemas determinsticos son aproximaciones de lo que realmente son,

principalmente por tres motivos: No existe un modelo matemtico perfecto de un sistema real. Existen perturbaciones que no se pueden modelar de una forma determinstica. Los sensores no son perfectos.

Esto nos lleva a plantearnos una serie de preguntas:

Cmo desarrollar modelos de sistemas que tengan en cuenta las incertidumbres. Cmo estimo de una forma ptima los datos que me interesan de un sistema mal

modelado y con datos alterados por el ruido. Cmo controlo de una forma ptima sistemas estocsticos.

Qu es el Filtro de Kalman

Es un algoritmo de procesado de datos ptimo recursivo. ptimo porque minimiza uncriterio determinado y porque incorpora toda la informacin que se le suministra paradeterminar el filtrado. Recursivo porque no precisa mantener los datos previos, lo quefacilita su implementacin en sistemas de procesado en tiempo real. Por ltimo,algoritmo de procesado de datos, ya que es un filtro, pensado para sistemas discretos.

El objetivo del filtro de Kalman es estimar los estados de una manera ptima, de maneraque se minimiza el ndice del error cuadrtico medio.


2/16

- 2 -

Un sistema lineal se puede representar as:

De manera que su evolucin es expresada en espacio de estados por:

x(k+1) = Ax(k) + Bu(k) + v(k)

y(k) = Cx(k) + w(k)

Siendo:

x(k) Estadoy(k) Salida u observacin del sistema.w(k) proceso estocstico asociado a la medidav(k) proceso estocsticas asociado al sistemaA, B, C matrices determinsticas que definen la dinmica del sistema

Se deben asumir las condiciones siguientes:

E[x(0)] = x0E[w(k)] = 0 kE[v(k)] = 0 kE[x(0),w(k)] = 0 k

E[x(0),v(k)] = 0 kE[v(k),w(j)] = 0 kE[w(k),w(k)] = R(k)E[w(k),w(k)] = 0 k jE[v(k),v(k)] = Q(k)E[w(k),w(j)] = 0 k jE[x(0),x(k)] = P0 k

Las matrices de covarianza Q(k) y R(k) son diagonales y por tanto simtricas.

En el sistema real podremos observar el valor de y(k) de manera directa con lossensores adecuados. Esta medida incorporar una serie de incertidumbres asociadas: la

incertidumbre del sensor y la del sistema. Por otro lado solo podremos acceder al valory(k). En caso de necesitar la evolucin completa del estado x(k) y/o de precisar el valorde la observacin ajena a las variaciones provocadas a la incertidumbre, tendremos queestimar de alguna manera indirecta sus valores.


3/16

- 3 -

El filtro de Kalman propone un mtodo para obtener un estimador ptimo del estado. Sisuponemos que )( kx es la estimacin en el instante k del estado. El filtro de Kalmanbuscar obtener ese valor de estimacin de manera que se minimice el error cuadrticomedio. Definiendo el error como la diferencia entre el valor real del estado y laestimacin:

)()()( kxkxke = . [1]

El objetivo por tanto ser minimizar

{ })().()( neneEnP T= . [2]

A la matriz P(n) se la conoce como matriz de covarianza del error. Dependiendo delvalor que tome n en [2], tendremos distintas representaciones del filtro de Kalman:

Si n = k+1, el filtrado ser de prediccin.Si n = k, el filtrado ser de alisado.

Filtro de Kalman con filtrado:

El objetivo consiste en determinar los valores de )(kx

conocidas las medidascontaminadas y(0),y(1),,y(k) para que P(k) sea mnima, o visto desde otro punto devista, el objetivo es determinar los valores de )1( +kx a partir de las medidas

contaminadas de la observacin )1( +ky para que la matriz )1( +kP sea mnima.

El filtrado de )1( +kx propuesta por Kalman y Bucy, se realizar a partir del estadoanterior y de un factor de correccin que ser funcin del error.

El algoritmo tiene dos pasos que son ejecutados de forma iterativa. Prediccin: Antes detener la mediday(k+1) y Correccin o actualizacin del estado.

En la siguiente figura se puede ver el proceso de clculo:


4/16

- 4 -

Paso 1: Prediccin-Extrapolacin-Estimacin

Calculamos una prediccin del estado x(k+1), lo notaremos como x(k+1). El valor dela prediccin es calculado a partir del valor ms actualizado del estado (posteriormenteveremos que es el estado corregido en la parte final del algoritmo)

)()()1(' kBukxAkx +=+ [3]

Predecimos el valor de la matriz de covarianza del error previo a la medida P(k+1)(recordamos que estamos haciendo una previsin del estado). Como se indic en [2], elerror estar definido por:

e(k+1) = x(k+1) x(k+1). [4]

Por tanto e(k+1) en esta etapa de prediccin vale:

)()()()()()1(' kBukxAkvkBukAxke ++=+

[ ][ ]TkvkxkxAkvkxkxAEkP )())()(()())()(()1(' ++=+

Operando:

[ ][ ] { }TT kvkvEkxkxAkxkxAEkP )()()()(()()(()1(' +=+

{ }TkvkvE )()( Se define como la matriz de covarianza asociada al proceso: Q(k)

)()()1(' kBukxAkx +=+

2

QAkAPkP T +=+ )()1('

[ ] 1)1()1(')1(')1( ++++=+ kRCkCPkCPkK T

[ ])1(')1()1()1(')1( +++++=+ kxCkykKkxkx

[ ] )1(')1( +=+ kPKCIkP

Valores iniciales para )( kx y P(k)


5/16

- 5 -

Queda por tanto:

QAkAPkPT+=+ )()1(' (k) [5]

Paso 2: Innovacin-Actualizacin-Correccin.

Decamos antes que el valor del estado se va a calcular a travs del estado anterior y deuna correccin que es funcin del error. Esto es:

[ ])1(')1()1()1(')1( +++++=+ kxCkykKkxkx [6]

Siendo el factor de correccin:

[ ])1(')1()1( +++ kxCkykK

y: y(k+1) es el ltimo valor observado.x(k+1) es el valor ms actualizado disponible del estado (calculado en la fase de

prediccin.Buscaremos el valor de K(k+1) para conseguir un valor ptimo de )1( +kx tal que lamatriz de covarianza del error sea mnima. Volviendo a definir el error a partir de [1]:

)1()1()1( ++=+ kxkxke

Substituyendo:

[ ])1(')1()1(')1()1( ++++=+ kCxkyKkxkxke

La observaciny(k+1) tiene el valor

)1()1()1( +++=+ kwkCxky

Por tanto:

)1(')1()1()1(')1()1( ++++++=+ kKCxkKwkKCxkxkxke

Agrupando:

[ ] )1())1(')1(()1( +++=+ kKwkxkxKCIke

As pues, segn [2]:

{ [ ] )1())1(')1(()1( +++=+ kKwkxkxKCIEkP

[ ] }T

kKwkxkxKCI )1())1(')1(( +++


6/16

- 6 -

{ [ ] [ ] }+++++=+T

kxkxKCIkxkxKCIEkP ))1(')1(())1(')1(()1(

{ }kkwkkwE T)1()1( ++

{ }TkwkwE )1()1( ++ : Matriz de covarianza asociada a la medida:R(k+1)

[ ] { }[ ]TKCIkxkxkxkxEKCIkP ++++=+ )1(')1()1(')1()1(' TKkKR )1( ++

Anteriormente ya calculamos que:

TkxkxkxkxEkP )1(')1()1(')1()1(' ++++=+

Por lo que :

[ ] [ ] TT KRKKCIkPKCIkP ++=+ )1(')1( [7]

Si Llamamos P1 a P(k+1) y P(k+1) = P:

TTTTT KRKKKCPCKPCKCPPP ++++= 1

TTT TrKRKKCPCTrKTrKCPTrPTrP +++= )(21 (Tr traza de la matriz)

Diferenciamos respecto a K para obtener el mnimo de la expresin e igualamos a cero

0222 =++=

KRTrCPCTrCP

K

TrP T

[ ] 1 += RCPCCPKT

Es decir:

[ ] 1)1()1(')1(')1( ++++=+ kRCkCPkCPkK T [8]

Se puede comprobar que es el mnimo a travs del Hessiano.

Substituyendo [8] en [7]:

[ ] )1(')1( +=+ kPKCIKP [9]

Inicializacin del algoritmo:

Para iniciar el algoritmo es necesario conocer las siguientes condiciones de contorno:Un valor inicial del estado: x(0). El valor de la matriz de covarianza del error, que paraP(0). Podemos asumir que P(0) = Q. Y los valores de las matrices de covarianzaasociadas al sistema y a la medida: Q y R.


7/16

- 7 -

Resumiendo, el algoritmo tiene los siguientes pasos:

En la iteracin k:

1 Prediccin:

)()()1(' kBukxAkx +=+ QAkAPkP T +=+ )()1('

2 Correccin

[ ] 1)1()1(')1(')1( ++++=+ kRCkCPkCPkK T observacin.y(k+1)

[ ])1(')1()1()1(')1( +++++=+ kxCkykKkxkx [ ] )1(')1( +=+ kPKCIKP

Ntese que el concepto de filtrado es debido a que los valores del estado ptimo)1( +kx y de la observacin se producen en el mismo instante.


8/16

- 8 -

Ejemplo 1. Filtrado de una constante

Supongamos que queremos estimar el valor de una constante que al ser medida tieneuna serie de desviaciones debidas a la incertidumbre del aparato de medida.

El sistema ser entonces:

x(k+1) = Ax(k) + Bu(k) + v(k)

y(k) = Cx(k) + w(k)

A=1

B=0

C=1

x(k+1) = x(k)+ v(k)

y(k) = x(k) + w(k)

Supongamos que la desviacin tpica de la medida es 5.0=w . La covarianza (varianza

en este caso ser Rw == 25.02

. Y consideremos tambin que el sistema tiene una

desviacin muy pequea 001.0=v

. La varianza asociada al sistema es 612 = ev .

P(0) = 1e-6. En la siguiente grfica se puede observar el efecto del filtro de Kalman parauna constante de 30. x(0) = 25.


9/16

- 9 -

Cdigo utilizado para generar la grfica:

!"#$%!"$%$$!"#"#&"#!'(""&"! ')

""#! '$*+,$$''''''''''''''''''''"+-+! '"+-"+-"! ''''''''''''''''''''..+-"+-/"+--"#! '0#1+-$-"#&!

'+-"+--.+-&1+-2"+-!'"+-2.+&"+-!

Ejemplo 1. Filtrado del estado para un sistema de segundo orden

Supongamos el siguiente sistema formado por un circuito RLC serie.

Si analizamos el sistema en el dominio de Laplace, obtendremos que:

1

1

)(

)(2

0

++=

RCsLCssE

sE

i

Pasando a espacio de estados, (nota: el sistema es continuo):


10/16

- 10 -

[ ] wx

xy

v

vsE

LCx

x

L

R

LCx

xi

+

=

+

+

=

2

1

2

1

2

1

2

1

01

0

0)(1

01

10

Siendo:

x1 = E0 x2 =dt

dE0

Tomemos los valores

L = 1 H.

C = 1000 F

R = 30

As pues:

=

30100010A ;

=1000

0B ; [ ]01=C

Si presumimos un tiempo de muestreo de T = 0.01 s.

=

7001.04963.8

0085.09550.0)(kA ;

=

4963.8

0450.0)(kB ; [ ]01)( =kC

Suponemos una desviacin del observador de 1 voltioSuponemos una desviacin del sistema de 0.01 voltio y 0.01 voltio/segundo.

01.01 =v ; 01.02 =v ; 1=w

por lo que :

R = 1; y

=

0001.00

00001.0Q

En la siguiente grfica se puede ver el resultado de la estimacin


11/16

- 11 -

Cdigo utilizado para generar la grfica:

'32!4!($!'5

6$!2/4&2(/47!86$!/4&7!6$7!3$!919:8::3!';*;$%$%!6:7"!'"#! '##@"$%$! '#$%$@+-"&! '"&"&B-)! ''''''''''''''''''''#11"+-"#&!'.

.&"&&"&B-(! '"-.B&12&"!'C+-!C+-!


12/16

- 12 -

Filtro de Kalman con estimacin:

Partimos de un sistema lineal representado por las ecuaciones:

)()()()1( kvkBukAxkx ++=+ [10])()()( kwkCxky +=

Como ya se indic con anterioridad, la solucin que propusieron Kalman y Bucy para elpredictor del estado, vendr determinada por el estado en el instante anterior y por unfactor de correccin que ser funcin del error entre la observacin y la estimacin.

[ ])()()()()()1( kykykKkBukxAkx +=+ [11])()( kxCky =

Asumidas todas las condiciones iniciales, anlogas al caso de filtrado, se trata aqu dedeterminar la estimacin )1( +kx , conociendo las medidas contaminadas de ruidoy(0),

y(1), y(2),..., y(k), para que la matriz P(k+1) de covarianza del error, en el instante k+1,sea mnima.

El error en el instante k+1 se determinar por:

)1()1()1( ++=+ kxkxke [12]

Substituyendo en [12] las expresiones [10] y [11]

[ ])()()()()()()()()()1( kxCkwkCxkKkBukxAkvkBukAxke++++=+

[ ] [ ] )()()()()()()()()1( kvkwkKkxkxkCKkxkxAke +=+

[ ])()()( kxkxke =

[ ] )()()()()()1( kvkwkKkeCkKAke +=+

Llamando:

CkKAkA )()( = [13]

tenemos:

)()()()()()1( kvkwkKkekAke +=+


13/16

- 13 -

As pues la matriz de covarianza del error tal y como se haba definido en [2]

{ }=++=+ TkekeEkP )1()1()1(

Substituyendo y operando:

{ [ ][ ] }=++T

kvkwkKkekAkvkwkKkekAE )()()()()()()()()()(

{ + TTTTT vkekAkwkKkekAkAkekekAE )()()()()()()()()()(

+TTTTT kvkwkKkekAkwkKkKkwkwkK )()()()()()()()()()()(

}TTTTT kvkvkwkKkvkekAkv )()()()()()()()( ++

y como:

{ } { } )()()()()()()()()()()( kAkPkAkAkekeEkAkAkekekAE TTTT ==

{ } { } 0)()()()()()()()( == TTTT kekAkwkKEkwkKkekAE

{ } { } 0)()()()()()( == TTT kekAkvEkvkekAE

{ } { } TTTTT kRKkKkKkwkwEkKkKkwkwkKE )()()()()()()()()()( ==

{ } { } 0)()()()()()( == TTT kwkKkvEkvkwkKE

{ } QkvkvE T =)()(

Queda:

QkRKkKkAkPkAkPT

++=+ )()()()()()1(

Teniendo en cuenta el valor de )(kAT

[ ] [ ] QkRKkKCkKAkPCkKAkP T ++=+ )()()()()()1( [14]

Una vez que hemos calculado la expresin de la matriz de covarianza del error, tenemosque encontrar el valor de K(k) que hace que esa matriz sea mnima:

[ ] [ ] QkRKkKCkKAkPCkKAkP T ++=+ )()()()()()1(

++=+TTTTTT CkKkCPkKAkCPkKCkKkAPAkAPkP )()()()()()()()()1(

QkRKkK +)()(


14/16

- 14 -

++=+ ))()()(())()(())(()1( TTTT CkKkCPkKTrAkCPkKTrAkAPTrkTrP

)()()(( QTrkRKkKTr +

Si diferenciamos respecto a K(k)

))((2))()((2))((2)(

)1(RkKTrCkCPkKTrAkCPTr

kK

kTrP TT++=

+

Igualando a cero para buscar el mnimo:

0))((2))()((2))((2 =++ RkKTrCkCPkKTrAkCPTr TT

Al ser P(k) yR(k) simtricas:

TT AkCPRCkCPkK )(2)()( =+

[ ] 1)()()( += RCkCPAkCPkK TT [15]

Substituyendo este valor en [14] obtenemos P(k+1)

[ ] TAkPCkKAQkP )()()1( +=+ [16]

Resumiendo, el proceso iterativo a seguir es:

1.- Clculo de la ganancia:

[ ] 1)()()( += RCkCPAkCPkK TT

2.- Estimacin del estado:

[ ])()()()()()1( kxCkykKkBukxAkx +=+

3.- Actualizacin de la matriz de covarianza del error

[ ] TAkPCkKAQkP )()()1( +=+

Ntese que el concepto de estimacin es debido a que cuando tenemos la observacindel sistemay(k) los valores obtenidos para el estado ptimo son los de )1( +kx .


15/16

- 15 -

Si representamos el filtro en forma de bloques, obtenemos la siguiente representacin:


16/16

- 16 -

Documentacin[1] Rafael Molina Soriano. Bases Del Filtro de Kalman.

http://decsai.ugr.es/vip/doctorado/pvd/T7bn.pdf

[2] Tema XVII El filtro de Kalman

http://www.depeca.uah.es/wwwnueva/docencia/ ING-CA/ctr_avz/VVEE17.PDF

[3] Greg Welch, Gary Bishop. Introduction to the Kalman Filter.Peter S. Haybeck. Stochastic models, estimation and control. Volume 1Greg Welch, Gary Bishop. SCAAT. Incremental tracking with IncompleteInformationhttp://knaan.com/Lev/SIGGRAPH2001_CoursePack_08.pdf

[4] Kalman Filter Derivation.http://www.aticourses.com/kalman_filter.pdf

filt rode kalman

Documents