Filtro de Kalman

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