Navegación Aérea - Tema 6: Sistemas de navegación ...aero.us.es/na/files1112/T6NA.pdf ·...

Sistemas de navegacion integradosFiltrado optimo de sistemas lineales: el filtro de Kalman.

Navegacion AereaTema 6: Sistemas de navegacion integrados. El filtro de

Kalman.


Fusion de sensores. Ejemplo: el canal vertical.INS-GPS

Fusion de sensores.

Una aeronave actual dispone de una gran diversidad desensores y sistemas de navegacion, que pueden obtener total oparcialmente las variables de navegacion PVAT.

Por ejemplo hemos visto el INS, que a partir de las medidasde la IMU, el modelo de Tierra y gravedad, y una estimacioninicial, nos da posicion, velocidad y actitud en todo momento.

Tambien hemos visto el GPS, que igualmente es capaz dedarnos todos estos datos, o al menos (si no disponemos demultiples 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 navegacionintegrados, consiste en obtener una unica estimacion PVAT apartir de todas las anteriores, tal que el error sea el menorposible.

2 / 26



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 altitudobtenida de medidas barometricas, hB . Se denomina“estimador baro-inercial de la altitud”.

Recordemos que las ecuaciones del canal vertical venıan dadaspor:

˙h = −VD ,˙VD = ρz +

µe

(Re + h)2,

donde ρz es la componente z de −(ωnn/e + 2ωn

e/i )×vn + an

NG .

3 / 26



Estimador baro-inercial de la altitud I

Se modifica el canal vertical del INS de la siguiente forma,usando hB :

˙h = −VD − C1(h − hB),

˙VD = ρz +µe

(Re + h)2+ C2(h − hB) + C3

∫ t

0(h(τ)− hB(τ))dτ,

donde C1, C2 y C3 son ganancias a determinar.

Calculando como en el tema 4 el error de altitud ydespreciando el error en el termino ρz , obtenemos:

δh = −δVD + C1(h − hB),

δVD ≈ −2g0

Reδh − C2(h − hB)− C3

∫ t

0(h(τ)− hB(τ))dτ,

y observese que h − hB = h − h + h − hB = −(δh − δhB),donde δhB es el error de estimacion barometrico, quesuponemos aproximadamente constante.

4 / 26



Estimador baro-inercial de la altitud II

Por tanto:

δh = −δVD − C1(δh − δhB),

δVD ≈ −2g0

Reδh + C2(δh − δhB) + C3

∫ t

0(δh − δhB)dτ,

y tomando derivada en la primera ecuacion y sustituyendo lasegunda, obtenemos:

δh =2g0

Reδh − C2(δh − δhB)− C3

∫ t

0(δh − δhB)dτ − C1δh.

Tomando otra derivada y reescribiendo la ecuacion:

δ...h + C1δh + (C2 −

2g0

Re)δh + C3δh = C3δhB .

Los autovalores de esta ecuacion vienen dados por las raıcesdel polinomio s3 + C1s

2 + (C2 − 2g0Re

)s + C3.5 / 26



Estimador baro-inercial de la altitud III

Tıpicamente se eligen lo valores de C1, C2 y C3 para que losautovalores tengan parte real negativa (es decir, la ecuacionde δh sea estable). Una eleccion clasica es fijar un autovalor alvalor −λ y los otros dos a los valores −λ+ jλ y −λ− jλ.

El polinomio caracterıstico serıa entonces:

(s + λ)(s + λ− jλ)(s + λ+ jλ)

= (s + λ)(s2 + 2λs + 2λ2)

= s3 + 3λs2 + 4λ2s + 2λ3

Sustituyendo en el polinomio en funcion de los coeficientesestos valores, se llega a: C1 = 3λ, C2 = 4λ2 + 2g

Re, C3 = 2λ3.

Un valor tıpico elegido de λ es λ = 0,01.

6 / 26



El caso INS-GPS

El sistema de navegacion INS y el GPS son particularmentecomplementarios.

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 eltiempo.El error esta acotado.Posee un bajo ancho de banda (Hz).

Una primera solucion serıa resetear el INS cada vez que seobtenga una medida GPS. Pero la medida GPS tampoco esexacta.

Por tanto hay que intentar, de algun modo, combinar el INS yel GPS para minimizar el error final.

7 / 26



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, obteneruna estimacion comun.Es la forma mas simple de integrar GPS e INS.No requiere modificar las estimaciones internas de ambossistemas.

Tight Integration:Este tipo de integracion emplea las senales de entrada al INS yGPS, es decir, las medidas de giroscopos y acelerometros y losobservables 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 laintegracion es el Filtro de Kalman y sus extensiones (FiltroExtendido de Kalman). 8 / 26


Deduccion 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 hungaro nacionalizado estadounidense.Presento su filtro a la NASA en 1960; la NASA buscaba unalgoritmo de fusion de sensores para el programa espacialApollo.Finalmente una version del KF fue utilizada en las misionesApollo para integrar las diferentes medidas de los sensores delvehıculo espacial.A dıa de hoy, el KF se emplea no solo en navegacion sino enmultitud de sistemas en los que se desea reconstruir una senalque evoluciona en el tiempo, a partir de medidas con ruido,por ejemplo en telefonos moviles.Realmente el KF solo sirve para sistemas lineales. Puesto quemuchos sistemas reales son no lineales, se han desarrolladoextensiones no lineales, conocidas como Filtro Extendido deKalman (EKF); en Navegacion se emplean este tipo de filtros.Nos limitaremos a entender el KF lineal y sus fundamentos. 9 / 26



Procesos dinamicos discretos con medidas

PROCESO: Consideremos el siguiente modelo discreto de unproceso: x(tk+1) = Akx(tk) + Bkε(tk), donde x es un procesogaussiano con dimension nx , Ak es una matriz (que puedecambiar en cada instante de tiempo tk) de dimension nx × nx ,ε(tk) es ruido blanco gaussiano de dimension nε y varianza Qk

(el ruido del proceso), y Bk es una matriz (que puede cambiaren cada instante de tiempo tk) de dimension nx × nε.MEDIDA: En cada instante tambien consideramos que serealiza una medida, representada por z , y definida de lasiguiente forma: z(tk+1) = Hk+1x(tk+1) + ν(tk+1), donde zes la medida, de dimension nz , Hk es una matriz (que puedecambiar en cada instante de tiempo tk) de dimension nz × nx ,y ν(tk) es ruido blanco gaussiano de dimension nν y varianzaRk (el ruido de medida).Ademas suponemos que ν(tk) y ε(tk) son independientes, yque sabemos que la condicion inicial de x esx(t0) ∼ Nnx (x0,P0). 10 / 26



Ecuaciones del proceso y la medida

Resumiendo las ecuaciones:

x(tk+1) = Akx(tk) + Bkε(tk),

z(tk+1) = Hk+1x(tk+1) + ν(tk+1),

E [ε(tk)] = E [ν(tk)] = 0,

E [ε(tk)εT (tj)] = δkjQk ,

E [ν(tk)νT (tj)] = δkjRk ,

E [ε(tk)νT (tj)] = 0,

x(t0) ∼ Nnx (x0,P0).

Definimos la estimacion en tk de x(tk) como x(tk).Definimos la covarianza del error de estimacion comoP(tk) = E [(x(tk)− x(tk))(x(tk)− x(tk))T ].El objetivo del filtro de Kalman es, empleando elconocimiento de las ecuaciones arriba formuladas, y a partirde las medidas z(tk), obtener la mejor estimacion posible, esdecir, el valor de x(tk) que minimiza P(tk). 11 / 26



El filtro de Kalman I

Si solo tuvieramos el proceso, podemos calcular su media ytomamos x como dicha media; por tanto,x(tk) ∼ Nnx (x(tk),Pk), donde:

x(tk+1) = Ak x(tk),

Pk+1 = AkPkATk + BkQkBT

k .

La idea de Kalman es decir: la estimacion arriba escrita esvalida antes de tomar la medida z(tk+1). Denotamos dichaestimacion “a priori” como x−(tk+1) y su covarianza comoP−

k+1.

Ahora, si la estimacion fuera perfecta y la medida no tuvieraerror, se tendrıa que z(tk+1) = Hk+1x

−(tk+1). Como no esası, se actualiza la estimacion (“a posteriori”) de formaproporcional a la discrepancia:x+(tk+1) = x−(tk+1) + Kk+1(z(tk+1)− Hk+1x

−(tk+1)).12 / 26



El filtro de Kalman II

En la ecuacionx+(tk+1) = x−(tk+1) + Kk+1(z(tk+1)− Hk+1x

−(tk+1)) lounico que no conocemos es Kk+1, que es la ganancia deKalman. Esta se determina para garantizar que la covarianzade x+(tk+1), P+

k+1, sea la menor posible.

Calculemos P+k+1:

P+k+1 = E [(x(tk+1)− x+(tk+1))(x(tk+1)− x+(tk+1))T ], y

sustituyendo la ecuacion de x+(tk+1):

P+k+1 = E

"„x(tk+1) − x+(tk+1)

« „x(tk+1) − x+(tk+1)

«T#

= 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−(tk+1))

«T#

Sustituyendo ahora z(tk+1) = Hk+1x(tk+1) + ν(tk+1):

P+k+1 = E

h“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−(tk+1))

”T–

13 / 26



El filtro de Kalman III

Simplificando, obtenemos:

P+k+1 = E

h“(I − Kk+1Hk+1)(x(tk+1)− x−)− Kk+1ν(tk+1)

”×“

(I − Kk+1Hk+1)(x(tk+1)− x−)− Kk+1ν(tk+1)”T–

= (I − Kk+1Hk+1)P−k+1(I − Kk+1Hk+1)T + Kk+1Rk+1KTk+1

Es necesario encontrar el valor de Kk+1 que minimiza laanterior expresion. Usando calculo matricial, se encuentra que

Kk+1 = P−k+1H

Tk+1

(Hk+1P

−k+1H

Tk+1 + Rk+1

)−1

Sustituyendo esta expresion se llega a que:P+

k+1 = (I − Kk+1Hk+1)P−k+1. Esta es la covarianza mınima.

14 / 26



Algoritmo del filtro de Kalman

El algoritmo queda como sigue:1 En el instante de tiempo tk+1, suponemos que tenemos la

anterior estimacion que incluyo tambien la ultima medida:x+(tk) y su covarianza P+tk . Para k = 0 tomamosx+(t0) = x0 y P+

0 = P0.2 Fase de propagacion; usamos la ecuacion del sistema dinamico

para calcular la estimacion a priori:

x−(tk+1) = Ak x+(tk),

P−k+1 = AkP+k AT

k + BkQkBTk .

3 Preparandonos para la medida, calculamos la ganacia de

Kalman: Kk+1 = P−k+1HTk+1

(Hk+1P

−k+1H

Tk+1 + Rk+1

)−1.

4 Tomamos la medida y calculamos la estimacion a posteriori:

x+(tk+1) = x−(tk+1) + Kk+1(z(tk+1)− Hk+1x−(tk+1)),

P+k+1 = (I − Kk+1Hk+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 tk ,sino que en ciertos instantes se hagan medidas, y en otros nose haga ninguna medida.

Por ejemplo podemos tener un sensor con bajo ancho debanda (como el GPS) mientras que nuestro tiempo demuestreo ∆t representa una elevada frecuencia.

Una forma de solucionarlo es tomar Hk = 0, luego Kk = 0 enlos instantes tk en los que no se realizan medidas. Por tantono es necesario realizar ninguna actualizacion yx+(tk) = x−(tk), P+(tk) = P−(tk).

16 / 26



El caso INS-GPS

En el caso INS-GPS no podemos aplicar el Filtro de Kalmandirectamente 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 laforma: δx(tk+1) = Akδx(tk) + Bkε(tk), donde el vector δx(tk)contiene los errores de posicion, velocidad y actitud en tk yε(tk) son las fuentes de error.Por otro lado en el tema del GPS obtuvimos ecuaciones de laforma: ∆ρ(tk+1) = Hk+1∆x(tk+1) +ν(tk+1), donde ∆x(tk+1)eran errores de posicion (y velocidad, si tambien estimamosvelocidad) respecto a una estimacion inicial y ∆ρ(tk+1) lasdiferencias entre los observables medidos y los estimados.Por tanto usando la medida del INS como estimacion para elGPS, ya tenemos los errores linealizados escritos de una formaadecuada para implementar el filtro de Kalman!El error estimado se suma a la posicion estimada por el INS,para conseguir la mejor estimacion final posible. 17 / 26



El caso INS-GPS

Esquema de la integracion INS-GPS (loose):

18 / 26



Ejemplo 1-D del filtro de Kalman I

Para entender mejor el filtro de Kalman consideremos unsistema sencillo. Imaginemos un vehıculo que solo se puedemover en una direccion, con un acelerometro de un ancho debanda de 100Hz que mide la aceleracion en dicha direccion, ycon un sensor con un ancho de banda de 1Hz que estima laposicion en dicha direccion.El modelo del sistema sera:x = a. Llamando v a la velocidad:

d

dt

»xv

–=

»0 10 0

– »xv

–+

»0a

–

El modelo del error sera:d

dt

»δxδv

–=

»0 10 0

– »δxδv

–+

»01

–δa

Pasando a tiempo discreto y teniendo en cuenta queddt x(t) ≈ x(tk+1)−x(tk )

∆t :»δx(tk+1)δv(tk+1)

–=

»1 ∆t0 1

– »δx(tk )δv(tk )

–+

»0

∆t

–δa(tk )

19 / 26



Ejemplo 1-D del filtro de Kalman II

Por otro lado el modelo de medida sera: z = x + ν, luego elmodelo de error sera: δz = δx + ν.Escribiendolo todo:»

δx(tk+1)δv(tk+1)

–=

»1 ∆t0 1

– »δx(tk )δv(tk )

–+

»0

∆t

–δa(tk+1)

δz(tk+1) = δx(tk+1) + ν(tk+1)

Ademas las medidas solo se hacen con una frecuencia de 1Hz(cada segundo), mientras que la frecuencia del acelerometroes 100 Hz con lo que deberıamos tomar ∆t = 0,01.

Supongamos ademas que la precision de los instrumentos es:σ2δa = 0,1, σ2

ν = 0,01, y que se verifican las hipotesis del KF(ruidos blancos gaussianos, independientes, etc...).En la nomenclatura que hemos usado para el KF, tendremos:

Ak =

»1 0,010 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 tomanmedidas cada segundo, pero no en fracciones de segundo).

20 / 26



Ejemplo 1-D del filtro de Kalman IIIPor tanto las ecuaciones del filtro de Kalman diran, para cadainstante de tiempo tk+1:»

δx−(tk+1)

δv−(tk+1)

–=

»1 0,010 1

– »δx+(tk )

δv+(tk )

–P−k+1 =

»1 0,010 1

–P+

k

»1 0

0,01 1

–+ 0,1

»0

0,01

– ˆ0 0,01

˜Si tk+1 = n, es decir, tiene un valor entero, significa que hahabido medida. Entonces, calcular la ganancia de Kalman:Kk+1 = P−

k+1

»10

–„ˆ0 1

˜P−

k+1

»01

–+ 0,01

«−1.

Tomamos la medida y calculamos la estimacion a posteriori:»δx+(tk+1)

δv+(tk+1)

–=

»δx−(tk+1)

δv−(tk+1)

–+ Kk+1(δz(tk+1)−

ˆ1 0

˜ » δx−(tk+1)

δv−(tk+1)

–,

P+k+1 = (I − Kk+1

ˆ1 0

˜)P−k+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)

–, P+

k+1 = P−k+1.

Actualizamos x(tk+1) = x(tk+1) + δx+(tk+1). Iteramos paralos siguientes valores de k. 21 / 26



Ejemplo 1-D del filtro de Kalman: simulacion I

Simulacion de la posicion (exacta) y medidas:

0 20 40 60 80 100 1200

50

100

150

200

250

300

350

t

posicionmedidas

22 / 26



Ejemplo 1-D del filtro de Kalman: simulacion II

Usando las medidas para estimar la posicion, el resultado esbueno porque el sensor es preciso y el movimiento en x eslento.

Si intentamos estimar la velocidad con la formulav(tk) =

x(tk )−x(tk−1)∆t se obtiene una estimacion muy mala:! "! #! $! %! &!! &"!

!'!

!

'!

&!!

&'!

"!!

"'!

(!!

('!

)

*

*

! "! #! $! %! &!! &"!!"

!&

!

&

"

(

#

'

$

+

%

)

*

*

,-./0/-1

234/45.

637-0/454

6*3.)/2545*43*234/45.

23 / 26



Ejemplo 1-D del filtro de Kalman: simulacion III

Comportamiento de la estimacion y del error sin filtro deKalman:

0 20 40 60 80 100 1200

100

200

300

400

t

0 20 40 60 80 100 120−2

0

2

4

6

8

t

posicionestimacion de posicion

velocidadestimacion de velocidad

24 / 26



Ejemplo 1-D del filtro de Kalman: simulacion IV

Comportamiento de la estimacion y del error con filtro deKalman:

0 20 40 60 80 100 1200

100

200

300

400

t

0 20 40 60 80 100 120−2

0

2

4

6

t

posicionestimacion de posicion (KF)

velocidadestimacion de velocidad (KF)

25 / 26



Ejemplo 1-D del filtro de Kalman: simulacion V

Comparacion de errores con y sin filtro de Kalman:

0 20 40 60 80 100 1200

10

20

30

40

t

0 20 40 60 80 100 1200

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

Navegación Aérea - Tema 6: Sistemas de navegación ...aero.us.es/na/files1112/T6NA.pdf ·...

Documents

Transcript of Navegación Aérea - Tema 6: Sistemas de navegación ...aero.us.es/na/files1112/T6NA.pdf ·...