1. No category

Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Fusión de sensores. Ejemplo: el canal vertical.
INS-GPS
Ejemplo: el canal vertical.
Se vio en el tema 4 que el canal vertical del INS es inestable.
Una forma de estabilizar el canal es usar la medida de altitud
obtenida de medidas barométricas, hB . Se denomina
“estimador baro-inercial de la altitud”.
Fundamentos de Navegación Aérea
Tema 11: Sistemas de navegación integrados. El filtro de
Kalman.
Recordemos que las ecuaciones del canal vertical venı́an dadas
por:
ĥ˙ = −V̂D ,
V̂˙ D
= ρ̂z +
µe
(Re + ĥ)2
,
donde ρ̂z es la componente z de −(ω̂ nn/e + 2ω̂ ne/i )× v̂ n + ânNG .
3 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Fusión de sensores. Ejemplo: el canal vertical.
INS-GPS
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Fusión de sensores.
Fusión de sensores. Ejemplo: el canal vertical.
INS-GPS
Estimador baro-inercial de la altitud I
Se modifica el canal vertical del INS de la siguiente forma,
usando hB :
ĥ˙ = −V̂D − C1 (ĥ − hB ),
� t
µe
˙
V̂D = ρ̂z +
+ C2 (ĥ − hB ) + C3
(ĥ(τ ) − hB (τ ))dτ,
(Re + ĥ)2
0
Una aeronave actual dispone de una gran diversidad de
sensores y sistemas de navegación, que pueden obtener total o
parcialmente las variables de navegación PVAT.
Por ejemplo hemos visto el INS, que a partir de las medidas
de la IMU, el modelo de Tierra y gravedad, y una estimación
inicial, nos da posición, velocidad y actitud en todo momento.
También hemos visto el GPS, que igualmente es capaz de
darnos todos éstos datos, o al menos (si no disponemos de
múltiples antenas), la posición y la velocidad.
Puede haber otros sistemas (DME-DME, etc...)
Cada sistema dará una estimación diferente, sujeta a error.
La idea de fusión de sensores y de los sistemas de navegación
integrados, consiste en obtener una única estimación PVAT a
partir de todas las anteriores, tal que el error sea el menor
posible.
donde C1 , C2 y C3 son ganancias a determinar.
Calculando como en el tema 4 el error de altitud y
despreciando el error en el término ρz , obtenemos:
δ ḣ = −δVD + C1 (ĥ − hB ),
� t
2g0
δ V̇D ≈ −
δh − C2 (ĥ − hB ) − C3
(ĥ(τ ) − hB (τ ))dτ,
Re
0
y obsérvese que ĥ − hB = ĥ − h + h − hB = −(δh − δhB ),
donde δhB es el error de estimación barométrico, que
suponemos aproximadamente constante.
2 / 35
4 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Fusión de sensores. Ejemplo: el canal vertical.
INS-GPS
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Estimador baro-inercial de la altitud II
El caso INS-GPS
Por tanto:
El sistema de navegación INS y el GPS son particularmente
complementarios.
El INS:
δ ḣ = −δVD − C1 (δh − δhB ),
� t
2g0
δ V̇D ≈ −
δh + C2 (δh − δhB ) + C3
(δh − δhB )dτ,
Re
0
Da una estimación continua en el tiempo.
Su error crece con el tiempo.
Posee un elevado ancho de banda (KHz).
y tomando derivada en la primera ecuación y sustituyendo la
segunda, obtenemos:
� t
2g0
δ ḧ =
δh − C2 (δh − δhB ) − C3
(δh − δhB )dτ − C1 δ ḣ.
Re
0
El GPS:
Proporciona una medida de alta precisión pero discreta en el
tiempo.
El error está acotado.
Posee un bajo ancho de banda (Hz).
Tomando otra derivada y reescribiendo la ecuación:
Una primera solución serı́a resetear el INS cada vez que se
obtenga una medida GPS. Pero la medida GPS tampoco es
exacta.
Por tanto hay que intentar, de algún modo, combinar el INS y
el GPS para minimizar el error final.
...
2g0
δ h + C1 δ ḧ + (C2 −
)δ ḣ + C3 δh = C3 δhB .
Re
Los autovalores de esta ecuación vienen dados por las raı́ces
0
del polinomio s 3 + C1 s 2 + (C2 − 2g
Re )s + C3 .
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
5 / 35
Fusión de sensores. Ejemplo: el canal vertical.
INS-GPS
Existen dos formas de llevar a cabo la integración:
Loose Integration:
Éste tipo de integración permite tomar dos sistemas separados,
un INS y un GPS, y a partir de las salidas de ambos, obtener
una estimación común.
Es la forma más simple de integrar GPS e INS.
No requiere modificar las estimaciones internas de ambos
sistemas.
(s + λ)(s + λ − jλ)(s + λ + jλ)
= (s + λ)(s 2 + 2λs + 2λ2 )
2
= s + 3λs + 4λ s + 2λ
Fusión de sensores. Ejemplo: el canal vertical.
INS-GPS
Tight Integration y Loose Integration
Tı́picamente se eligen lo valores de C1 , C2 y C3 para que los
autovalores tengan parte real negativa (es decir, la ecuación
de δh sea estable). Una elección clásica es fijar un autovalor al
valor −λ y los otros dos a los valores −λ + jλ y −λ − jλ.
El polinomio caracterı́stico serı́a entonces:
2
7 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Estimador baro-inercial de la altitud III
3
Fusión de sensores. Ejemplo: el canal vertical.
INS-GPS
Tight Integration:
Éste tipo de integración emplea las señales de entrada al INS y
GPS, es decir, las medidas de giróscopos y acelerómetros y los
observables GPS, y los integra directamente.
Es más complejo de desarrollar.
No se emplean los algoritmos que hemos visto de GPS e INS,
sino un único algoritmo que integra los dos sistemas a la vez.
Se obtienen estimaciones más precisas que en la tipo loose.
3
Sustituyendo en el polinomio en función de los coeficientes
3
estos valores, se llega a: C1 = 3λ, C2 = 4λ2 + 2g
Re , C3 = 2λ .
Un valor tı́pico elegido de λ es λ = 0,01.
Observación: otra forma de diseñar un estimador baro-inercial
serı́a con un Filtro de Kalman, aplicando la teorı́a que veremos
a continuación.
6 / 35
En ambos casos, la herramienta clave para desarrollar la
integración es el Filtro de Kalman y sus extensiones (Filtro
Extendido de Kalman).
8 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
El filtro de Kalman
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Ejemplo 1-D: propagación del error de un giróscopo
El filtro de Kalman (KF) fue desarrollado por Rudolph E.
Kalman, un ingeniero húngaro nacionalizado estadounidense.
Presentó su filtro a la NASA en 1960; la NASA buscaba un
algoritmo de fusión de sensores para el programa espacial
Apollo.
Finalmente una versión del KF fue utilizada en las misiones
Apollo para integrar las diferentes medidas de los sensores del
vehı́culo espacial.
A dı́a de hoy, el KF se emplea no sólo en navegación sino en
multitud de sistemas en los que se desea reconstruir una señal
que evoluciona en el tiempo, a partir de medidas con ruido,
por ejemplo en teléfonos móviles.
Realmente el KF sólo sirve para sistemas lineales. Puesto que
muchos sistemas reales son no lineales, se han desarrollado
extensiones no lineales, conocidas como Filtro Extendido de
Kalman (EKF); en Navegación se emplean éste tipo de filtros.
Nos limitaremos a entender el KF lineal y sus fundamentos.
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
El error de estimación δθ = θ − θ̂ será el siguiente:
δ θ̇ = ω − ω̂ = ν
Suponiendo ν ruido blanco (unidimensional) de varianza Q y
δθ(0) ≈ N(0, P0 ), encontramos (usando la teorı́a de procesos
expuesta en las primeras transparencias) que el error es un
proceso estocástico gaussiano, δθ ≈ N(m, P), donde:
ṁ = 0 −→ m = δθ0 = 0,
y
Ṗ = σν2 −→ P = P0 + Qt
Por tanto, aunque la media del error permanece fija en cero,
la varianza del error crece linealmente con el tiempo y
eventualmente se dispara, siendo por tanto este estimador
inútil a medio plazo.
9 / 35
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
11 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Ejemplo 1-D: propagación del error de un giróscopo
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Medida externa
Para entender el filtro de Kalman a nivel conceptual, vamos a
ejemplificarlo en el caso más sencillo: un único grado de
libertad de giro. Por tanto hay un sólo ángulo θ, cuya
ecuación diferencial cinemática es
θ̇ = ω
Un giróscopo proporcionará una medida de ω que denotamos
por ω̂. En realidad no será exactamente ω, sino que
estará contaminado por un cierto ruido blanco ν:
ω = ω̂ + ν
Si intentamos estimar θ (denotamos a la estimación θ̂)
directamente de ω̂, tendremos:
˙
θ̂ = ω̂
10 / 35
Supongamos que se tiene una medida externa adicional del
ángulo. Suponemos que cada ciertos instantes discretos t = tk
se realiza una medida del ángulo θ̂(tk ), que llamamos θ̂kmed
con algún otro dispositivo, que también tendrá un cierto error
asociado, de forma que θk = θ̂kmed + �, donde � es ruido
blanco, de varianza R.
Como el tiempo entre medidas puede ser grande, no es buena
idea decir θ̂(t) = θ̂kmed para t ∈ [tk , tk+1 ).
Otra idea es resetear el estimador de las anteriores
transparencias cuando se llega a t = tk , es decir, combinar las
medidas de la siguiente forma:
θ̂˙ = ω̂, θ̂(tk ) = θ̂kmed , t ∈ [tk , tk+1 ),
Por tanto cada nueva medida externa se reinicia la condición
inicial de la ecuación diferencial y se vuelve a integrar.
Es fácil ver que el error obtenido de esta forma serı́a
δθ ≈ N(m, P), con m = 0 y Ṗ = Q, para t ∈ [tk , tk+1 ), con
P(tk ) = R, luego P = R + Q(t − tk ).
12 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Filtro de Kalman
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Filtro de Kalman
Con la anterior forma que el error serı́a máximo justo antes de
una medida, obteniendo P = R + Q(tk+1 − tk ) en dicho
instante.
El problema es que se ha despreciado la estimación que daba
la ecuación diferencial, cuando entre tk y tk+1 no ha dado
tiempo a que se el error crezca demasiado. La idea del filtro
de Kalman es combinar la estimación de la ecuación
diferencial justo antes de la medida externa, con la medida
externa, de forma que la covarianza del error sea mı́nima.
A la estimación justo antes de la medida se le llama
estimación a priori θ̂k− .
La estimación después de la medida (estimación a posteriori),
se denota θ̂k+ y se calcula como:
Resumiendo el algoritmo:
Para t ∈ [tk , tk+1 ), se integra usando la medida de los
giróscopos, partiendo de la última estimación a posteriori:
θ̂˙ = ω̂, θ(tk ) = θk+ ,
También se propaga la covarianza del error:
Ṗ = Q,
Al llegar a t = tk+1 , se obtiene de estas ecuaciones
−
−
θk+1
= θ(tk+1 ) y Pk+1
, y se obtiene una medida externa
med
θ̂k+1 . Aplicamos el filtro de Kalman:
+
−
−
med
θ̂k+1
= θ̂k+1
+ K (θ̂k+1
− θ̂k+1
),
θ̂k+ = θ̂k− + K (θ̂kmed − θ̂k− )
donde K es la ganancia de Kalman y el paréntesis es la
diferencia entre la estimación que se tenı́a y la medida externa.
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
P(tk ) = Pk+ ,
donde K =
13 / 35
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
−
Pk+1
−
Pk+1 +R
+
, y también obtenemos Pk+1
=
−
Pk+1
+R
.
Repetimos y volvemos a integrar las ecuaciones diferenciales
hasta la nueva medida en t = tk+2 .
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Filtro de Kalman
−
Pk+1
R
15 / 35
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Filtro de Kalman: casos extremos
K se calcula para minimizar la covarianza del error a
posteriori.
Si la medida del giróscopo es muy mala, es decir, Q es muy
grande, entonces Pk− → ∞, y se puede ver que Pk+ → R,
K → 1, y por tanto θk+ = θk (es decir se coge la medida del
sensor externo despreciando el resultado de integrar la
ecuación diferencial).
La covarianza a priori será Pk− .
A posteriori, calculando la covarianza de la ecuación de θk+ :
Pk+ = (1 − K )2 Pk− + K 2 R
Si la medida del sensor externo es muy mala, es decir, R es
muy grande, entonces se puede ver que Pk+ → Pk− , K → 0, y
por tanto θk+ = θk− (es decir se coge la estimación resultado
de integrar la ecuación diferencial despreciando la medida del
sensor externo ).
Derivando con respecto a K e igualando a cero:
0 = −2(1 − K )Pk− + 2KR, luego K =
Pk−
.
−
Pk +R
Por tanto la covarianza a posterior será, sustituyendo K :
P −R
Pk+ = −k
Pk + R
Se ve fácilmente que Pk+ es menor que R y menor que Pk−
(recordar que ambas son positivas): por tanto se ha
conseguido mejorar tanto la estimación anterior como la que
se tenı́a de la medida!
Si resulta que Pk− = R, es decir, la estimación a priori tiene el
mismo error que es sensor externo, entonces se puede ver que
θ +θ −
Pk+ = R/2, K → 1/2, y por tanto θk+ = k 2 k (es decir se
toma la media entre la estimación resultado de integrar la
ecuación diferencial y la medida del sensor externo ).
14 / 35
16 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman
Ecuaciones del proceso y la medida
Resumiendo las ecuaciones:
A continuación se explicará el filtro de Kalman para sistemas
lineales discretos.
En realidad, todos los sistemas a efectos prácticos son
discretos, por lo que tiene más sentido realizar la explicación
general en el caso discreto.
En la nomenclatura del KF, al sistema se le llama “proceso”.
En el desarrollo es importante tener presente el ejemplo 1-D
anteriormente desarrollado, ya que conceptualmente lo que se
hace es similar.
La mayor dificultad viene por la nomenclatura y la cantidad de
matrices que aparecen.
El KF trasciende la navegación aérea y es una herramiente
ampliamente utilizada en muchos contextos de ingenierı́a,
cuando se quieren conocer (estimar) ciertas cantidades a las
que no se tiene acceso directo sino que se tiene modelos y
medidas de otras cantidades relacionadas.
x(tk+1 ) = Ak x(tk ) + Bk �(tk ),
z(tk+1 ) = Hk+1 x(tk+1 ) + ν(tk+1 ),
E [�(tk )] = E [ν(tk )] = 0,
E [�(tk )�T (tj )] = δkj Qk ,
E [ν(tk )ν T (tj )] = δkj Rk ,
E [�(tk )ν T (tj )] = 0,
x(t0 ) ∼ Nnx (x̂0 , P0 ).
17 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Definimos la estimación en tk de x(tk ) como x̂(tk ).
Definimos la covarianza del error de estimación como
P(tk ) = E [(x(tk ) − x̂(tk ))(x(tk ) − x̂(tk ))T ].
El objetivo del filtro de Kalman es, empleando el
conocimiento de las ecuaciones arriba formuladas, y a partir
de las medidas z(tk ), obtener la mejor estimación posible, es
decir, el valor de x̂(tk ) que minimiza P(tk ).
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Procesos dinámicos discretos con medidas
PROCESO: Consideremos el siguiente modelo discreto de un
proceso: x(tk+1 ) = Ak x(tk ) + Bk �(tk ), donde x es un proceso
gaussiano con dimensión nx , Ak es una matriz (que puede
cambiar en cada instante de tiempo tk ) de dimensión nx × nx ,
�(tk ) es ruido blanco gaussiano de dimensión n� y varianza Qk
(el ruido del proceso), y Bk es una matriz (que puede cambiar
en cada instante de tiempo tk ) de dimensión nx × n� .
MEDIDA: En cada instante también consideramos que se
realiza una medida, representada por z, y definida de la
siguiente forma: z(tk+1 ) = Hk+1 x(tk+1 ) + ν(tk+1 ), donde z
es la medida, de dimensión nz , Hk es una matriz (que puede
cambiar en cada instante de tiempo tk ) de dimensión nz × nx ,
y ν(tk ) es ruido blanco gaussiano de dimensión nν y varianza
Rk (el ruido de medida).
Además suponemos que ν(tk ) y �(tk ) son independientes, y
que sabemos que la condición inicial de x es
x(t0 ) ∼ Nnx (x̂0 , P0 ).
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
19 / 35
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
El filtro de Kalman I
Si sólo tuviéramos el proceso, podemos calcular su media y
tomamos x̂ como dicha media; por tanto,
x(tk ) ∼ Nnx (x̂(tk ), Pk ), donde:
x̂(tk+1 ) = Ak x̂(tk ),
T
Pk+1 = Ak Pk AT
k + B k Qk B k .
18 / 35
La idea de Kalman es decir: la estimación arriba escrita es
válida (lo mejor que podemos obtener) antes de tomar la
medida z(tk+1 ). Denotamos dicha estimación “a priori” como
−
x̂ − (tk+1 ) y su covarianza como Pk+1
.
Ahora, si la estimación fuera perfecta y la medida no tuviera
error, se tendrı́a que z(tk+1 ) = Hk+1 x̂ − (tk+1 ). Como no es
ası́, se actualiza la estimación (“a posteriori”) de forma
proporcional a la discrepancia entre lo que esperamos y lo que
obtenemos:
x̂ + (tk+1 ) = x̂ − (tk+1 ) + Kk+1 (z(tk+1 ) − Hk+1 x̂ − (tk+1 )).
20 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
El filtro de Kalman II
El filtro de Kalman III
Desarrollando:
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
+
de x̂ + (tk+1 ), Pk+1
, sea la menor posible.
+
Calculemos Pk+1 :
+
Pk+1
= E [(x(tk+1 ) − x̂ + (tk+1 ))(x(tk+1 ) − x̂ + (tk+1 ))T ], y
sustituyendo la ecuación de x̂ + (tk+1 ):
+
Pk+1
=
E
=
E
��
��
×
x(tk+1 ) − x̂
x(tk+1 ) − x̂
�
x(tk+1 ) − x̂
+
(tk+1 )
−
−
��
x(tk+1 ) − x̂
+
(tk+1 )
�
T
+
Tr[Pk+1
]
+
∂Tr[Pk+1
]
∂Kk+1
(tk+1 ) − Kk+1 (z(tk+1 ) − Hk+1 x̂
−
−
(tk+1 )
+
Pk+1
=
E
×
�
x(tk+1 ) − x̂
x(tk+1 ) − x̂
−
−
�
(tk+1 ) − Kk+1 (Hk+1 x(tk+1 ) + ν(tk+1 ) − Hk+1 x̂
(tk+1 ) − Kk+1 (Hk+1 x(tk+1 ) + ν(tk+1 ) − Hk+1 x̂
−
−
T
T
= 2(Rk+1 + Hk+1 Pk+1
Hk+1
)Kk+1
− 2Hk+1 Pk+1
−
−
T
T
Kk+1
= (Rk+1 + Hk+1 Pk+1
Hk+1
)−1 Hk+1 Pk+1
Por tanto:
�
(tk+1 ))
−
−
T
T
Tr[Kk+1 (Rk+1 + Hk+1 Pk+1
Hk+1
)Kk+1
] − 2Tr[Kk+1 Hk+1 Pk+1
]
Igualando a cero:
�
(tk+1 ) − Kk+1 (z(tk+1 ) − Hk+1 x̂
=
Por tanto:
T
−
−
T
T
Kk+1 = Pk+1
Hk+1
(Rk+1 + Hk+1 Pk+1
Hk+1
)−1
�
+
Y sustituyendo en Pk+1
para encontrar el valor minimizado, se
llega a:
Sustituyendo ahora z(tk+1 ) = Hk+1 x(tk+1 ) + ν(tk+1 ):
��
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
−
−
(tk+1 )
�
(tk+1 ))
�T �
+
−
Pk+1
= (I − Kk+1 Hk+1 )Pk+1
21 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
El filtro de Kalman III
+
=
E
×
=
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Algoritmo del filtro de Kalman
Simplificando, obtenemos:
Pk+1
23 / 35
��
�
(I − Kk+1 Hk+1 )(x(tk+1 ) − x̂
(I − Kk+1 Hk+1 )(x(tk+1 ) − x̂
−
−
−
) − Kk+1 ν(tk+1 )
) − Kk+1 ν(tk+1 )
(I − Kk+1 Hk+1 )Pk+1 (I − Kk+1 Hk+1 )
T
El algoritmo queda como sigue:
�
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 P0+ = P0 .
2 Fase de propagación; usamos la ecuación del sistema dinámico
para calcular la estimación a priori:
1
�T �
T
+ Kk+1 Rk+1 Kk+1
Es necesario encontrar el valor de Kk+1 que minimiza la
anterior expresión. No obstante no podemos “minimizar una
matriz”. Puesto que la diagonal de la matriz de covarianzas
son las varianzas de las variables individuales, es suficente con
minimizar es la traza (suma de los elementos de la diagonal,
es decir, suma de las varianzas, que es una cantidad positiva,
luego si se minimiza las varianzas estarán lo más cerca posible
de cero).
¿Cómo se deriva respecto a una matriz? Se tiene:
∂Tr[ABAT ]
= 2BAT ,
A
x̂ − (tk+1 )
=
Ak x̂ + (tk ),
−
Pk+1
=
T
Ak Pk+ AT
k + B k Qk B k .
Preparándonos para la medida, calculamos la ganancia de
�
�−1
−
−
T
T
Kalman: Kk+1 = Pk+1
Hk+1
Hk+1 Pk+1
Hk+1
+ Rk+1
.
4 Tomamos la medida y calculamos la estimación a posteriori:
3
∂Tr[AB]
=B
A
5
22 / 35
x̂ + (tk+1 )
=
+
Pk+1
=
x̂ − (tk+1 ) + Kk+1 (z(tk+1 ) − Hk+1 x̂ − (tk+1 )),
−
(I − Kk+1 Hk+1 )Pk+1
.
Iteramos para los siguientes valores de k.
24 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
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
El caso INS-GPS
Esquema de la integración INS-GPS (loose):
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 ).
25 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
El caso INS-GPS
En el caso INS-GPS no podemos aplicar el Filtro de Kalman
directamente porque los sistemas y medidas son no lineales.
Lo que se hace es aplicar la solución al error de navegación.
Recordemos que derivamos para el INS una ecuación de la
forma: δx(tk+1 ) = Ak δx(tk ) + Bk �(tk ), donde el vector δx(tk )
contiene los errores de posición, velocidad y actitud en tk y
�(tk ) son las fuentes de error.
Por otro lado en el tema del GPS obtuvimos ecuaciones de la
forma: ∆ρ(tk+1 ) = Hk+1 ∆x(tk+1 ) + ν(tk+1 ), donde ∆x(tk+1 )
eran errores de posición (y velocidad, si también estimamos
velocidad) respecto a una estimación inicial y ∆ρ(tk+1 ) las
diferencias entre los observables medidos y los estimados.
Por tanto usando la medida del INS como estimación para el
GPS, ya tenemos los errores linealizados escritos de una forma
adecuada para implementar el filtro de Kalman!
El error estimado se suma a la posición estimada por el INS,
para conseguir la mejor estimación final posible.
27 / 35
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Ejemplo 2-D del filtro de Kalman I
Para entender mejor el filtro de Kalman consideremos un
sistema sencillo. Imaginemos un vehı́culo que sólo se puede
mover en una dirección, con un acelerómetro de un ancho de
banda de 100Hz que mide la aceleración en dicha dirección, y
con un sensor con un ancho de banda de 1Hz que estima la
posición en dicha dirección.
El modelo del sistema será:ẍ = a. Llamando v a la velocidad:
d
dt
�
�
x
v
=
�
0
0
��
1
0
x
v
�
+
�
0
a
�
δx
δv
�
+
�
0
1
�
El modelo del error será:
d
dt
�
δx
δv
�
=
�
0
0
1
0
��
δa
Pasando a tiempo discreto y teniendo en cuenta que
x(tk+1 )−x(tk )
d
:
dt x(t) ≈
∆t
�
26 / 35
δx(tk+1 )
δv (tk+1 )
�
=
�
1
0
∆t
1
��
δx(tk )
δv (tk )
�
+
�
0
∆t
�
δa(tk )
28 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Ejemplo 2-D del filtro de Kalman II
Ejemplo 2-D del filtro de Kalman: simulación I
Simulación de la posición (exacta) y medidas:
Por otro lado el modelo de medida será: z = x + ν, luego el
modelo de error será: δz = δx + ν.
Escribiéndolo todo:
�
�
=
�
δz(tk+1 )
=
δx(tk+1 ) + ν(tk+1 )
δx(tk+1 )
δv (tk+1 )
1
0
∆t
1
��
�
δx(tk )
δv (tk )
+
�
�
0
∆t
('!
�
1
0
�
0,01
1
, Bk =
�
0
0,01
�
*
δa(tk+1 )
+,-./.,0
123.34-
(!!
Además las medidas sólo se hacen con una frecuencia de 1Hz
(cada segundo), mientras que la frecuencia del acelerómetro
es 100 Hz con lo que deberı́amos tomar ∆t = 0,01.
Supongamos además que la precisión de los instrumentos es:
2 = 0,1, σ 2 = 0,01, y que se verifican las hipótesis del KF
σδa
ν
(ruidos blancos gaussianos, independientes, etc...).
En la nomenclatura que hemos usado para el KF, tendremos:
Ak =
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
, Qk = 0,1, Rk = 0,01, Hk =
� �
1
"'!
"!!
&'!
&!!
�
0
, tk = n
0, tk =
� n.
'!
donde n es cualquier entero (para modelar que se toman
medidas cada segundo, pero no en fracciones de segundo).
!*
!
"!
#!
$!
)
%!
&!!
&"!
29 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Ejemplo 2-D del filtro de Kalman III
�
=
Pk+1
=
δx̂ − (tk+1 )
δv̂ − (tk+1 )
−
�
�
��
�
δx̂ + (tk )
δv̂ + (tk )
�
�
0,01
1
+
Pk
1
0,01
1
0
0,01
1
1
0
('!
"'!
"!!
0
1
�
+ 0,1
�
�
0
0,01
�
0
0,01
�
1
0
0
1
� −
Pk+1
0
1
+ 0,01
−1
�
&'!
&!!
δx̂ + (tk+1 )
δv̂ + (tk+1 )
�
=
+
Pk+1
=
�
δx̂ − (tk+1 )
δv̂ − (tk+1 )
�
1
(I − Kk+1
�
!
!'!*
!
0
−
)Pk+1 .
�
1
δx̂ + (tk+1 )
δv̂ + (tk+1 )
�
=
�
,-./0/-1
234/45.
"!
#!
$!
)
%!
&!!
&"!
δx̂ − (tk+1 )
δv̂ − (tk+1 )
0
�
�
δx̂ − (tk+1 )
δv̂ − (tk+1 )
�
%
*
637-0/454
6*3.)/2545*43*234/45.
+
$
,
'
#
(
"
donde δz(tk+1 ) = z(tk+1 ) − Hk+1 (x̂(tk+1 ) +
Si no hubo medida, entonces simplemente:
�
Si intentamos estimar la velocidad con la fórmula
x(t )−x(t
)
v (tk ) = k ∆t k−1 se obtiene una estimación muy mala:
.
+ Kk+1 (δz(tk+1 ) −
�
Usando las medidas para estimar la posición, el resultado es
bueno porque el sensor es preciso y el movimiento en x es
lento.
'!
Tomamos la medida y calculamos la estimación a posteriori:
�
*
(!!
Si tk+1 = n, es decir, tiene un valor entero, significa que ha
habido medida.
Entonces,� calcular
la ganancia de Kalman:
�
��
�
�
−
Kk+1 = Pk+1
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Ejemplo 2-D del filtro de Kalman: simulación II
Por tanto las ecuaciones del filtro de Kalman dirán, para cada
instante de tiempo tk+1 :
�
31 / 35
�
δx̂ − (t
k+1 )).
&
!
!&
!"*
!
+
−
"!
#!
$!
)
%!
&!!
&"!
, Pk+1 = Pk+1 .
Actualizamos x̂(tk+1 ) = x̂(tk+1 ) + δx̂ + (tk+1 ). Iteramos para
los siguientes valores de k.
30 / 35
32 / 35
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Ejemplo 2-D del filtro de Kalman: simulación III
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Ejemplo 2-D del filtro de Kalman: simulación V
Comportamiento de la estimación y del error sin filtro de
Kalman:
Comparación de errores con y sin filtro de Kalman:
#!
#!!
'!!
)
'!
*+,-.-+/
0,(-12.-+/)30)*+,-.-+/
)
,--.-)/,)0.1232.4)5124)678
,--.-)/,)0.1232.4)53.4)678
"!
"!!
&!
&!!
!)
!
!)
!
"!
#!
$!
(
%!
&!!
"!
#!
&"!
$!
(
%!
&!!
&*+
%
$
&"!
)
,--.-)/,)9,:.32/;/)5124)678
,--.-)/,)9,:.32/;/)53.4)678
)
405+.-323
0,(-12.-+/)30)405+.-323
&
#
!*+
"
!
ï")
!
!)
!
"!
#!
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
$!
(
%!
&!!
&"!
33 / 35
Deducción del filtro de Kalman. Ecuaciones.
Ejemplo de un filtro de Kalman
Ejemplo 2-D del filtro de Kalman: simulación IV
Comportamiento de la estimación y del error con filtro de
Kalman:
#!!
)
'!!
"!!
&!!
!)
!
*+,-.-+/
0,(-12.-+/)30)*+,-.-+/)4567
"!
#!
$!
(
%!
&!!
$
&"!
)
#
"
!
ï")
!
809+.-323
0,(-12.-+/)30)809+.-323)4567
"!
#!
$!
(
%!
&!!
&"!
34 / 35
"!
#!
$!
(
%!
&!!
&"!
35 / 35