Geodesia. Cartografía. Sistemas de referencia. Tiempos.

Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
El filtro de Kalman II
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
En la ecuación
ˆx + (tk+1) = ˆx − (tk+1) + Kk+1(z(tk+1) − Hk+1ˆx − (tk+1)) lo
único que no conocemos es Kk+1, que es la ganancia de
Kalman. Ésta se determina para garantizar que la covarianza
, sea la menor posible.
de ˆx + (tk+1), P +
k+1
Calculemos P +
k+1 :
P +
k+1 = E[(x(tk+1) − ˆx + (tk+1))(x(tk+1) − ˆx + (tk+1)) T ], y
sustituyendo la ecuación de ˆx + (tk+1):
P + k+1
" „
= E x(tk+1 ) − ˆx + « „
(tk+1 ) x(tk+1 ) − ˆx + « #
T
(tk+1 )
»„
= E x(tk+1 ) − ˆx − (tk+1 ) − Kk+1 (z(tk+1 ) − Hk+1 ˆx − «
(tk+1 )
„
× x(tk+1 ) − ˆx − (tk+1 ) − Kk+1 (z(tk+1 ) − Hk+1 ˆx − « #
T
(tk+1 ))
Sustituyendo ahora z(tk+1) = Hk+1x(tk+1) + ν(tk+1):
P +
k+1 =
h“
E x(tk+1) − ˆx − (tk+1) − Kk+1(Hk+1x(tk+1) + ν(tk+1) − Hk+1ˆx − ”
(tk+1) “
× x(tk+1) − ˆx − (tk+1) − Kk+1(Hk+1x(tk+1) + ν(tk+1) − Hk+1ˆx − ” –
T
(tk+1)) Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
El filtro de Kalman III
Simplificando, obtenemos:
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
P +
k+1 =
h“
E (I − Kk+1Hk+1)(x(tk+1) − ˆx − ”
) − Kk+1ν(tk+1) “
× (I − Kk+1Hk+1)(x(tk+1) − ˆx − ” –
T
) − Kk+1ν(tk+1) = (I − K k+1H k+1)P −
k+1 (I − K k+1H k+1) T + K k+1R k+1K T
k+1
Es necesario encontrar el valor de Kk+1 que minimiza la
anterior expresión. Usando cálculo matricial, se encuentra que
Kk+1 = P −
k+1HT
k+1
Hk+1P −
k+1HT −1 k+1 + Rk+1
Sustituyendo ésta expresión se llega a que:
P +
−
k+1 = (I − Kk+1Hk+1)Pk+1 . Ésta es la covarianza mínima.
13 / 26
14 / 26
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Algoritmo del filtro de Kalman
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
El algoritmo queda como sigue:
1 En el instante de tiempo tk+1, suponemos que tenemos la
anterior estimación que incluyó también la última medida:
ˆx + (tk) y su covarianza P + tk. Para k = 0 tomamos
ˆx + (t0) = ˆx 0 y P + 0
= P0.
2 Fase de propagación; usamos la ecuación del sistema dinámico
para calcular la estimación a priori:
ˆx − (tk+1) = Ak ˆx + (tk),
P −
k+1
= AkP +
k AT k + BkQkB T k .
3 Preparándonos para la medida, calculamos la ganacia de
Kalman: Kk+1 = P −
k+1HT
k+1
Hk+1P −
k+1HT −1. k+1 + Rk+1
4 Tomamos la medida y calculamos la estimación a posteriori:
ˆx + (tk+1) = ˆx − (tk+1) + Kk+1(z(tk+1) − Hk+1ˆx − (tk+1)),
P +
k+1
= (I − Kk+1Hk+1)P −
k+1 .
5 Iteramos para los siguientes valores de k.
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Sobre las medidas
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Observación: es posible que no se realice una medida cada tk,
sino que en ciertos instantes se hagan medidas, y en otros no
se haga ninguna medida.
Por ejemplo podemos tener un sensor con bajo ancho de
banda (como el GPS) mientras que nuestro tiempo de
muestreo ∆t representa una elevada frecuencia.
Una forma de solucionarlo es tomar Hk = 0, luego Kk = 0 en
los instantes tk en los que no se realizan medidas. Por tanto
no es necesario realizar ninguna actualización y
ˆx + (tk) = ˆx − (tk), P + (tk) = P − (tk).
15 / 26
16 / 26