T6NA

Sistemas de navegacion integrados
Filtrado optimo de sistemas lineales: el ltro de Kalman.

Navegacion Aerea
Tema 6: Sistemas de navegacion integrados. El ltro de
Kalman.
Fusi on de sensores. Ejemplo: el canal vertical.
INS-GPS
Fusi on de sensores.
Una aeronave actual dispone de una gran diversidad de
sensores y sistemas de navegacion, que pueden obtener total o
parcialmente las variables de navegacion PVAT.
Por ejemplo hemos visto el INS, que a partir de las medidas
de la IMU, el modelo de Tierra y gravedad, y una estimacion
inicial, nos da posicion, velocidad y actitud en todo momento.
Tambien hemos visto el GPS, que igualmente es capaz de
darnos todos estos datos, o al menos (si no disponemos de
m ultiples antenas), la posicion y la velocidad.
Puede haber otros sistemas (DME-DME, etc...)
Cada sistema dara una estimacion diferente, sujeta a error.
La idea de fusion de sensores y de los sistemas de navegacion
integrados, consiste en obtener una unica estimacion PVAT a
partir de todas las anteriores, tal que el error sea el menor
posible.
2 / 26
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 barometricas, h
B
. Se denomina
estimador baro-inercial de la altitud.
Recordemos que las ecuaciones del canal vertical venan dadas
por:
h =
V
D
,
V
D
=
z
+

e
(R
e
+

h)
2
,
donde
z
es la componente z de (
n
n/e
+ 2
n
e/i
)
v
n
+ a
n
NG
.
3 / 26
INS-GPS
Estimador baro-inercial de la altitud I
Se modica el canal vertical del INS de la siguiente forma,
usando h
B
:
h =
V
D
C
1
(
h h
B
),
V
D
=
z
+

e
(R
e
+

h)
2
+ C
2
(
h h
B
) + C
3
t
0
(
h() h
B
())d,
donde C
1
, C
2
y C
3
son ganancias a determinar.
Calculando como en el tema 4 el error de altitud y
despreciando el error en el termino
z
, obtenemos:
h = V
D
+ C
1
(
h h
B
),

V
D

2g
0
R
e
h C
2
(
h h
B
) C
3
t
0
(
h() h
B
())d,
y observese que

h h
B
=

h h + h h
B
= (h h
B
),
donde h
B
es el error de estimacion barometrico, que
suponemos aproximadamente constante.
4 / 26
INS-GPS
Estimador baro-inercial de la altitud II
Por tanto:
h = V
D
C
1
(h h
B
),

V
D

2g
0
R
e
h + C
2
(h h
B
) + C
3
t
0
(h h
B
)d,
y tomando derivada en la primera ecuacion y sustituyendo la
segunda, obtenemos:
h =
2g
0
R
e
h C
2
(h h
B
) C
3
t
0
(h h
B
)d C
1
h.
Tomando otra derivada y reescribiendo la ecuacion:
...
h
+ C
1
h + (C
2
2g
0
R
e
)
h + C
3
h = C
3
h
B
.
Los autovalores de esta ecuacion vienen dados por las races
del polinomio s
3
+ C
1
s
2
+ (C
2
2g
0
R
e
)s + C
3
.
5 / 26
INS-GPS
Estimador baro-inercial de la altitud III
Tpicamente se eligen lo valores de C
1
, C
2
y C
3
para que los
autovalores tengan parte real negativa (es decir, la ecuacion
de h sea estable). Una eleccion clasica es jar un autovalor al
valor y los otros dos a los valores + j y j .
El polinomio caracterstico sera entonces:
(s + )(s + j )(s + + j )
= (s + )(s
2
+ 2s + 2
2
)
= s
3
+ 3s
2
+ 4
2
s + 2
3
Sustituyendo en el polinomio en funcion de los coecientes
estos valores, se llega a: C
1
= 3, C
2
= 4
2
+
2g
R
e
, C
3
= 2
3
.
Un valor tpico elegido de es = 0,01.
6 / 26
INS-GPS
El caso INS-GPS
El sistema de navegacion INS y el GPS son particularmente
complementarios.
El INS:
Da una estimacion continua en el tiempo.
Su error crece con el tiempo.
Posee un elevado ancho de banda (KHz).
El GPS:
Proporciona una medida de alta precision pero discreta en el
tiempo.
El error esta acotado.
Posee un bajo ancho de banda (Hz).
Una primera solucion sera 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 un modo, combinar el INS y
el GPS para minimizar el error nal.
7 / 26
INS-GPS
Tight Integration y Loose Integration
Existen dos formas de llevar a cabo la integracion:
Loose Integration:
Este tipo de integracion permite tomar dos sistemas separados,

un INS y un GPS, y a partir de las salidas de ambos, obtener
una estimaci on com un.
Es la forma mas simple de integrar GPS e INS.
No requiere modicar las estimaciones internas de ambos
sistemas.
Tight Integration:
Este tipo de integracion emplea las se nales de entrada al INS y

GPS, es decir, las medidas de giroscopos y acelerometros y los
observables GPS, y los integra directamente.
Es mas complejo de desarrollar.
No se emplean los algoritmos que hemos visto de GPS e INS,
sino un unico algoritmo que integra los dos sistemas a la vez.
Se obtienen estimaciones mas precisas que en la tipo loose.
En ambos casos, la herramienta clave para desarrollar la
integracion es el Filtro de Kalman y sus extensiones (Filtro
Extendido de Kalman).
8 / 26
Deduccion del ltro de Kalman. Ecuaciones.
Ejemplo de un ltro de Kalman
El ltro de Kalman
El ltro de Kalman (KF) fue desarrollado por Rudolph E.
Kalman, un ingeniero h ungaro nacionalizado estadounidense.
Presento su ltro a la NASA en 1960; la NASA buscaba un
algoritmo de fusion de sensores para el programa espacial
Apollo.
Finalmente una version del KF fue utilizada en las misiones
Apollo para integrar las diferentes medidas de los sensores del
vehculo espacial.
A da de hoy, el KF se emplea no solo en navegacion sino en
multitud de sistemas en los que se desea reconstruir una se nal
que evoluciona en el tiempo, a partir de medidas con ruido,
por ejemplo en telefonos moviles.
Realmente el KF solo 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 Navegacion se emplean este tipo de ltros.
Nos limitaremos a entender el KF lineal y sus fundamentos.
9 / 26
Procesos dinamicos discretos con medidas
PROCESO: Consideremos el siguiente modelo discreto de un
proceso: x(t
k+1
) = A
k
x(t
k
) + B
k
(t
k
), donde x es un proceso
gaussiano con dimension n
x
, A
k
es una matriz (que puede
cambiar en cada instante de tiempo t
k
) de dimension n
x
n
x
,
(t
k
) es ruido blanco gaussiano de dimension n
y varianza Q
k
(el ruido del proceso), y B
k
es una matriz (que puede cambiar
en cada instante de tiempo t
k
) de dimension n
x
n
.
MEDIDA: En cada instante tambien consideramos que se
realiza una medida, representada por z, y denida de la
siguiente forma: z(t
k+1
) = H
k+1
x(t
k+1
) + (t
k+1
), donde z
es la medida, de dimension n
z
, H
k
es una matriz (que puede
cambiar en cada instante de tiempo t
k
) de dimension n
z
n
x
,
y (t
k
) es ruido blanco gaussiano de dimension n
y varianza
R
k
(el ruido de medida).
Ademas suponemos que (t
k
) y (t
k
) son independientes, y
que sabemos que la condicion inicial de x es
x(t
0
) N
n
x
( x
0
, P
0
).
10 / 26
Ecuaciones del proceso y la medida
Resumiendo las ecuaciones:
x(t
k+1
) = A
k
x(t
k
) + B
k
(t
k
),
z(t
k+1
) = H
k+1
x(t
k+1
) + (t
k+1
),
E[(t
k
)] = E[(t
k
)] = 0,
E[(t
k
)
T
(t
j
)] =
kj
Q
k
,
E[(t
k
)
T
(t
j
)] =
kj
R
k
,
E[(t
k
)
T
(t
j
)] = 0,
x(t
0
) N
n
x
( x
0
, P
0
).
Denimos la estimacion en t
k
de x(t
k
) como x(t
k
).
Denimos la covarianza del error de estimacion como
P(t
k
) = E[(x(t
k
) x(t
k
))(x(t
k
) x(t
k
))
T
].
El objetivo del ltro de Kalman es, empleando el
conocimiento de las ecuaciones arriba formuladas, y a partir
de las medidas z(t
k
), obtener la mejor estimacion posible, es
decir, el valor de x(t
k
) que minimiza P(t
k
).
11 / 26
El ltro de Kalman I
Si solo tuvieramos el proceso, podemos calcular su media y
tomamos x como dicha media; por tanto,
x(t
k
) N
n
x
( x(t
k
), P
k
), donde:
x(t
k+1
) = A
k
x(t
k
),
P
k+1
= A
k
P
k
A
T
k
+ B
k
Q
k
B
T
k
.
La idea de Kalman es decir: la estimacion arriba escrita es
valida antes de tomar la medida z(t
k+1
). Denotamos dicha
estimacion a priori como x
(t
k+1
) y su covarianza como
P
k+1
.
Ahora, si la estimacion fuera perfecta y la medida no tuviera
error, se tendra que z(t
k+1
) = H
k+1
x
(t
k+1
). Como no es
as, se actualiza la estimacion (a posteriori) de forma
proporcional a la discrepancia:
x
+
(t
k+1
) = x
(t
k+1
) + K
k+1
(z(t
k+1
) H
k+1
x
(t
k+1
)).
12 / 26
El ltro de Kalman II
En la ecuacion
x
+
(t
k+1
) = x
(t
k+1
) + K
k+1
(z(t
k+1
) H
k+1
x
(t
k+1
)) lo
unico que no conocemos es K
k+1
, que es la ganancia de
Kalman.

Esta se determina para garantizar que la covarianza
de x
+
(t
k+1
), P
+
k+1
, sea la menor posible.
Calculemos P
+
k+1
:
P
+
k+1
= E[(x(t
k+1
) x
+
(t
k+1
))(x(t
k+1
) x
+
(t
k+1
))
T
], y
sustituyendo la ecuacion de x
+
(t
k+1
):
P
+
k+1
= E
"
x(t
k+1
) x
+
(t
k+1
)

x(t
k+1
) x
+
(t
k+1
)
T
#
= E
x(t
k+1
) x
(t
k+1
) K
k+1
(z(t
k+1
) H
k+1
x
(t
k+1
)
x(t
k+1
) x
(t
k+1
) K
k+1
(z(t
k+1
) H
k+1
x
(t
k+1
))
T
#
Sustituyendo ahora z(t
k+1
) = H
k+1
x(t
k+1
) + (t
k+1
):
P
+
k+1
= E
h
x(t
k+1
) x
(t
k+1
) K
k+1
(H
k+1
x(t
k+1
) + (t
k+1
) H
k+1
x
(t
k+1
)
x(t
k+1
) x
(t
k+1
) K
k+1
(H
k+1
x(t
k+1
) + (t
k+1
) H
k+1
x
(t
k+1
))
13 / 26
El ltro de Kalman III
Simplicando, obtenemos:
P
+
k+1
= E
h
(I K
k+1
H
k+1
)(x(t
k+1
) x
) K
k+1
(t
k+1
)
(I K
k+1
H
k+1
)(x(t
k+1
) x
) K
k+1
(t
k+1
)
= (I K
k+1
H
k+1
)P
k+1
(I K
k+1
H
k+1
)
T
+ K
k+1
R
k+1
K
T
k+1
Es necesario encontrar el valor de K
k+1
que minimiza la
anterior expresion. Usando calculo matricial, se encuentra que
K
k+1
= P
k+1
H
T
k+1
H
k+1
P
k+1
H
T
k+1
+ R
k+1
1
Sustituyendo esta expresion se llega a que:
P
+
k+1
= (I K
k+1
H
k+1
)P
k+1
.

Esta es la covarianza mnima.
14 / 26
Algoritmo del ltro de Kalman
El algoritmo queda como sigue:
1 En el instante de tiempo t
k+1
, suponemos que tenemos la
anterior estimacion que incluyo tambien la ultima medida:
x
+
(t
k
) y su covarianza P
+
t
k
. Para k = 0 tomamos
x
+
(t
0
) = x
0
y P
+
0
= P
0
.
2 Fase de propagacion; usamos la ecuacion del sistema dinamico
para calcular la estimacion a priori:
x
(t
k+1
) = A
k
x
+
(t
k
),
P
k+1
= A
k
P
+
k
A
T
k
+ B
k
Q
k
B
T
k
.
3 Preparandonos para la medida, calculamos la ganacia de
Kalman: K
k+1
= P
k+1
H
T
k+1
H
k+1
P
k+1
H
T
k+1
+ R
k+1
1
.
4 Tomamos la medida y calculamos la estimacion a posteriori:
x
+
(t
k+1
) = x
(t
k+1
) + K
k+1
(z(t
k+1
) H
k+1
x
(t
k+1
)),
P
+
k+1
= (I K
k+1
H
k+1
)P
k+1
.
5 Iteramos para los siguientes valores de k.
15 / 26
Sobre las medidas
Observacion: es posible que no se realice una medida cada t
k
,
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 H
k
= 0, luego K
k
= 0 en
los instantes t
k
en los que no se realizan medidas. Por tanto
no es necesario realizar ninguna actualizacion y
x
+
(t
k
) = x
(t
k
), P
+
(t
k
) = P
(t
k
).
16 / 26
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 solucion al error de navegacion.
Recordemos que derivamos para el INS una ecuacion de la
forma: x(t
k+1
) = A
k
x(t
k
) +B
k
(t
k
), donde el vector x(t
k
)
contiene los errores de posicion, velocidad y actitud en t
k
y
(t
k
) son las fuentes de error.
Por otro lado en el tema del GPS obtuvimos ecuaciones de la
forma: (t
k+1
) = H
k+1
x(t
k+1
) +(t
k+1
), donde x(t
k+1
)
eran errores de posicion (y velocidad, si tambien estimamos
velocidad) respecto a una estimacion inicial y (t
k+1
) las
diferencias entre los observables medidos y los estimados.
Por tanto usando la medida del INS como estimacion para el
GPS, ya tenemos los errores linealizados escritos de una forma
adecuada para implementar el ltro de Kalman!
El error estimado se suma a la posicion estimada por el INS,
para conseguir la mejor estimacion nal posible.
17 / 26
El caso INS-GPS
Esquema de la integracion INS-GPS (loose):
18 / 26
Ejemplo 1-D del ltro de Kalman I
Para entender mejor el ltro de Kalman consideremos un
sistema sencillo. Imaginemos un vehculo que solo se puede
mover en una direccion, con un acelerometro de un ancho de
banda de 100Hz que mide la aceleracion en dicha direccion, y
con un sensor con un ancho de banda de 1Hz que estima la
posicion en dicha direccion.
El modelo del sistema sera: x = a. Llamando v a la velocidad:
d
dt
x
v
0 1
0 0

x
v
0
a
El modelo del error sera:

d
dt
x
v
0 1
0 0

x
v
0
1
a
Pasando a tiempo discreto y teniendo en cuenta que
d
dt
x(t)
x(t
k+1
)x(t
k
)
t
:
x(t
k+1
)
v(t
k+1
)
1 t
0 1

x(t
k
)
v(t
k
)
0
t
a(t
k
)
19 / 26
Ejemplo 1-D del ltro de Kalman II
Por otro lado el modelo de medida sera: z = x + , luego el
modelo de error sera: z = x + .
Escribiendolo todo:
x(t
k+1
)
v(t
k+1
)
1 t
0 1

x(t
k
)
v(t
k
)
0
t
a(t
k+1
)
z(t
k+1
) = x(t
k+1
) + (t
k+1
)
Ademas las medidas solo se hacen con una frecuencia de 1Hz
(cada segundo), mientras que la frecuencia del acelerometro
es 100 Hz con lo que deberamos tomar t = 0,01.
Supongamos ademas que la precision de los instrumentos es:
2
a
= 0,1,
2
= 0,01, y que se verican las hipotesis del KF

(ruidos blancos gaussianos, independientes, etc...).
En la nomenclatura que hemos usado para el KF, tendremos:
A
k
=
1 0,01
0 1
, B
k
=
0
0,01
, Q
k
= 0,1, R
k
= 0,01, H
k
=

1 0
, t
k
= n
0, t
k
= n.
donde n es cualquier entero (para modelar que se toman
medidas cada segundo, pero no en fracciones de segundo).
20 / 26
Ejemplo 1-D del ltro de Kalman III
Por tanto las ecuaciones del ltro de Kalman diran, para cada
instante de tiempo t
k+1
:
(t
k+1
)
v
(t
k+1
)
1 0,01
0 1

x
+
(t
k
)
v
+
(t
k
)
k+1
=
1 0,01
0 1
P
+
k
1 0
0,01 1
+ 0,1
0
0,01
0 0,01
Si t
k+1
= n, es decir, tiene un valor entero, signica que ha
habido medida. Entonces, calcular la ganancia de Kalman:
K
k+1
= P
k+1
1
0

0 1
k+1
0
1
+ 0,01
1
.
Tomamos la medida y calculamos la estimacion a posteriori:
x
+
(t
k+1
)
v
+
(t
k+1
)
(t
k+1
)
v
(t
k+1
)
+ K
k+1
(z(t
k+1
)
1 0
(t
k+1
)
v
(t
k+1
)
,
P
+
k+1
= (I K
k+1
1 0
)P
k+1
.
donde z(t
k+1
) = z(t
k+1
) H
k+1
( x(t
k+1
) + x
(t
k+1
)).
Si no hubo medida, entonces simplemente:
x
+
(t
k+1
)
v
+
(t
k+1
)
(t
k+1
)
v
(t
k+1
)
, P
+
k+1
= P
k+1
.
Actualizamos x(t
k+1
) = x(t
k+1
) + x
+
(t
k+1
). Iteramos para
los siguientes valores de k.
21 / 26
Ejemplo 1-D del ltro de Kalman: simulacion I
Simulacion de la posicion (exacta) y medidas:
0 20 40 60 80 100 120
0
50
100
150
200
250
300
350
t

posicion
medidas
22 / 26
Ejemplo 1-D del ltro de Kalman: simulacion II
Usando las medidas para estimar la posicion, el resultado es
bueno porque el sensor es preciso y el movimiento en x es
lento.
Si intentamos estimar la velocidad con la formula
v(t
k
) =
x(t
k
)x(t
k1
)
t
se obtiene una estimacion muy mala:
0 20 40 60 80 100 120
!50
0
50
100
150
200
250
300
350
t

0 20 40 60 80 100 120
!2
!1
0
1
2
3
4
5
6
7
8
t

posicion
medidas
velocidad
v estimada de medidas
23 / 26
Ejemplo 1-D del ltro de Kalman: simulacion III
Comportamiento de la estimacion y del error sin ltro de
Kalman:
0 20 40 60 80 100 120
0
100
200
300
400
t

0 20 40 60 80 100 120
2
0
2
4
6
8
t

posicion
estimacion de posicion
velocidad
estimacion de velocidad
24 / 26
Ejemplo 1-D del ltro de Kalman: simulacion IV
Comportamiento de la estimacion y del error con ltro de
Kalman:
0 20 40 60 80 100 120
0
100
200
300
400
t

0 20 40 60 80 100 120
2
0
2
4
6
t

posicion
estimacion de posicion (KF)
velocidad
estimacion de velocidad (KF)
25 / 26
Ejemplo 1-D del ltro de Kalman: simulacion V
Comparacion de errores con y sin ltro de Kalman:
0 20 40 60 80 100 120
0
10
20
30
40
t

0 20 40 60 80 100 120
0
0.5
1
1.5
t

error de posicion (sin KF)
error de posicion (con KF)
error de velocidad (sin KF)
error de velocidad (con KF)
26 / 26

T6NA

Cargado por

Información del documento

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

T6NA

Cargado por

Copyright:

Formatos disponibles

Sistemas de navegacion integrados

Filtrado optimo de sistemas lineales: el ltro de Kalman.

Este tipo de integracion permite tomar dos sistemas separados,

Este tipo de integracion emplea las se nales de entrada al INS y

El modelo del error sera:

= 0,01, y que se verican las hipotesis del KF

También podría gustarte