Proyecto Fin de CarreraIngeniería de Telecomunicación

Formato de Publicación de la Escuela TécnicaSuperior de Ingeniería

Autor: F. Javier Payán Somet

Tutor: Juan José Murillo Fuentes

Dep. Teoría de la Señal y ComunicacionesEscuela Técnica Superior de Ingeniería

Universidad de Sevilla

Sevilla, 2013

Proyecto Fin de CarreraIngeniería Aeronáutica

Diseño y simulación en Matlab de los algoritmosEKF y UKF aplicados a la trayectoria de unaaeronave equipada con un sistema VOR

Autor: Roberto Fermín García MenaTutor: Mª Ángeles Martín Prats

Dep. Ingeniería ElectrónicaEscuela Técnica Superior de Ingeniería

Universidad de Sevilla

Sevilla, 2016

Proyecto Fin de CarreraIngeniería Aeronáutica

Diseño y simulación en Matlab de los algoritmosEKF y UKF aplicados a la trayectoria de una

aeronave equipada con un sistema VOR

Autor:

Roberto Fermín García Mena

Tutor:

Mª Ángeles Martín PratsProfesor Titular

Dep. Ingeniería ElectrónicaEscuela Técnica Superior de Ingeniería

Universidad de SevillaSevilla, 2016

Proyecto Fin de Carrera: Diseño y simulación en Matlab de los algoritmos EKF y UKF aplicadosa la trayectoria de una aeronave equipada con un sistema VOR

Autor: Roberto Fermín García MenaTutor: Mª Ángeles Martín Prats

El tribunal nombrado para juzgar el trabajo arriba indicado, compuesto por los siguientes profesores:

Presidente:

Vocal/es:

Secretario:

acuerdan otorgarle la calificación de:

El Secretario del Tribunal

Fecha:

ÍndiceObjetivos 9

1. Introducción 91.1. Filtro de Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2. Descripción del vector de estado 142.1. Sistemas de referencia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142.2. Actitud . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162.3. Posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3. Descripción de los sensores 183.1. Giroscopio . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183.2. GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183.3. Acelerómetro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193.4. Presión atmosférica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193.5. VOR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

4. Algoritmos de fusión de datos 234.1. Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 244.2. Unscented Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

5. Resultados y simulación 275.1. Referencia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 275.2. Efecto del tiempo de muestreo . . . . . . . . . . . . . . . . . . . . . . . . . 30

5.2.1. ∆t = 1s . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 305.2.2. ∆t = 0,25s . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 315.2.3. ∆t = 0,125s . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

5.3. Efecto de la posición de las antenas VOR . . . . . . . . . . . . . . . . . . . 345.3.1. 1 antena exterior cercana . . . . . . . . . . . . . . . . . . . . . . . . 345.3.2. 1 antena exterior lejana . . . . . . . . . . . . . . . . . . . . . . . . 365.3.3. 2 antenas exteriores cercanas . . . . . . . . . . . . . . . . . . . . . . 385.3.4. 2 antenas exteriores lejanas . . . . . . . . . . . . . . . . . . . . . . 405.3.5. 2 antenas exteriores laterales cercanas . . . . . . . . . . . . . . . . . 425.3.6. 2 antenas exteriores laterales lejanas . . . . . . . . . . . . . . . . . 44

5.4. Efecto del GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 465.5. Análisis comparativo del tiempo de ejecución . . . . . . . . . . . . . . . . . 48

6. Análisis de los resultados 49

7. Conclusiones y líneas futuras 52

Anexos 53

A. Cálculo con cuaterniones 53

B. Cálculo numérico de matrices jacobianas 55

C. Programas y modelos de Simulink 56

Referencias 67

8

ObjetivosEl objetivo de este proyecto es analizar y comparar el comportamiento de dos algoritmos

de estimación de estado para sistemas no lineales. En concreto nos centraremos en losllamados Extended Kalman Filter (EKF) y el Unscented Kalman Filter (UKF) y losusaremos para estudiar el movimiento de un avión a partir de las medidas de una seriede sensores, entre los cuales nos centraremos especialmente en el uso de un VHF OmniDirectional Radio Range (VOR).

1. IntroducciónLa aparición de los Unmanned Aerial Vehicles (UAV) y los aviones cada vez más

electrónicos ha hecho que los sistemas digitales de geolocalización y control de actitudsean más necesarios que nunca. Además la industria demanda que estos sistemas tenganlos menores costes posibles sin renunciar en ningún momento a los estándares de seguridad,que son cada día más estrictos. Para conseguir cumplir estos requerimientos, se estáconvirtiendo en una necesidad la integración de sensores digitales cuya señal deber sertratada e integrada para obtener de ellos los datos necesarios para la navegación.

La fusión de toda esta información se realiza gracias a un algoritmo conocido comofiltro de Kalman (KF). Este algoritmo resulta de gran utilidad en ingeniería ya que nosproporciona un método para reducir el ruido de uno de los sensores instalados en unsistema valiéndose de las medidas, tanto actuales como pasadas, del resto de ellos, ynos permite además hacerlo de la forma matemáticamente más óptima. Además, paraincrementar la seguridad, gracias a este algoritmo podemos integrar varios sensores, tantoiguales como distintos, con el objetivo de medir una misma variable de estado a través dediferentes métodos. En nuestro caso por ejemplo, para medir la posición de la aeronavevamos a utilizar tres sistemas distintos: inerciales (acelerómetro), por satélite (GPS) yradioayudas (VOR), aunque todos ellos serán descritos más en detalle en la sección 3.

A continuación pasamos a explicar el funcionamiento del filtro de Kalman con elobjetivo de facilitar la posterior comprensión del EKF y el UKF.

1.1. Filtro de KalmanEl filtro de Kalman es un algoritmo desarrollado en 1960 por el matemático e ingeniero

Rudolf E. Kalman. La principal función de este algoritmo es la de estimar el estado deun sistema lineal a partir de una serie de medidas que pueden ser a su vez combinaciónlineal de las variables de estado y pudiendo estar tanto unas como otras afectadas por unruido blanco de características conocidas.

Los algoritmos UKF y EKF son evoluciones del filtro de Kalman original pero en estecaso tanto la función de transición de estado como las funciones de medida pueden ser nolineales. En las secciones 4.1 y 4.2 hablaremos más en profundidad de cada uno de ellos.

Partiremos de un sistema lineal de la siguiente forma.

xk = Fk−1xk−1 + Bk−1uk−1 + wk−1

zk = Hkxk + vk

9

Las variables que aparecen en estas fórmulas son:

x es el estado del sistema.

F es la matriz de transición de estado.

B y u son la matriz y el vector de control.

w es el vector de ruido del estado.

z es el vector de medidas.

H es la matriz de medida.

v es el vector de ruido de la medida.

En nuestro caso además haremos las siguientes consideraciones.

No tenemos variables de control así que de ahora en adelante omitiremos el términoBk−1uk−1.

w es un ruido blanco modelado como una distribución normal de media cero ycovarianza Q.

v es un ruido blanco modelado como una distribución normal de media cero ycovarianza R.

Hay que destacar que el ruido w no tiene que ser un ruido real, también se puede usarpara compensar errores en el modelado del sistema. Por ejemplo, en nuestro caso vamos aconsiderar que el avión tendrá aceleración constante pero estableceremos un ruido ficticiocambiando los valores de la matriz Q para permitir variaciones. Mientras mayores seanestos valores, más rápidas serán las variaciones de la señal estimada. Esto permite capturarmejor dinámicas rápidas pero puede ser un inconveniente si la señal tiene mucho ruido,ya que no lo filtrará adecuadamente y dejará muchos picos (ver figura 1).

0 50 100 150-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

RealEstimadaMedida

Figura 1: Filtro de Kalman con Q demasiado alta.

10

Por el contrario, si Q es demasiado baja, la señal filtrada será más suave pero seadaptará con mucho retraso provocando errores igualmente (ver figura 2).

0 50 100 150-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

RealEstimadaMedida

Figura 2: Filtro de Kalman con Q demasiado baja.

Para concretar, si tenemos ruidos de medida bajos o variables que cambian rápidamentede valor, necesitaremos valores de Q altos, mientras que si tenemos ruidos de medida altoso variables que cambien lentamente, es recomendable valores de Q bajos. Teniendo estoen cuenta, tras unas pocas simulaciones podremos ajustar un valor adecuado de esteparámetro (ver figura 3).

0 50 100 150-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

RealEstimadaMedida

Figura 3: Filtro de Kalman con Q adecuada.

Antes de continuar, vamos a explicar la notación que vamos a utilizar.

x es la estimación del estado, para distinguirlo de x, que es el estado real.

11

P es la matriz de covarianza de la estimación. Es una medida de la precisión de laestimación.

xa|b significa que la variable x está evaluada en el instante a conocidas las medidashasta el instante b.

El filtro de Kalman tiene dos partes claramente diferenciadas: predicción y actualización.

Figura 4: Esquema del filtro de Kalman.

Predicción

En la etapa de predicción calcularemos una aproximación al estado actual partiendode la estimación en el instante anterior y aplicando la matriz de transición de estados.

xk|k−1 = Fkxk−1|k−1

Pk|k−1 = FkPk−1|k−1FTk + Qk

Actualización

En la etapa de actualización comparamos la medida en el instante actual con lapredicción que hemos realizado en la etapa anterior y corregimos nuestra estimaciónen consecuencia. Para que la corrección sea óptima se utiliza la llamada ganancia deKalman (K) que establece como de fiable es nuestra predicción en comparación con lanueva medida. Esto lo hace comparando las matrices de covarianza de la medida (Rk) yla matriz de covarianza de la predicción (Pk|k−1). Mientras mayor sea la segunda, mayorserá Kk y por lo tanto mayor será la corrección.

Kk = Pk|k−1HTk

(HkPk|k−1HT

k + Rk

)−1

zk = HTk xk|k−1

xk|k = xk|k−1 + Kk (zk − zk)Pk|k = (I −KkHk) Pk|k−1

Condiciones iniciales

Además de los datos del sistema y de las medidas, necesitaremos unas condicionesiniciales para las variables x0|0 y P0|0. Si podemos conseguir una buena aproximación delestado real, podemos suponer un valor de P bajo, pero si por el contrario no disponemos

12

de datos, podemos usar un valor arbitrario de x y un valor alto de P. En cualquier caso,tras las primeras iteraciones ambos valores tenderán a sus valores reales, tal como se puedever en la figura 5.

k0 5 10 15 20 25

P

0.04

0.06

0.08

0.1

0.12

0.14

0.16

0.18

0.2

Figura 5: Evolución de P respecto al número de iteraciones.

13

2. Descripción del vector de estadoEl sistema que vamos a analizar es un avión pero desde un punto de vista físico

usaremos un sistema de seis grados de libertad (6DoF) por lo que los resultados obtenidospueden ser aplicados a cualquier otro que se pueda modelar de forma similar.

Para las ecuaciones cinemáticas tendremos en cuenta que deberemos discretizarlas yeso lo haremos de forma similar a la que se usa en el método de Euler para resolverecuaciones diferenciales de forma numérica.

xk = f (xk)xk ≈ xk+1−xk

∆t

}⇒ f (xk) ≈

xk+1 − xk∆t

xk+1 ≈ xk + f (xk) ·∆t (1)Donde ∆t representa el incremento de tiempo entre dos medidas.

tk+1 = tk + ∆t

Además, en nuestro caso, el vector de estado lo dividiremos en dos, uno para lasvelocidades angulares y la actitud y otro para la posición, la velocidad y la aceleración.A continuación explicaremos con más detalle cada uno de ellos y detallaremos comocalcularlos, pero antes vamos a realizar una serie de aclaraciones sobre los sistemas dereferencia en los que nos vamos a basar.

2.1. Sistemas de referenciaPara calcular la posición utilizaremos la Hipótesis de Tierra Plana. Según esta hipótesis

se considera que la tierra es plana, que cualquier sistema de referencia fijada a ella esinercial y que la gravedad es constante en cualquier punto, en nuestro caso consideraremosque es cero ya que solo nos introduciría un término constante.

Definiremos los ejes ligados a tierra (Xe, Ye, Ze) de la siguiente forma a partir de unpunto Oe en la superficie de la Tierra.

Figura 6: Sistema de referencia de Tierra plana.

Xe contenido en el plano tangente a la superficie de la tierra en el punto Oe.

14

Ze perpendicular al plano tangente a la superficie de la tierra en el punto Oe ydirigido hacia el centro del planeta.

Ye perpendicular a los otros dos eje y formando un triedro a derechas.

Figura 7: Sistema de referencia North-East-Down.

Para más información sobre la transformación de las coordenadas en ejes de TierraPlana a Longitud, Latitud y Altitud dejamos la referencia en la bibliografía [4]. A modo deresumen, los ejes que vamos a usar son similares al sistema de referencia North-East-Down(NED) pero girado un ángulo ψ (positivo en sentido horario) para generalizar. La relaciónmatemática entre ambos sistemas es la siguiente. N

ED

=

cosψ −sinψ 0sinψ cosψ 0

0 0 1

xeyeze

Por otra parte tenemos los ejes cuerpo (Xb, Yb, Zb) definidos a partir del punto O que

es el centro de gravedad de la aeronave.

Figura 8: Sistema de referencia de Ejes cuerpo.

Xb contenido en el plano de simetría del avión, según una línea de referencialongitudinal y dirigida hacia el morro.

Zb contenido en el plano de simetría del avión, perpendicular a Xe y dirigido haciaabajo en la actitud normal de vuelo.

15

Yb completa un triedro a derechas (es ortogonal al plano de simetría, dirigido segúnel ala derecha del avión).

2.2. ActitudPara calcular la actitud de la aeronave respecto de los ejes tierra utilizaremos cuaterniones

por dos razones. La primera es que de esta forma evitamos discontinuidades como las quese producen al usar los ángulos de Euler al pasar de −π a π. Además las ecuacionescinemáticas son más sencillas, lo que permite una implantación más fácil y reduce eltiempo de cálculo al ejecutar los programas. Sin embargo, también es cierto que losresultados tienen una interpretación física mucho menos intuitiva que los ya mencionadosángulos de Euler, así que la transformación de unos a otros la realizaríamos a posteriorisi fuera necesario.

Además consideraremos las velocidades angulares de los ejes cuerpo respeto de los ejestierra medidas en ejes cuerpo. De nuevo tenemos dos razones principales para hacerlo deesta forma. Por una parte la ecuación cinemática en término de cuaterniones se calculausando las velocidades angulares expresadas en estos ejes y, por otra, es la medida directaque nos proporcionan los giróscopos por lo que la función de medida será más fácil deimplementar y más rápida de ejecutar.

Figura 9: Velocidades angulares.

Con todas estas consideraciones el vector de estado de la actitud queda de la siguienteforma.

x1 =[

qωb

]=

q0q1q2q3ωxωyωz

La ecuación cinemática que proporciona la evolución de los cuaterniones de actitud

tiene la siguiente forma (ver referencia [3]).

q =

q0q1q2q3

= 12

−q1 −q2 −q3q0 −q3 q2q3 q0 −q1−q2 q1 q0

ωxωyωz

Pero aplicando la fórmula 1 obtenemos este resultado.

16

qk = qk−1 + ∆t2

−q1 −q2 −q3q0 −q3 q2q3 q0 −q1−q2 q1 q0

k−1

ωxωyωz

k−1

Donde [ · ]k−1 representa que dicho vector o matriz se evalúan en el instante k-1.

2.3. PosiciónPara calcular la posición plantearemos sobre cada eje las ecuaciones de un movimiento

uniformemente acelerado y posteriormente, según explicamos en la sección 1.1, aplicaremosun error ficticio para modelar posibles variaciones en la aceleración.

Nótese que vamos a plantearlas sobre los ejes ligados a tierra. De esta forma, paracada eje tenemos el siguiente sistema de ecuaciones:

xk = xk−1 + vk−1∆t+ 12ak−1 (∆t)2

vk = vk−1 + ak−1∆tak = ak−1

O en forma matricial xva

k

=

1 ∆t 12 (∆t)2

0 1 ∆t0 0 1

xva

k−1

17

3. Descripción de los sensoresPara poder corregir adecuadamente las estimaciones, los filtros de Kalman necesitan

una serie de datos o medidas. Como ya hemos comentado, en nuestro caso tenemos comosistema un avión por lo que vamos a considerar que tenemos señales de los sensores máshabituales que se suelen utilizar en este tipo de vehículo. A continuación vamos a explicarcada uno de ellos y vamos a especificar las ecuaciones que vamos a utilizar para modelarlos.

3.1. Giroscopio

Figura 10: Giroscopio electrónico visto a través de un microscopio. [5]

El giróscopo o giroscopio es un aparato que permite medir las velocidades angulares.Como se monta fijo a la aeronave, dichas velocidades angulares se calcularán en ejes cuerpoy, dado que una de las variables de nuestro sistema es precisamente la velocidad angularmedida en ejes cuerpo, la función de transición de estados es directamente la identidad.

ωb = ωg

donde ωg son las velocidades angulares medidas por el giroscopio.

3.2. GPSEl Global Positioning System (GPS) es un sistema que permite calcular la posición

de la aeronave mediante el uso de señales transmitidas por satélite. Los satélites deGPS emiten constantemente una señal con la hora y su posición con una precisión muyalta. Comparando las señales de varios satélites es posible calcular nuestra posición portriangulación.

En nuestro modelo consideraremos que el GPS nos da una señal que es directamentela posición del avión respecto al punto de referencia Oe expresada en ejes Tierra. Comode nuevo esta es una de las variables de nuestro sistema, la función de medida vuelve a

18

ser la identidad. xyz

=

xyz

GPS

Figura 11: Posición GPS.

3.3. AcelerómetroUn acelerómetro es un sensor que permite medir las aceleraciones del vehículo en las

tres direcciones del espacio. Junto con el giróscopo, son los sensores básicos de una Unidadde Medidas Inerciales (IMU) y, al igual que este, se instalan unidos físicamente al avión,por lo que las medidas de aceleración se expresarán en ejes cuerpo (ab).

Como vimos en las secciones 2.2 y 2.3, dos de las variables de estado de nuestro sistemason el cuaternión q, que representa el giro de ejes tierra a ejes cuerpo, y el vector deaceleraciones en ejes tierra (ax, ay, az)e. Usando la ecuación 3 del anexo A y definiendo uncuaternión con las aceleraciones ae = [0, ax, ay, az]Te , podemos expresar las aceleracionesen ejes cuerpo como

ab = q∗aeqHay que destacar que la ecuación de este sensor involucra variables de posición y de

actitud. Este hecho provoca un acoplamiento entre los dos sistemas de ecuaciones quede otra forma serían independientes. En el apartado 4 explicaremos como resolver esteproblema.

3.4. Presión atmosféricaEl sensor de presión atmosférica nos permite medir la altura de vuelo de forma

indirecta. En nuestro caso usaremos el modelo deAtmósfera Estándar Internacional (ISA)para calcular la altura de vuelo en función de la presión.

La atmósfera ISA establece que la evolución de la presión frente a la altura sigue lasiguiente función para 0km ≤ h ≤ 11km (ver referencia [6] pág 107)

p (h) = p0

(1 + α

T0(h− h0)

)− gαRg

donde los coeficientes tienen los siguientes valores

19

α = −0,0065 K/km g = 9,80665 m/s2 Rg = 287,05307 J/kg Kp0 = 101325 Pa T0 = 288,15 K h0 = 0 km

Recordemos además que dado que nuestro vector Ze está definido hacia abajo, tenemosque z = −h.

p/p0

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1

h [k

m]

0

1

2

3

4

5

6

7

8

9

10

11

Figura 12: Presión atmosférica.

3.5. VOREl sistema VOR es un tipo de radioayuda que se utiliza en navegación para determinar

la posición sin necesidad de utilizar GPS. La antena de VOR emite tres señales, la primeraes un identificador y las otras dos señales son ondas senoidales. La diferencia es que unade esas ondas se emite con una fase constante, mientras que la otra tiene un desfase quevaría según la dirección en la que se emite. Midiendo la diferencia de fase entre las dosseñales podemos determinar en qué dirección se encuentra la antena de VOR y, usandolas señales de varias antenas y conocidas sus localizaciones, podemos aproximar nuestraposición.

20

Figura 13: Esquema de funcionamiento de un VOR.

En nuestro caso supondremos que el dato que nos ofrece es el ángulo que forma nuestraposición con la línea de referencia. Esta línea suele considerarse como la dirección queva hacia el norte pero, para simplificar la implementación, vamos a suponer que es laque va hacia el este. Para hacerlo usaremos el comando de Matlab llamado atan2(Y,X).Este comando nos devuelve el valor de α para cualquier punto visto desde la posiciónde la antena de VOR. Usaremos este comando porque nos proporciona un ángulo en elintervalo [−π, π] mientras que un comando arcotangente normal nos devolverá un valoren [−π/2, π/2].

Figura 14: Resultados del comando atan2.

De esta forma podemos calcular nuestro dato como

α = atan2 (py − y, px − x)

donde [px, py] es la posición de la antena de VOR y [x, y] es la posición de la aeronavesobre el plano horizontal.

No obstante, dado que la función arcotangente así definida tiene una discontinuidad enα = π, usaremos mejor la dirección dada en forma de número complejo de módulo unidad,algo similar a lo que hemos explicado con cuaterniones pero en el plano en lugar de en

21

el espacio. Recordando que eiϕ = cosϕ + i sinϕ tenemos que por cada antena de VORobtendremos dos señales, la parte real y la parte imaginaria de este número complejo, asíque la función de medida queda como sigue

hV OR

(x) =[

sinαcosα

]

Recordemos que α = α(x).

22

4. Algoritmos de fusión de datosVamos a implementar dos algoritmos de filtrado para sistemas no lineales, los conocidos

como Extended Kalman Filter y Unscented Kalman Filter. A continuación desarrollaremosen más detalle el funcionamiento de cada uno pero antes vamos a explicar como vamosa realizar la simulación. Partiremos de un modelo de seis grados de libertad simuladoen Simulink que representará nuestra aeronave (ver anexo C). A este modelo le daremoscomo entradas una serie de fuerzas y momentos y obtendremos como resultado todaslas variables de estado de nuestro sistema. Usando las funciones descritas en la sección3 obtenemos las medidas de los sensores, a las cuales posteriormente les sumamos unruido gaussiano que simula la posible incertidumbre de los sensores reales. De esta formaobtenemos dos vectores de datos, uno con medidas para calcular la actitud (giróscopo) yotro para calcular la posición (GPS, acelerómetro, sensor de presión y VOR).

Primero realizaremos la estimación de la actitud usando un filtro que se alimentarácon las medidas del giróscopo y la estimación del estado en el instante anterior. En lafigura 15 hemos representado esta realimentación usando un z−1 que representa el retrasoentre que se realiza una estimación y se realiza la siguiente. Por último, realizamos elfiltrado de las medidas de los sensores de posición. Hay que hacerlo en este orden porque,tal como vimos en el apartado 3.3, para aplicar la función de medida del acelerómetronecesitamos una estimación de la actitud de vuelo.

Es muy importante destacar que los filtros de Kalman tanto de actitud como deposición son en realidad el mismo programa, lo que los diferencia realmente son lasfunciones y las variables que les introducimos como datos.

Figura 15: Esquema del sistema de filtrado.

Para los siguientes apartados vamos a suponer que tenemos un sistema de la siguienteforma

xk = f (xk−1) + wk−1

zk = h (xk) + vk

donde f (xk−1) es la función de transición de estados en el instante k-1 y h (xk) es lafunción de medida en el instante k.

23

4.1. Extended Kalman FilterEl algoritmo del EKF es muy similar al del filtro de Kalman original. Las principales

diferencias radican en que tanto la estimación como la medida se realizan mediantefunciones no lineales y, a la hora de calcular las matrices de covarianza de la estimación,realizaremos una linealización de las ecuaciones mediante el uso de matrices jacobianas.El cálculo de estas matrices vamos a hacerlo de forma numérica (ver anexo B) porquepermite modificar tanto el sistema que estamos usando como los sensores de forma mássencilla.

Predicción

xk|k−1 = f(xk−1|k−1

)Pk|k−1 = FkPk−1|k−1FT

k + Qk

Donde los términos de la matriz jacobiana F se definen como

F = ∇f (x)

Actualización

Kk = Pk|k−1HTk

(HkPk|k−1HT

k + Rk

)−1

zk = h(xk|k−1

)xk|k = xk|k−1 + Kk (zk − zk)Pk|k = (I −KkHk) Pk|k−1

De forma similar a la F la matriz H se calcula como

H = ∇h (x)

4.2. Unscented Kalman FilterEl UKF es un filtro de Kalman basado en la llamada Unscented Tranformation (UT).

La UT es un proceso matemático por el cual podemos calcular una estimación de la mediay la covarianza de la distribución de probabilidad de una variable aleatoria a la que se leha aplicado una transformación no lineal. Para hacer esto se vale de una serie de muestras,a las que se le llaman puntos sigma, las cuales se propagan usando la función no linealque estemos estudiando. La colocación de estos puntos sigma se realiza de forma que, altransformarlos, su media y covarianza se ajusten lo más posible a los de la distribuciónoriginal.

24

Figura 16: Unscented Transformation.[8]

Puntos sigma

A continuación vamos a detallar el método para el cálculo de los puntos sigma asícomo unos pesos que nos servirán para ponderar más adelante la medida en la que dichospuntos aportan al resultado final. Empezaremos definiendo los puntos sigma como

χ0k−1|k−1 = xk−1|k−1

χik−1|k−1 = xk−1|k−1 +(√

(L+ λ) Pk−1|k−1

)i, i = 1, . . . , L

χik−1|k−1 = xk−1|k−1 −(√

(L+ λ) Pk−1|k−1

)i−L

, i = L+ 1, . . . , 2L

donde:

L es el número de variables que definen el estado de nuestro sistema, o dicho de otraforma, la longitud del vector x.

λ se conoce como factor de escala y se define

λ = α2 (L+ κ)− L

α determina la dispersión de los puntos sigma y suele tomar un valor pequeño, ennuestro caso α = 10−3, y κ es un segundo parámetro de escala que suele tomar elvalor 0 [8].(√

(L+ λ) Pk−1|k−1)irepresenta la columna i de la matriz raíz cuadrada de (L+ λ) Pk−1|k−1.

Esta matriz se calcula comúnmente usando la llamada factorización de Cholesky quenos permite obtener una matriz L a partir de una matriz A de forma que A = LLT .Esto es posible gracias a que P es definida positiva y hermítica (en nuestro caso real

25

y simétrica). En Matlab realizaremos esta descomposición como L = chol (A)′. Esimportante trasponer porque el programa nos devuelve LT .

Para calcular los pesos que mencionamos anteriormente usamos las siguientes fórmulas

W 0s = λ

L+ λ

W 0c = λ

L+ λ+(1− α2 − β

)W is = W i

c = λ

2 (L+ λ) , i = 1, . . . , 2L

donde:W is son los pesos para el cálculo de la estimación del estado y la medida.

W ic son los pesos para el cálculo de las matrices de covarianza.

β es un parámetro para incorporar el conocimiento previo de la distribución de x.En el caso de distribución gaussiana, como es el nuestro, el óptimo es β = 2 [8].

Predicción

En esta etapa se propagan los puntos sigma según la función de transición de estadosf y posteriormente se combinan los resultados para calcular la predicción del estado y sucovarianza.

χik|k−1 = f(χik−1|k−1

), i = 0, . . . , 2L

xk|k−1 =2L∑i=0

W isχ

ik|k−1

Pk|k−1 =( 2L∑i=0

W ic

[χik|k−1 − xk|k−1

] [χik|k−1 − xk|k−1

]T)+ Qk

Actualización

En esta etapa se propagan los puntos sigma según la función de observación h yposteriormente se combinan los resultandos para calcular la nueva estimación del estadoy su covarianza.

γik = h(χik|k−1

), i = 0, . . . , 2L

zk =2L∑i=0

W isγ

ik

Pzkzk=( 2L∑i=0

W ic

[γik − zk

] [γik − zk

]T)+ Rk

Pxkzk=

2L∑i=0

W ic

[χik|k−1 − xk|k−1

] [γik − zk

]TKk = Pxkzk

P−1zkzk

xk|k = xk|k−1 + Kk (zk − zk)Pk|k = Pk|k−1 −KkPzzKT

k

26

5. Resultados y simulaciónA continuación vamos a estudiar una serie de trayectorias para ver el comportamiento

de cada filtro. Para comparar los errores vamos a usar la Root Mean Square Deviation(RMSD) que se define a partir del error cuadrático medio (ECM) de la siguiente forma

ECM = 1n

n∑i=1

(X − X

)2

RMSD =√ECM

donde X es el valor real y X es la aproximación. En este caso, nuestra variable serála posición estimada, por lo que tenemos(

X − X)2

= (x− x)2 + (y − y)2 + (z − z)2

Para calcular un valor de estos parámetros, realizaremos un promediado de los resultadosobtenidos mediante 100 simulaciones de Monte Carlo.

5.1. ReferenciaPara comparar, tomaremos como referencia un vuelo horizontal, circular de radio

100m, con inicio en el punto [0,−100], con velocidad constante, con velocidad angularadecuada de forma que la orientación de la nave sea tangente a la trayectoria y realizandoel recorrido en sentido antihorario. Además colocaremos una antena de VOR en el centrodel recorrido (ver figura 17), desactivaremos la señal del GPS y tomaremos un tiempo demuestreo ∆t = 0,5s.

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100

120

TrayectoriaAntena VOR

Figura 17: Trayectoria de referencia.

27

La desviación típica del ruido de cada sensor se ha considerado de forma razonablepero no está basada en ningún modelo de sensor concreto, no obstante, los resultadosseguirán teniendo valor desde un punto de vista cualitativo. En la tabla 1 recogemos losdatos usados para la simulación.

En la figura 18 podemos ver el recorrido predicho por cada filtro y en la figura 19 loserrores cometidos, ambos para una simulación típica, pero para hacernos una mejor ideausaremos los resultados del RMSD que se muestran en la figura 20. Para las siguientesconfiguraciones mostraremos las mismas gráficas.

σGiróscopo 0.01

GPS 3Acelerómetro 0.05Barómetro 0.2

VOR 0.1

Tabla 1: Desviación típica de los sensores.

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100RealUKFEKF

Figura 18: Estimación.

28

0 10 20 30 40 50 60 70 80 90 100

|x-x|

0

5

UKFEKF

0 10 20 30 40 50 60 70 80 90 100

|y-y|

0

5

10

t0 10 20 30 40 50 60 70 80 90 100

|z-z|

0

0.5

1

Figura 19: Errores.

Simulación0 10 20 30 40 50 60 70 80 90 100

RM

SD

2

2.2

2.4

2.6

2.8

3

3.2

3.4

UKFEKF

UKF EKFRMSD 3.006 2.494

Figura 20: RMSD.

29

5.2. Efecto del tiempo de muestreo5.2.1. ∆t = 1s

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100RealUKFEKF

Figura 21: Estimación (∆t = 1s).

0 10 20 30 40 50 60 70 80 90 100

|x-x|

0

5

10

UKFEKF

0 10 20 30 40 50 60 70 80 90 100

|y-y|

0

5

10

t0 10 20 30 40 50 60 70 80 90 100

|z-z|

0

1

2

Figura 22: Errores (∆t = 1s).

30

Simulación0 10 20 30 40 50 60 70 80 90 100

RM

SD

5

5.2

5.4

5.6

5.8

6

6.2

UKFEKF

UKF EKFRMSD 5.567 5.692

Figura 23: RMSD (∆t = 1s).

5.2.2. ∆t = 0,25s

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100

RealUKFEKF

Figura 24: Estimación (∆t = 0,25s).

31

0 10 20 30 40 50 60 70 80 90 100

|x-x|

0

2

4

UKFEKF

0 10 20 30 40 50 60 70 80 90 100

|y-y|

0

2

4

t0 10 20 30 40 50 60 70 80 90 100

|z-z|

0

0.5

1

Figura 25: Errores (∆t = 0,25s).

Simulación0 10 20 30 40 50 60 70 80 90 100

RM

SD

0.8

1

1.2

1.4

1.6

1.8

2

2.2

2.4

UKFEKF

UKF EKFRMSD 1.991 1.202

Figura 26: RMSD (∆t = 0,25s).

32

5.2.3. ∆t = 0,125s

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100

RealUKFEKF

Figura 27: Estimación (∆t = 0,125s).

0 10 20 30 40 50 60 70 80 90 100

|x-x|

0

2

4

UKFEKF

0 10 20 30 40 50 60 70 80 90 100

|y-y|

0

2

4

t0 10 20 30 40 50 60 70 80 90 100

|z-z|

0

0.5

1

Figura 28: Errores (∆t = 0,125s).

33

Simulación0 10 20 30 40 50 60 70 80 90 100

RM

SD

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

UKFEKF

UKF EKFRMSD 1.591 0.712

Figura 29: RMSD (∆t = 0,125s).

5.3. Efecto de la posición de las antenas VOR5.3.1. 1 antena exterior cercana

x-150 -100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100

120

TrayectoriaAntena VOR

Figura 30: Posición de las antenas.

34

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100RealUKFEKF

Figura 31: Estimación.

0 10 20 30 40 50 60 70 80 90 100

|x-x|

0

5

UKFEKF

0 10 20 30 40 50 60 70 80 90 100

|y-y|

0

5

10

t0 10 20 30 40 50 60 70 80 90 100

|z-z|

0

0.5

1

Figura 32: Errores.

35

Simulación0 10 20 30 40 50 60 70 80 90 100

RM

SD

3

3.5

4

4.5

5 UKFEKF

UKF EKFRMSD 4.404 3.562

Figura 33: RMSD.

5.3.2. 1 antena exterior lejana

x-200 -150 -100 -50 0 50 100

y

-100

-50

0

50

100TrayectoriaAntena VOR

Figura 34: Posición de las antenas.

36

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100RealUKFEKF

Figura 35: Estimación.

0 10 20 30 40 50 60 70 80 90 100

|x-x|

0

5

UKFEKF

0 10 20 30 40 50 60 70 80 90 100

|y-y|

0

5

t0 10 20 30 40 50 60 70 80 90 100

|z-z|

0

0.5

1

Figura 36: Errores.

37

Simulación0 10 20 30 40 50 60 70 80 90 100

RM

SD

2

2.5

3

3.5

4

4.5

UKFEKF

UKF EKFRMSD 3.327 3.161

Figura 37: RMSD.

5.3.3. 2 antenas exteriores cercanas

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100

120

TrayectoriaAntena VOR

Figura 38: Posición de las antenas.

38

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100

RealUKFEKF

Figura 39: Estimación.

0 10 20 30 40 50 60 70 80 90 100

|x-x|

0

0.5

1

UKFEKF

0 10 20 30 40 50 60 70 80 90 100

|y-y|

0

2

4

t0 10 20 30 40 50 60 70 80 90 100

|z-z|

0

1

2

Figura 40: Errores.

39

Simulación0 10 20 30 40 50 60 70 80 90 100

RM

SD

0.6

0.8

1

1.2

1.4

1.6

UKFEKF

UKF EKFRMSD 1.251 0.874

Figura 41: RMSD.

5.3.4. 2 antenas exteriores lejanas

x-200 -150 -100 -50 0 50 100 150 200

y

-150

-100

-50

0

50

100

150TrayectoriaAntena VOR

Figura 42: Posición de las antenas.

40

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100

RealUKFEKF

Figura 43: Estimación.

0 10 20 30 40 50 60 70 80 90 100

|x-x|

0

2

4

UKFEKF

0 10 20 30 40 50 60 70 80 90 100

|y-y|

0

2

4

t0 10 20 30 40 50 60 70 80 90 100

|z-z|

0

1

2

Figura 44: Errores.

41

Simulación0 10 20 30 40 50 60 70 80 90 100

RM

SD

0.6

0.8

1

1.2

1.4

1.6

1.8

2

2.2

UKFEKF

UKF EKFRMSD 1.582 1.315

Figura 45: RMSD.

5.3.5. 2 antenas exteriores laterales cercanas

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100

TrayectoriaAntena VOR

Figura 46: Posición de las antenas.

42

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100

RealUKFEKF

Figura 47: Estimación.

0 10 20 30 40 50 60 70 80 90 100

|x-x|

0

2

4

UKFEKF

0 10 20 30 40 50 60 70 80 90 100

|y-y|

0

1

2

t0 10 20 30 40 50 60 70 80 90 100

|z-z|

0

0.5

1

Figura 48: Errores.

43

Simulación0 10 20 30 40 50 60 70 80 90 100

RM

SD

0.7

0.8

0.9

1

1.1

1.2

1.3

1.4

UKFEKF

UKF EKFRMSD 1.068 0.910

Figura 49: RMSD.

5.3.6. 2 antenas exteriores laterales lejanas

x-250 -200 -150 -100 -50 0 50 100 150 200 250

y

-200

-150

-100

-50

0

50

100

150

200

TrayectoriaAntena VOR

Figura 50: Posición de las antenas.

44

x-100 -50 0 50 100

y

-100

-80

-60

-40

-20

0

20

40

60

80

100

RealUKFEKF

Figura 51: Estimación.

0 10 20 30 40 50 60 70 80 90 100

|x-x|

0

1

2

UKFEKF

0 10 20 30 40 50 60 70 80 90 100

|y-y|

0

1

2

t0 10 20 30 40 50 60 70 80 90 100

|z-z|

0

0.5

1

Figura 52: Errores.

45

Simulación0 10 20 30 40 50 60 70 80 90 100

RM

SD

1

1.2

1.4

1.6

1.8

2

2.2

UKFEKF

UKF EKFRMSD 1.492 1.610

Figura 53: RMSD.

5.4. Efecto del GPS

x-100 -50 0 50 100 150

y

-100

-80

-60

-40

-20

0

20

40

60

80

100 RealUKFEKFGPS

Figura 54: Estimación.

46

0 10 20 30 40 50 60 70 80 90 100

|x-x|

0

5

10

UKFEKF

0 10 20 30 40 50 60 70 80 90 100

|y-y|

0

5

10

t0 10 20 30 40 50 60 70 80 90 100

|z-z|

0

5

10

Figura 55: Errores.

Simulación0 10 20 30 40 50 60 70 80 90 100

RM

SD

2.5

3

3.5

4

4.5

5

UKFEKF

UKF EKFRMSD 3.638 3.630

Figura 56: RMSD.

47

5.5. Análisis comparativo del tiempo de ejecución

Simulación0 10 20 30 40 50 60 70 80 90 100

Tie

mpo

1.14

1.16

1.18

1.2

1.22

1.24

1.26

UKFEKF

UKF EKFTiempo (s) 1.218 1.160

Figura 57: Tiempo de simulación.

48

6. Análisis de los resultadosA la vista de estos resultados podemos observar algunos comportamientos interesantes.Con respecto al tiempo de muestreo podemos ver como el error cae según disminuimos

el tiempo de muestreo. Esto parece razonable por dos motivos, lo primero es que para unmismo trayecto tenemos más puntos de medida y por lo tanto nuestra estimación puedeser más precisa y, por otra parte, el método que usamos para aproximar las ecuaciones quedescribimos en la sección 2, es más exacto cuanto menor es el incremento de tiempo quetomemos. Además otro dato a remarcar es el hecho de que para frecuencias de muestreoaltas funciona mejor el EKF mientras que para frecuencias bajas funciona ligeramentemejor el UKF.

∆t (s) UKF EKF1.000 5.567 5.6920.500 3.006 2.4940.250 1.991 1.2020.125 1.591 0.712

Tabla 2: RMSD para distintos tiempos de muestreo (1 antena central).

Con respecto a la posición de las antenas vamos a ir analizando los distintos escenarios.Si comparamos, los errores cometidos al usar una antena son bastante superiores a losobtenidos al usar dos, esto parece razonable ya que de esta forma tenemos más información,algo similar a lo que pasa al reducir el tiempo de muestreo.

Posición UKF EKF

1 antenaInterior Central 3.006 2.494

Exterior Cercana 4.404 3.562Lejana 3.327 3.161

2 antenasCentrada Cercana 1.251 0.874

Lejana 1.582 1.315

Lateral Cercana 1.068 0.910Lejana 1.492 1.610

Figura 58: RMSD para distintas posiciones de las antenas (∆t = 0,5s).

Si nos centramos solo en el caso de usar una antena veremos que el caso de referenciacon una antena central es el que tiene mayor precisión. Esto puede ser debido a quetodos los puntos están a una distancia intermedia de la antena, mientras que en el casode las antenas exteriores hay ciertos puntos que están relativamente cerca pero otrosestán más lejos que en el caso de la antena central. Si comparamos los resultados de unaantena cercana y una lejana podemos ver que los errores cometidos en el primer caso sonmayores. Si miramos en las figuras 31, 32, 35 y 36 podemos hacernos una idea de lo queha sucedido. En el caso de la antena cercana, cuando nos acercamos a la antena (t ∼ 75s),el ángulo α varía bruscamente, como se ve en la figura 59, y el filtro no es capaz de seguirla trayectoria. Esto hace que la predicción oscile alrededor de la posición real provocandoque el error en valor absoluto aumente. Por otra parte, cuando la antena está un pocomás lejos, esta variación es mucho más suave por lo que el filtro si que tiene tiempo de

49

adaptarse, no se produce una oscilación tan pronunciada y por lo tanto la corrección esun poco mejor.

t0 10 20 30 40 50 60 70 80 90 100

α [r

ad]

-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

CercanaLejana

Figura 59: α para una antena exterior.

Pero para conseguir una estimación más acertada queda claro que es mejor usar másde una antena simultáneamente. Si empezamos por los casos en los que las antenasestán centradas podemos ver como la estimación sí que es mejor cuando están a unadistancia menor. Al usar dos antenas conseguimos evitar el efecto adverso provocado porel cambio brusco en la medida del ángulo del VOR, ya que cuando nos acercamos muchoa una antena, la otra es capaz de seguir corrigiendo nuestra trayectoria. Por otra partetenemos las simulaciones con las antenas laterales. En este caso los resultados del UKFmejoran mientras que los del EKF empeoran y esto puede deberse a que hay dos efectoscontrapuestos. Por una parte al desplazar las antenas lateralmente tenemos puntos de latrayectoria más lejanos y, como acabamos de ver, esto empeora la estimación. Sin embargo,en la figura 48 podemos ver como cuando nos encontramos entre las dos antenas (t ∼ 60s)el error de medida en esa dirección empeora mucho (en nuestro caso en el eje x). Esto es asíporque, tal como dijimos cuando explicamos el funcionamiento del VOR (subsección 3.5),este instrumento nos indica la dirección en la que se encuentran las antenas pero no nosda información sobre la distancia a la que están. Esto hace que cuando nos encontramosjusto entre dos antenas no podamos triangular nuestra situación y, aunque podamos saberque nos encontramos en esa línea, no podamos calcular nuestra posición exacta (figura60). Para evitar este problema podemos hacerlo de varias formas. Lo primero es intentarevitar cruzar entre dos antenas, como en el caso de 2 antenas exteriores laterales lejanas.Si no podemos evitar cruzar esta línea lo ideal es hacerlo de forma perpendicular parapermanecer el menor tiempo posible en la zona de incertidumbre y, por último, la formamás sencilla es utilizar, siempre que sea posible, la señal de una tercera antena no alineadacon las dos primeras.

50

(a) Posición fiable. (b) Posición no fiable.

Figura 60: Zona de error del VOR.

La simulación con GPS también nos proporciona algunos datos interesantes. Observandolos resultados mostrados en la tabla 3 podría dar la sensación que el sensor GPS empeoralas estimaciones pero esto tiene una explicación. Lo primero es que hemos simulado estesensor con un ruido de la señal bastante grande y, dado que nos proporciona una medidadirecta de una de nuestras variables de estado, esta señal es tenida más en cuenta quela del VOR a la hora de estimar la posición. No obstante, en zonas con pocas antenasde VOR o cuando nos movemos mucho tiempo lejos de las antenas, es posible acumularerrores mucho mayores si nos movemos usando solamente este sensor, mientras que elGPS nos proporciona un método para mantener el error acotado.

UKF EKFSin GPS 3.006 2.494Con GPS 3.638 3.630

Tabla 3: RMSD usando GPS.

Además hay que recordar que cuando se utilizan las medidas del GPS debemos reducirlos valores de la matriz de covarianza Q ya que, como comentamos en la la sección 1.1,cuando el ruido es muy alto la estimación puede presentar variaciones demasiado bruscas.

UKF EKFTiempo (s) 1.218 1.160

Tabla 4: Tiempos medios de simulación.

Con respecto a los tiempos de simulación podemos observar como los del UKF sonligeramente superiores a los del EKF. Esto no es de extrañar pues, aunque la cantidad depasos para ejecutar cada algoritmo es similar, el UKF incorpora algunos sumatorios y lafactorización de Cholesky que necesitan de una mayor cantidad de cálculos.

51

7. Conclusiones y líneas futurasEn la mayor parte de las simulaciones que hemos realizado el EKF ha obtenidounos resultados más precisos que UKF, siendo además sus tiempos de simulaciónmenores. Esto convierte al EKF en la opción preferible para una implementaciónpráctica, no obstante hay que tener algunas cosas en consideración. Por la formaen que hemos definido nuestras variables de estado, las ecuaciones de transición delas variables de posición (posición, velocidad y aceleración) son lineales y, como elEKF se basa en la linealización de ecuaciones, es posible que este filtro sea capaz decaptar mejor la evolución del sistema. Sería interesante la realización de simulacionesen sistemas de coordenadas geodésicos (latitud, longitud, altitud), en los cuales lasecuaciones de transición son no lineales, y estudiar el comportamiento de los filtros,ya que el UKF está específicamente diseñado para este tipo de sistemas.

Para obtener los mejores resultados necesitaremos tener sensores precisos, tiemposde muestreo bajos y trayectorias que permitan el uso de la mayor cantidad deseñales de VOR posible, tratando de evitar alinearnos con dos antenas. Además seríaconveniente el uso de GPS en determinadas circunstancias para evitar acumulacionesde error.

Con respecto a la actitud, ambos filtros presentan un comportamiento similar debidoen parte a que el giróscopo tiene un margen de error pequeño. Se ha observadoque para simulaciones suficientemente largas sí que pueden apreciarse pequeñoserrores en los últimos momentos. Sería interesante el uso de algún sensor extracapaz de detectar la actitud de forma directa sin necesidad de integración. Entreotros podría usarse una brújula, un detector de campo magnético terrestre o unsensor de gravedad.

0 10 20 30 40 50 60 70 80 90 100

Yaw

-2

0

2

Ángulos de Euler

RealUKFEKF

0 10 20 30 40 50 60 70 80 90 100

Pitc

h

-2

0

2

t0 10 20 30 40 50 60 70 80 90 100

Rol

l

-2

0

2

Figura 61: Ángulos de Euler.

52

A. Cálculo con cuaternionesLos cuaterniones son una generalización de los números complejos, y al igual que

estos pueden usarse para representar giros, solo que mientras que los números complejosrepresentas giros en el plano, los cuaterniones representan giros en el espacio. En estasección vamos a explicar como se realizan algunas de las operaciones comunes que podemosaplicarles.

Un cuaternión tiene la siguiente forma

q = q0 + q1i+ q2j + q3k

donde q0, q1, q2 y q3 son números reales y i2 = j2 = k2 = −1. Estos coeficientes semultiplican siguiendo estas normas.

ij = −ji = kjk = −kj = iki = −ik = j

En la siguiente tabla de multiplicación se resumen todas las combinaciones.

1 i j k1 1 i j ki i -1 k -jj j -k -1 ik k j -i -1

Tabla 5: Multiplicación de términos de un cuaternión.

Multiplicación

Dados dos cuaterniones a = a0 + a1i+ a2j + a3k y b = b0 + b1i+ b2j + b3k, se defineel producto de cuaterniones usando la propiedad conmutativa de la multiplicación y loscriterios de la Tabla 5.

ab = (a0b0 − a1b1 − a2b2 − a3b3) + (a0b1 + a1b0 + a2b3 − a3b2) i+ (a0b2 − a1b3 + a2b0 + a3b1) j + (a0b3 + a1b2 − a2b1 + a3b0) k

Otra forma de representarlo es la forma matricial, en la cual un cuaternión se representaríaen forma de vector

a =

a0a1a2a3

y el producto se puede realizar como vemos a continuación

ab =

a0 −a1 −a2 −a3a1 a0 −a3 a2a2 a3 a0 −a1a3 −a2 a1 a0

b0b1b2b3

53

Norma

Se define la norma de un cuaternión como

|q| =√q2

0 + q21 + q2

2 + q23

Decimos que un cuaternión está normalizado o es unitario si cumple que |q| = 1. Sino es así, podemos normalizar un cuaternión de esta forma.

u = q|q|

Rotación

Los cuaterniones unitarios pueden usarse para representar rotaciones. Según la fórmulade Euler-Rodrigues [3], dado un cuaternión q que representa un giro, la matriz de cosenosdirectores (DCM) asociada tiene la siguiente forma

C (q) =

q20 + q2

1 − q22 − q2

3 2 (q1q2 + q0q3) 2 (q1q3 − q0q2)2 (q1q2 − q0q3) q2

0 − q21 + q2

2 − q23 2 (q2q3 + q0q1)

2 (q1q3 + q0q2) 2 (q2q3 − q0q1) q20 − q2

1 − q22 + q2

3

Así, para calcular el vector v′ = [v′1, v′2, v′3]T , que es el resultado de girar un vector

v = [v1, v2, v3]T según q, tenemosv′ = C (q) v (2)

Pero hay una forma en la que podemos realizar el giro sin pasar por la DCM. Siconsideramos los cuaterniones qv = [0, v1, v2, v3]T y qv′ = [0, v′1, v′2, v′3]T , entonces podemoscalcular qv′ de la siguiente forma

qv′ = q∗qvq (3)

donde q∗ = [q0,−q1,−q2,−q3] y se conoce como cuaternión conjugado.Además tenemos que, gracias a las propiedades de la matriz de giro y el producto

de cuaterniones, invertir la transformación es sencillo. En el caso de la ecuación 2 solotenemos que utilizar la matriz transpuesta.

v = CTv′

Y para la fórmula 3 se invierte el orden de multiplicación.

qv = qqv′q∗

54

B. Cálculo numérico de matrices jacobianasLa matriz jacobiana de una función es una matriz formada por las derivadas parciales

de dicha función respecto de sus variables. Dicha matriz es una generalización para variasdimensiones del concepto de derivada. Para el caso general supongamos

y = f (x)

siendo y = [y1, . . . , ym]T y x = [x1, . . . , xn]T .Se define la matriz jacobiana como

∇f =

∂f1

∂x1· · · ∂f1

∂xn... . . . ...∂fm∂x1

· · · ∂fm∂xn

donde

yi = fi(x)

Para calcular esta matriz de forma numérica vamos a utilizar el vector auxiliar

xj =

x1...

xj + ε...xn

De esta forma tenemos que la columna j de la matriz jacobiana queda

(∇f)j ≈f (xj)− f (x)

ε

Mientras más pequeño sea el valor de ε mejor será la aproximación, pero hay que teneren cuenta que a la hora de programarlo, valores demasiado pequeños pueden ser ignoradosdebido al redondeo. En nuestro caso tomaremos ε = 10−10 .

55

C. Programas y modelos de Simulink

x

3Fxyz (N)

Mxyz (N-m)

Ve (m/s)

Xe (m)

φ θ ψ (rad)

DCMbe

Vb (m/s)

ω (rad/s)

dω/dt

Ab (m/s2)

BodyQuaternion

FixedMass

6DoF (Quaternion)

3

3

3

3

3

3

3

3

[3x3]

3

a

3

du/dt

Derivative

3

3

w

3

Ve

3

ae

3

ae

ve

xe

DCM

w

Ab

Señales3

3

[3x3]

3

3

3

Terminator13

Terminator23

ab

3

F

M

Momentos y Fuerzas

3

3

XY Graph

Terminator

3

100

DisplayClock

Figura 62: signal_KF_Aero.slx

PulseGenerator

Sine Wave

-K-

Gain

3

3

Sine Wave1-K-

Gain13

3

3

3

Sine Wave2

Sine Wave3

Sine Wave4

Sine Wave5

1F

3

2M

3

F1

3

M1

3

Product

Terminator

-C-

Constant

Figura 63: Momentos y Fuerzas

56

sV

Estado real16

3

93

3Interpreted

MATLAB Fcnx v a

9

9

qDCMbeDCM2q

4

[3x3]4

DCM

[3x3]

5w

3

3xe

3

2ve

3

1ae

3

6Ab

3

InterpretedMATLAB Fcn

Ruido

12

12

16

9

7

4 7

3

zV

Medida12

12

2

333

Scope

3Interpreted

MATLAB FcnInterpreted MATLAB

Function2

10

3

3

4

3

10

InterpretedMATLAB Fcn

VOR

2

3

In1 Out1

Presion

3

Figura 64: Señales

1In1

31

Out1Selector

3h (m)

T (K)a (m/s)P (Pa)

ρ (kg/m3)ISA

ISA Atmosphere Model Terminator

Terminator1

Terminator2

-K-

Gain

0

Display

101.3

Display1

Figura 65: Presión

57

ukf_exp_p.m

% programa para e j e c u t a r UKFclear a l l

%% v a r i a b l e s

n = 16 ; % tamaño d e l v e c t o r de es tadonh = 9 ; % tamaño d e l v e c t o r de medidaAt = 0 . 5 ; % incremento de tiempoN = f ix (100/At) ; % no de puntoskc = 0 ; % puntos has ta e l f a l l o d e l GPS

% pos i c i on de l a s antenas VORp = [ 0 , 0 ] ;np = s ize (p , 1 ) ; % cant idad de antesa VOR

% matriz de covar ianza d e l e s tadoQ1 = diag ( [ 1 e−4∗ones (1 , 4 ) ,1 e−3∗ones (1 , 3 ) ] ) ; % covar ianza de l a a c t i t u d

qq = [1 e1 , 1 e−3,1e−1] ; % sin GPSQ2 = diag ( [ qq , qq , qq . ∗ [ 1 e−5,1e0 , 1 e −1 ] ] ) ;

qq = [1 e−7,1e−7,1e−0] ; % con GPSQ3 = diag ( [ qq , qq , qq . ∗ [ 1 e−1,1e−5,1e−1 ] ] ) ;

multip = 1 ;% matriz de covar ianza de l a medidarw = 0.01∗multip ; % des . t i p i c a d e l g i ro scoporx = 3∗multip ; % des . t i p i c a d e l GPSra = 0.05∗multip ; % des . t i p i c a d e l ace lerometrorv = 0.1∗multip ; % des . t i p i c a d e l VORrP = 2e−1∗multip ; % des . t i p i c a d e l barometroR = diag ( [ rw^2∗ ones (1 , 3 ) , rx ^2∗ ones (1 , 3 ) , ra ^2∗ ones (1 , 3 ) , rP , rv∗ ones (1 ,2∗np) ] ) ;

R1 = R( 1 : 3 , 1 : 3 ) ; % covar ianza de g i roscopoR2 = R(4:10+2∗np ,4:10+2∗np) ; % covar ianza de pos i c i onR3 = R(7:10+2∗np ,7:10+2∗np) ; % covar ianza de pos i c i on s in GPS

m1 = @(q ) 0.5∗ [−q (2 ) ,−q (3 ) ,−q (4 ) ;q (1 ) ,−q (4 ) , q (3 ) ;q (4 ) , q (1 ) ,−q (2 ) ;−q (3 ) , q (2 ) , q (1 ) ] ;

fq = @(x ) [m1(x ( 1 : 4 ) ) ∗ [ x (5 ) ; x (6 ) ; x (7 ) ]∗At + [ x (1 ) ; x (2 ) ; x (3 ) ; x (4 ) ] ; . . .x (5 ) ; x (6 ) ; x (7 ) ] ; % ecuacion de t r a n s i c i o n de es tados angu lar

fx = @(x ) [ x (1 ) + x (2) ∗At + 0.5∗ x (3 ) ∗At^2;x (2 ) + x (3) ∗At ;x (3 ) ] ; % ecuacion de t r a n s i c i o n de es tados 1D

fp = @(x ) [ fx ( x ( 1 : 3 ) ) ; fx ( x ( 4 : 6 ) ) ;fx ( x ( 7 : 9 ) ) ] ; % ecuacion de t r a n s i c i o n de es tados 3D

f = @(x ) [ fq ( x ( 1 : 7 ) ) ; fp ( x ( 8 : 1 6 ) ) ] ;

hq = @(x )x ( [ 5 , 6 , 7 ] ) ; % funcion de medida d e l g i ro scopo

ha = @(x , y ) quat ro ta te ( ( y ( 1 : 4 ) ’ ) , x ( [ 3 , 6 , 9 ] ) ’ ) ’ ; % funcion de medida d e l ace lerometro

58

hv = @(x ) [ sin (atan2 ( ( x (2 )−p ( : , 2 ) ) , ( x (1 )−p ( : , 1 ) ) ) ) ; cos (atan2 ( ( x (2 )−p ( : , 2 ) ) , . . .( x (1 )−p ( : , 1 ) ) ) ) ] ; % funcion de medida d e l VOR

hP = @(h) 101 .325∗ ( (288 .15 −6 .5∗h/1000) /288 .15) ^ . . .−(9.80665/(−6.5/1000∗287) ) ; % funcion de medida d e l barometro

hp = @(x , y ) [ x ( [ 1 , 4 , 7 ] ) ; ha (x , y ) ; hP(x (7 ) ) ; hv (x ( [ 1 , 4 ] ) ) ] ; % funcion de medida de pos i c i on

h = @(x ) [ hq (x ( 1 : 7 ) ) ; hp (x ( 8 : 1 6 ) , x ( 1 : 4 ) ) ] ; % funcion de medida

% e j ecuc ion de s imu l i k para ob tener l o s r e s u l t a d o s de medida y es tadoi f ~exist ( ’zV ’ , ’ var ’ )

sim ( ’ signal_KF_Aero . s l x ’ ,N∗At)sV = sV ’ ;zV = zV ’ ;

end

s = sV ( : , 1 ) ; % cond ic iones i n i c i a l e sx1 = s ( 1 : 7 ) ; % c . i . de a c t i t u d para UKFx2 = x1 ; % c . i . de a c t i t u d para EKFx3 = s ( 8 : 1 6 )+sV (8 : 1 6 , 2 ) . ∗ [ 0 , 0 , 1 , 0 , 0 , 1 , 0 , 0 , 1 ] ’ ; % c . i . de po s i c i on para UKFx4 = x3 ; % c . i . de po s i c i on para EKF

% covar ianza d e l es tado i n i c i a lP1 = eye (7 ) ∗0 . 0 01 ; % de a c t i t u d para UKFP2 = P1 ; % de a c t i t u d para EKFP3 = eye (9 ) ∗0 . 0 01 ; % de pos i c i on para UKFP4 = P3 ; % de pos i c i on para EKF

N = length ( sV) ; % numero de pasos t o t a l e s

%% proceso

xV1 = zeros (n ,N) ;xV2 = zeros (n ,N) ;xV1 ( : , 1 ) = [ x1 ; x3 ] ; % guardar l a es t imacionxV2 ( : , 1 ) = [ x2 ; x4 ] ; % guardar l a es t imacion

% reordenar cua tern ionesfor k = 2 :N

[~ , ind ] = max(abs ( sV ( 1 : 4 , k ) ) ) ;i f sign ( sV( ind , k ) ) ~= sign ( sV( ind , k−1) ) ;

sV ( 1 : 4 , k ) = −sV ( 1 : 4 , k ) ;end

end

P5 = P4 ;for k=2:N

z = zV ( : , k ) ; % medida

i f k <= kc | | k >= N−kc+1% b l u c l e con GPSz1 = z ( 1 : 3 ) ; % medida a c t i t u d[ x1 , P1 ] = ukf fun2 ( fq , x1 , P1 , hq , z1 ,Q1 ,R1) ; % ukf[ x2 , P2 ] = ekf fun22 ( fq , x2 , P2 , hq , z1 ,Q1 ,R1) ; % e k f con jacob iano numerico

59

% normal i zac ion d e l cuatern ionx1 ( 1 : 4 ) = quatnormal ize ( x1 ( 1 : 4 ) ’ ) ’ ;x2 ( 1 : 4 ) = quatnormal ize ( x2 ( 1 : 4 ) ’ ) ’ ;

z2 = z ( 4 :end) ; % medida de pos i c i onh3 = @(x )hp(x , x1 ( 1 : 4 ) ) ;h4 = @(x )hp(x , x2 ( 1 : 4 ) ) ;[ x3 , P3 ] = ukf fun2 ( fp , x3 , P3 , h3 , z2 ,Q3 ,R2) ; % ukf[ x4 , P4 ] = ekf fun22 ( fp , x4 , P4 , h4 , z2 ,Q3 ,R2) ; % e k f con jacob iano numerico

else% buc l e s in GPSz1 = z ( 1 : 3 ) ; % medida de a c t i t u d[ x1 , P1 ] = ukf fun2 ( fq , x1 , P1 , hq , z1 ,Q1 ,R1) ; % ukf[ x2 , P2 ] = ekf fun22 ( fq , x2 , P2 , hq , z1 ,Q1 ,R1) ; % e k f con jacob iano numerico% normal i zac ion d e l cuatern ionx1 ( 1 : 4 ) = quatnormal ize ( x1 ( 1 : 4 ) ’ ) ’ ;x2 ( 1 : 4 ) = quatnormal ize ( x2 ( 1 : 4 ) ’ ) ’ ;

z2 = z ( 7 :end) ; % medida de pos i c i onh3 = @(x ) [ ha (x , x1 ( 1 : 4 ) ) ; hP(x (7 ) ) ; hv (x ( [ 1 , 4 ] ) ) ] ;h4 = @(x ) [ ha (x , x2 ( 1 : 4 ) ) ; hP(x (7 ) ) ; hv (x ( [ 1 , 4 ] ) ) ] ;[ x3 , P3 ] = ukf fun2 ( fp , x3 , P3 , h3 , z2 ,Q2 ,R3) ; % ukf[ x4 , P4 ] = ekf fun22 ( fp , x4 , P4 , h4 , z2 ,Q2 ,R3) ; % e k f con jacob iano numerico

end

xV1 ( : , k ) = [ x1 ; x3 ] ; % guardar l a es t imacionxV2 ( : , k ) = [ x2 ; x4 ] ; % guardar l a es t imacion

end

[ yaw , pitch , r o l l ] = quat2angle ( sV ( 1 : 4 , : ) ’ ) ;ar = [ yaw , pitch , r o l l ] ’ ; % angu los r e a l e s

[ yawe , p i tche , r o l l e ] = quat2angle (xV1 ( 1 : 4 , : ) ’ ) ;ae = [ yawe , p i tche , r o l l e ] ’ ; % angu los est imados UKF[ yawe , p i tche , r o l l e ] = quat2angle (xV2 ( 1 : 4 , : ) ’ ) ;ae2 = [ yawe , p i tche , r o l l e ] ’ ; % angu los est imados EKF2

%% repre sen tac ion de l o s r e s u l t a d o s

tpau = 0 . 0 1 ; % tiempo de pausavt = ( 0 :N−1)∗At ; % vec to r de tiempo

% cambiar l o s c o l o r e s por d e f e c t oco = [0 0 1 ;

0 0 .5 0 ;1 0 0 ;0 0 .75 0 . 7 5 ;0 .75 0 0 . 7 5 ;0 .75 0 .75 0 ;0 .25 0 .25 0 . 2 5 ] ;

set ( groot , ’ de faultAxesColorOrder ’ , co )

% cuatern ionesf igure (1 )set ( gcf , ’name ’ , ’Quat ’ , ’ numbert i t l e ’ , ’ o f f ’ )for k=1:4

60

subplot (2 , 2 , k )plot ( vt , sV(k , : ) , ’− ’ , vt , xV1(k , : ) , ’−− ’ , vt , xV2(k , : ) , ’−− ’ )t i t l e (num2str(k−1, ’q_%1.0 f ’ ) )i f k == 1 , legend ( ’ Real ’ , ’UKF’ , ’EKF’ ) , end

endshg , pause ( tpau )

% angu los de e u l e rf igure (2 )set ( gcf , ’name ’ , ’Ang Euler ’ , ’ numbert i t l e ’ , ’ o f f ’ )for k=1:3

subplot (3 , 1 , k )plot ( vt , ar (k , : ) , ’− ’ , vt , ae (k , : ) , ’−− ’ , vt , ae2 (k , : ) , ’−− ’ )i f k == 1 , t i t l e ( ’ Ángulos de Euler ’ ) , endi f k == 1 , legend ( ’ Real ’ , ’UKF’ , ’EKF’ ) , endylim ([−pi , pi ] ∗ 1 . 1 )i f k == 1 , ylabel ( ’Yaw ’ ) , endi f k == 2 , ylabel ( ’ Pitch ’ ) , endi f k == 3 , ylabel ( ’ Ro l l ’ ) , xlabel ( ’ t ’ ) , end

endshg , pause ( tpau )

% v e l o c i d a d e s angu laresf igure (3 )set ( gcf , ’name ’ , ’V Ang ’ , ’ numbert i t l e ’ , ’ o f f ’ )for k=5:7

subplot (3 , 1 , k−4)plot ( vt , sV(k , : ) , ’− ’ , vt , xV1(k , : ) , ’−− ’ , vt , xV2(k , : ) , ’−− ’ , vt , . . .

zV(k−4 , : ) , ’ . ’ )t i t l e (num2str(k−4, ’ \\omega_%1.0 f ’ ) )i f k == 5 , legend ( ’ Real ’ , ’UKF’ , ’EKF’ , ’Medida ’ ) , end

endshg , pause ( tpau )

nom = ’ xyz ’ ;

% x , v , afor kk = 1 :3

f igure ( kk+3)set ( gcf , ’name ’ ,nom(kk ) , ’ numbert i t l e ’ , ’ o f f ’ )k = (kk−1)∗3 + 8 ;

subplot ( 3 , 1 , 1 )plot ( vt , sV(k , : ) , ’− ’ , vt , xV1(k , : ) , ’−− ’ , vt , xV2(k , : ) , ’−− ’ , . . .

vt ( [ 1 : kc ,N−kc :N] ) , zV(3+kk , [ 1 : kc ,N−kc :N] ) , ’ . ’ )t i t l e (nom(kk ) )legend ( ’ Real ’ , ’UKF’ , ’EKF’ , ’Medida ’ )

subplot ( 3 , 1 , 2 )plot ( vt , sV(k+1 , : ) , ’− ’ , vt , xV1(k+1 , : ) , ’−− ’ , vt , xV2(k+1 , : ) , ’−− ’ )t i t l e ( [ ’v_ ’ nom(kk ) ] )

subplot ( 3 , 1 , 3 )plot ( vt , sV(k+2 , : ) , ’− ’ , vt , xV1(k+2 , : ) , ’−− ’ , vt , xV2(k+2 , : ) , ’−− ’ )t i t l e ( [ ’a_ ’ nom(kk ) ] )

shg , pause ( tpau )

61

end

% errore sf igure (7 )set ( gcf , ’name ’ , ’ Error ’ , ’ numbert i t l e ’ , ’ o f f ’ )for kk = 1 :3

subplot (3 , 1 , kk )k = (kk−1)∗3 + 8 ;errU = sV(k , : )−xV1(k , : ) ;errE = sV(k , : )−xV2(k , : ) ;MSEU = sum( errU .^2 ) / length ( errU ) ;MSEE = sum( errE .^2 ) / length ( errE ) ;fpr intf ( ’RMSD(UKF) = %4.3 f ; RMSD(EKF) = %4.3 f \n ’ , sqrt (MSEU) , sqrt (MSEE) )plot ( vt , abs ( errU ) , ’− ’ , vt , abs ( errE ) )ylabel ( [ ’ $ | ’ ,nom(kk ) , ’ $−$\hat{ ’ ,nom(kk ) , ’ } | $ ’ ] , ’ f o n t s i z e ’ ,16 , ’ i n t e r p r e t e r ’ , ’ l a t e x ’ )gridi f kk == 1 , legend ( ’UKF’ , ’EKF’ ) , end

endxlabel ( ’ t ’ )shg , pause ( tpau )

% pres ionf igure (8 )set ( gcf , ’name ’ , ’ Pres ion ’ , ’ numbert i t l e ’ , ’ o f f ’ )[ ~ ,~ , pres , ~ ] = atmosisa ( sV ( 1 4 , : ) ) ;plot ( vt , zV ( 1 0 , : ) , vt , pres /1000)t i t l e ( ’Medida pre s i on ’ )shg , pause ( tpau )

% angu los vorf igure (9 )angvor = zeros (2∗np ,N) ;for k = 1 :N, angvor ( : , k ) = hv (sV ( [ 8 , 1 1 ] , k ) ) ; endset ( gcf , ’name ’ , ’Medida VOR’ , ’ numbert i t l e ’ , ’ o f f ’ )plot ( vt , (zV(end−2∗np+1:end , : ) ) , ’ . ’ )hold onset (gca , ’ ColorOrderIndex ’ , 1)plot ( vt , angvor )hold o f fgridshg , pause ( tpau )

% pos vorf igure (10)set ( gcf , ’name ’ , ’ Pos VOR’ , ’ numbert i t l e ’ , ’ o f f ’ )plot ( sV ( 8 , : ) , sV ( 1 1 , : ) , p ( : , 1 ) , p ( : , 2 ) , ’ o ’ )legend ( ’ Trayector ia ’ , ’ Antena VOR’ )xlabel ( ’ x ’ ) , ylabel ( ’ y ’ )gridaxis equalshg , pause ( tpau )

% pos est imadaf igure (11)set ( gcf , ’name ’ , ’ Pos Estimada ’ , ’ numbert i t l e ’ , ’ o f f ’ )plot ( sV ( 8 , : ) , sV ( 1 1 , : ) , xV1 ( 8 , : ) , xV1 ( 1 1 , : ) , xV2 ( 8 , : ) , xV2 ( 1 1 , : ) , . . .

zV ( 4 , [ 1 : kc ,N−kc :N] ) ,zV ( 5 , [ 1 : kc ,N−kc :N] ) , ’ . ’ )

62

legend ( ’ Real ’ , ’UKF’ , ’EKF’ , ’GPS ’ )xlabel ( ’ x ’ ) , ylabel ( ’ y ’ )gridaxis equalshg , pause ( tpau )

% alphaf igure (12)plot ( vt , atan2 ( angvor ( 1 , : ) , angvor ( 2 , : ) ) )xlabel ( ’ t ’ ) , ylabel ( ’ \ alpha [ rad ] ’ )set ( gcf , ’name ’ , ’ Alpha ’ , ’ numbert i t l e ’ , ’ o f f ’ )grid

% da/ dtf igure (13)plot ( vt ( 1 :end−1) , d i f f (atan2 ( angvor ( 1 , : ) , angvor ( 2 , : ) ) ) . / d i f f ( vt ) )t i t l e ( ’d\ alpha /dt ’ )xlabel ( ’ t ’ )set ( gcf , ’name ’ , ’ dAlpha dt ’ , ’ numbert i t l e ’ , ’ o f f ’ )grid

% r e s e t e a r l o s c o l o r e s por d e f e c t oset ( groot , ’ de faultAxesColorOrder ’ , ’ remove ’ )

63

ukffun2.m

function [ xs , Ps ] = ukf fun2 ( f , xe , Pe , h , z ,Q,R)%{Entradas :f : func ion de t r a n s i c i o n de es tadosxe : estado estimado i n i c i a lPe : matr iz de covar ianza de l a e s t imac ion i n i c i a lh : func ion de medidaz : medidaQ: matr iz de covar ianza de l a medidaR: matr iz de covar ianza de l estado

Sa l i d a s :xs : estado estimadoPs : matr iz de covar ianza de l estado estimado

%}

%% cons tan te s

L = length ( xe ) ; % tamaño d e l v e c t o r de es tadoac = 1e−3; % v a r i a b l e a lphakc = 0 ; % v a r i a b l e kappabc = 2 ; % v a r i a b l e be talambda = ac ^2∗(L+kc)−L ; % v a r i a b l e lambda

%% reordenamiento de l a s v a r i a b l e s de entrada

xe = xe ( : ) ; % estado a n t e r i o rz = z ( : ) ; % medida

%% Predicc ion

maux = sqrt (L+lambda )∗ chol (Pe ) ’ ; % matriz a u x i l i a r r a i z cuadradaX0 = [ xe xe ( : , ones (1 , length ( xe )))+maux . . .

xe ( : , ones (1 , length ( xe )))−maux ] ; % puntos sigma en torno a xe

Ws = [ lambda /(L+lambda ) , 1/ (2∗ (L+lambda ) )∗ ones (1 ,2∗L ) ] ; % pesos d e l es tadoWc = [ lambda /(L+lambda ) + (1−ac^2+bc ) , . . .

1/(2∗ (L+lambda ) )∗ ones (1 ,2∗L ) ] ; % pesos de l a covar ianza

X1 = zeros ( s ize (X0 ) ) ;for k = 1:2∗L+1

X1 ( : , k ) = f (X0 ( : , k ) ) ; % propagacion de l o s puntos sigma segun fend

x1 = (Ws∗X1 ’ ) ’ ; % pred i c c i on d e l e s tado

P1 = zeros ( length ( x1 ) ) ;for k = 1:2∗L+1

% matriz de covar ianza d e l e s tado est imadoP1 = P1 + Wc(k )∗ (X1 ( : , k)−x1 )∗ (X1 ( : , k)−x1 ) ’ ;

endP1 = P1 + Q;

%% Actua l i z ac i on

64

% maux = s q r t (L+lambda )∗ cho l (P1 ) ’ ; % matr iz a u x i l i a r r a i z cuadrada% X2 = [ x1 x1 ( : , ones (1 , l e n g t h ( x1 )))+maux . . .% x1 ( : , ones (1 , l e n g t h ( x1 )))−maux ] ; % puntos sigma en torno a x1

X2 = X1 ;

gm = zeros ( length ( z ) , length (X2 ) ) ;for k = 1:2∗L+1

gm( : , k ) = h(X2 ( : , k ) ) ; % proyecc ion de l o s puntos sigma segun hend

z2 = (Ws∗gm’ ) ’ ; % pred i c c i ón de l a medida

Pzz = zeros ( length ( z2 ) ) ;Pxz = zeros ( length ( x1 ) , length ( z2 ) ) ;for k = 1:2∗L+1

% matriz de covar ianza de l a p red i c c i on de l a medidaPzz = Pzz + Wc(k )∗ (gm( : , k)−z2 )∗ (gm( : , k)−z2 ) ’ ;% matriz de covar ianza cruzada d e l e s tado y l a medidaPxz = Pxz + Wc(k )∗ (X1 ( : , k)−x1 )∗ (gm( : , k)−z2 ) ’ ;

endPzz = Pzz + R;

K = Pxz/Pzz ; % ganancia de Kalman

xs = x1 + K∗( z − z2 ) ; % estado a c t u a l i z a d o

Ps = P1 − K∗Pzz∗K’ ;

65

ekffun22.m

function [ xs , Ps ] = ekf fun22 ( f , xe , Pe , h , z ,Q,R)%{Entradas :f : func ion de t r a n s i c i o n de es tadosxe : estado estimado i n i c i a lPe : matr iz de covar ianza de l a e s t imac ion i n i c i a lh : func ion de medidaz : medidaQ: matr iz de covar ianza de l a medidaR: matr iz de covar ianza de l estado

Sa l i d a s :xs : estado estimadoPs : matr iz de covar ianza de l estado estimado

%}

%% reordenamiento de l a s v a r i a b l e s de entrada

xe = xe ( : ) ; % estado a n t e r i o rz = z ( : ) ; % medida

%% Calcu lo de F

ep = 1e−10; % incremento de xF = zeros ( length ( xe ) ) ;for k = 1 : length ( xe )

vp = xe ;vp (k ) = vp (k)+ep ; % estado mas e l incrementoF( : , k ) = ( f ( vp)− f ( xe ) )/ ep ; % cons t rucc ion de F por columnas

end

%% Predicc ion

x1 = f ( xe ) ; % pred i c c i on d e l e s tadoP1 = F∗Pe∗F’ + Q; % matriz de covar ianza de l a es t imacion

%% Calcu lo de H

H = zeros ( length ( z ) , length ( x1 ) ) ;for k = 1 : length ( x1 )

vp = x1 ;vp (k ) = vp (k)+ep ; % estado mas e l incrementoH( : , k ) = (h(vp)−h( x1 ) )/ ep ; % cons t rucc ion de H por columnas

end

%% Actua l i z ac i on

K = P1∗H’ / (H∗P1∗H’ + R) ; % ganancia de Kalmanxs = x1 + K∗( z − h( x1 ) ) ; % a c t u a l i z a c i o n d e l e s tadoPs = (eye ( s ize (K∗H)) − K∗H)∗P1 ; % a c t u a l i z a c i o n de l a matiz de cobvar ianza

66

Referencias[1] Basic concept of Kalman filtering.svg [Imagen]. St. Petersburg (FL): Wikimedia

Foundation, Inc. 2011. [Consulta: 7 de septiembre de 2015]. Disponible en: https://commons.wikimedia.org/wiki/File:Basic_concept_of_Kalman_filtering.svg

[2] Esteban Roncero, Sergio; Gavilán Jiménez, Francisco. Mecánica del Vuelo del Avión.Parte I: Actuaciones del Avión. [en línea]. Sevilla: Universidad de Sevilla, 2009.[Consulta: 7 de septiembre de 2015]. Disponible en: http://aero.us.es/AVE/archivos/Y0910/Tema5_parteI.pdf

[3] Vázquez Valenzuela, Rafael. Vehículos Espaciales y Misiles. Tema 3: Cinemática yDinámica de la Actitud. Parte I: Cinemática [en línea]. Sevilla: Universidad de Sevilla,2012. [Consulta: 7 de septiembre de 2015]. Disponible en: http://www.aero.us.es/vem/files1112/t3p1.pdf 2.2, A

[4] Flat Earth to LLA. [en línea]. The MathWorks, Inc. 2015. [Consulta: 7 deseptiembre de 2015]. Disponible en: http://es.mathworks.com/help/aeroblks/flatearthtolla.html 2.1

[5] iPhone 4 Gyroscope Teardown. [Imagen]. Ifixit, 2015. [Consulta: 7 de septiembre de2015]. Disponible en: https://www.ifixit.com/Teardown/iPhone+4+Gyroscope+Teardown/3156 10

[6] Esmat Bekir. Introduction to Modern Navigation Systems. Singapore: WorldScientific Publishing, 2007. ISBN-13 978-981-270-765-9 3.4

[7] Weisstein, Eric W. Quaternion. [en línea]. Wolfram Research, Inc. 2015 [Consulta:7 de septiembre de 2015]. Disponible en: http://mathworld.wolfram.com/Quaternion.html

[8] Eric A. Wan; Rudolph van der Merwe. The Unscented Kalman Filter forNonlinear Estimation. [en línea]. Beaverton: Oregon Graduate Institute of Science &Technology. [Consulta: 7 de septiembre de 2015]. Disponible en: https://www.seas.harvard.edu/courses/cs281/papers/unscented.pdf 16, 4.2

[9] Gabriel A. Terejanu. Unscented Kalman Filter Tutorial [en línea]. Buffalo: Universityat Buffalo. [Consulta: 7 de septiembre de 2015]. Disponible en: https://cse.sc.edu/~terejanu/files/tutorialUKF.pdf

[10] Jouni Hartikainen; Arno Solin; Simo Särkkä. Optimal Filtering with Kalman Filtersand Smoothers [en línea]. Espoo, Finland: Aalto University School of Science, 2011.[Consulta: 7 de septiembre de 2015]. Disponible en: http://becs.aalto.fi/en/research/bayes/ekfukf/documentation.pdf

67

(Página en blanco)

68