Kalman Para Drones

7/29/2019 Kalman Para Drones

1/67

Desarrollo, implementacin y pruebade un filtro de Kalman del tipo UKFpara un vehculo areo no tripulado

HERNANDO LEYTON VLic. en Matemticas y Fsica

Ing. Electricista

Director: Carlos Mario Vlez S., Dr.

Maestra en Matemticas Aplicadas

Departamento de Ciencias Bsicas

Escuela de Ciencias y Humanidades

Medelln - Colombia

Mayo 13 de 2009


2/67


3/67

Nota de aceptacin

________________________________________

________________________________________

________________________________________

________________________________________

________________________________________

________________________________________

Francisco Ivn Zuluaga

Coordinador de la Maestra

________________________________________

Carlos Mario Vlez S.

Director del proyecto

Medelln, mayo 13 de 2009


4/67


5/67

Dedicatoria

Arduo anlisis, cooperacin y constancia han sido las piezas claves para la realizacin de este documento.Dedico su finalizacin a la Universidad San Buenaventura quien me brindo apoyo econmico, abri un

espacio de trabajo conjunto con la Universidad EAFIT y me permiti combinar mi labor docente con laejecucin de mi maestra. A mi tutor, el Dr. Carlos Mario Vlez, por su constante acompaamientocientfico y la correccin final del manuscrito, el cual refleja su capacidad y experiencia en el rea. Y a mifamilia y amigos por su apoyo incondicional y su buen nimo.


6/67


7/67

Resumen

En este trabajo se presenta el desarrollo matemtico, diseo, implementacin en Matlab/Simulink y anlisisde una versin del filtro de Kalman denominada Filtro de Kalman Unscented (UKF), para la estimacinptima y en tiempo real (Chow et al., 2007) de las variables de estado de inters (posicin, velocidad yactitud) de un mini-helicptero robot a partir de las medidas ruidosas indirectas que de l se conocen(posicin y velocidad a partir del GPS y rumbo dado por un magnetmetro). El trabajo hace parte delProyecto Colibr (2009) del grupo de investigacin "Sistemas de Control Digital" de la Universidad EAFIT deMedelln, en el cual se ha construido una plataforma (mini-helicptero, avinica y software) para laaplicacin de diversos mtodos matemticos.

A partir de una revisin bibliogrfica exhaustiva sobre el filtro de Kalman y sus aplicaciones a aeronaves,se hace el desarrollo matemtico del filtro UKF con la versin original propuesta por Julier y Uhlmann(1997) en lo referente al manejo de los puntos sigma y la Transformacin Unscented" (UT), herramientasmatemticas fundamentales propuestas por ellos para la extensin del Filtro de Kalman a sistemas nolineales. Despus, y mediante la utilizacin de bibliografa ms aplicada, como los trabajos de Oh yJohnson (2006) y Vlez y Agudelo (2006), se implementa y documenta el filtro completamente enMatlab/Simulink (Matlab, 2009; Simulink, 2009) con la posibilidad de ejecucin en tiempo real dentro de lametodologa de prototipado rpido de software (Vlez y Agudelo, 2006).

A partir de las pruebas realizadas en diferentes condiciones (Teixeira et al, 2008) se pudo concluir que el

filtro UKF brinda mejores resultados que los obtenidos con el Filtro de Kalman Extendido (EKF) utilizado enla actualidad en el proyecto Colibr. Se puede resaltar del UKF implementado la invariabilidad de sucomportamiento frente a la variacin de las condiciones iniciales del vector de estado y la matriz decovarianzas, as como la reduccin en el valor del error absoluto de las componentes del vector de estadoen cada instante y el error cuadrtico medio (RMSE). Como nica desventaja podemos destacar elaumento del costo computacional frente al EKF debido al elevado nmero de operaciones para cada unode los vectores de la matriz de puntos sigma.


8/67


9/67

Desarrollo, implementacin y prueba de un filtro de Kalman del tipo UKF para un vehculo areo no tripulado ix

Contenido

Dedicatoria .................................................................................................................................................... vResumen ...................................................................................................................................................... viiContenido ..................................................................................................................................................... ixLista de figuras ............................................................................................................................................ xiLista de tablas ............................................................................................................................................ xiiiCaptulo 1. Introduccin ............................................................................................................................... 1

1.1 Descripcin y formulacin del problema ........................................................................................................... 31.2 Objetivos ........................................................................................................................................................... 31.2.1 Objetivo general ...................................................................................................................................... 3

1.2.2 Objetivos especficos .............................................................................................................................. 41.3 Justificacin ...................................................................................................................................................... 41.4 Limitaciones de la investigacin ....................................................................................................................... 41.5 Hiptesis o pregunta de investigacin .............................................................................................................. 51.6 Planteamiento metodolgico ............................................................................................................................ 5

1.6.1 Tipo de investigacin .............................................................................................................................. 51.6.2 Mtodo de investigacin ......................................................................................................................... 51.6.3 Tcnicas e instrumentos para la recoleccin de la informacin .............................................................. 5

1.7 Usuarios potenciales ........................................................................................................................................ 61.8 Modelo dinmico del mini-helicptero .............................................................................................................. 6

1.8.1

Componentes del modelo matemtico .................................................................................................... 8

1.8.2 Ecuaciones de movimiento ..................................................................................................................... 91.8.3 Componentes de fuerzas y momentos ................................................................................................. 11

1.9 Organizacin del trabajo ................................................................................................................................. 12Captulo 2. Filtro de Kalman Unscented (UKF) ..................................................................................... 14

2.1 Introduccin .................................................................................................................................................... 142.2 El filtro Kalman ............................................................................................................................................... 142.3 La Transformacin Unscented (UT) ............................................................................................................. 18


10/67

Desarrollo, implementacin y prueba de un filtro de Kalman del tipo UKF para un vehculo areo no tripulado x

2.4 Filtro de Kalman Unscented (UKF) ................................................................................................................. 202.5 Anlisis del filtro UKF ..................................................................................................................................... 232.6 Resumen del capitulo ..................................................................................................................................... 24

Captulo 3. Resultados y anlisis en la implementacin y aplicacin del UKF ..................................... 253.1 Introduccin .................................................................................................................................................... 253.2 Codificacin del UKF en Matlab/Simulink ....................................................................................................... 253.3 Pruebas y ANLISIS de resultados ................................................................................................................ 31

3.3.1 Efecto de la seleccin del tiempo de muestreo sT ............................................................................... 333.3.2 Efecto de la seleccin de la matriz de covarianzas inicial 0P .............................................................. 353.3.3 Obtencin y comparacin de los valores RMSE para los EKF y UKF .................................................. 373.3.4 Anlisis del tiempo de operacin .......................................................................................................... 44

3.4 Resumen del captulo ..................................................................................................................................... 44Captulo 4. Conclusiones y recomendaciones ......................................................................................... 45Anexo ........................................................................................................................................................... 48

Bibliografa .................................................................................................................................................. 51ndice ........................................................................................................................................................... 54


11/67

Desarrollo, implementacin y prueba de un filtro de Kalman del tipo UKF para un vehculo areo no tripulado xi

Lista de figuras

Figura 1.1 Ejemplo en 2D del comportamiento de los puntos sigma. Grfica tomada de Wan y Van derMerwe (2001). ......................................................................................................................................... 2

Figura 1.2 ngulos de Euler. .......................................................................................................................... 6Figura 1.3 Mini-helicptero X-Cell en vuelo ................................................................................................... 7Figura 1.4 Modelo dinmico del mini-helicptero implementado en Matlab/Simulink .................................... 7Figura 1.5 Fuerzas externas y momentos que actan en el helicptero ........................................................ 9Figura 3.1 Modelo del mini-helicptero trabajado en el proyecto Colibr....................................................... 26Figura 3.2 Filtro EKF incorporado al modelo del mini-helicptero trabajado en el proyecto Colibr .............. 26Figura 3.3 Modelo final en Matlab Simulink del filtro UKF ............................................................................. 27Figura 3.4 Implementacin en Matlab Simulink de los puntos sigma ........................................................... 27Figura 3.5 Elemento Implementado en Matlab/Simulink para la determinacin dex(k|k-1) ....................... 28Figura 3.6 Implementacin en Matlab Simulink para la prediccin de la matriz de covarianzas | 1P k k

.............................................................................................................................................................. 28Figura 3.7 Implementacin en Matlab Simulink de la etapa de prediccin del UKF ..................................... 28Figura 3.8 Elemento Implementado en Matlab/Simulink para la determinacin de | 1y k k ................. 29Figura 3.9 Implementacin en Matlab Simulink para la determinacin de

yyP ............................................. 29Figura 3.10 Elemento Implementado en Matlab Simulink para la determinacin de xyP ............................. 29Figura 3.11 Implementacin en Matlab Simulink para la determinacin de |K k k ................................ 30Figura 3.12 Implementacin en Matlab Simulink para la correccin del vector de estado |x k k ........... 30Figura 3.13 Implementacin en Matlab Simulink para la correccin de la matriz de covarianzas |P k k 30Figura 3.14 Implementacin en Matlab Simulink de la etapa de correccin del UKF .................................. 31Figura 3.15 Parmetros y condiciones iniciales de trabajo para el UKF ..................................................... 32Figura 3.16 Comparacin de las trayectorias en XY del UKF y EKF con la referencia para un Ts=0.01 s .. 33Figura 3.17 Comparacin de las trayectorias en XY del UKF y EKF con la referencia para un Ts=0.02 s .. 34Figura 3.18 Comparacin de las trayectorias en XY del UKF y EKF con la referencia para un Ts=0.04 s .. 34Figura 3.19 Comparacin de las trayectorias en XY del UKF y EKF con la referencia para un Ts=0.08 s .. 35Figura 3.20 Comparacin de las trayectorias en XY del UKF y EKF con la referencia para 0 0P ............ 36


12/67

Desarrollo, implementacin y prueba de un filtro de Kalman del tipo UKF para un vehculo areo no tripulado xii

Figura 3.21 Comparacin de las trayectorias en XY del UKF y EKF con la referencia para 0P diag

((5/3)2; (5/3)2; (7/3)2; 1; 1; (5/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2;(0.01/3)2; (0.01/3)2; (0.01/3)2;

(0.01/3)2; (0.01/3)2; (0.01/3)2 ). .............................................................................................................. 36Figura 3.22 Comparacin de las trayectorias en XY con Po=0 para el EKF y Po= diag((5/3)2; (5/3)2;

(7/3)2;1;1;(5/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2;(0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2;(0.01/3)2; (0.01/3)2 ) para el UKF. .......................................................................................................... 37

Figura 3.23 Comparacin de la componente x para el EKF Y UKF con la referencia ................................ 38Figura 3.24 Comparacin de la componente y para el EKF Y UKF con la referencia ................................ 38Figura 3.25 Comparacin de la componente zpara el EKF Y UKF con la referencia ................................. 39Figura 3.26 Comparacin de las velocidades en x para el EKF Y UKF con la referencia .......................... 39Figura 3.27 Comparacin de las velocidades en y para el EKF Y UKF con la referencia .......................... 40Figura 3.28 Comparacin de las velocidades en z para el EKF Y UKF con la referencia .......................... 40Figura 3.29 Comparacin de los ngulos de roll, pitch y yaw del modelo del mini-helicptero con el UKF . 41Figura 3.30 Comparacin de los ngulos de roll, pitch y yaw del modelo del mini-helicptero con el EKF . 41Figura 3.31 Estimacin de los errores de posicin x , y yz en los filtros EKF y UKF ............................... 42Figura 3.32 Estimacin de los errores de velocidad xv , yv y zv en los filtros EKF y UKF .......................... 43Figura 3.33 Estimacin de los errores en las medidas de los ngulos de roll, pitch y yaw en los filtros EKF y

UKF ....................................................................................................................................................... 43


13/67

Desarrollo, implementacin y prueba de un filtro de Kalman del tipo UKF para un vehculo areo no tripulado xiii

Lista de tablas

Tabla 1.1 Parmetros del modelo del mini-helicptero .................................................................................. 9Tabla 3.1 Parmetros utilizados para el UKF en bibliografa relacionada con el control de aeronaves ...... 32Tabla 3.2 Comparacin de los errores RMSE en los filtros EKF y UKF de las componentes de posicinvelocidad y ngulos en el mini-helicptero ............................................................................................ 42


14/67

Desarrollo, implementacin y prueba de un filtro de Kalman del tipo UKF para un vehculo areo no tripulado 1

Captulo 1. Introduccin

En la navegacin de un vehculo autnomo como un UAV (Unmanned Aerial Vehicle), la posicin yvelocidad pueden ser determinadas con un sistema de posicionamiento global (Global Positioning

System- GPS) y el rumbo con un magnetmetro. Sin embargo, los errores asociados a estas medidas nopermiten que ste sea el nico sistema de medida utilizado en la aeronave. Surge entonces la necesidadde implementar herramientas que determinen el estado del sistema (posicin, velocidad y actitud oinclinacin) en cualquier instante y realicen las acciones de control necesarias para cumplir con una tarea otrayectoria determinada en forma ptima y recursiva, teniendo en cuenta el modelo interno del sistema. Laaplicacin de un filtro de Kalman es clave en esta etapa, ya que permite la fusin de la informacin delGPS, del magnetmetro y del sistema de referencia inercial (INS), compuesto por acelermetros ygirscopos, para determinar el estado del UAV.

El filtro de Kalman apareci a principios de la dcada de los sesenta y es un mtodo muy utilizado paraestimar de manera ptima los estados de un sistema dinmico en tiempo real, a partir de las medidasruidosas indirectas que de l se van tomando (Chow et al., 2007). Estas estimaciones en tiempo real del

estado del sistema son valiosas en s mismas cuando el sistema opera en lazo abierto; sin embargo, si seconsidera la operacin de lazo cerrado, las mismas pueden ser utilizadas para un control que mantenga laaeronave en el vuelo deseado (Aguirre-Gil et al., 2005).

En la actualidad, el manejo de modelos, en su mayora no lineales, ha hecho necesario adaptar o extendernuevas versiones del filtro de Kalman para que trabajen con la linealizacin de los modelos o permitan elmanejo de densidades de distribucin no necesariamente lineales (Pascual, 2006). Dentro de lasmodificaciones antes mencionadas tenemos el filtro extendido de Kalman (Extended Kalman Filter - EKF)y el Filtro de Kalman Unscented(Unscented Kalman Filter - UKF), que no presenta traduccin definidaal espaol. La idea esencial del EKF fue propuesta por Schmidt (1976) y fue utilizada hasta hace pococomo la opcin estndar en aplicaciones no-lineales (Grewal y Andrews, 2001). Se caracteriza por

responder a las alinealidades del sistema equivalente linealizado empleando sus ltimos valores estimadosy asumiendo que el error provocado al ignorar los trminos de orden superior es muy pequeo encomparacin con los trminos de primer orden. La prdida de trminos de orden superior puede evitarseusando todas las ecuaciones no lineales, aunque una descripcin completa del estado condiciona ladensidad de probabilidad a requerir de un elevado nmero de parmetros no siempre bien determinados(Oh y Johnson, 2006).


15/67


El UKF, propuesto inicialmente por Julier y Uhlmann (1997) y luego perfeccionado por Wan y Van derMerwe (2001), es una extensin del filtro de Kalman que se emplea para la estimacin de sistemas no

lineales sin tener que linealizar el sistema como en el EKF. En este caso se implementa un mtododenominado transformacin unscented que se utiliza para calcular los primeros momentos de la densidadde distribucin de probabilidad de una variable aleatoria y(x) que resulta de aplicar una transformacin nolinealfa una variable aleatoriax de estadstica conocida.

Un conjunto de puntos, denominados puntos sigma, se eligen de forma tal que su media y covarianzacoincidan con la esperanza y covarianza de la variable aleatoria x. Luego, la transformacin no lineal fseaplica a cada punto sigma para generar una nube de puntos transformados. En la figura 1.1 se visualiza laidea del comportamiento de la media y la covarianza de estos puntos sigma mediante su comparacin conlos valores reales y la aproximacin obtenida con los EKF. De la estadstica de los puntos transformadosse pueden estimar ahora la esperanza y covarianza de la variable aleatoria =f() (Pascual, 2006; Vander Merwe y Wan, 2001).

Figura 1.1 Ejemplo en 2D del comportamiento de los puntos sigma. Grfica tomada de Wan y Van der Merwe (2001).

Este enfoque da al UKF mejores caractersticas de convergencia y precisin para los sistemas no linealesque las obtenidas con el EKF. Adems, con el UKF se maneja directamente la informacin de los

instrumentos de medida sin requerir de un procesamiento adicional, como el cmputo de la matrizjacobiana utilizada en el EKF (Oh y Johnson, 2006). Otra importante ventaja del UKF que lo hace atractivopara su implementacin en aeronaves (sistemas inherentemente no lineales) es la habilidad para estimarparmetros con bastante precisin en las alinealidades del sistema.

Actualmente, el mini-helicptero X-Cell 60 del Proyecto Colibr (2009) de la Universidad EAFIT de Medellnopera con base en la estimacin de variables obtenidas mediante un filtro EKF. Las medidas tomadas de laUnidad de Medicin Inercial o IMU (velocidad angular y la aceleracin en cada eje del vehculo), el GPS


16/67


(posicin geodsica y velocidades respecto a la Tierra) y el magnetmetro (rumbo), son empleadas paraobtener, mediante ecuaciones diferenciales no lineales, la posicin, velocidad y actitud (ngulos de Euler)

de la aeronave respecto al sistema de coordenados fijo a la Tierra (sistema de navegacin NED: North-East-Down). La IMU est formada por tres acelermetros y tres girscopos encargados de entregarle alSistema Inercial de Navegacin (INS) los datos de aceleracin y velocidad respectivamente en cada eje(Vlez 2006; Vlez y Agudelo, 2006).

En el presente trabajo, mediante la utilizacin de Matlab/Simulink, se implement el UKF como unobservador de estado aplicado a las ecuacionescinemticas que modelan el mini-helicptero X-Cell 60con el fin de mejorar su desempeo y operacin, ya que su utilizacin est orientada principalmente arealizar entre otras actividades: monitoreo ambiental, inspeccin y vigilancia de redes, inspecciones detrfico, bsqueda y rescate, fotografa area, inteligencia y reconocimiento areo. Este trabajo es unacontinuacin de los trabajos realizados previamente dentro del grupo en el tema de la estimacin delestado (lvarez, 2005; Vlez, 2007).

1.1 DESCRIPCIN Y FORMULACIN DEL PROBLEMA

El proyecto Colibr del grupo de investigacin "Sistemas de Control Digital" de la Universidad EAFIT deMedelln tiene entre sus objetivos aprovechar las extensas posibilidades del programa Matlab (con suscajas de herramientas o "toolboxes") para el modelado, identificacin, adaptacin y control de sistemas. Delos proyectos y desarrollos investigativos del grupo surgi el inters por disear un control que logremantener una trayectoria deseada y la actitud del helicptero, es decir, la posicin con respecto al sistemade coordenadas de navegacin NED.

El filtro utilizado por el grupo en la actualidad es un EKF que se basa en la informacin de un gradiente deprimer orden derivado de un sistema no lineal, que no presenta una ptima precisin por ser unaaproximacin de primer orden. Con la implementacin del UKF se incorpora la informacin de un gradientede orden superior, mejorando as la precisin. Adems, con el UKF se maneja directamente la informacinde los instrumentos de medida sin requerirse de un procesamiento adicional, como el cmputo de la matrizjacobiana (Oh y Johnson, 2006), aunque por lo extenso del mtodo, el procesamiento adicional sedestinaria al clculo y procesamiento de los puntos sigma.

1.2 OBJETIVOS

1.2.1 Objetivo general

Disear, implementar, analizar y probar el comportamiento en simulacin de un filtro de KalmanUnscented (UKF) sobre el modelo de un mini-helicptero robot.


17/67


1.2.2 Ob jeti vo s esp ecfic os

Presentar un desarrollo terico de los Filtros Kalman Unscented (UKF) Desarrollar tericamente el UKF para un mini-helicptero robot

Obtener el diagrama de simulacin en Matlab/Simulink del filtro UKF

Simular el control del mini-helicptero mediante la utilizacin del UKF en Matlab

Analizar el comportamiento del UKF en la simulacin y documentar los resultados

1.3 JUSTIFICACIN

La teora de control no lineal y sus aplicaciones en la estimacin del estado en tiempo real para losmodelos cada vez ms complejos en ingeniera, se ha visto estimulada por el desarrollo tecnolgico de lacomputacin, que mediante el clculo numrico facilita dichos anlisis e implementaciones. Un casoespecfico de las aplicaciones est relacionado con los vehculos areos no tripulados (UAV) autnomos,que permiten aplicar el filtro de Kalman para la estimacin del estado y, seguramente, con una aplicacinposterior al control automtico. El proyecto Colibr brinda la posibilidad de desarrollar investigaciones atravs de la matemtica aplicada, buscando un buen nivel de desarrollo en lo referente a la modelacin y elcontrol de mini-helicpteros. En este proyecto se desarrolla una de las tcnicas de estimacin del estadoms actuales (el UKF) para el mini-helicptero X-Cell 60 del Proyecto Colibr (2009), permitiendo laaplicacin y comparacin de avanzados mtodos matemticos y su aplicacin a un problema real.

1.4 LIMITACIONES DE LA INVESTIGACIN

El proyecto est orientado a la implementacin de un UKF en Matlab/Simulink y su simulacin paradiferentes condiciones iniciales con el fin de analizar su desempeo frente al EKF existente en laactualidad, para considerar su posible implementacin dentro del sistema de control del mini-helicpterodel proyecto Colibr. En las simulaciones no se recurre a pruebas con todos los datos reales, por lasmayores dificultades inherentes a la toma de ellos (calibracin de sensores, problemas electrnicos,

reduccin de ruidos) y por el alcance matemtico de este trabajo.


18/67


1.5 HIPTESIS O PREGUNTA DE INVESTIGACIN

Es el UKF una mejor alternativa de estimacin del estado de un mini-helicptero robot que el EKF usadoen la actualidad en el proyecto Colibr?

1.6 PLANTEAMIENTO METODOLGICO

1.6.1 Tipo de investig acin

Esta investigacin es de tipo aplicada, ya que el UKF implementado en Matlab se utilizar en la simulacindel comportamiento de la aeronave en trayectorias y condiciones especficas. Sin embargo, la aplicacinrequiere de un enfoque completamente matemtico basado en la teora del filtro.

1.6.2 Mtodo de in vest ig ac in

En el desarrollo de la investigacin se siguieron las siguientes fases:

Revisin bibliogrfica sobre el tema

Estudio del modelo del mini-helicptero con el fin de unificar la estructura del modelo y las variablesutilizadas en su definicin

Desarrollo matemtico del filtro y, con base en los resultados, implementacin del modelo enMatlab/Simulink

Documentacin de la versin final del UKF

Simulaciones del UKF para diferentes condiciones iniciales con el fin de validar en Matlab/Simulink suaplicacin al control del mini-helicptero

Redaccin del documento final

1.6.3 Tcn icas e ins trumento s para la reco lecc in de la inf ormacin

Se utilizaron artculos cientficos, libros especializados y la asesora directa del director del proyecto, eldoctor Carlos Mario Vlez S. Para la implementacin del UKF y su simulacin se trabaj con Matlab ydiferentes herramientas, algunas desarrolladas por el grupo de investigacin.


19/67


1.7 USUARIOS POTENCIALES

El filtro UKF desarrollado puede ser utilizado por ingenieros que quieran profundizar en el manejo desistemas no lineales y su extensin a otros campos de investigacin, as como por estudiantes de cienciasque deseen ver la aplicacin directa de las teoras matemticas y leyes fsicas en procesos reales.Tambin la industria, que empieza a creer en el material humano de este pas, puede considerar estosdesarrollos para iniciar procesos de innovacin tecnolgica e investigacin en el rea de la estimacin delestado y los sensores virtuales.

1.8 MODELO DINMICO DEL MINI-HELICPTERO

Los helicpteros son vehculos que pueden ser representados como sistemas complejos no lineales conmltiples entradas y mltiples salidas (MIMO) y con fuertes modos acoplados, lo que hace que sumodelado matemtico y control presenten un alto grado de dificultad y sea un sistema muy interesantepara su estudio por medio de diferentes estrategias de control (Aguirre-Gil, 2005).

Estos sistemas funcionan bajo un amplio rango de condiciones aerodinmicas, pero en los helicpterosminiatura muchos de estos efectos no son tenidos en cuenta por la gran influencia que presenta el rotor enel modelo, as como los momentos casi instantneos en los que el rotor controla las entradas. Una versinmenos compleja del modelo se puede encontrar con mtodos de identificacin mientras se encuentra envuelo estacionario, que consiste en mantener al helicptero en el aire con la misma orientacin y altitudconstantes; dicha actitud est determinada por los ngulos del alabeo o "roll", cabeceo o "pitch" y guiada

o "yaw", los cuales se conocen tambin como ngulos de Euler (figura 1.2).

Figura 1.2 ngulos de Euler.

En este documento se presentan las ecuaciones que modelan el comportamiento del mini-helicpteroX-Cell 60 presentado en la figura 1.3 y que es utilizado para el desarrollo del proyecto Colibr de laUniversidad EAFIT de Medelln. Las pruebas del UKF se realizan con dicho modelo. El modelo del mini-helicptero fue desarrollado en Matlab/Simulink por el grupo de investigacin "Sistemas de Control Digital"y se presenta en la figura 1.4; ver (Gavrilets, 2003) y (Vlez, 2006) para mayores detalles.


20/67


Figura 1.3 Mini-helicptero X-Cell en vuelo

Figura 1.4 Modelo dinmico del mini-helicptero implementado en Matlab/Simulink

GndFlag

12

Omega

11

a1 b1

10

u v w

9

(vx vy vz )ECEF8

(x y z)ECEF

7

x y z

6

p q r

5

ax ay az

4

roll pitch yaw

3

vx vy vz

2

lat lon alt

1

Translational

equations

p q rX Y ZgDCMMa

u v w

ax ay az

Total Moment

MmrMtrMempQe

L M N

Total Force

FmrFtrFfusFemp

X Y Z

Tail rotor and

empennage

Deltasp q ru v wwindOmegarhoV i_mr

Ftr

Femp

Mtr

Memp

Qtr

Rotational

equations

Ix Iy Iz Ixz

L M N

p q r

p' q' r'

Powerplant

p' q' r'QmrQtrFuel_outthrottle

Qe

Omega

Main rotor

Deltas

p q ru v wwindOmegarho

Fmr

MmrQmr

V i_mr

a1 b1

Integration and

axis transformation

u v w

p q r

x y z

vx vy vz

Lat Lon Alt

(x y z )ECEF

(vx vy vz )ECEF

Euler

DCM

Ground flag

Inertia

MaIx Iy Iz Ixz

Fuel_out

Fuselage

u v w

wind

V i_mr

rho

Ffus

Earth

g

rho

Wind

3

Throttle

(0,1)

2

Deltas (-1,1)

1


21/67


A continuacin se presenta en detalle el desarrollo de las ecuaciones que describen el modelo matemticodel mini-helicptero, as como el anlisis de sus componentes de fuerzas y momentos.

1.8.1 Com ponentes del modelo matemtico

El modelo matemtico del mini-helicptero, con el que se trabaja en este proyecto, se basa en el modelopropuesto por Gavrilets (2003) y Padfield (1996) y se desarroll usando la teora bsica de un helicptero,teniendo en cuenta las caractersticas particulares de uno en miniatura; usando datos obtenidos enpruebas a partir de experimentos sencillos de vuelo se determinaron parmetros como los radios de losrotores, la masa y la velocidad del viento entre otros por medio de mediciones directas; mientras quealgunos como los momentos de inercia, el radio de influjo y la velocidad inducida fueron estimados.

La exactitud del modelo fue verificada en pruebas de simulacin en tiempo real con un piloto de

helicpteros de radiocontrol y actualmente se estn usando comparaciones entre las respuestaspredictivas de ste y las respuestas colectadas durante las pruebas.

El mini-helicptero es un sistema con ocho grados de libertad correspondientes a tres desplazamientos,tres rotaciones y dos aleteos del rotor principal, y es modelado como un sistema de cinco entradas y quincevariables de estado, como se explica a continuacin.

Las entradas de control se establecen con el vector presentado en (1.1) (lvarez y Vlez, 2007; Gavrilets,2003; Vlez, 2005)

col lon lat r t, , , , 1.1T

Cuyas componentes corresponden respectivamente al colectivo del rotor principal, cclico longitudinal,

cclico lateral, rotor de cola y acelerador.El vector de estado es (en realidad , x, y, z se pueden omitir, pero se mencionan aqu con carcterinformativo)

1 1, , , , , ,,,, , , , , ,T

mru v w p q r x y z a b (1.2)

Y sus componentes se definen como:

, ,u v w : Velocidad del vehculo en el marco de referencia del cuerpo.

, ,p q r : Velocidades angulares del vehculo en el marco de referencia del cuerpo.

,, : ngulos de Euler (roll, pitch, yaw) que determinan la actitud del vehculo.

, ,x y z : Posicin del vehculo en el marco de navegacin

1 1, ( , )lon lat a b : ngulos de aleteo ("flapping") longitudinal y lateral del rotor principal.

mr : Velocidad angular del rotor principal.

Los momentos de inercia alrededor de los ejes del cuerpo de la aeronave que pasan por el centro degravedad del vehculo fueron determinados usando un experimento de tensin pendular; los momentos de


22/67


inercia en el cruce axial fueron difciles de medir sin un dispositivo de balanceo, y como son usualmentecercanos a cero no fueron tenidos en cuenta (Molinar-Monterrubio, 2007).

El rotor principal y el rotor de cola del X-Cell, as como la barra estabilizadora, tienen lminas simtricaspara aire ("airfoils"), donde su curva de inclinacin fue estimada de acuerdo a sus respectivos aspectos devariacin. La torsin efectiva en el vuelo estacionario fue estimada por la proporcin angular de respuestapara los pasos de comando en el cclico.

1.8.2 Ecuaciones de mo vimiento

Empezamos presentando en la figura 1.5 (Gavrilets, 2003) algunas de las variables, fuerzas y momentosdel mini-helicptero que son utilizados en la construccin del modelo, as como en la tabla 1.1 lapresentacin de los parmetros que no han sido definidos, con sus unidades respectivas.

Figura 1.5 Fuerzas externas y momentos que actan en el helicptero

Tabla 1.1 Parmetros del modelo del mini-helicptero

Parmetro Descripcin Unidadesm Masa Kg

I Momento de inercia Kg.m2

e Constante de tiempo para el rotor S

Radio de avance M

, ,w w wu v w Componentes de la velocidad del aire en , ,x y z m/s

v fF Fuerza vertical N

fusF Fuerza sobre el fuselaje N

mrT Empuje del rotor principal. N

t rT Empuje del rotor trasero N


23/67


Las ecuaciones de Newton-Euler se derivan del principio de la conservacin del momentum lineal yangular. Dichas ecuaciones se presentan en (1.3) y (1.4) para un vehculo de masa constante y

momento de inercia , aunque es necesario tener en cuenta que en el modelo real la masa es variable yaque el combustible se consume. En las siguientes ecuaciones , ,X Y ZF es el vector de fuerzas

actuando en el centro de gravedad del vehculo y , ,L M NM es el vector de momentos externosalrededor de los ejes x , y y z (Mettler, 2003).

1.3d

mdt

v

F

1.4d

Idt

w

M

En un helicptero las fuerzas externas y los momentos son producidos por el rotor principal y el rotor decola; por ello las fuerzas gravitacionales aerodinmicas del fuselaje se pueden presentar como en laecuacin (1.5) y para la superficie trasera como en la ecuacin (1.6).

1.5m m v v w F

1.6I I w w w M

Los vectores [ , , ]Tu v wv y [ , , ]Tp q rw tambin son los encargados de describir el movimientotraslacional y rotacional del helicptero con respecto a sus tres ejes de referencia como se muestra en lasecuaciones diferenciales de (1.7), con las que podemos determinar la aceleracin lineal y angular delsistema.

(1.7)

yy zz

xx xx

zz xx

yy yy

xx yy

zz zz

I IX Lu vr wq p qr

m I I

I IY Mv wp ur q pr

m I I

I IZ Nw uq vp r pq

m I I

Estas ecuaciones describen un sistema de aceleraciones en el que [L, M,N] son funciones no lineales

que determinan el estado de la aeronave y en las siguientes ecuaciones, Q es el torque producido por elmotor para contrarrestar el torque aerodinmico en las aspas del rotor principal.

En (1.8) se establecen las anteriores ecuaciones en funcin de las entradas de control y las variables deestado segn lo planteado en Gavrilets (2003):


24/67


sin( )

mr fusX Xu vr wq g

m

sin( )cos( )

mr fus tr vf Y Y Y Y v wp ur g

m

cos( )cos( )

mr fus ht Z Z Zw uq vp g

m

yy zz mr vf trxx xx

I I L L Lp qr

I I

1.8zz xx mr ht

yy yy

I I M M

q pr I I

xx yy vf tr ezz zz

I I N N Qr pq

I I

1 1 11

1 w w lonlon

e e mr mr z mr mr e

u u w wa a aa q

R R

1 11

1 w lat lat

e e v mr mr e

v vb bb p

R

Por ltimo, para la definicin de las ecuaciones de movimiento del mini-helicptero es necesario establecer,como en (1.9), la relacin existente entre la variacin de los ngulos de Euler y las velocidades angularesde la aeronave.

1 tan( )sin( ) tan( )cos( )

0 cos( ) sin( ) 1.9

0 sin( ) / cos( ) cos( ) / cos( )

p

q

r

1.8.3 Compon entes de fuerzas y momentos

Las componentes de las fuerzas y de los momentos esenciales para el modelo requieren asumir que parael empuje del rotor el influjo es constante y uniforme. De acuerdo con Padfield (1996), se establecern lossiguientes supuestos y expresiones para el modelo:

La constante de tiempo para la respuesta transitoria del influjo (modelo de primer orden) en vueloestacionario presentado en (1.10) es inversamente proporcional al producto del radio del influjo con lavelocidad angular del rotor principal y est dado por:


25/67


0.849

1.104 trim mr

La velocidad inducida en condicin de vuelo estacionario se determina con la densidad del aire como:

2.

1.112

imr

mr

m gV

R

El coeficiente de empuje presentado en (1.12) est definido por el empuje del rotor Tque omitiendo elsubndice mr se determina por:

2 2 1.12T

TC

R R

Por ltimo, la solucin iterativa presentada para el radio de influjo y el coeficiente mximo del empuje seobtienen de las ecuaciones (1.13) y (1.14).

0

22

0

1.132

T

w Z

C

maxmax

2 21.14T

TC

R R

Donde se calcula con (1.15) y z corresponde a la componente normal del flujo de aire calculada con

(1.16).

2 2

1.15wind wind u u v v

R

1.16windzw w

R

1.9 ORGANIZACIN DEL TRABAJO

En el Captulo 1 se definieron las caractersticas del trabajo, sus objetivos y alcances, as como tambin sedescribi el punto de partida del proyecto mediante el planteamiento del modelo del mini-helicptero y susecuaciones dinmicas.

En el Captulo 2 se desarrolla la teora bsica de los Filtros Kalman y el mtodo de la TransformacinUnscented, mtodos necesarios para el desarrollo de los Filtros Kalman Unscented.


26/67


En el Captulo 3 se presenta como resultado el desarrollo del UKF para el modelo del mini-helicptero consu respectiva implementacin en Matlab/Simulink y una relacin de pruebas, esto con el fin de validar el

modelo del UKF y su significativa superioridad frente al modelo existente del EKF.Por ltimo en el Capitulo 4 se presentan las conclusiones y recomendaciones deducidas delcomportamiento del filtro UKF en el modelo del mini-helicptero utilizado en el proyecto Colibr de laUniversidad EAFIT de Medelln.


27/67


Captulo 2. Filtro de KalmanUnscented (UKF)

2.1 INTRODUCCIN

En este captulo se desarrollar un estudio detallado del Filtro Kalman y de una de sus extensiones a lossistemas no lineales correspondiente al Filtro Kalman Unscented (UKF), a travs de la utilizacin de laTransformacin Unscented (UT). Con dicho estudio se establecern la dinmica del filtro y suscaractersticas para luego implementar el filtro en Matlab/Simulink y probarlo y analizarlo en simulacin conel modelo del mini-helicptero.

2.2 EL FILTRO KALMAN

El filtro de Kalman es un algoritmo recursivo, ptimo, de procesado de datos que minimiza el ndice delerror cuadrtico y permite estimar los estados de un sistema ms apropiadamente. El error cuadrtico

medio o RMSE de su traduccin al ingls Root Mnimum Mean-Squared Error representa la medida tpicadel error de prediccin y es particularmente sensible a las discrepancias grandes entre el valor real y elvalor predicho. Este error se calcula con la expresin

2

1

12.1

Nmod ref

i i i

i

RMSE x x xN


28/67


Donde modix es el valor obtenido por el modelo yref

ix ser el valor tomado como real o valor de referencia

(el error se utilizar en las pruebas finales realizadas en el captulo 3 al modelo del filtro UKF

implementadas en Matlab/Simulink).La evolucin de un sistema se puede expresar en el espacio de estados como (se omite la negrita paravectores y matrices):

( 1) ( ) ( ) ( )

( ) ( ) ( ) (2.2)

x k A x k Bu k v k

y k Cx k w k

En el cual:

( )x k es elestado.

( )y k es la salida.( )w k es el proceso estocstico asociado a la medida.

( )v k es el proceso estocstico asociado al sistema., ,A B C son las matrices determinsticas que definen la dinmica del sistema.

( )v k y ( )w k tambin son habitualmente conocidos como el ruido de proceso y el ruido de la medidarespectivamente. El primer ruido ( )v k afecta la dinmica del modelo y representa los aspectos nomodelados de la dinmica de la planta real, mientras el segundo ruido ( )w k influye sobre la salidaaccesible del modelo y representa la incertidumbre propia del proceso de medida de la salida de la plantareal. Adems, el filtro Kalman debe establecer las siguientes condiciones para los ruidos de proceso, de lamedida y la condicin inicial (0)x en trminos de la esperanza (definida en la ecuacin 2.8):

0[ (0)]

[ ( )] [ ( )] 0; 0

[ (0), ( )] [ (0), ( )] 0; 0

[ ( ), ( )] 0; 2.3

[ ( ), ( )] ( )

[ ( ), ( )] ( )

[ (0), ( )] (0); 0

E x x

E w k E v k k

E x w k E x v k k

E v k w k k

E w k w k R k

E v k v k Q k

E x x k P k

Donde ( )Q k y ( )R k representarn en adelante las matrices de covarianza del ruido del proceso y de lamedida respectivamente. Dichas matrices son diagonales simtricas y se calculan segn (2.4).

, 2.4Cov x y E x E x y E y Tambin se debe garantizar que la condicin inicial y los ruidos del proceso y de la medida seanmutuamente no correlacionados (Pascual, 2006); tal condicin se presenta en (2.5).


29/67


0 , 0 , 0; 0x v kP Cov x v k k

0 , 0 , 0; 0 2.5x w kP Cov x w k k

, , 0; , 0v k w k P Cov v k w k k j Con lo anterior, el objetivo principal del empleo de un filtro Kalman es estimar en cada instante 1k el

estado x k de un sistema, empleando la sucesin de entrada conocida 0: 0 1, ,...,k ku u u u y las

observaciones 1: 1 2, ,...,k ky y y y (Kalman, 1960).

Si se supone que x k (o |x k k ) es la estimacin en el instante kdel estado, el filtro Kalman buscarobtener este valor de manera que se minimice el error cuadrtico medio (RMSE).

Al considerar el error como

2.6e k x k x k

el objetivo es minimizar P k que en adelante se identificar como la matriz de covarianza del error y sedefine en (2.7) como:

2.7TP k E e k e k

La estimacin ptima presentada en (2.8) viene dada entonces por la esperanza condicional

1: 1:| ( | ) . | 2.8RMSE

k k k k k k k kx E x y x P x y dx

que requiere del conocimiento de la densidad de probabilidad a posteriori 1:|k kP x y que se calculamediante la utilizacin de la constante de normalizacin 1: 1|k kP y y y mediante el enfoque Bayesianopuede calcularse recursivamente como:

1: 1

1:

1: 1

1: 1 1 1 1| 1 1

1: 1 1: 1

| ||

|

| | | 2.9

| | |

k k k k

k k

k k

k k k k k k k

k k k k k k k

P x y P y xP x y

P y y

P x y P x x P x y dx

P y y P x y P y x dx

Este proceso recursivo permite calcular la densidad de probabilidad del estado actual en funcin de la

densidad previa y su medida ms reciente. 1|k kP x x y |k kP y x se determinan a partir de P v k y P w k respectivamente, la entrada kU y las expresiones presentadas en (2.2).Como el clculo de 1: 1|k kP y y (entendida en este documento como yyP ) es de gran dificultad, sepropone aplicar el filtro de Kalman para el modelo inicial representado en las ecuaciones (2.2) con


30/67


densidades de distribucin gaussianas y caracterizado por un vector de esperanza E x x y una matriz

de covarianzas P k definida en (2.7).

Llamando | 1x k k a la prediccin del estado calculado del valor ms actualizado del estado

1| 1x k k y | 1P k k a la prediccin de la matriz de covarianzas calculada de la matriz de

covarianzas ms actualizada 1| 1P k k , lo que propone el filtro de Kalman para su parte predictiva(ecuaciones (2.10) y (2.11)) y correctiva (ecuaciones (2.12), (2.13) y (2.14)) se muestra a continuacin.

Prediccin:

| 1 1| 1 1 (2.10)x k k Ax k k Bu k

| 1 1| 1 (2.11)T

P k k AP k k A Q

Correccin:

1

| 1 | 1 (2.12)TK k CP k k CP k k C R

| | 1 | 1 (2.13)x k k x k k K k y k Cx k k

| | 1 (2.14)P k k I K k C P k k

En una aplicacin aeroespacial, la trayectoria nominal puede ser definida en el vuelo (on the fly), como lamejor estimacin corriente de la trayectoria actual. A este tipo de aproximacin se le conoce como filtro deKalman extendido o EKF de su traduccin en ingles Extended Kalman Filter (Grewal y Andrews, 2001).Este filtro est destinado al manejo de modelos no lineales en donde las densidades de distribucin no sonnecesariamente gaussianas. En este caso la esperanza o primer momento de la distribucin se propaga atravs del modelo no lineal y el segundo momento de la distribucin o covarianza se propaga a travs de laversin linealizada del modelo, todo esto mediante la utilizacin de matrices jacobianas en sus procesosrecursivos. Como ventaja se tiene que las perturbaciones son incluidas nicamente en la estimacin de loserrores del estado (generalmente pequeas) y con la desventaja del aumento en el costo computacionaldebido al proceso de liberalizacin del modelo. De (Vlez, 2006), el EKF se puede presentar para su etapade prediccin como:

| 1 1| 1 , 1

| 1 1| 1 (2.15)T T

x k k f x k k u k

P k k AP k k A Q

Y para su etapa de correccin como:


31/67


1

| 1 | 1 | 1

| 1 | 1

| | 1 | 1

| | 1 (2.16)

, ,

T T

x k k x k k x k k

K k P k k C CP k k C R

x k k x k k K k y k h x k k

P k k I K k C P k k

f f hA B C

x U x

Donde K k es llamada Matriz Kalman encargada de la correccin del estado y fy hson los modelosmatemticos que representan respectivamente la dinmica del sistema y la salida del sistema.

El EKF es el filtro utilizado actualmente en el proyecto Colibr de la Universidad EAFIT de Medelln. Comouna alternativa diferente a la utilizada en el EKF para el clculo de las densidades de distribucin de

probabilidad de la variable aleatoria, se plantea la estimacin de la esperanza mediante la aplicacin deuna transformacin no lineal a una nueva variable aleatoria de estadstica desconocida. Dichoprocedimiento se denomina Transformacin Unscented (UT) por sus siglas en ingles UnscentedTransformation y es fundamental para el entendimiento del filtro de Kalman Unscented (UKF), objetivoprincipal de este trabajo y que ser desarrollado ms adelante con el fin de determinar sus principalescaractersticas y fortalezas.

2.3 LA TRANSFORMACIN UNSCENTED (UT)

El problema de predecir el estado futuro de un sistema puede ser expresado de la siguiente forma.Suponiendo quex es una variable aleatoria con media x y covarianza xxP ; una segunda variable aleatoria

y se relaciona conx a travs de la funcin no lineal

2.17y f x

A la funcin anterior debe calculrsele su media y y su covarianza yyP . La estadstica de la variable

transformada es consistente si se satisface la desigualdad encontrada por Julier y Uhlmann (2002 y 2004)en trminos de la covarianza y la esperanza matemtica presentada en (2.18).

0 2.18T

yyP E y y y y

Si no se cumple lo anterior se podr considerar a yyP sub-estimada. Sin embargo, la consistencia no

necesariamente implica buenos resultados ya que los clculos finales estn sujetos a la minimizacin delerror cuadrtico medio.


32/67


La UT es un novedoso mtodo utilizado para calcular la estadstica de una variable aleatoria que ha sidosometida a una transformacin no lineal. Este mtodo est fundamentado en la idea intuitiva que plantea

que es ms fcil aproximar una distribucin gaussiana que aproximar una funcin no lineal arbitraria(Uhlmann, 1994). El mtodo consiste en elegir un conjunto de puntos llamados puntos sigma, con lacondicin que su media y su covarianza coincida con las de la variable aleatoria del proceso. Luego, la

transformacin no-lineal fse aplica a cada punto sigma y a estos puntos y f x se les calcula suesperanza E y y covarianza P y .

Six es una variable aleatoria de dimensinL con esperanza E x x y covarianza xxP , entonces para

calcular la esperanza E y y y la covarianza yyP y P se construye la matriz de puntos sigma 0 con (2L + 1) vectores columna (Chow et al., 2007; Matthew et al., 2004), tales que:

0 x

; 1,..., (2.19)i xxi

x L P i L

; 1,..., 2i xxi L

x L P i L L

donde 2 L k L y (generalmente41 10 1

) determinan la dispersin de los puntos

alrededor de x , k es una constante relacionada con el parmetro de escala (generalmente 0 3k L )

y xxL P es la i-sima columna de la matriz cuadrada xxL P .El clculo de la esperanza y covarianza de la variable aleatoria tambin se puede lograr mediante lautilizacin de una descomposicin de Cholesky como se plantea en el trabajo de Oh y Johnson (2006) yser en definitiva la que se utilizar para la implementacin del Filtro UKF en Matlab/Simulink.

Se puede verificar que los puntos sigma capturan la esperanza y covarianza de x en las siguientesecuaciones que describen en forma simplificada la idea final del mtodo propuesto en Julier y Uhlmann(2002 y 2004):

2

0

2.20L

UT m

i i

i

x W x

2

0

2.21L T

UT c UT UT xx i i i xx

i

P W x x P

Donde ciW y m

iW son escalares que asignan un peso estadstico a la medida y dependen de los

parmetros , k y como se plantea en las ecuaciones (2.22). Los valores de los parmetros son


33/67


determinados segn el tipo de dispersin y la distribucin de probabilidad propuesta en el desarrollo de

cada modelo. Los pesos estadsticos ciW y m

iW se definen como:

0

mW

L

20 1 2.22c

WL

1

; 1,..., 22

m c

i iW W i LL

donde es el parmetro que incorpora el conocimiento que se tiene de antemano acerca de ladistribucin x (generalmente 2 si se trata de distribuciones Gaussianas). Ahora, la transformacin no-linealfse aplica a cada conjunto de puntos sigma con el fin de generar una nube de puntos transformados(2.23) y de su estadstica, estimar la esperanza (2.24).

; 0,..., 2 2.23i iY f i L

2

0

2.24L

UT m

i i

i

y y W Y

La covarianza, se determina mediante la utilizacin de las ecuaciones (2.6), (2.7) y los pesos asignados acada grupo de puntos sigma como se plantea en (2.25).

2

0

2.25L T

UT c UT UT

yy yy i i i

i

P P W Y y Y y

2.4 FILTRO DE KALMAN UNSCENTED (UKF)

El Filtro de Kalman unscented" puede considerarse como el resultado de incorporar la transformacinUnscented al filtro EKF con el fin de mejorar las aproximaciones que se hacen de los dos primerosmomentos de una variable aleatoria que resulta de propagar otra variable aleatoria tomada como

gaussiana a travs de la transformacin no lineal que representa al sistema (Pascual, 2006).Ahora, el clculo de los valores predichos y corregidos del filtro para el vector de estado y la matriz decovarianza respectivamente | 1x k k , | 1P k k , |x k k y |P k k implica propagar los puntossigma a travs del modelo como se explica a continuacin (Gove y Hollinger, 2005).


34/67


Se define una nueva variable aleatoria (2.26) llamada vector de estado aumentado ( 1)ax k , que incluyeel ruido del proceso y el ruido de la medida, con esperanza y covarianza dadas en (2.27) y (2.28).

.

1

1 1 2.26

1

a

x k

x k v k

w k

1

1 0 2.27

0

a

x k

x k

1 0 0

1 0 0 2.28

0 0

a P kP k Q

R

Por las caractersticas propias de este trabajo y por simplicidad, se simplifica el vector de estado x(k) al notener en cuenta los ruidos del proceso ni de la medida, igual a lo trabajado en Oh y Johnson (2006).

Lo primero que se hace es generar el conjunto de puntos sigma para cada paso k= 1,2,,N.

| 1 1| 1 1| 1 | 1 1| 1 | 1 2.29k k x k k x k k s k k x k k s k k

| 1 1| 1 2.30T

s k k Chol P k k

Dondes es una factorizacin de Cholesky en forma de matriz triangular inferior y es un parmetro decontrol de la medida de dispersin de la media.

Siguiendo la metodologa dada en el captulo 2.3, se pasan los puntos sigma a travs defy h, los modelosmatemticos que representan respectivamente la dinmica del sistema y la salida del sistema, obteniendorespectivamente la esperanza y la covarianza de la variable aleatoria x y la variable transformada y(Pascual, 2006).

Transformando el conjunto de puntos sigma a travs del modelo que representa la dinmica del sistema:

| 1 | 1 , 1 2.31X k k f k k U k Ahora calculando su esperanza y covarianza, a las que se denominarn ecuaciones de prediccin para elvector de estado y la matriz de covarianza respectivamente:


35/67


2

1: 1

0

2

0

1: 1

| 1 | 1 | (2.32)

| 1 | 1 | 1 | 1 | 1 (2.33)

, |

Lm

i i k k

i

L Tc

i i i

i

k k k

x k k W X k k E x y

P k k W X k k x k k X k k x k k Q

Cov x x y

los puntos sigma son transformados ahora a travs del modelo que representa la salida del sistema:

| 1 | 1 (2.34)Y k k h k k

Y con ellos se calculan la esperanza y covarianza como:

2

1: 10| 1 | 1 | (2.35)

Lm

i i k k iy k k W Y k k E y y

2

0

1: 1

| 1 | 1 | 1 | 1 (2.36)

, |

LTc

yy i i i

i

k k k

P W Y k k y k k Y k k y k k R

Cov y y y

Puesto que el objetivo del UKF es adaptar la UT a los EKF, slo falta definir la matriz de Kalman, para locual es necesario primero definir mediante la ecuacin (2.37) un nuevo elemento que se denomina lacovarianza cruzadaPxy :

2

0

1: 1

| 1 | 1 | 1 | 1 (2.37)

, |

LTc

xy i i i

i

k k k

P W X k k x k k Y k k y k k

Cov x y y

Con la covarianza se define la matriz de Kalman como:

1| (2.38)xy xyK k k P P

Finalmente se definen las ecuaciones de correccin para el vector de estado y la matriz de covarianzaplanteadas en (2.16) como:

1:

1:

| | 1 | | 1 | (2.39)

| | 1 | | , | (2.40)

k k

T

yy k k k

x k k x k k K k k y k y k k E x y

P k k P k k K k k P K k k Cov x x y

Obteniendo as el filtro UKF con su respectiva etapa de prediccin y correccin.


36/67


2.5 ANLISIS DEL FILTRO UKF

En las ecuaciones de prediccin y correccin para el EKF (2.15), (2.16) y para el UKF (2.31) a (2.40), sedestaca la ventaja del filtro UKF en la no linealizacin del sistema y con ello la no prdida de informacinimportante al momento de corregir y predecir el vector de estado y la matriz de covarianzas mediante lautilizacin de una tcnica de muestreo determinstica conocida como transformacin unscented, como semuestran en el trabajo de Hartikainen y Srkk (2008); dicha tcnica trabaja con un nmero mnimo depuntos alrededor de la media llamados puntos sigma, presentados en las ecuaciones (2.29) y (2.30).

Si bien la transformacin UT es similar a los mtodos de muestreo Monte Carlo, existe una diferenciafundamental en la toma de muestras, ya que estas no se toman al azar sino de acuerdo a un algoritmodeterminista. De esta forma, se captura informacin de alto orden de la distribucin de probabilidad,tomando slo un nmero pequeo de puntos como plantean Julier y Uhlmann (2005).

El resultado es un filtro que es ms preciso capturando la verdadera media y covarianza de los datostransformados a travs del modelo no lineal, evitando as el clculo analtico de jacobianos que pueden sercomplicados y de alto costo computacional en el caso de funciones complejas. Aunque el costocomputacional del uso del UKF no es que se reduzca, las operaciones son ms simples, repetitivas y ladesviacin estndar de sus aproximaciones son menores al 1% como se obtuvo en el trabajo de Gove yHollinger (2005).

Otra de las ventajas del filtro UKF consiste en la posibilidad de trabajar con diferentes valores para losparmetros , k y relacionados con el tipo de dispersin y el tipo de distribucin como se explic en laseccin 2.3. Es as como el parmetro k provee un grado de libertad extra al proceso que repercutedirectamente en la reduccin del error de prediccin y en la definicin positiva o no positiva de la matriz de

covarianzas Pyy como se trabaja en Julier y Uhlmann (1997); esta condicin es importante para lafactorizacin de Cholesky, ya que para dicha transformacin la matriz debe ser definida positiva comoplantean Oh y Johnson (2006).

Tal vez una desventaja que presenta el filtro UKF es la relacionada con el tiempo de operacin como seobtuvo en Teixeira, Santillo, Erwin y Bernstein (2008), ya que los clculos no slo se realizan a un vector

de estado, sino a los 2 1L vectores obtenidos de la matriz de puntos sigma, con L la dimensin delvector de estado.

Por ltimo, se plantea como ventaja del UKF la reduccin del error cuadrtico medio (RMSE) y el mejorseguimiento de la trayectoria de referencia. Estas ventajas sern estudiadas en la seccin 3.3 del siguientecaptulo, cuando se pruebe la implementacin final de filtro en Matlab/Simulink.

Es necesario establecer que para las estimaciones se usan los cuaterniones en lugar de los ngulos deEuler, pues evitan singularidades cuando el vehculo se encuentra en posiciones de cabeceo vertical. Uncuaternin ("quaternion") se puede tomar como una extensin de los nmeros complejos inventado por W.R. Hamilton (1843) y se define como un elemento de magnitud unitaria con cuatro componentes, en dondeuna de ella es real y con las otras tres forman se forma un vector en el espacio imaginario , ,i j kcomo sepresenta en (2.41).


37/67


0 1 2 3 0 1 2 3, , , 2.41q q q q q q i q j q k

2.6 RESUMEN DEL CAPITULO

En este captulo se present el desarrollo matemtico del filtro UKF mediante la utilizacin de latransformacin Unscented y los puntos sigma. Tambin se establecieron las componentes y ecuacionesque permitirn llevar a cabo la implementacin del filtro UKF en Matlab/Simulink. Por ltimo, y del anlisisrealizado al filtro en diferentes fuentes bibliogrficas, se pudo inducir que el comportamiento del UKFmejorar frente al EKF en lo referente a las variaciones de los parmetros y condiciones iniciales, aunqueel costo computacional temporal puede ser mayor debido al incremento en el nmero de clculos. Estas

caractersticas servirn de marco al anlisis en simulacin del filtro aplicado a la estimacin del estado delmini-helicptero, de lo que tratar el siguiente captulo.


38/67


Captulo 3. Resultados yanlisis en la implementacin y

aplicacin del UKF

3.1 INTRODUCCIN

En este captulo se describe la codificacin del UKF en Matlab/Simulink y se analiza su comportamiento ensimulacin en la estimacin del estado de un mini-helicptero y su aplicacin en el control en lazo cerradopara el seguimiento de una trayectoria circular. El control se realiza con reguladores PID (proporcional-integral-derivativo) que fueron desarrollados por el grupo de investigacin "Sistemas de Control Digital" ycuyos detalles se omiten aqu por estar fuera del alcance del presente trabajo. Se utiliza el mismo esquemadel diagrama utilizado por el grupo para el EKF, pero adaptndolo al UKF obtenido, lo que facilita el anlisisde los resultados y de las ventajas y desventajas del UKF con respecto al EKF. El anlisis se realizateniendo en cuenta los aspectos ms importantes mencionados en la bibliografa, algunos de los cuales semencionan al final del captulo 2.

3.2 CODIFICACIN DEL UKF EN MATLAB/SIMULINK

La codificacin en Matlab/Simulink se implement con base en las ecuaciones desarrolladas en el captuloanterior y en el trabajo de Oh y Johnson (2006). El mini-helicptero del proyecto Colibr de la UniversidadEAFIT de Medelln posee una caja de avinica que permite el clculo, mediante la solucin de ecuaciones


39/67


diferenciales no lineales, la posicin, velocidad y actitud del vehculo. La caja de avinica, sujeta al mini-helicptero por medio de unos aisladores de vibracin (amortiguadores), contiene una unidad de medicin

inercial (IMU), un GPS y un magnetmetro. El sistema de navegacin inercial (INS) est compuesto por elsistema de adquisicin de datos de los sensores y un filtro extendido de Kalman (EKF) que integraadecuadamente las distintas frecuencias y medidas en un nico estado para el vehculo. El sistema deadquisicin de datos, el EKF y el control fueron desarrollados en Matlab/Simulink, con un enfoque deprogramacin visual de fcil comprensin y con la posibilidad de generacin automtica del cdigo quereduce ostensiblemente el tiempo de desarrollo.

El modelo en Matlab/Simulink con el que se trabaja se presenta en la figura 3.1 y es el punto de partidapara cualquier investigacin del grupo. En dicho esquema se presenta el modelo del mini-helicptero, elsistema de navegacin inercial y el bloque destinado al control del modelo, en este caso mediante lautilizacin de un control PID.

Figura 3.1 Modelo del mini-helicptero trabajado en el proyecto Colibr

Explorando el bloque del sistema de navegacin inercial se puede encontrar el filtro EKF utilizado por elgrupo, tal y como se observa en la figura 3.2.

Figura 3.2 Filtro EKF incorporado al modelo del mini-helicptero trabajado en el proyecto Colibr

col,lon ,lat ,ped

x,y,z

vx ,vy,vz

roll ,pitch ,yaw

u_manual

[0 0 0 0 ]

Trajectory

planning

(circle)

ref

Mini -helicopter

u_col (-1,1)

u_lon (-1,1)

u_lat (-1,1)

u_ped (-1,1)

lla

vxvyvz

yaw

axayaz

pqr

Integral PID

control

mode

u_manual

ref

xyz

vxvyvz

euler

u_col

u_lon

u_lat

u_ped

INS with sequential

measurement 1

lla,gps

vxvyvz ,gps

hxhyhz ,mag

axayaz ,accel

pqr,gyro

xyz _est

vxvyvz _est

euler_est

0 (manual )

1 (auto )

1

x = [x y z vx vy vz q0 q1 q 2 q3 abx aby abz pb qb rb ]'

euler _est

3

vxvyvz _est

2

xyz_est

1

Time update

("Prediction")

u(k-1)

x(k-1|k-1)

P(k-1|k-1)

x(k|k-1)

P(k|k-1)

Sequential measurement

update ("Correction")

y(k)

x(k|k-1)

P(k|k-1)

x(k|k)

P(k|k)

Quaternions Euler

LLA to Navigation

LLA xy z

pqr,gyro

5

axayaz,accel

4

hxhyhz,mag3

vxvyvz ,gps

2

lla,gps

1


40/67


Como puede verse, el filtro de Kalman toma las variables de los sensores (aceleraciones y velocidadesangulares en el sistema de referencia del cuerpo, entregadas por los acelermetros y girscopos; posicin

geodsica y velocidad en el sistema de referencia, entregados por el GPS; componentes del campomagntico entregados por el magnetmetro) y obtiene la posicin, velocidad y actitud (en cuaterniones) enel sistema de referencia. Adicionalmente se obtienen seis estimaciones de los sesgos de las aceleracionesy velocidades angulares, los cuales se utilizan para corregir las dems estimaciones (Vlez, 2007).

Como se presento en el captulo 2, el UKF posee una etapa de prediccin y una de correccin, semantendr el esquema del filtro EKF presentado en la figura anterior, pero con la diferencia que esnecesario adicionar el manejo de los puntos sigma. En la figura 3.3 se presenta el modelo final del filtroUKF con las dimensiones de cada una de sus variables, el cual se explicar en este captulo.

Figura 3.3 Modelo final en Matlab Simulink del filtro UKF

Para la etapa de prediccin, la transformacin UT requiere de la obtencin de los puntos sigma a partir del

vector de estado 1| 1x k k y la matriz de covarianza 1| 1P k k como se presenta en lasecuaciones (2.29) y (2.30). La implementacin se presenta en la figura 3.4.

Figura 3.4 Implementacin en Matlab Simulink de los puntos sigma

x = [x y z vx vy vz q 0 q1 q2 q 3 abx aby abz pb qb rb ]'

euler _est

3

vxvyvz _est

2

xyz_est

1

Time update("Prediction")

U(k-1)

x(k-1|k-1)

P(k-1|k-1)

x(k|k-1)

P(k|k-1)

X(k|k-1)

Sequential measurementupdate ("Correction")

y(k)

x(k|k-1)

P(k|k-1)

X(k|k-1)

x(k|k)

P(k|k)

Quaternions Euler

LLA to Navigation

LLA x y z

pqr,gyro5

axayaz,accel

4

hxhyhz,mag

3

vxvyvz ,gps

2

lla,gps

13

[3x1]

3

3

[7x1]

3

[3x1]

4

[6x1]

3

[16x16]

[16x16]

[16x1]

[16x16]

6

6

[16x33]

[16x33]

[3x1]

[16x1]

[16x1]

[16x1]

X(k|k-1)

11/z

1/z

uT

x(k-1|k-1) x(k-1|k-1)[16,16]

Matrix

Concatenation

2Gamma

Extract Triangular

Matrix

A L

Cholesky

Factorization

S = LL'

S LL'

P(k-1|k-1)

2

x(k-1|k-1)

1


41/67


Ahora, es necesario transformar cada una de las columnas de la matriz de puntos sigma 16 33 a travsdel modelo que representa la dinmica del sistema segn la ecuacin (2.31). La transformacin para cada

una de las columnas se implement como se muestra en la figura 3.5.

Figura 3.5 Elemento Implementado en Matlab/Simulink para la determinacin dex(k|k-1)

Sumando los 33 vectores de la matriz de los puntos sigma transformados, cada uno de ellos multiplicados

por su respectivo peso asignado por la transformacin UT como se presenta en la ecuacin (2.32), seobtiene la prediccin del vector de estado. De acuerdo con la ecuacin (2.33) la implementacin para laprediccin de la matriz de covarianza se presenta en la figura 3.6.

Figura 3.6 Implementacin en Matlab Simulink para la prediccin de la matriz de covarianzas | 1P k k

Se obtiene as la etapa de prediccin, que en el modelo final se presenta en la figura 3.7.

Figura 3.7 Implementacin en Matlab Simulink de la etapa de prediccin del UKF

u(k-1)

x(k-1|k-1)x(k|k-1) Wom

X(k|k-1)2 U(k-1)1

P(k|k-1)

1X(k|k-1)

x(k|k-1)

Sum(Wi[ X(k-1)x(k-1)][X(k-1)x(k-1)]T)

Q

Matrix

symmetrization

In OutX(k|k-1)

2

x(k|k-1)

1

X(k|k-1)

3

P(k|k-1)

2

x(k|k-1)

1

Time update

U(k-1)

X(k|k-1)

x(k|k-1)

P(k|k-1)

Sigma Points

x(k-1|k-1)

P(k-1|k-1)

X(k|k-1)

P(k-1|k-1)

3

x(k-1|k-1)

2

U(k-1)

1


42/67


Utilizando el procedimiento anterior, pero ahora con el modelo que representa la salida del sistema, seobtiene y(k|k-1) y Pyy de las ecuaciones (2.35) y (2,36) respectivamente. En las figuras 3.8 y 3.9 se

presenta la implementacin de estas ecuaciones.

Figura 3.8 Elemento Implementado en Matlab/Simulink para la determinacin de | 1y k k

Figura 3.9 Implementacin en Matlab Simulink para la determinacin de yyP

Como se enunci en el captulo 2, el propsito fundamental del filtro UKF es el de implementar latransformacin Unscented en el filtro UKF definido mediante las ecuaciones (2.15) y (2.11) para susetapas de prediccin y correccin respectivamente. Inicialmente, se implementa la matriz de covarianzacruzada definida en (2.37) mediante la utilizacin de 33 veces el arreglo presentado en la figura 3.10.

Figura 3.10 Elemento Implementado en Matlab Simulink para la determinacin de xyP

u(k-1)

x(k-1|k-1)

x(k|k-1) Wom

X(k|k-1)2 U(k-1)1

Pyy

1

transformed covariance

X(k|k-1)

y(k|k-1)

Sum(Wi[Y(k-1)-y (k-1)][Y (k-1)-y (k-1)]T)

RX(k|k-1)

2

y(k|k-1)

1

Xk-xk

Yk-ykWoc(Xk-xk)(Yk -yk )T

y(k|k-1)4Y(k|k-1)3

x(k|k-1)2X(k|k-1)1


43/67


Con la matriz de covarianza cruzada Pxy se implementa la matriz de Kalman definida en (2.38) ypresentada en la figura 3.11.

Figura 3.11 Implementacin en Matlab Simulink para la determinacin de |K k k

Finalmente, se implementa la correccin del vector de estado y la matriz de covarianzas como se

muestran en las figuras (2.39) y (2.40).

Figura 3.12 Implementacin en Matlab Simulink para la correccin del vector de estado |x k k

Figura 3.13 Implementacin en Matlab Simulink para la correccin de la matriz de covarianzas |P k k

La parte correspondiente a la correccin del UKF se presenta en forma simplificada en la figura 3.14.

K(k|k)

1Inv

Matrix

Multiply

Pyy

2

Pxy

1

x(k|k)

1Matrix

MultiplyK(k|k)

4

y(k|k-1)3 x(k|k-1)2

y(k)

1

P(k|k)

1

Matrix

Multiply

uT

Pyk

3

K(k|k)

2

P(k|k-1)

1


44/67


Figura 3.14 Implementacin en Matlab Simulink de la etapa de correccin del UKF

De esta manera se ha codificado el UKF utilizando los bloques de Simulink, lo cual permite la generacinautomtica de cdigo utilizando otras herramientas de Matlab como el Real-Time Workshop. El diseorequiri de la comprensin matemtica profunda del UKF y de su implementacin algortmica.

3.3 PRUEBAS Y ANLISIS DE RESULTADOS

El modelo final del filtro UKF implementado en Matlab/Simulink que se present en la figura 3.3, se probpara diferentes valores de parmetros, tiempos de muestreo y condiciones iniciales para el vector deestado y matriz de covarianzas, con el fin de establecer las condiciones para un comportamiento adecuadoy, adems, saber si se podan trabajar ambos filtros con las condiciones ya probadas en el EKF existentesen el grupo de investigacin (Colibr, 2009). Adicionalmente, con la variacin en las condiciones iniciales sepretende comprobar lo planteado por Teixeira, Santillo, Erwin y Bernstein (2008) con respecto a lainvariancia del comportamiento del filtro ante dichas variaciones.

Al realizar varias pruebas en lazo cerrado con reguladores PID se estableci que el mejor comportamientodel filtro durante el seguimiento de una trayectoria circular tomada como referencia, corresponde a los

valores 1 , 2 y 0k . Estos valores concuerdan con los valores de los parmetros trabajados envarios documentos similares en donde se aplican los UKF a la estimacin del estado para el control deaeronaves. Algunos de los trabajos referenciados se presentan en la tabla 3.1. Las condiciones inicialesutilizados en el filtro UKF de este trabajo se presentan en la figura 3.15 que se obtiene directamente deMatlab/Simulink.

A continuacin se presentan los resultados y el anlisis de las distintas pruebas realizadas.

P(k|k)

2

x(k|k)

1

Transformed

cross-covariance

x(k|k-1)

X(k|k-1)

y(k|k-1)

Pxy

Pyy

State correction

y(k)

x(k|k-1)

y(k|k-1)

K(k|k)

x(k|k)

Kalman Matrix

Pxy

Pyy

K(k|k)

Covariance correction

K(k|k)

Pyk

P(k|k-1)

P(k|k)

X(k|k-1)

4

P(k|k-1)

3

x(k|k-1)

2

y(k)

1


45/67


Tabla 3.1 Parmetros utilizados para el UKF en bibliografa relacionada con el control de aeronaves

Documento k

(La Viola, 2002) 1 0 0(Van der Merwe y Wan, 2004) 1 2 0(Matthew et al., 2004) 1 2 -13(Julier y Uhlmann,2004) 1 2 0(Teixeira, Santillo, Erwin y Bernstein, 2008) 1 2 0

Figura 3.15 Parmetros y condiciones iniciales de trabajo para el UKF


46/67


3.3.1 Efecto de la seleccin del tiemp o de muestreo sT

La primera prueba realizada al modelo UKF permite establecer el efecto en la respuesta que produce lavariacin del tiempo de muestreo sT . Se busca el mejor comportamiento del filtro siguiendo la trayectoria

circular de referencia (en xy ) y el menor valor RMSE definido en (2.1) para las medidas del vector de

estado definido ahora como0 1 2 3, , , , , , , , , , , , , , ,x y zx y z a a a p q rx y z v v v q q q q b b b b b b

.

La comparacin de las trayectorias del UKF y EKF se realiza manteniendo, para este ltimo, el valor de0.02sT s fijo, mientras para el UKF se toman los siguientes valores: 0.01, 0.02, 0.04 y 0.08 segundos.

Con 0.01sT s se obtienen los resultados dados en la figura 3.16 con un valor RMSE de 5.832935.




Con base en lo anterior, y teniendo en cuenta el menor valor de error RMSE, en adelante se trabajar conun 0.02sT s que es igual al valor que se trabaja en el modelo existente del EKF.

Figura 3.16 Comparacin de las trayectorias en XY del UKF y EKF con la referencia para un Ts=0.01 s

-10 -8 -6 -4 -2 0 2 4 6 8 10

-20

-18

-16

-14

-12

-10

-8

-6

-4

-2

0

i l i l il l i

x (m)

y(m)

Ref.

UKF

EKF


47/67




-10 -8 -6 -4 -2 0 2 4 6 8 10

-20

-18

-16

-14

-12

-10

-8

-6

-4

-2

0

i l i l il l i

x (m)

y(m)

Ref.

UKF

EKF

-10 -8 -6 -4 -2 0 2 4 6 8 10

-20

-18

-16

-14

-12

-10

-8

-6

-4

-2

0

i l i l il l i

x (m)

y(m)

Ref.

UKF

EKF


48/67



3.3.2 Efecto de la seleccin de la matriz de covarian zas inicial 0P

En esta prueba se determin el efecto que tiene en cada filtro la variacin del valor inicial de la matriz decovarianzas o densidad de distribucin, relacionada directamente con los errores que se trabajan en elmodelo y la medida. Inicialmente se tom 0 0P (matriz cuadrada de orden 16) para ambos filtros, Los

resultados se presentan en la figura 3.20.

Luego, de la bibliografa relacionada con el tema, especficamente la proporcionada por Oh y Johnson(2006) se trabaja con 0P diag [(5/3)2; (5/3)2; (7/3)2;1;1;(5/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2;

(0.01/3)2;(0.01/3)2;(0.01/3)2;(0.01/3)2] Los resultados se muestran en la figura 3.21.

Por ltimo, se probaron los filtros con 0 0P en el EKF y el 0P propuesto por Oh y Johnson (2006) en el

UKF. Los resultados se presentan en la figura 3.22.

Con base en lo observado para las anteriores trayectorias, se decidi trabajar con 0 0P tanto en el UKF

como en el EKF ya que el EKF presenta con este valor su mejor comportamiento y el UKF muestrainvariancia en su comportamiento ante el valor inicial de la variacin en la matriz de covarianzas. Resultadomuy interesante que concuerda con los ya establecidos para el EKF dentro del grupo y el obtenido porTeixeira, Santillo, Erwin y Bernstein (2008).

-10 -8 -6 -4 -2 0 2 4 6 8 10

-20

-18

-16

-14

-12

-10

-8

-6

-4

-2

0

i l i l il l i

x (m)

y(m)

Ref.

UKF

EKF


49/67


Figura 3.20 Comparacin de las trayectorias en XY del UKF y EKF con la referencia para 0 0P

Figura 3.21 Comparacin de las trayectorias en XY del UKF y EKF con la referencia para 0P diag ((5/3)2; (5/3)2; (7/3)2; 1; 1;(5/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2;(0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2 ).

-10 -8 -6 -4 -2 0 2 4 6 8 10

-20

-18

-16

-14

-12

-10

-8

-6

-4

-2

0

i l i l il l i

x (m)

y(m)

Ref.

UKF

EKF

-10 -8 -6 -4 -2 0 2 4 6 8 10

-20

-18

-16

-14

-12

-10

-8

-6

-4

-2

0

i l i l il l i

x (m)

y(m)

Ref.

UKF

EKF


50/67


Figura 3.22 Comparacin de las trayectorias en XY con Po=0 para el EKF y Po= diag((5/3)2; (5/3)2; (7/3)2;1;1;(5/3)2; (0.01/3)2;(0.01/3)2; (0.01/3)2; (0.01/3)2;(0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2; (0.01/3)2 ) para el UKF.

3.3.3 Obtencin y comp aracin de los valores RMSE para los EKF y UKF

El objetivo de esta prueba es determinar si el comportamiento del vector de estado con el UKF mejorafrente al EKF mientras sigue la trayectoria circular de referencia. Se ejecutan de manera simultnea losfiltros EKF y UKF y se visualizan los comportamientos de las componentes de posicin (x, y, z) y velocidad(vx, vy, vz), as como las medidas de los ngulos de Euler.

En estas pruebas se presentaron inconvenientes relacionados con el tamao de las mediciones para losngulos de Euler, ya que no coincidan y por tanto fue necesario trabajar por separado y ser comparadoscon respecto a los valores obtenidos por el modelo del mini-helicptero. Dicho tratamiento se puedeanalizar ms detalladamente del programa en Matlab que se presenta como Anexo 1. El comportamientode cada una de las variables de estado en cada filtro es presentado respectivamente en las figuras 3.23 a3.30.

-10 -8 -6 -4 -2 0 2 4 6 8 10

-20

-18

-16

-14

-12

-10

-8

-6

-4

-2

0

i l i l il l i

x (m)

y(m)

Ref.

UKF

EKF


51/67


Figura 3.23 Comparacin de la componente x para el EKF Y UKF con la referencia

Figura 3.24 Comparacin de la componente y para el EKF Y UKF con la referencia

0 10 20 30 40 50 60

-10

-8

-6

-4

-2

0

2

4

6

8

10

t (s)

x(m)

Ref.

UKF

EKF

0 10 20 30 40 50 60

-20

-18

-16

-14

-12

-10

-8

-6

-4

-2

0

t (s)

y(m)

Ref.

UKF

EKF


52/67


Figura 3.25 Comparacin de la componente zpara el EKF Y UKF con la referencia

Figura 3.26 Comparacin de las velocidades en x para el EKF Y UKF con la referencia

0 10 20 30 40 50 60-62

-61.5

-61

-60.5

-60

-59.5

-59

-58.5

-

t (s)

z(m)

Ref.

UKF

EKF

0 10 20 30 40 50 60

-1

-0.5

0

0.5

1

1.5

t (s)

Vx(m/s)

Ref.

UKF

EKF


53/67


Figura 3.27 Co

Documents

Kalman Para Drones