Sistemas de navegación integrados. El filtro de Kalman.

Sistemas de navegación integrados. El filtro de Kalman.

Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Navegación Aérea
Tema 6: Sistemas de navegación integrados. El filtro de
Kalman.
Rafael Vázquez Valenzuela
Departamento de Ingenierı́a Aeroespacial
Escuela Superior de Ingenieros, Universidad de Sevilla
[email protected]
26 de abril de 2010
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Fusión de sensores. Ejemplo: el canal vertical.
INS-GPS
Fusión de sensores.
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.
2 / 26
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”.
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 / 26
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
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 ),
Z t
µe
˙
V̂D = ρ̂z +
+ C2 (ĥ − hB ) + C3
(ĥ(τ ) − hB (τ ))dτ,
2
(Re + ĥ)
0
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 ),
Z 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.
4 / 26
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Fusión de sensores. Ejemplo: el canal vertical.
INS-GPS
Estimador baro-inercial de la altitud II
Por tanto:
δ ḣ = −δVD − C1 (δh − δhB ),
Z t
2g0
δ V̇D ≈ −
δh + C2 (δh − δhB ) + C3
(δh − δhB )dτ,
Re
0
y tomando derivada en la primera ecuación y sustituyendo la
segunda, obtenemos:
Z t
2g0
δ ḧ =
δh − C2 (δh − δhB ) − C3
(δh − δhB )dτ − C1 δ ḣ.
Re
0
Tomando otra derivada y reescribiendo la ecuación:
...
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 .
5 / 26
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Fusión de sensores. Ejemplo: el canal vertical.
INS-GPS
Estimador baro-inercial de la altitud III
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:
(s + λ)(s + λ − jλ)(s + λ + jλ)
= (s + λ)(s 2 + 2λs + 2λ2 )
= s 3 + 3λs 2 + 4λ2 s + 2λ3
Sustituyendo en el polinomio en función de los coeficientes
3.
estos valores, se llega a: C1 = 3λ, C2 = 4λ2 + 2g
,
C
=
2λ
3
Re
Un valor tı́pico elegido de λ es λ = 0,01.
6 / 26
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Fusión de sensores. Ejemplo: el canal vertical.
INS-GPS
El caso INS-GPS
El sistema de navegación INS y el GPS son particularmente
complementarios.
El INS:
Da una estimación continua en el tiempo.
Su error crece con el tiempo.
Posee un elevado ancho de banda (KHz).
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).
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.
7 / 26
Sistemas de navegación integrados
Filtrado óptimo de sistemas lineales: el filtro de Kalman.
Fusión de sensores. Ejemplo: el canal vertical.
INS-GPS
Tight Integration y Loose Integration
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.
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.
En ambos casos, la herramienta clave para desarrollar la
integración es el Filtro de Kalman y sus extensiones (Filtro
Extendido de Kalman).
8 / 26
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
El filtro de Kalman
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.
9 / 26
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
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 ).
10 / 26
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
Ecuaciones del proceso y la medida
Resumiendo las ecuaciones:
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 ).
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 ).
11 / 26
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
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
+
B
Q
B
k k k .
k
La idea de Kalman es decir: la estimación arriba escrita es
válida 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:
x̂ + (tk+1 ) = x̂ − (tk+1 ) + Kk+1 (z(tk+1 ) − Hk+1 x̂ − (tk+1 )).
12 / 26
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
El filtro de Kalman II
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 ), Pk+1
+
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
#
(tk+1 ) − Kk+1 (z(tk+1 ) − Hk+1 x̂
(tk+1 ) − Kk+1 (z(tk+1 ) − Hk+1 x̂
−
−
«
(tk+1 )
«
(tk+1 ))
T
#
Sustituyendo ahora z(tk+1 ) = Hk+1 x(tk+1 ) + ν(tk+1 ):
+
Pk+1
=
E
h“
”
−
−
x(tk+1 ) − x̂ (tk+1 ) − Kk+1 (Hk+1 x(tk+1 ) + ν(tk+1 ) − Hk+1 x̂ (tk+1 )
“
”T –
−
−
× x(tk+1 ) − x̂ (tk+1 ) − Kk+1 (Hk+1 x(tk+1 ) + ν(tk+1 ) − Hk+1 x̂ (tk+1 ))
13 / 26
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
El filtro de Kalman III
Simplificando, obtenemos:
+
Pk+1
=
E
h“
(I − Kk+1 Hk+1 )(x(tk+1 ) − x̂
−
) − Kk+1 ν(tk+1 )
”
“
”T –
−
× (I − Kk+1 Hk+1 )(x(tk+1 ) − x̂ ) − Kk+1 ν(tk+1 )
=
−
(I − Kk+1 Hk+1 )Pk+1 (I − Kk+1 Hk+1 )
T
T
+ Kk+1 Rk+1 Kk+1
Es necesario encontrar el valor de Kk+1 que minimiza la
anterior expresión. Usando cálculo matricial, se encuentra que
−1
−
−
T
T
Kk+1 = Pk+1 Hk+1 Hk+1 Pk+1 Hk+1 + Rk+1
Sustituyendo ésta expresión se llega a que:
+
−
Pk+1
= (I − Kk+1 Hk+1 )Pk+1
. Ésta es la covarianza mı́nima.
14 / 26
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
Algoritmo del filtro de Kalman
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
x̂ − (tk+1 )
−
Pk+1
= Ak x̂ + (tk ),
T
= Ak Pk+ AT
+
B
Q
B
k
k
k
k .
Preparándonos para la medida, calculamos la ganacia 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
5
x̂ + (tk+1 )
=
x̂ − (tk+1 ) + Kk+1 (z(tk+1 ) − Hk+1 x̂ − (tk+1 )),
+
Pk+1
=
−
(I − Kk+1 Hk+1 )Pk+1
.
Iteramos para los siguientes valores de k.
15 / 26
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
Sobre las medidas
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 ).
16 / 26
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
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.
17 / 26
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
El caso INS-GPS
Esquema de la integración INS-GPS (loose):
18 / 26
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
Ejemplo 1-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
»
–
x
v
dt
»
0
0
=
–»
1
0
x
v
–
δx
δv
–
»
0
a
–
»
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
x(t)
≈
:
dt
∆t
»
δx(tk+1 )
δv (tk+1 )
–
»
=
1
0
∆t
1
–»
δx(tk )
δv (tk )
–
»
+
0
∆t
–
δa(tk )
19 / 26
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
Ejemplo 1-D del filtro de Kalman II
Por otro lado el modelo de medida será: z = x + ν, luego el
modelo de error será: δz = δx + ν.
Escribiéndolo todo:
»
δx(tk+1 )
δv (tk+1 )
–
»
=
δz(tk+1 )
=
1
0
∆t
1
–»
δx(tk )
δv (tk )
–
»
+
0
∆t
–
δa(tk+1 )
δx(tk+1 ) + ν(tk+1 )
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 =
1
0
0,01
1
–
»
, Bk =
0
0,01
–
 ˆ
, Qk = 0,1, Rk = 0,01, Hk =
1
˜
0
, tk = n
0, tk =
6 n.
donde n es cualquier entero (para modelar que se toman
medidas cada segundo, pero no en fracciones de segundo).
20 / 26
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
Ejemplo 1-D del filtro de Kalman III
Por tanto las ecuaciones del filtro de Kalman dirán, para cada
instante de tiempo tk+1 :
»
δx̂ − (tk+1 )
δv̂ − (tk+1 )
–
»
1
0
»
1
0
=
−
Pk+1
=
δx̂ + (tk )
δv̂ + (tk )
–
»
0,01
1
+
Pk
1
0,01
0,01
1
–»
–
0
1
–
»
+ 0,1
–
0
0,01
ˆ
0
0,01
˜
Si tk+1 = n, es decir, tiene un valor entero, significa que ha
habido medida.
Entonces,» calcular
la ganancia de Kalman:
»
–„
–
«
Kk+1 =
1
0
−
Pk+1
ˆ
0
−1
0
1
˜ −
1
Pk+1
+ 0,01
.
Tomamos la medida y calculamos la estimación a posteriori:
»
δx̂ + (tk+1 )
δv̂ + (tk+1 )
–
δx̂ − (tk+1 )
δv̂ − (tk+1 )
ˆ
1
(I − Kk+1
»
=
+
Pk+1
=
–
+ Kk+1 (δz(tk+1 ) −
0
˜
ˆ
1
0
˜
»
δx̂ − (tk+1 )
δv̂ − (tk+1 )
–
,
−
)Pk+1 .
donde δz(tk+1 ) = z(tk+1 ) − Hk+1 (x̂(tk+1 ) + δx̂ − (tk+1 )).
Si no hubo medida, entonces simplemente:
»
δx̂ + (tk+1 )
δv̂ + (tk+1 )
–
»
=
δx̂ − (tk+1 )
δv̂ − (tk+1 )
–
+
−
, Pk+1 = Pk+1 .
Actualizamos x̂(tk+1 ) = x̂(tk+1 ) + δx̂ + (tk+1 ). Iteramos para
los siguientes valores de k.
21 / 26
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
Ejemplo 1-D del filtro de Kalman: simulación I
Simulación de la posición (exacta) y medidas:
350
posicion
medidas
300
250
200
150
100
50
0
0
20
40
60
t
80
100
120
22 / 26
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
Ejemplo 1-D del filtro de Kalman: simulación II
('!
*
(!!
"'!
"!!
&'!
&!!
Usando las medidas para estimar la posición, el resultado es
bueno porque el sensor es preciso y el movimiento en x es
lento.
'!
!
!'!*
!
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:
,-./0/-1
234/45.
"!
#!
$!
)
%!
&!!
%
&"!
*
637-0/454
6*3.)/2545*43*234/45.
+
$
'
#
(
"
&
!
!&
!"*
!
"!
#!
$!
)
%!
&!!
&"!
23 / 26
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
Ejemplo 1-D del filtro de Kalman: simulación III
Comportamiento de la estimación y del error sin filtro de
Kalman:
400
posicion
estimacion de posicion
300
200
100
0
0
20
40
60
t
80
100
120
60
t
80
100
120
8
velocidad
estimacion de velocidad
6
4
2
0
−2
0
20
40
24 / 26
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
Ejemplo 1-D del filtro de Kalman: simulación IV
Comportamiento de la estimación y del error con filtro de
Kalman:
400
300
200
100
0
posicion
estimacion de posicion (KF)
0
20
40
60
t
80
100
120
6
4
2
0
−2
velocidad
estimacion de velocidad (KF)
0
20
40
60
t
80
100
120
25 / 26
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
Ejemplo 1-D del filtro de Kalman: simulación V
Comparación de errores con y sin filtro de Kalman:
40
error de posicion (sin KF)
error de posicion (con KF)
30
20
10
0
0
20
40
60
t
80
100
120
60
t
80
100
120
1.5
error de velocidad (sin KF)
error de velocidad (con KF)
1
0.5
0
0
20
40
26 / 26