Tema 6: Sistemas de navegación integrados. El filtro de Kalman.

Navegación Aérea Tema 6: Sistemas de navegación integrados. 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

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, h B. 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 (R e + ĥ) 2, donde ˆρ z es la componente z de (ˆω n n/e + 2ˆωn e/i ) ˆv n + â n NG. 3 / 26

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 h B : ĥ = ˆV D C 1 (ĥ h B ), ˆV D = ˆρ z + µ e (R e + ĥ)2 + C 2(ĥ h B) + C 3 t 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 término ρ z, obtenemos: δḣ = δv D + C 1 (ĥ h B ), δ V D 2g R e δh C 2 (ĥ h B ) C 3 t (ĥ(τ) h B(τ))dτ, (ĥ(τ) h B (τ))dτ, y obsérvese que ĥ h B = ĥ h + h h B = (δh δh B ), donde δh B es el error de estimación barométrico, que suponemos aproximadamente constante. 4 / 26

Fusión de sensores. Ejemplo: el canal vertical. INS-GPS Estimador baro-inercial de la altitud II Por tanto: δḣ = δv D C 1 (δh δh B ), δ V D 2g R e δh + C 2 (δh δh B ) + C 3 t (δh δh B )dτ, y tomando derivada en la primera ecuación y sustituyendo la segunda, obtenemos: δḧ = 2g R e δh C 2 (δh δh B ) C 3 t (δh δh B )dτ C 1 δḣ. Tomando otra derivada y reescribiendo la ecuación: δ... h + C 1 δḧ + (C 2 2g R e )δḣ + C 3 δh = C 3 δh B. Los autovalores de esta ecuación vienen dados por las raíces del polinomio s 3 + C 1 s 2 + (C 2 2g R e )s + C 3. 5 / 26

Fusión de sensores. Ejemplo: el canal vertical. INS-GPS Estimador baro-inercial de la altitud III Típicamente se eligen lo valores de C 1, C 2 y C 3 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 estos valores, se llega a: C 1 = 3λ, C 2 = 4λ 2 + 2g R e, C 3 = 2λ 3. Un valor típico elegido de λ es λ =,1. 6 / 26

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

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

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 196; 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

Procesos dinámicos 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 dimensión n x, A k es una matriz (que puede cambiar en cada instante de tiempo t k ) de dimensión n x n x, ɛ(t k ) es ruido blanco gaussiano de dimensión 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 dimensión n x n ɛ. MEDIDA: En cada instante también consideramos que se realiza una medida, representada por z, y definida de la siguiente forma: z(t k+1 ) = H k+1 x(t k+1 ) + ν(t k+1 ), donde z es la medida, de dimensión n z, H k es una matriz (que puede cambiar en cada instante de tiempo t k ) de dimensión n z n x, y ν(t k ) es ruido blanco gaussiano de dimensión n ν y varianza R k (el ruido de medida). Además suponemos que ν(t k ) y ɛ(t k ) son independientes, y que sabemos que la condición inicial de x es x(t ) N nx (ˆx, P ). 1 / 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 )] =, E[ɛ(t k )ɛ T (t j )] = δ kj Q k, E[ν(t k )ν T (t j )] = δ kj R k, E[ɛ(t k )ν T (t j )] =, x(t ) N nx (ˆx, P ). Definimos la estimación en t k de x(t k ) como ˆx(t k ). Definimos la covarianza del error de estimación como P(t k ) = E[(x(t k ) ˆx(t k ))(x(t k ) ˆx(t k )) T ]. El objetivo del filtro de Kalman es, empleando el conocimiento de las ecuaciones arriba formuladas, y a partir de las medidas z(t k ), obtener la mejor estimación posible, es decir, el valor de ˆx(t k ) que minimiza P(t k ). 11 / 26

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(t k ) N nx (ˆ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 kq k B T k. La idea de Kalman es decir: la estimación arriba escrita es válida antes de tomar la medida z(t k+1 ). Denotamos dicha estimación a priori como ˆx (t k+1 ) y su covarianza como P k+1. Ahora, si la estimación fuera perfecta y la medida no tuviera error, se tendría que z(t k+1 ) = H k+1ˆx (t k+1 ). Como no es así, se actualiza la estimación ( 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 filtro de Kalman II En la ecuación ˆx + (t k+1 ) = ˆx (t k+1 ) + K k+1 (z(t k+1 ) H k+1ˆx (t k+1 )) lo único que no conocemos es K k+1, que es la ganancia de Kalman. Ésta 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 ecuación de ˆx + (t k+1 ): P + k+1 = E = E " x(t k+1 ) ˆx + (t k+1 )«x(t k+1 ) ˆx + «# T (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 ) x(t k+1 ) ˆx (t k+1 ) K k+1 (z(t k+1 ) H k+1 ˆx «# T (t k+1 )) Sustituyendo ahora z(t k+1 ) = H k+1 x(t k+1 ) + ν(t k+1 ): P + k+1 = h E 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 (t k+1 )) 13 / 26

El filtro de Kalman III Simplificando, obtenemos: P + k+1 = h E (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 T ) K k+1 ν(t k+1 ) = (I K k+1 H k+1 )P k+1 (I K k+1h 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 expresión. Usando cálculo matricial, se encuentra que K k+1 = P ( k+1 HT k+1 Hk+1 P k+1 HT k+1 + R ) 1 k+1 Sustituyendo ésta expresión se llega a que: P + k+1 = (I K k+1h k+1 )P k+1. Ésta es la covarianza mínima. 14 / 26

Algoritmo del filtro de Kalman El algoritmo queda como sigue: 1 En el instante de tiempo t k+1, suponemos que tenemos la anterior estimación que incluyó también la última medida: ˆx + (t k ) y su covarianza P + t k. Para k = tomamos ˆx + (t ) = ˆx y P + = P. 2 Fase de propagación; usamos la ecuación del sistema dinámico para calcular la estimación a priori: ˆx (t k+1 ) = A k ˆx + (t k ), P k+1 = A k P + k AT k + B k Q k B T k. 3 Preparándonos para la medida, calculamos la ganacia de Kalman: K k+1 = P k+1 HT k+1 ( Hk+1 P k+1 HT k+1 + R k+1) 1. 4 Tomamos la medida y calculamos la estimación 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 Observación: 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 =, luego K k = en los instantes t k en los que no se realizan medidas. Por tanto no es necesario realizar ninguna actualización 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 solución al error de navegación. Recordemos que derivamos para el INS una ecuación 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 posición, 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 posición (y velocidad, si también estimamos velocidad) respecto a una estimación inicial y ρ(t k+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

El caso INS-GPS Esquema de la integración INS-GPS (loose): 18 / 26

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 1Hz 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 El modelo del error será: d dt δx δv = = 1 1 x v δx δv + a + 1 δa Pasando a tiempo discreto y teniendo en cuenta que d dt x(t) x(t k+1) x(t k ) t : δx(tk+1 ) δv(t k+1 ) = 1 t 1 δx(tk ) δv(t k ) + t δa(t k ) 19 / 26

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(t k+1 ) = 1 t 1 δx(tk ) δv(t k ) δz(t k+1 ) = δx(t k+1 ) + ν(t k+1 ) + t δa(t k+1 ) Además las medidas sólo se hacen con una frecuencia de 1Hz (cada segundo), mientras que la frecuencia del acelerómetro es 1 Hz con lo que deberíamos tomar t =,1. Supongamos además que la precisión de los instrumentos es: σδa 2 =,1, σ2 ν =,1, y que se verifican las hipótesis del KF (ruidos blancos gaussianos, independientes, etc...). En la nomenclatura que hemos usado para el KF, tendremos: A k = 1,1 1, B k =,1, Q k =,1, R k =,1, H k = j ˆ 1, tk = n, t k n. donde n es cualquier entero (para modelar que se toman medidas cada segundo, pero no en fracciones de segundo). 2 / 26

Ejemplo 1-D del filtro de Kalman III Por tanto las ecuaciones del filtro de Kalman dirán, para cada instante de tiempo t k+1 : δˆx (tk+1 ) δˆv (t k+1 ) P k+1 = = 1,1 1 1,1 1 δˆx + (tk ) δˆv + (t k ) P + 1 k,1 1 +,1,1 ˆ,1 Si t k+1 = n, es decir, tiene un valor entero, significa que ha habido medida. Entonces, calcular la ganancia de Kalman: K k+1 = P k+1 1 ˆ 1 P k+1 1 1 +,1«. Tomamos la medida y calculamos la estimación a posteriori: δˆx + (tk+1 ) δˆv + (t k+1 ) = δˆx (tk+1 ) δˆv (t k+1 ) P + k+1 = (I K k+1 ˆ 1 )P k+1. + K k+1 (δz(t k+1 ) ˆ 1 δˆx (t k+1 ) δˆv (t 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 + (tk+1 ) δˆv + (t k+1 ) = δˆx (tk+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 filtro de Kalman: simulación I Simulación de la posición (exacta) y medidas: 35 posicion medidas 3 25 2 15 1 5 2 4 6 8 1 12 t 22 / 26

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 v(t k ) = x(t k) x(t k 1 ) se obtiene ) una estimación muy mala:!'!*! "! #! $! %! &!! &"! t,-.//-1 234/45. % + $ 637-/454 6*3.)/2545*43*234/45. * ' # ( " &!!&!"*! "! #! $! %! &!! &"! ) 23 / 26

Ejemplo 1-D del filtro de Kalman: simulación III Comportamiento de la estimación y del error sin filtro de Kalman: 4 3 posicion estimacion de posicion 2 1 2 4 6 8 1 12 t 8 6 velocidad estimacion de velocidad 4 2 2 2 4 6 8 1 12 t 24 / 26

Ejemplo 1-D del filtro de Kalman: simulación IV Comportamiento de la estimación y del error con filtro de Kalman: 4 3 2 1 posicion estimacion de posicion (KF) 2 4 6 8 1 12 t 6 4 2 velocidad estimacion de velocidad (KF) 2 2 4 6 8 1 12 t 25 / 26

Ejemplo 1-D del filtro de Kalman: simulación V Comparación de errores con y sin filtro de Kalman: 4 3 error de posicion (sin KF) error de posicion (con KF) 2 1 2 4 6 8 1 12 t 1.5 1 error de velocidad (sin KF) error de velocidad (con KF).5 2 4 6 8 1 12 t 26 / 26