El Filtro de Kalman: La idea fundamental del filtro de Kalman es la actualización. La llegada de una nueva observación supone un cambio en la mejor estimación mínimo cuatrática del parámetro x. Se desea calcular los cambios que actualizan la estimación de x^. El procedimiento será eficiente si se puede expresar la estimación como una combinación lineal de la estimación anterior x^old y de la nueva observación. bnew: x^new = L x^old + K bnew Lo primero que hay que resaltar es que el procedimiento es recursivo. Se evita de esta forma el almacenamiento de las observaciones viejas bold, usadas en las estimaciones de x^ old. Lo segundo es que la fórmula anterior se puede escribir de distintas formas equivalentes. Esto hace que la comprensión del filtro de Kalman sea compleja. Para ayudar a la comprensión de éstas se diferencian las fases de predicción y corrección. x^new = x^ old + K(bnew – Anew x^old) De la comparación de ambas expresiones se deduce que L = I – K Anew. La ganancia de Kalman K multiplica a bnew – Anew x^old , que se interpreta como la diferencia entre la estimación vieja y la nueva medida. La estimación de la nueva observación se realiza a través de Anew x^old . La diferencia entre la predicción y la observación se multiplica por la ganancia del filtro K para proporcionar una corrección a x^. La ganancia del filtro, matriz K, contiene los estadísticos de la nueva observación bnew y la estimación anterior x^old , proporcionando un peso para la misma. (Es imprescindible que la matriz de covarianzas P se actualice con cada nueva observación). La tercera parte es la confianza de la estimación x^. Esta se define con la matriz de covarianza del error P = Σ x^ para la estimación. La matriz P proporciona información estadística de x^ basada en las propiedades estadísticas de b. Ésta no depende de una observación o de sus errores sino que es función de un conjunto de muestras de las posibles observaciones o de sus errores. Se asume que posee una distribución Normal (Gausiana) de la que se conoce la matriz de covarianza Σ e. La solución mínimo cuadrática de x^ de un sistema estático Ax = b es ponderada por Σ e-1 , de tal forma que P = (AT Σ e-1 A) -1 es la matriz de covarianzas del error para x^. Esta es la matriz que se debe actualizar cada vez que se obtiene una nueva observación bnew con Σ e,new . Pnew = (I –K A) Pold (I – KA)T + K Σ e,new KT Para obtener esta expresión con L = I – K Anew se ha considerado que el error en la nueva observación es estadísticamente independiente de los errores viejos. La matriz de covarianza para b ,

(bold) es Σ e = (Σ e,old 0 ) (bnew) ( 0 Σ e,new ) 1

La cuarta parte posee una importancia fundamental. El problema de mínimos cuadrados descrito es estático. Se debe actualizar la estimación del mismo x^ vector de parámetros x, por lo que se convierte en un método recursivo de mínimos cuadrados. Si se dispone de observaciones exactas x = x^ old = x^ new, se puede solucionar el problema como un sistema sobredeterminado en esta situación estática: Aold * x = bold

y (Aold Anew)T * (x) = (bold bnew)T .

A este sistema se le pueden añadir más filas con nuevas observaciones. Si la mejor estimación x^old a través del sistema es igual a la nueva observación bnew = Anew x^old , quiere decir que la ganancia K del filtro ha de ser multiplicada por 0, para que no varíe la estimación. El filtro de kalman está diseñado para sistemas dinámicos. El nuevo estado xk generalmente no es el mismo que xk-1 . El estado cambia, y se va a suponer que lo hace de forma lineal, con su propio error ε k : Estado: Xk = Fk-1 Xk-1 + ε k . La expresión que liga el nuevo estado, las observaciones y el error del modelo con las estimaciones es: Nuevo estado: Bk = Ak xk + ek El filtro se va a modelar en dos fases: predicción y corrección.: Predicción: Corrección:

x^k|k-1 = Fk-1 x^k-1|k-1 . x^k|k = x^ k|k-1 + Kk (bk – Ak x^k|k-1 ).

Se desea conocer la matriz de covarianzas del error del segundo paso de actualización P (o su inversa). La predicción Pk|k-1 se convierte en corrección Pnew = Pk|k y dicha predicción procede de la ecuación de estado: Pk|k-1 = Fk-1 Pk-1 FT k-1 + Σ ε,k El nuevo paso Pk|k combina las nuevas covarianzas Σ e, k con Σ ε, k . Estas describen el conjunto de los errores de ambos ek y ε k (errores de estado y observaciones). Se esta considerando que ε k es estadísticamente independiente de todos los ε j y de todos los ej. Esta consideración hace que toda la matriz Σ de covarianzas sea una matriz de bloque diagonal, un bloque para cada ek y ε k . Dicho bloque describe los errores en la ecuación de estado y en la de observaciones.

2

Análisis del sistema A x = b – e . La expresión representa el sistema de ecuaciones, dependiente del tiempo en el que el estado inicial es x0 = x^ 0 junto a su matriz de covarianza del error P0|0 . En cada paso se introducen dos nuevos bloques (filas) en la matriz A y un nuevo bloque (columna) correspondiente al nuevo xk en el vector de estado desconocido x k .

Ao -Fo

I A1 -F1

I .... .... ....

-Fk-1

I Ak

x0 x1 . . . xk-1 xk

=

b0 0 b1 0 . . . . . 0 bk

e0

ε1 e1

ε2

-

. . . .

εk ek

Las dos últimas filas se corresponden con la ecuación de estado y la de observaciones del filtro de kalman. Este sistema se resuelve recursivamente por el método de mínimos cuadrados ponderados. Las ponderaciones son las matrices de covarianzas de los errores ek y ε k . El resultado es el conjunto de las estimaciones del estado (x^ 0 ,...x^ k ) y sus matrices de covarianzas (P0 ,...Pk ). Estos podrían haberse obtenido con la fórmula P = (AT Σ −1 A)-1 pero no es así. Las fórmulas recursivas determinan x^k y Pk a partir de bk , x^ k-1 , Pk-1 , Σε,κ y Σe,k. Las diferentes maneras de representar los filtros de kalman proceden de las distintas formas en las que se puede representar la matriz (AT C A). Esta matriz banda tridiagonal es simétrica, definida positiva y admite factorización de Cholesky L LT . También se puede factorizar por el método de Gram-Schmidt , ortogonalizando las columnas de la matriz A, obteniendose así la descomposición Q R. Todas estas son formas de manipular la matriz completa A. No se debe olvidar que lo que se desea resolver es el sistema mostrado antes, por medio de mínimos cuadrados ponderados. El objetivo de las ecuaciones recursivas es hacer más sencillo el manejo de las expresiones, utilizando sólo los estados para t = k-1 y k. Se determinan las matrices a partir de la fórmula de actualización. Se hace especial énfasis en la matriz K llamada ganancia del filtro de Kalman para el estado k. Hasta el momento sólo se ha estudiado un modelo estático, del que a continuación se pone un ejemplo: Un médico toma muestras b1 ,...bk del pulso de un paciente x. Cada medida posee un error cuadrático (x –bj)2 cuya estimación o varianza es σ2 . Se desea obtener la mejor aproximación de x^ k y de Pk de forma recursiva a partir de x^ k-1 y Pk-1 .

3

Las ecuaciones de las medidas son: x=b1 , x=b2 , ... x=bn o (11...1)T (x) = (b1 b2 ... bk )T La matriz de covarianzas es Σ b = σ2 * I. La matriz P-1 = (AT Σ −1 b A) y AT C b son: P-1 = (1 ...1) I / σ2 (1...1)T = k/ σ2 (1 ...1)( I / σ2 )(b1 b2 ... bk )T = (b1+ b2 ...+ bk )/ σ2 La mejor aproximación, como era de esperar, es la media de las observaciones: x^k = P(AT C b) = (σ 2 / k) (b1+ b2 ...+ bk )/ σ2 = (b1+ b2 ...+ bk ) / k. Se itera partiendo con los valores correctos en k-1 : X^old = (1/k-1) (b1+ b2 ...+ bk-1 ) y Pold = σ2 / (k−1) Las nuevas medidas reducen P e incrementan P-1 : Pk –1 = Pk-1 –1 + (1/ σ2 )= (k-1)/ σ2 + 1/ σ2 = k/ σ2 . Se puede observar que el incremento de Pk como: AnewT Σ −1 k Anew = (1)( 1/ σ2 )(1).

–1

es 1/ σ2 valor que ha sido obtenido

Kk = Pk Ak T Σ −1 k =( σ2 /k)(1)(1/ σ2 )=(1/k) x^k = x^ k-1 + 1/k (bk – x^k-1 )= ((k-1)/k) (b1+ b2 ...+ bk-1 /k-1) + (1/k) bk El mismo problema se puede plantear haciendo que el error de la muestra k sea distinto de las anteriores σk 2 . Pk –1 = (k-1)/ σ2 + 1/ σk 2 . x^k = Pk (b1/ σ2 + b2/ σ2 ...+ bk-1/ σ2 + bk / σk 2 ) Kk = Pk Ak T Σ −1 k = Pk / σk 2 . x^k = x^ k-1 + Pk (bk – x^k-1 )/ σk 2 . x^k = Pk (Pk-1 x^k-1 + (bk – x^k-1 )/ σk 2

Modelo de actualizaciones dinámicas: La primera medida es b0 = A0 x0 + e0 . Se estima x0 usando este conjunto de observaciones b0 ( x^0 ). La ecuación Ao x0 = b0 probablemente estará sobredeterminada. La solución mínimo cuadrática ponderada se obtendrá usando la matriz inversa de las covarianzas C = Σ −1 e,0 (de los residuos de b0 ), de la siguiente manera: x^0 = (A0 T Σ e, 0−1 A0 )-1 (A0 T Σ e, 0−1 A0 ) El error x0 - x^0 tiene como media 0 y su matriz de covarianzas es: Σ 0 = (A0 T Σ e, 0−1 A0 )-1 4

C = Σ e−1 donde: | Σ e,0 0 | Σe = | 0 Σ e, 1| matriz de covarianza de los residuos

| e0 | | e1 |

La matriz Σ e es diagonal porque e1 es independiente de e0 . Por lo tanto la matriz de coeficientes AT Σ e−1 A para x^ 1 es: Σe =

( A0 T A1 T )

| Σ e,0 | 0

0 | -1 | A0 | Σ e, 1| | A1 | = A0 T Σ e, 0−1 A0 + A1 T Σ e, 1−1 A1

Teniendo en cuenta que x^1 es la mejor aproximación para el sistema, las ecuaciones normales se convierten en: AT Σ e−1 A x^ = AT Σ e−1 b. Escribiendo P o P1 o Pnew para la matriz inversa (AT Σ e−1 A)-1 , la solución óptima se escribe: x^1 = P1 (A0 T A1 T ) Σ e (b0 b1 )T = P1 (A0 T Σ e, 0−1 b0 + A1 T Σ e, 1−1 b1 ) Si se intentan utilizar los valores obtenidos en las estimaciones anteriores, x^0 en vez de b0 , se pueden ir actualizando los coeficientes de la matriz de ecuaciones normales por medio de : P1 –1 = P0 –1 + A1 T Σ e, 1−1 A1 La incorporación de nuevas medidas debería reducir los coeficientes de la matriz de covarianzas. Si se escribe la expresión de la estimación x^1 en términos de los datos conocidos de la observación anterior, se obtiene lo siguiente: x^1 = P1 (P0 –1 x^0 + A1 T Σ e, 1−1 b1 ) = P 1 (P1 –1 x^0 - A1 T Σ e, 1−1 A1 x^0 + A1 T Σ e, 1−1 b1 ) = x^ 0 + K1 (b1 – A1 x^0 ) è x^1 = x^ 0 + K1 (b1 – A1 x^0 ) Esta fórmula identifica la matriz de ganancia K, que multiplica a las innovaciones. K1 = P1 A1 T Σ e, 1−1 De esta forma se ha convertido el modelo en un sistema recursivo, del que se conocen las expresiones que estiman por mínimos cuadrados, las varianzas y la ganancia del filtro. Pk –1 = Pk-1 –1 + Ak T Σ e, κ−1 Ak x^k= x^k-1 + Kk (bk – Ak x^k-1 ) Kk = Pk Ak T Σ e, κ−1

5

El siguiente paso consistirá en modelar el sistema por un vector de estado x(t) dependiente del tiempo. Los cambios serán modelados por un sistema de ecuaciones en diferencias que supondremos lineales. Este sistema opera en tiempos discretos, en los que se comete un error por cada paso: xk = Fk-1 xk-1 + ε k La matriz conocida F k-1 es la matriz de cambio de estado. El vector desconocido x k es el estado en cada instante de tiempo. La ecuación que relaciona las observaciones con el estado es la siguiente: b k = A k x k + e k para t = 0,1,.....,k La matriz ponderación o peso es siempre la inversa de la de covarianzas. Se asume que los errores ε k y e k son independientes y Gaussianos , con media cero y matrices de covarianzas Σ e,κ y Σ ε,κ ; E{εκ εκΤ} = Σ ε,κ y Ε{ek ,ek Τ} = Σ e,κ . Ahora el problema de Kalman ya está completamente definido. Este se debe solucionar construyendo las matrices. Ao -Fo

I A1 -F1

I .... .... ....

-Fk-1

I Ak

x0 x1 . . . xk-1 xk

=

b0 0 b1 0 . . . . . 0 bk

e0

ε1 e1

ε2

-

. . . .

εk ek

La estimación óptima x^ se obtiene resolviendo las ecuaciones normales: AT Σ −1 A x^ = AT Σ −1 b La matriz de covarianzas Σ es una banda diagonal, por haber asumido que los vectores de error son independientes, aunque pueden estar correlados entre ellos. La solución es un vector que contiene la mejor estimación de toda la secuencia. x^k = (x^ 0|k ...... x^ k-1|k x^k|k )T Hay que destacar que para la estimación del estado t=k se ha utilizado toda la información disponible. Este hecho supone que las estimaciones de los estados iniciales se ven afectados por las medidas posteriores, por lo que se deben interpretar independientemente cada uno. A este hecho se le conoce como aplanado de los estados anteriores. Se distinguen tres nomenclaturas para los estados: x^j|k (j < k): Valores aplanados o suavizados. x^k|k : Valor del filtro de Kalman. x^k+1|k : Valor predicho.

6

El filtro posee dos pasos: predicción y corrección, que se reflejan en las siguientes expresiones: Predicción: Corrección:

x^k|k-1 = Fk-1 x^k-1|k-1 x^k|k = x^ k|k-1 + Kk (bk – Ak x^k|k-1 )

La matriz de ganancia y de covarianzas predichas y corregidas se expresan: Pk|k-1 = Fk-1 Pk-1|k-1 Fk-1 T + Σ ε,κ Kk = Pk|k-1 Ak T (Ak Pk|k-1 Ak T + Σ e, κ) Pk|k = (I – Kk Ak )Pk|k-1

Covarianza predicción: Matriz de ganancia: Covarianza corregida:

Ejemplo comparativo entre mínimos cuadráticos (Gauss ) y el filtro de Kalman. Supongamos conocidas una serie de muestras u observaciones (números reales ) de un mismo sistema x. Se suponen independientes las observaciones con varianza σ2 =1 . 1º) Estimación mínimo cuadrática. La solución del sistema de n ecuaciones x = bk es la media: x^n = (b1+ b2 ...+ bn ) /n , con varianza P = 1/n. 2º) Estimación usando el filtro de Kalman, comenzando con el modelo xk = xk-1 y Fk = 1 constante para todas las ecuaciones. Aplicando el filtro para las dos primeras ecuaciones se obtiene: xk = bk y xk = xk-1. Aplicando las ecuaciones normales AT A x^ = AT b 1 0 -1 1 0 1

x0 x1

=

b0 0 b1

2 -1

x^0|1 x^1|1

-1 2

Resolviendo el sistema de ecuaciones de la derecha, (AT A) obtiene: x^0|1 = (2b0 +b1 ) / 3 x^1|1 = (b0 +2b1 ) /3

−1

b0 b1

=

= 1/3 (2 1

1

2)

se

Repitiendo el proceso para las tres primeras:

L

1 -1 0 0 0

0 1 1 -1 0

0 0 0 1 1

AT A

2 -1 0

–1 0 3 -1 -1 2

(A T A)-1

1/8

5 2 1 2 4 2 1 2 5 7

x^0|2 = (5b0

+2b1 +b2 ) / 8 x^1|2 = (2b0 +4b1 +2b2 ) /8 x^2|2 = (b0 +2b1 +5b2 ) / 8 Se puede observar cómo cada nueva observación cambia la ponderación del resto de observaciones en cada paso. También se puede observar cómo los P1 = 2/3 y P2 =5/8 aumentan, mientras que en mínimos cuadrados permanecían constantes. Se observa que los errores estimados en cada paso decrecen.

Asíntotas del modelo: Se desea conocer cómo se comporta el filtro cuando el número de observaciones crece, manteniendo Σ ε = 1 y Σ e = 1 y permitirendo distintos valores de σε2 σe2 . La matriz que se debe estudiar es aquella que es tridiagonal, cuyos elementos distintos de 0 son: -1, 3, -1 excepto en la primera y última, con sólo los valores 2 en la diagonal. Esta matriz es simétrica y definida positiva y por lo tanto admite factorización de Cholesky de la forma L LT . Estas dos matrices son bidiagonales. Si se considera ahora tan solo la matriz, con los elementos numerados en las filas primera y última de valor 3 (en la posición correspondiente a la diagonal), se pueden considerar dos límites: uno para los elementos de la diagonal principal (a y b) y otro para los de las diagonales secundarias. a2 + b2 = 3 en la diagonal ab = -1 fuera de ella. Sustituyendo: b2 = 1/a2 , en la primera resulta: a4 – 3a2 +1 = 0. De esta se puede deducir que a2 = (3 + (5)1/2 ) / 2 y b2 = (3 – (5)1/2 ) /2 P/1 + P = (3 – (5)1/2 ) /2 por lo que P (aproximadamente ) = 0.618 . Se define así la asíntota de P y, por tanto, se modela el comportamiento del filtro respecto al aumento de observaciones o iteraciones. A continuación se muestran algunos comportamientos del filtro de Kalman con series de datos (observaciones) y estadísticos iniciales distintos. Se comparan con la estimación proporcionada por Gauss con los mínimos cuadrados.

8

En el primer ensayo se dispone de 50 observaciones de un mismo fenómeno y se conoce, del mismo, que el error estimado en las observaciones es: 0.01 , asimismo se estima un error en el modelo es: 0.00001 . Se supone un valor inicial Xo = 1 y un Po =1: Representación de las observaciones: Observaciones

1,12

1,1

Observación

1,08

1,06 Observaciones 1,04

1,02

1

0,98 0

10

20

30

40

50

60

Nº Observación

Representación de la estimación de Gauss para cada nueva observación. Minimos Cuadrados 1,12

1,1

1,08

1,04 Gauss Observaciones 1,02

1

0,98

0,96

49

47

45

43

41

39

37

35

33

31

29

27

25

23

21

19

17

15

13

9

11

7

5

3

0,94 1

Estimación Gauss

1,06

Nº Observación

9

Representación de la estimación de Gauss frente a la de Kalman.

Gauss vs Kalman 1,06

1,055

1,05

1,045

Estimaciones

1,04

1,035 Gauss Kalman 1,03

1,025

1,02

1,015

1,01

49

47

45

43

41

39

37

35

33

31

29

27

25

23

21

19

17

15

13

9

11

7

5

3

1

1,005 Nº Observación

Representación de la estimación final de Gauss y de Kalman, con la última observación. Gauss-Kalman 1,06

1,055

1,05

1,045

1,035 Kalman Final Gauss 1,03

1,025

1,02

1,015

1,01

49

47

45

43

41

39

37

35

33

31

29

27

25

23

21

19

17

15

13

11

9

7

5

3

1,005 1

Kalman

1,04

Nº Observación

10

Representación de la Ganacia del filtro. k kalman 1,2

Ganancia de Kalman

1 0,8 0,6

k kalman

0,4 0,2

49

46

43

40

37

34

31

28

25

22

19

16

13

10

7

4

1

0 Nº Observación

Representación de la matriz de covarianzas de los errores.

Covarianza

0,012

0,01

0,006

Covarianza

0,004

0,002

49

47

45

43

41

39

37

35

33

31

29

27

25

23

21

19

17

15

13

11

9

7

5

3

0 1

Covarianza del error

0,008

Nº Observación

11

En el segundo ensayo se dispone de 50 observaciones de un mismo fenómeno y se conoce, del mismo, que el error estimado en las observaciones es: 1 , así mismo se estima un error en el modelo es: 1. Se supone un valor inicial Xo = 1 y un Po =1: Representación de las observaciones: Observaciones

1,12

1,1

Observación

1,08

1,06 Observaciones 1,04

1,02

1

0,98 0

10

20

30

40

50

60

Nº Observación

Estimación de Gauss con cada nueva observación:

Minimos Cuadrados 1,12

1,1

1,08

1,04 Gauss Observaciones 1,02

1

0,98

0,96

49

47

45

43

41

39

37

35

33

31

29

27

25

23

21

19

17

15

13

9

11

7

5

3

0,94 1

Estimación Gauss

1,06

Nº Observación

12

Representación de la estimación de Gauss frente a la de Kalman. Gauss vs Kalman 1,12

1,1

1,08

Estimaciones

1,06

1,04 Gauss Kalman 1,02

1

0,98

0,96

49

47

45

43

41

39

37

35

33

31

29

27

25

23

21

19

17

15

13

9

11

7

5

3

1

0,94 Nº Observación

Representación de la estimación final de Gauss y de Kalman, con la última observación.

Gauss vs Kalman 1,1

1,08

1,04 Filtro Kalman Estimación Gauss 1,02

1

0,98

49

47

45

43

41

39

37

35

33

31

29

27

25

23

21

19

17

15

13

11

9

7

5

3

0,96 1

Estimaciones

1,06

Nº Observación

13

Representación de la Ganacia del filtro.

K de Kalman

0,7

0,6

Ganancia de Kalman

0,5

0,4 K de Kalman 0,3

0,2

0,1

49

47

45

43

41

39

37

35

33

31

29

27

25

23

21

19

17

15

13

9

11

7

5

3

1

0 Nº Observación

Representación de la matriz de covarianzas de los errores.

Covarianzas

0,7

0,6

0,4

Covarianzas 0,3

0,2

0,1

49

47

45

43

41

39

37

35

33

31

29

27

25

23

21

19

17

15

13

9

11

7

5

3

0 1

Covarizanzas de los errores

0,5

Nº Observación

14