Estudio y aplicación del Filtro de Kalman en fusión de sensores en UAVs

Equation Chapter 1 Section 1 Trabajo Fin de Grado Grado en Ingeniería Aeroespacial Estudio y aplicación del Filtro de Kalman en fusión de sensores en UAVs Autora: María Esther Aranda Romasanta Tutor a: Juana María Martínez Heredia Dpto. Ingeniería Electrónica Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2017 1 Trabajo Fin de Grado Grado en Ingeniería Aeroespacial Estudio y aplicación del Filtro de Kalman en fusión de sensores en UAVs Autora: María Esther Aranda Romasanta Tutora: Juana María Martínez Heredia Dpto. de Ingeniería Electrónica Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2017 3 ÍNDICE Índice de Figuras 3 1. Motivación 5 2. Introducción 6 3. Descripción general del hardware 8 3.1 GPS 8 3.2 IMU 9 3.3 Arduino 11 4. Adquisición de datos 13 4.1 Módulo GPS de la compañía Adafruit 13 4.2 Módulo IMU de la compañía Adafruit 14 4.3 Montaje del sistema de medición 14 4.4 Programa de obtención de datos 15 4.5 Dónde fueron realizadas las medidas 16 4.6 Incidencias en la toma de los registros 18 5. Tratamiento de los datos obtenidos 19 5.1 Tratamiento de la posición 19 5.1.1 Fórmula de Haversine 19 5.1.2 Rotación de las aceleraciones de la IMU 21 5.1.3 Integración de las aceleraciones de la IMU 23 5.2 Tratamiento de la velocidad 25 5.3 Fusión de datos 26 6. Filtro de Kalman lineal 27 6.1 Introducción 27 6.2 Filtro de Kalman monodimensional 28 6.3 Definición de varianza y covarianza 30 6.4 Filtro de Kalman multidimensional 31 7. Aplicación del filtro de Kalman lineal 35 7.1 Modelo dinámico lineal 35 7.2 Inicialización de las matrices 35 7.3 Cálculo de las matrices de covarianza Q y R 37 7.4 Análisis de los resultados obtenidos 38 7.4.1 Entrada de datos a 100Hz en tramo 1 39 7.4.2 Entrada de datos a 50 Hz en tramo 2 42 7.4.3 Inicialización con distintas matrices de covarianza iniciales 45 7.4.4 Efecto de los vectores de ruido W y Z 48 8. Filtro de Kalman Extendido 51 8.1 Introducción 51 8.2 Idoneidad y limitaciones 53 9. Aplicación del filtro de Kalman Extendido 54 9.1 Modelo dinámico 54 9.2 Modelo utilizado en la medida 55 9.3 Inicialización de las matrices 55 1 9.4 Análisis de los resultados obtenidos 57 9.4.1 Entrada de datos a 100Hz en tramo 1 57 9.4.2 Entrada de datos a 50 Hz en tramo 2 60 9.4.3 Inicialización con distintas matrices de covarianza iniciales 63 9.4.4 Efecto de los vectores de ruido W y Z 66 10. Comparativa de los resultados obtenidos 69 11. Otros filtros 73 11.1. La transformación Unscented. 73 11.2. Filtro de Kalman Unscented 76 11.3. El filtro de Kalman como ejemplo de filtro adaptativo. 77 11.4. Filtro Schmidt-Kalman. 78 12. Ecuaciones para modelos UAV 79 12.1. Descripción del UAV 79 12.2. Modelado del UAV 79 12.3. Formulación por Newton-Euler 82 13. Conclusiones y líneas futuras 86 Apéndice A: Programa Arduino 88 Apéndice B: Tratamiento de medidas 92 1 Latitud, longitud y altitud a coordenadas 𝒙𝒙,𝒚𝒚,𝒛𝒛. 92 2 Conversión de un vector en función para poder ser integrado 92 3 Rotación de las aceleraciones de la IMU 93 Apéndice C: Filtro de Kalman Lineal 94 Apéndice D: Filtro de Kalman Extendido 97 Apéndice E: Filtro de Kalman Matlab 100 Bibliografía 102 2 ÍNDICE DE FIGURAS Figura 2.1: Dron utilizado para fotografía. 7 Figura 3.1: Constelación de satélites GPS 9 Figura 3.2: IMU en formato modular 10 Figura 3.3: Giróscopo MPU-6050, acelerómetro Arduino Adx1335 y magnetómetro HMC5883L 10 Figura 3.4: Placa Arduino Due 12 Figura 4.1: Imagen del sensor GPS utilizado 13 Figura 4.2: Imagen del sensor IMU utilizado 14 Figura 4.3: Diagrama de las conexiones entre la placa Arduino, la IMU y el receptor GPS. 14 Figura 4.4: Montaje del sistema para realizar la medición 15 Figura 4.5: Muestra de archivo de registro de datos 16 Figura 4.6: Tramo 1 de la prueba 17 Figura 4.7: Tramo 2 de la prueba 17 Figura 5.1: Triángulo esférico y curva ortodrómica 19 Figura 5.2: Esquema del ángulo y dirección de avance medido por el GPS 21 Figura 5.3: Ejes de la IMU sobre el vehículo del experimento 22 Figura 5.4: Vista de los ejes de la IMU desde arriba 22 Figura 5.5: Regla del trapecio 24 Figura 5.6: Regla de Simpson 24 Figura 5.7: Esquema del ángulo y velocidad medida por el GPS 25 Figura 6.1: Diagrama de la estructura de un filtro de Kalman 28 Figura 6.2: Esquema de un filtro de Kalman multidimensional 32 Figura 7.1: Posición en el eje x – filtro lineal – 100Hz 39 Figura 7.2: Posición en el eje y – filtro lineal – 100Hz 39 Figura 7.3: Posición en el eje z – filtro lineal – 100Hz 40 Figura 7.4: Velocidad en el eje x - filtro lineal - 100Hz 40 Figura 7.5: Velocidad en el eje y - filtro lineal - 100Hz 41 Figura 7.6: Velocidad en el eje z - filtro lineal - 100Hz 41 Figura 7.7: Posición en el eje x – filtro lineal – 50Hz 42 Figura 7.8: Posición en el eje y – filtro lineal – 50Hz 42 Figura 7.9: Posición en el eje z – filtro lineal – 50Hz 43 Figura 7.10: Velocidad en el eje x - filtro lineal - 50Hz 43 Figura 7.11: Velocidad en el eje y - filtro lineal - 50Hz 44 Figura 7.12: Velocidad en el eje z - filtro lineal - 50Hz 44 Figura 7.13: Posición en el eje x – 𝑃𝑃2– 50Hz 45 Figura 7.14: Posición en el eje x – 𝑃𝑃3– 50Hz 46 Figura 7.15: Posición en el eje x – 𝑃𝑃4– 50Hz 46 Figura 7.16: Posición en el eje x – 𝑃𝑃5– 50Hz 47 Figura 7.17: Posición en el eje x – 𝑃𝑃6– 50Hz 48 Figura 7.18: Posición en el eje x – Efecto de W – 50Hz 49 Figura 7.19: Posición en el eje x – Efecto de Z – 50Hz 49 Figura 7.20: Posición en el eje x – Efecto de W y Z – 50Hz 50 Figura 9.1: Posición en el eje x – filtro extendido – 100Hz 57 3 Figura 9.2: Posición en el eje y – filtro extendido – 100Hz 58 Figura 9.3: Posición en el eje z – filtro extendido – 100Hz 58 Figura 9.4: Velocidad en el eje x – filtro extendido – 100Hz 59 Figura 9.5: Velocidad en el eje y – filtro extendido – 100Hz 59 Figura 9.6: Velocidad en el eje z – filtro extendido – 100Hz 60 Figura 9.7: Posición en el eje x – filtro extendido – 50Hz 60 Figura 9.8: Posición en el eje y – filtro extendido – 50Hz 61 Figura 9.9: Posición en el eje z – filtro extendido – 50Hz 61 Figura 9.10: Velocidad en el eje x – filtro extendido – 50Hz 62 Figura 9.11: Velocidad en el eje y – filtro extendido – 50Hz 62 Figura 9.12: Velocidad en el eje z – filtro extendido – 50Hz 63 Figura 9.13: Posición en el eje x – 𝑃𝑃2– 50Hz 64 Figura 9.14: Posición en el eje x – 𝑃𝑃3– 50Hz 64 Figura 9.15: Posición en el eje x – 𝑃𝑃4– 50Hz 65 Figura 9.16: Posición en el eje x – 𝑃𝑃5– 50Hz 65 Figura 9.17: Posición en el eje x – 𝑃𝑃6– 50Hz 66 Figura 9.18: Posición en el eje x – Efecto de W – 50Hz 67 Figura 9.19: Posición en el eje x – Efecto de Z – 50Hz 67 Figura 9.20: Posición en el eje x – Efecto de W y Z – 50Hz 68 Figura 10.1: Comparativa de la posición en dirección x 69 Figura 10.2: Comparativa de la posición en dirección y 70 Figura 10.3: Comparativa de la posición en dirección z 70 Figura 10.4: Comparativa de la velocidad en dirección x 71 Figura 10.5: Comparativa de la velocidad en dirección y 71 Figura 10.6: Comparativa de la velocidad en dirección z 72 Figura 11.1: Puntos en el plano de la medida y en el transformado 75 Figura 11.2: Medias ("+" MC, "◊" LIN y "◊" UT) y contornos 1σ estimados para 𝑥𝑥,𝑦𝑦𝑦𝑦 76 Figura 11.3: Esquema completo del concepto general del filtrado de Kalman. 78 Figura 12.1: Modelo dinámico del UAV 79 4 1. MOTIVACIÓN Un UAV es un vehículo aéreo no tripulado, es decir, una aeronave que vuela sin tripulación. Es capaz de mantener de manera autónoma un nivel de vuelo controlado y sostenido, y es propulsado por un motor de explosión, eléctrico, o de reacción. Existen dos tipos principales, los controlados desde una ubicación remota y los que son capaces de realizar un vuelo autónomo, a partir de planes de vuelo pre-programados mediante automatización dinámica. Sus primeros usos fueron militares, ya que posibilitan sobrevolar áreas de alto riesgo o de difícil acceso, además de que no requieren la actuación de pilotos en zonas de combate. Sin embargo, presentan una serie de desventajas o problemas que aún están sin resolver. Los principales problemas técnicos que presentan derivan del uso de señales para enviar y recibir información, en el caso del control remoto. Las señales para navegación vía satélite pueden ser alteradas externamente, como podría ocurrir en el caso de una guerra, o verse afectados por condiciones atmosféricas adversas, o simplemente ser los causantes de ralentizar las comunicaciones entre el operador en tierra y la aeronave, provocando retrasos entre la emisión de instrucciones y su recepción. Es por ello que aparece como una necesidad el dotar a la aeronave de la mayor autonomía posible, siempre que esto no viole principios éticos como puede ser la decisión de atacar en el caso de una guerra. Para que esto sea realizable, es necesario equiparla con sensores que le permitan captar toda la información disponible a su alrededor y, por tanto, esta información debe ser lo más fiable posible. Mediante mejoras en los sistemas de medición y calibración, instalación de sensores que, con sistemas de medición diferentes, proporcionan la misma medida, y la aparición de herramientas que permiten estimar mejor los estados de la aeronave, están apareciendo cada vez más usos para estos sistemas, que ya no solo implican operaciones militares complejas, sino maniobras en zonas pobladas o bajo situación de catástrofe que requieren que la aeronave esté dotada de potentes sistemas de control, estabilizadores y datos correctos para actuar de manera adecuada en un entorno con condiciones complejas y cambiantes. 5 La idea bajo la que surgió Arduino fue la de acercar y facilitar el uso de la electrónica y la programación de sistemas a proyectos de diferentes disciplinas. Toda la plataforma Arduino libera sus componentes con licencia de código abierto, lo que permite libertad de acceso a ellos. El hardware de Arduino está integrado por una placa de circuito impreso que incluye un microcontrolador, puertos digitales y analógicos de entrada y salida, cuatro de los cuales pueden ser conectados a placas de expansión, que permiten ampliar las características de funcionamiento de la placa Arduino. También posee un puerto de conexión USB que permite conectar la placa con el ordenador y alimentarla. El software consiste en el entorno de desarrollo (IDE) previamente mencionado, que permite el desarrollo del código que programará la función para la que vaya a ser usada la placa. Este IDE está basado en un entorno de “Processing”, que es un lenguaje de programación y entorno de desarrollo integrado, también de código abierto, basado en Java, de fácil utilización y que sirve como medio para la enseñanza y producción de proyectos multimedia e interactivos de diseño digital. El microcontrolador incluido en la placa se programa mediante un ordenador. Arduino se puede utilizar para desarrollar objetos interactivos autónomos o puede ser conectado a algún software, tal como Adobe Flash. Una tendencia tecnológica es utilizar Arduino como tarjeta de adquisición de datos. Las placas pueden ser montadas a mano o adquiridas. El entorno de desarrollo integrado libre se puede descargar gratuitamente. Figura 3.4: Placa Arduino Due 12 4. ADQUISICIÓN DE DATOS El proceso de adquisición de datos se realizó a través de una placa Arduino Due conectada a una placa de desarrollo, sobre la que se encontraban insertados el conjunto de sensores, es decir, el GPS y la IMU. En este apartado se detallan las características concretas de los sensores utilizados, así como la estructura del montaje para el sistema de medición. Por otro lado, se comentan los cambios realizados sobre un programa, obtenido de la bibliografía, para obtener los datos a la frecuencia necesaria para el experimento. Por último, se detallará la estructura de los datos obtenidos y dónde fue realizado el experimento. 4.1 Módulo GPS de la compañía Adafruit Se trata de un módulo de alta calidad que puede realizar seguimiento de hasta 22 satélites sobre 66 canales. Su frecuencia de actualización de datos va de 1 a 10Hz, aunque esta última es demasiado alta, ya que no tiene tiempo suficiente de tomar las medidas y enviarlas. Es por ello que en este trabajo la frecuencia del GPS será fijada a 1Hz. La precisión con la que toma las medidas será de menos de 3m para el caso de la posición, y del orden de 0.1m/s para el caso de la velocidad. El tiempo de arranque del sensor es de unos 34s y la velocidad máxima que es capaz de medir es 515m/s. El resto de características técnicas se detallan a continuación: - Tamaño de la antena: 15mm x 15mm x 4mm. - Sensibilidad en adquisición: -145dBm. - Sensibilidad en seguimiento: -165dBm. - Rango de voltaje de entrada: 3.0 – 5.5VDC. - Datos de salida: NMEA 0183, 9600 baudios por defecto, 3V de salida a nivel lógico, 5V de entrada segura. - Soporte de DGPS/WAAS/EGNOS. - Detección y reducción de interferencias. - Detección y compensación de Multi-path. Figura 4.1: Imagen del sensor GPS utilizado 13 4.2 Módulo IMU de la compañía Adafruit Esta unidad de medidas inerciales combina tres de los sensores de mayor calidad disponibles en el mercado para ofrecer once ejes de datos: aceleración en tres ejes, tres ejes giroscópicos y tres ejes magnéticos, además de medidas barométricas de presión y altitud, así como medida de temperatura. Las características técnicas de los tres sensores que conforman la IMU son: - Giróscopo L3GD20H de tres ejes: ±250, ±500 o ±2000 grados por segundo de escala. - Brújula LSM303 de tres ejes: de ±1.3 a ±8.1 gauss de escala de campo magnético. - Acelerómetro LSM303 de tres ejes: ±2g, ±4g, ±8g o ±16g de escala seleccionables. - Presión barométrica/temperatura BMP180: de 300 a 1100hPa de presión. De -40 a 85°C de temperatura. 0.17m de resolución. Figura 4.2: Imagen del sensor IMU utilizado 4.3 Montaje del sistema de medición El montaje del hardware para la obtención de los datos durante el experimento ha sido realizado como aparece en el esquema de la Figura 4.3, según [2]. Figura 4.3: Diagrama de las conexiones entre la placa Arduino, la IMU y el receptor GPS. 14 Para que los ejes de la IMU estuvieran alineados con los ejes del movimiento del vehículo durante el experimento, se realizó un montaje sobre una caja de cartón indicando claramente los ejes del sensor (Figura 4.4). La antena del GPS (dispositivo negro situado fuera de la caja) fue pegada al techo del vehículo, facilitando así la capacidad de recibir señales por parte de los satélites, llegando a recibir en algunos tramos hasta diez señales. Figura 4.4: Montaje del sistema para realizar la medición 4.4 Programa de obtención de datos El programa de captura de datos ejecutado sobre Arduino fue obtenido de [2] y aparece detallado en el Apéndice A. Consta de una función setup, donde se configuran las velocidades de la comunicación serie entre Arduino y el ordenador (115200 baudios), y la velocidad entre el canal serie y el módulo GPS (9600 baudios). A continuación, en la función, se configura el GPS para que transmita los mensajes NMEA RMC (recommended mínimum – mínimo recomendado) y GGA (fix data - constantes) mediante la llamada a la librería del módulo GPS sendCommand. Mediante este mismo comando se habilita el envío de los mensajes a 1Hz y se solicita la actualización del status de la antena. Finalmente, se llama a la función initSensors, desde donde se inicializan el acelerómetro, el sensor magnético y el medidor de presión atmosférica y temperatura. Una vez inicializados los sensores, se inicia el bucle principal del programa, llamado loop (), el cual consiste básicamente en la programación de dos timers (temporizadores), que posibilitan la lectura a 1Hz de los datos procedentes del GPS, y a 100Hz de los datos procedentes de la IMU. Estos datos son empaquetados en una única línea de registro y transmitidos al terminal de Arduino. En esa línea se encuentran la última posición recibida del GPS y los datos recién leídos de la IMU. La línea contiene también los tiempos de recepción de los datos del GPS y de la IMU. La separación de los datos dentro de la línea se realiza mediante un carácter de tabulación. Al final de cada línea de registro se inserta un carácter retorno de carro, que hace que el terminal comience a escribir en la siguiente línea. La estructura de cada línea de registro, numerados de uno a dieciséis y en orden de aparición de los elementos que la componen, es la siguiente: 15 # Dato Descripción Unidades Freq. (Hz) 1 TimerGPS Instante de recepción de los datos del GPS Milisegundos 1 2 Fix Indicación de disponibilidad de posición - 1 3 Quality Tipo de posición 0 = No válido 1 = Posición GPS (SPS) 2 = Posición DGPS 3 = Posición PPS 4 = Navegación cinética satelital 5 = Navegación cinética satelital flotante 6 = Estima 7 = Modo manual 8 = Modo simulado - 1 4 Satellites Nº de satélites disponibles - 1 5 Latitude Latitud de la posición actual Grados sexag. 1 6 Longitude Longitud de la posición actual Grados sexag. 1 7 Altitude Altitud de la posición actual Metros 1 8 Speed Velocidad Nudos 1 9 Angle Ángulo del vector velocidad respecto al Norte Grados sexag. 1 10 TimerIMU Instante de recepción de los datos de la IMU Milisegundos 100* 11 Heading Rumbo Grados 100* 12 Pitch Ángulo de inclinación respecto al plano horizontal Grados 100* 13 Roll Ángulo de inclinación respecto al plano vertical Grados 100* 14 Acel X Aceleración en el eje X Grados 100* 15 Acel Y Aceleración en el eje Y Grados 100* 16 Acel Z Aceleración en el eje Z Grados 100* Tabla 4.1: Estructura del registro de datos * Algunos registros fueron realizados también a 50Hz. Una muestra de la estructura del registro de datos está representada en la Figura 4.5, donde aparecen las dieciséis columnas que posee cada línea. Figura 4.5: Muestra de archivo de registro de datos 4.5 Dónde fueron realizadas las medidas Para el experimento realizado se estudia el movimiento rectilíneo y uniforme, esto es, a velocidad constante, de un vehículo circulando sobre una carretera sin pendiente ni tramos curvos. El experimento fue realizado de noche, evitando así el tráfico, sin viento ni lluvia, y utilizando el modo velocidad de crucero del que dispone el vehículo para conseguir una velocidad constante. 16 El registro de datos fue realizado recorriendo la autovía de une San Fernando con Cádiz (España), aprovechando los dos tramos rectos de los que esta carretera dispone (Figura 4.6 y 4.7). Los tramos donde se realizó la prueba se encuentran señalados en amarillo. Figura 4.6: Tramo 1 de la prueba Figura 4.7: Tramo 2 de la prueba 17 Por facilidad en la identificación, se han denominado tramo 1 y tramo 2. Es relevante indicar las orientaciones de los tramos con el fin de analizar los resultados obtenidos posteriormente. El tramo 1 parte de San Fernando y finaliza en la curva que se dirige hacia Cádiz. En dirección a Cádiz, supone seguir un rumbo de 99⁰, y en dirección a San Fernando de 279⁰. El tramo 2 parte de la curva y finaliza en Cádiz. En dirección a Cádiz tiene un rumbo de 155’5⁰, mientras que en dirección a San Fernando de 335’5⁰. Observando la Figura 4.5 donde se muestran los datos de registro, las columnas nueve y once deberían mostrar los mismos valores, ya que el ángulo que mide el sensor GPS y el heading que mide el sensor IMU son los indicadores del rumbo. Sin embargo, no solo no coindicen, sino que difieren en un ángulo distinto de 180⁰. Investigando en la librería proporcionada por la compañía Adafruit sobre el módulo IMU, se ha concluido que el ángulo mostrado por el heading corresponde al arcotangente de las proyecciones de la medida tomada por el magnetómetro sobre los ejes 𝑥𝑥 e 𝑦𝑦 de la IMU. Al desconocer el valor de estas proyecciones, ha sido muy difícil determinar cuál es la referencia que toma el sensor para el cálculo del rumbo. Por ello, de ahora en adelante y durante los experimentos realizados en esta memoria, se tomará como valor del rumbo el ángulo proporcionado por el sensor GPS. Es importante destacar que la aplicación de los filtros Kalman a los datos capturados durante el experimento aquí descrito fue realizada off-line, es decir, se realizó con posterioridad a la captura de los datos. No obstante, el algoritmo de los filtros Kalman que serán implementados está programado para que, únicamente añadiendo un procesador de datos a tiempo real, los filtros puedan ser implementados mientras se toman las medidas. 4.6 Incidencias en la toma de los registros Es notable reseñar que la toma de registros se vio afectada por la capacidad limitada que el sistema proporcionaba en la captura de datos. Tras el inicio de un nuevo registro y transcurrido un tiempo variable entre 20 y 100 segundos, la captura de datos se interrumpía haciendo necesario un reset de la tarjeta Arduino (desconectando y volviendo a conectar el terminal USB a través del cual la tarjeta Arduino recibe la alimentación). No pudo determinarse si la interrupción del registro era motivada por la conexión serie entre el PC y la placa Arduino, debido al elevado volumen de los datos registrados (aproximadamente 100 líneas de registros por segundo), o por fallos en el acceso a los sensores de la IMU. En numerosas ocasiones, tras el reset de la tarjeta, al iniciar un nuevo registro, se producían fallos de acceso al módulo de la IMU, apareciendo la sentencia "Ooops, no LSM303 detected... Check your wiring!" a consecuencia de fallos en la inicialización del sensor. Otro efecto que ralentizó y perturbó la toma de registros era la necesidad de esperar entre 30 segundos y un minuto tras la alimentación de la placa para que el GPS finalizara el proceso de adquisición de satélites y se obtuviera una posición GPS disponible. 18 5. TRATAMIENTO DE LOS DATOS OBTENIDOS En esta sección se explicarán los cambios realizados sobre los datos obtenidos directamente de los sensores para adaptarlos a las unidades y sistemas de coordenadas en los que van a trabajar los filtros. Se comenzará hablando del tratamiento que han recibido los valores que determinan la posición, tanto de los obtenidos del GPS como de la IMU, para luego de qué manera se han obtenido los datos de velocidad. Finalmente se hablará de la fusión de datos, es decir, del método seguido para pasar a los filtros las medidas de posición y velocidad de los dos sensores al mismo tiempo. 5.1 Tratamiento de la posición Incluye tres grupos de operaciones, correspondientes a los programas que aparecen en el Apéndice B de esta memoria. Primero, se describe la conversión de las coordenadas geográficas (Lat, Long, Alt) en la que se recibe el dato de posición GPS a coordenadas cartesianas. Seguidamente, se continuará describiendo la rotación de ejes e integración de los valores proporcionados por la IMU. 5.1.1 Fórmula de Haversine La fórmula de Haversine calcula la distancia del círculo máximo entre dos puntos de una esfera, dadas la longitud y la latitud de dichos puntos. Esta fórmula es ampliamente utilizada en navegación, y representa un caso particular de la ley de Haversine utilizada en trigonometría esférica, que relaciona los lados y ángulos de los triángulos esféricos (Figura 5.1). Figura 5.1: Triángulo esférico y curva ortodrómica La distancia del círculo máximo entre dos puntos es la distancia más corta entre esos dos puntos. Aplicado a la superficie terrestre se hablaría de la ortodrómica (Figura 5.1). Una característica de esta curva es que presenta un ángulo diferente con cada meridiano, excepto cuando dicha ortodrómica coincide con un meridiano o con el ecuador. Es necesario destacar que para la aplicación de esta fórmula se está suponiendo que la superficie de la Tierra es representada por una esfera, cosa que no es del todo cierto ya que, debido al achatamiento de los polos, posee la forma de un elipsoide. Sin embargo, dado que 19 esta aproximación esférica supone un error entorno al 0.3%, se considerará como válida en el caso bajo estudio. Para cualquier pareja de puntos sobre una esfera, la función de Haversine para el ángulo central entre ellos, es decir, el ángulo que formarían esos dos puntos si estuviesen sobre una esfera del mismo radio, es: ℎ𝑎𝑎𝑎𝑎�𝑑𝑑𝑟𝑟�=ℎ𝑎𝑎𝑎𝑎(φ2−φ1)+cos 𝜑𝜑1cos 𝜑𝜑2ℎ𝑎𝑎𝑎𝑎(𝜆𝜆2−𝜆𝜆1) Donde: - hav: Función de Haversine: ℎ𝑎𝑎𝑎𝑎𝑎𝑎𝑟𝑟𝑟𝑟𝑟𝑟𝑟𝑟(𝜃𝜃)=sin(𝜃𝜃 2)2=(1−cos𝜃𝜃) 2 - d: distancia entre los dos puntos sobre el círculo máximo de la esfera. - r: radio de la esfera, que en este caso será el radio de la Tierra, 𝑅𝑅=6371𝐾𝐾𝐾𝐾. - 𝝋𝝋𝟏𝟏,𝝋𝝋𝟐𝟐: Latitud de los puntos 1 y 2, respectivamente, en radianes. - 𝝀𝝀𝟏𝟏,𝝀𝝀𝟐𝟐: Longitud de los puntos 1 y 2, respectivamente, en radianes. Resolviendo la ecuación para d: 𝑑𝑑=𝑟𝑟ℎ𝑎𝑎𝑎𝑎−1�ℎ𝑎𝑎𝑎𝑎�𝑑𝑑𝑟𝑟��= 2𝑟𝑟sin−1��ℎ𝑎𝑎𝑎𝑎(φ2−φ1)+cos 𝜑𝜑1cos 𝜑𝜑2ℎ𝑎𝑎𝑎𝑎(𝜆𝜆2−𝜆𝜆1)� Una vez conocido el valor de la distancia que separa dos puntos sobre la superficie terrestre dadas sus latitudes y longitudes que proporcionará el sensor GPS, es preciso adaptar esta idea al problema del experimento que se realiza en esta memoria. Tomando como latitud y longitud iniciales las del origen del movimiento, se calcularán los incrementos siempre respecto a ellas, de tal manera que en cada instante, el valor de la distancia (d) conocido sea el valor de la distancia total recorrida. Los ejes en los que se estudiará el movimiento del vehículo serán unos ejes cartesianos con origen en el punto de inicio de la trayectoria del vehículo y el eje Y orientado según el Norte geográfico y el X según el Este. Por ello, será necesario descomponer esta distancia recorrida en las componentes según las direcciones Norte-Sur y Este-Oeste. Para ello, será utilizada otra de las medidas proporcionadas por el GPS, el llamado ángulo de GPS (track angle o bearing en inglés). El bearing es el ángulo que va desde el Norte Geográfico a la dirección de avance del móvil. Como puede verse en la Figura 5.2, la distancia calculada anteriormente, que se corresponde con la distancia total recorrida por el móvil, tendrá la misma dirección que la dirección de avance en el caso de un movimiento rectilíneo. En el caso de que el sensor GPS no proporcione el valor del ángulo, este puede ser calculado mediante relaciones trigonométricas que involucran los valores de las diferencias entre las latitudes y las longitudes. 𝑏𝑏𝑎𝑎𝑎𝑎𝑟𝑟𝑟𝑟𝑟𝑟𝑏𝑏=𝜃𝜃=tan−1(sin 𝛥𝛥𝜆𝜆∗cos 𝜑𝜑2∗cos 𝜑𝜑1∗sin 𝜑𝜑2−sin 𝜑𝜑1∗cos 𝜑𝜑2∗cos 𝛥𝛥𝜆𝜆) 20 Figura 5.2: Esquema del ángulo y dirección de avance medido por el GPS Una vez obtenida la distancia recorrida y el ángulo que forma la dirección de avance con la dirección del Norte Geográfico, la descomposición de la distancia recorrida según estos ejes es el resultado de aplicar relaciones trigonométricas. Las componentes de la posición del vehículo son: 𝑥𝑥=𝐸𝐸𝐺𝐺=𝑑𝑑∗sin(𝜃𝜃) 𝑦𝑦=𝑁𝑁𝐺𝐺=𝑑𝑑∗cos(𝜃𝜃) En ningún momento en este desarrollo se ha hablado de la tercera componente que proporciona la posición del móvil, esto es, de la altura. El motivo es porque la altura proporcionada por el sensor GPS coincide con la altura en el sistema de ejes geográficos que se ha tomado como referencia, por lo que no es necesario efectuar ningún cambio sobre su valor. 𝑧𝑧=𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝑟𝑟𝑎𝑎 5.1.2 Rotación de las aceleraciones de la IMU El siguiente paso será la rotación de las aceleraciones proporcionadas por la IMU. Estas vienen expresadas en los ejes de la IMU, y para que las medidas puedan ser tratadas por los filtros Kalman a la vez que las de GPS, deben estar expresadas en los mismos ejes, es decir, en los ejes geográficos de la Tierra. Es preciso notar que los ejes de la IMU son ejes móviles respecto del sistema de referencia fijo que representan los ejes geográficos de la Tierra. Los ejes de la IMU orientados según la dirección de avance del vehículo aparecen representados en la Figura 5.3. Por tratarse de un movimiento rectilíneo y uniforme, las aceleraciones únicamente tendrán componente en la dirección del eje 𝑦𝑦, aunque serán muy pequeñas, ya que al realizarse a velocidad constante el experimento serán aceleraciones NORTE Á ESTE Á BEARING DIRECCIÓN DE AVANCE DEL MOVIMIENTO d 21 considerado una pequeña componente de viento en un movimiento de un vehículo a lo largo de una carretera. Se llama ruido o incertidumbre porque se trata de variaciones pequeñas sobre los resultados reales. Los errores en la calibración de los sensores o en el modelado del sistema dinámico que impliquen grandes variaciones sobre los resultados reales no están incluidos aquí, por lo que un filtro de Kalman no es capaz de eliminarlos. Es importante, por tanto, remarcar la idea de que un filtro de Kalman no es capaz de decidir si la medida o el modelo son acertados o erróneos. Su única tarea es estimar, dentro de los datos de los que dispone, una solución óptima libre de ruido aleatorio. 6.2 Filtro de Kalman monodimensional En la Figura 6.1, obtenida de [9], se puede ver un diagrama de las etapas que conforman un filtro de Kalman monodimensional: Figura 6.1: Diagrama de la estructura de un filtro de Kalman En una iteración normal del filtro el primer paso es la predicción, para el instante actual, del valor de la variable de estado, utilizando el modelo dinámico del sistema y en base al valor estimado de la variable (salida del filtro) en la iteración anterior. A continuación, se actualiza el error de la predicción realizada utilizando el error que se obtiene de la salida del filtro como resultado de nuestra estimación. En la primera iteración, al no disponerse de una salida del filtro, se utilizan valores iniciales previamente establecidos. Por otro lado, el filtro recibe como dato de entrada la medida de la variable, obtenida del sensor, que será almacenada como valor medido. Se actualiza también el error de este último valor medido. Siguen ahora los tres procesos principales en los que se basa el filtro. ERROR INICIAL ENTRADA DE LA MEDIDA PREDICCIÓN INICIAL MEDIDA PREDICCIÓN ERROR EN LA PREDICCIÓN ERROR EN LA MEDIDA CÁLCULO DE LA GANANCIA DE KALMAN CÁLCULO DE LA ESTIMACIÓN CÁLCULO DEL ERROR EN LA ESTIMACIÓN 1 2 3 28 El primero de ellos será calcular la Ganancia de Kalman que, utilizando los errores calculados, se encarga de ponderar entre la medida y la predicción, según se indica en el cuadro 1 de la Figura 6.1. Con la Ganancia, la predicción de la variable de estado para esta iteración y la medida, se obtiene la salida del filtro, es decir, se estima el valor de la variable de estado. Finalmente, se actualiza el error cometido en el cálculo de esta estimación. Ambos procesos son indicados en los cuadros 2 y 3 de la Figura 6.1. Tanto el valor de la estimación como de su error asociado son almacenados para ser utilizados en la siguiente iteración. - Ganancia de Kalman: Es un peso cuyo valor oscila entre cero y uno. Su función dentro del filtro es la de dar más importancia al valor medido o al valor estimado mediante la comparación de los errores (E) de cada uno. Se define por el siguiente cociente: 𝑲𝑲= 𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬Ó𝑵𝑵 𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬Ó𝑵𝑵 + 𝑬𝑬𝑬𝑬𝑬𝑬𝑴𝑴𝑬𝑬𝑴𝑴𝑬𝑬 ; 𝑲𝑲 ∈[𝟎𝟎,𝟏𝟏] Al comienzo del proceso iterativo, el error en la estimación será normalmente elevado respecto al de la medida, porque tanto el error como la estimación se corresponderán con un valor que ha sido inicializado previamente por el programador y que estará alejado de la realidad, o por lo menos, más alejado que la medida. Por esta razón, al ser 𝐸𝐸𝐼𝐼𝑀𝑀𝑀𝑀𝐼𝐼𝑀𝑀𝑀𝑀 ≪ 𝐸𝐸𝑀𝑀𝐺𝐺𝐸𝐸𝐼𝐼𝐼𝐼𝑀𝑀𝐸𝐸𝐼𝐼Ó𝑁𝑁, la Ganancia de Kalman tiende a un valor cercano a uno. Esto significará que el filtro dará más peso a la medida respecto de la estimación. A medida que avanza el proceso, la Ganancia de Kalman tiende a disminuir. En el extremo contrario, cuando su valor sea cercano a cero, significará que 𝐸𝐸𝐼𝐼𝑀𝑀𝑀𝑀𝐼𝐼𝑀𝑀𝑀𝑀≫ 𝐸𝐸𝑀𝑀𝐺𝐺𝐸𝐸𝐼𝐼𝐼𝐼𝑀𝑀𝐸𝐸𝐼𝐼Ó𝑁𝑁, lo que indicará que la estimación prevalece sobre la medida denotando la buena estimación del modelo. La rápida convergencia del filtro de Kalman permite que el valor de la ganancia alcance un valor próximo a cero en un número relativamente pequeño de iteraciones. Por esta razón es tan ampliamente utilizado como uno de los mejores métodos estimativos. - Estimación del estado actual: Una vez conocido cuál de los dos valores, si la estimación o la medida, es más fiable, y reflejado en el valor de la Ganancia de Kalman, K, tiene lugar la estimación del estado actual, o lo que es lo mismo, el cálculo de la variable que rige el comportamiento del sistema. Viene definida por la ecuación: 𝑬𝑬𝑬𝑬𝑬𝑬𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂= 𝑷𝑷𝑷𝑷𝑷𝑷𝑷𝑷𝑷𝑷𝒂𝒂𝒂𝒂𝑷𝑷ó𝒏𝒏 + 𝑲𝑲 [𝑬𝑬𝑷𝑷𝑷𝑷𝑷𝑷𝑷𝑷𝒂𝒂− 𝑷𝑷𝑷𝑷𝑷𝑷𝑷𝑷𝑷𝑷𝒂𝒂𝒂𝒂𝑷𝑷ó𝒏𝒏 ] Es importante volver a incidir en que el filtro de Kalman es un estimador o función estadística y, como tal, realizará el tratamiento de la medida. En este sentido, tratará las medidas como un conjunto de valores de una distribución normal para los cuales minimizará su error cuadrático. No puede, por tanto, mejorar mediciones que se 29 encuentren afectadas por sesgos o errores sistemáticos que no estén dentro del ámbito del ruido blanco. Analizando la ecuación, cuando la Ganancia de Kalman tiende a uno, la salida estará principalmente determinada por la medida. En el otro extremo, cuando la Ganancia de Kalman esté próxima a cero, la salida del filtro, es decir, la estimación, estará fuertemente condicionada por la predicción. - Error en la estimación actual: Es una indicación de cómo de próximos están los valores estimados a los medidos. Se calcula a partir de la siguiente ecuación: 𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂 =(𝑬𝑬𝑬𝑬𝑬𝑬𝑴𝑴𝑬𝑬𝑴𝑴𝑬𝑬)�𝑬𝑬𝑷𝑷𝑷𝑷𝑬𝑬𝑴𝑴𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬Ó𝑵𝑵� (𝑬𝑬𝑬𝑬𝑬𝑬𝑴𝑴𝑬𝑬𝑴𝑴𝑬𝑬)+ �𝑬𝑬𝑷𝑷𝑷𝑷𝑬𝑬𝑴𝑴𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬Ó𝑵𝑵� ; 𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂 =(𝟏𝟏−𝑲𝑲) 𝑬𝑬𝑷𝑷𝑷𝑷𝑬𝑬𝑴𝑴𝑬𝑬𝑬𝑬𝑬𝑬𝑬𝑬Ó𝑵𝑵 En este caso, cuando la Ganancia de Kalman tiende a uno, el error en la estimación actual va a ser muy pequeño. En efecto, cuando 𝐾𝐾= 1, el filtro considera que el error en la medida es muy pequeño, y el valor que estima es, por tanto, muy cercano a ella. Para la siguiente iteración, conseguirá comparar la medida nueva con un valor de su predicción que, por estar basado en el estado anterior, va a ser también muy próximo a la medida, con lo que K tenderá a cero. Cuando 𝐾𝐾= 0, el error en la medida es muy grande y el valor estimado es muy próximo al valor predicho, por lo que el error actual será el error de la predicción. Como conclusión, cuando K sea muy grande la estimación se acerca rápidamente al valor medido, mientras que si K es pequeña, la estimación se acercará más lentamente a la medida, lo que se traduce por pequeñas oscilaciones en el entorno de la misma. Es interesante observar que, si bien será muy extraño que K tome valores extremos, el error en la estimación actual siempre será más pequeño que el error en la predicción, que está basado en el error de la estimación anterior, lo que garantiza la convergencia del filtro. 6.3 Definición de varianza y covarianza El filtro de Kalman está basado en una serie de conceptos estadísticos. Su definición facilitará la comprensión del funcionamiento del filtro en los siguientes apartados: - Varianza: Medida de la dispersión de una variable respecto de su media. Se calcula como el cuadrado de la desviación típica, que representa la distancia promedio de cada punto respecto de la media. En una distribución normal, un intervalo de (-𝜎𝜎, +𝜎𝜎), centrado en la media representa aproximadamente 2/3 de los valores de la muestra, siendo 𝜎𝜎 la desviación típica. Si sustituimos el valor de la desviación típica por el de la varianza, el nuevo intervalo abarcará el 100 % de los valores de la muestra. Siendo 𝑥𝑥𝑖𝑖 los valores individuales, 𝑥𝑥 la media de los valores y 𝑥𝑥−𝑥𝑥𝑖𝑖 la desviación de cada valor respecto de la media: 30 𝐷𝐷𝑎𝑎𝑟𝑟𝑎𝑎𝑟𝑟𝑎𝑎𝐷𝐷𝑟𝑟ó𝑟𝑟 𝐴𝐴í𝑝𝑝𝑟𝑟𝐷𝐷𝑎𝑎=𝜎𝜎=�∑(𝑥𝑥−𝑥𝑥𝑖𝑖)2 𝑁𝑁 𝑖𝑖=1 𝑁𝑁 𝑉𝑉𝑎𝑎𝑟𝑟𝑟𝑟𝑎𝑎𝑟𝑟𝑧𝑧𝑎𝑎=𝜎𝜎2=∑(𝑥𝑥−𝑥𝑥𝑖𝑖)2 𝑁𝑁 𝑖𝑖=1 𝑁𝑁 - Covarianza: Indica el grado de variación conjunta de dos variables aleatorias respecto de sus medias, es decir, determina si existe algún tipo de dependencia entre ellas. La varianza se puede definir también como la covarianza de una variable respecto de sí misma. Si vale cero, significará que no existe una relación lineal entre ellas. Por otro lado, si grandes o pequeños valores de una se corresponden con grandes o pequeños valores de otra, esto es, varían conjuntamente, la covarianza tomará valores positivos. Si, por el contrario, grandes valores de una se corresponden con pequeños valores de la otra, o viceversa, la covarianza tendrá un valor negativo, expresando esto comportamientos opuestos entre las variables. 𝜎𝜎𝑥𝑥𝜎𝜎𝑦𝑦=∑(𝑥𝑥−𝑥𝑥𝑖𝑖)(𝑦𝑦�−𝑦𝑦𝑖𝑖) 𝑁𝑁 𝑖𝑖=1 𝑁𝑁 Cuando, de ahora en adelante, se mencionen en este trabajo las matrices de covarianza, serán aquellas matrices que incluyen las desviaciones de los parámetros que sean considerados en ese momento como muestra respecto a la media de dicha muestra. Tales matrices estarán formadas por los términos correspondientes a la varianza en la diagonal principal, y por las covarianzas de los parámetros fuera de ella. Un ejemplo de esto en 3D sería: 𝑀𝑀=�𝜎𝜎𝑥𝑥2𝜎𝜎𝑥𝑥𝜎𝜎𝑦𝑦𝜎𝜎𝑥𝑥𝜎𝜎𝑧𝑧 𝜎𝜎𝑦𝑦𝜎𝜎𝑥𝑥𝜎𝜎𝑦𝑦2𝜎𝜎𝑦𝑦𝜎𝜎𝑧𝑧 𝜎𝜎𝑧𝑧𝜎𝜎𝑥𝑥𝜎𝜎𝑧𝑧𝜎𝜎𝑦𝑦𝜎𝜎𝑧𝑧2� Las matrices de covarianza serán las encargadas de cuantificar el error cometido por el filtro, tanto en la predicción, como en las medidas o en la estimación del estado real. Por ello, quizás el término error no es del todo acertado, refiriéndonos al usarlo a la incertidumbre o dispersión que experimentarán los valores que estén siendo considerados respecto a su media. 6.4 Filtro de Kalman multidimensional En el apartado 6.2 se realizaba una pequeña introducción al funcionamiento de un filtro de Kalman de una variable. No obstante, en la realidad, cuando un filtro es implementado, no sólo se quiere conocer el valor real de una variable, sino de todas aquellas que sean capaces de medir los sensores y resulten útiles para alguna de las muchas aplicaciones del filtro. Implementar un filtro por cada una de estas medidas sería prácticamente imposible, ya que se necesitaría que fuesen realizadas en paralelo si se habla de una implementación a tiempo real. Es aquí donde nace la idea de realizar un filtro multidimensional, que estime el valor de más de una variable al mismo tiempo. Para ello, será necesario que el modelo lineal del sistema dinámico incluya todas estas variables, por lo que deberán guardar algún tipo de relación entre ellas. Además, será preciso definir una serie de matrices que permitirán el manejo conjunto de 31 todas las variables que estén siendo tratadas. En la Figura 6.2 se muestra un diagrama de las ecuaciones que son necesarias para elaborar un filtro de estas características, así como el orden de aplicación de las mismas. Figura 6.2: Esquema de un filtro de Kalman multidimensional Como puede observarse, la estructura es igual que la de un filtro de una variable. A continuación se detalla lo que representan cada una de estas matrices y la función que tienen: - Creación de un estado anterior: La salida del filtro, es decir, la estimación realizada por el filtro en la iteración anterior, pasa a convertirse en la nueva entrada del filtro (cuadro 5 de la Figura 6.1). En el caso de la primera iteración, al no existir una salida del filtro, se utilizan valores iniciales previamente establecidos. • X(0): Vector inicial de estado. En él se incluirán los valores iniciales de las variables que se quieren filtrar. • P(0): Matriz inicial de la covarianza del proceso. Esta matriz recoge los errores en el cálculo del vector inicial de estado. • X(k-1): Vector de estado correspondiente al estado anterior. Incluye la estimación de las variables realizada por el filtro en la iteración anterior (k-1). • P(k-1): Matriz de covarianza del proceso del estado anterior. Representa los errores que han sido cometidos en la estimación realizada en la iteración anterior (k-1). - Calculo de la predicción: El filtro, basándose en el modelo lineal definido, y utilizando como base el valor estimado de la iteración anterior, realiza una predicción de cuál será el siguiente estado del sistema (primera ecuación del cuadro 1 de la Figura 6.2). • X(kp): Vector de estado predicho (p) para la iteración actual (k). Vector que recoge el valor predicho de cada una de las variables de estado, obtenido en este caso mediante la aplicación del modelo lineal que representa el comportamiento del sistema en estudio. • A: Es la matriz de coeficientes de aquellos términos del modelo lineal que dependen de las variables de estado, es decir, es la matriz que relaciona las variables entre sí. • B: Es la matriz de coeficientes de aquellos términos del modelo lineal que no dependen de las variables de estado, es decir, de aquellos términos que 𝑋𝑋(0) 𝑃𝑃(0) ERR 𝑋𝑋(𝑘𝑘−1) 𝑃𝑃(𝑘𝑘−1) 𝑋𝑋(𝑘𝑘) 𝑃𝑃(𝑘𝑘) (5) 𝑋𝑋(𝑘𝑘𝑝𝑝)=𝐴𝐴 𝑋𝑋(𝑘𝑘−1)+𝐵𝐵 𝑈𝑈 (𝑘𝑘)+𝑊𝑊 (𝑘𝑘) 𝑃𝑃(𝑘𝑘𝑝𝑝)=𝐴𝐴 𝑃𝑃(𝑘𝑘−1) 𝐴𝐴𝐸𝐸+𝑄𝑄 (1) 𝑌𝑌(𝑘𝑘)=𝐶𝐶 𝑋𝑋(𝑘𝑘𝐾𝐾)+𝑍𝑍 (𝑘𝑘) (2) 𝐾𝐾= 𝐺𝐺(𝑘𝑘𝑘𝑘) 𝐻𝐻 𝐻𝐻 𝐺𝐺(𝑘𝑘𝑘𝑘) 𝐻𝐻𝑇𝑇+𝑅𝑅 (3) 𝑋𝑋(𝑘𝑘)=𝑋𝑋(𝑘𝑘𝑝𝑝)+𝐾𝐾 [𝑌𝑌(𝑘𝑘)−𝐻𝐻 𝑋𝑋(𝑘𝑘𝑝𝑝)] 𝑃𝑃(𝑘𝑘)=(𝐼𝐼− 𝐾𝐾𝐻𝐻) 𝑃𝑃(𝑘𝑘𝑝𝑝) (4) 32 permanecerán constantes durante la implementación del filtro. Puede servir también para modificar las dimensiones del vector U(k). • U(k): Vector de variables de control. Incluye los términos del modelo lineal que permanecen constantes durante la predicción de las variables, por ejemplo, aceleraciones conocidas como la gravedad o una fuerza de empuje constante. • W(k): Vector de ruido del modelo dinámico. Pasar de un modelo real a un modelo matemático implica errores de aproximaciones, redondeos o simplificaciones, como puede ser una pequeña componente de viento aleatorio que modificaría la trayectoria en un movimiento. En definitiva, se asume como ruido la diferencia entre el modelo real sobre el que se realiza el estudio, y el modelo matemático que lo representa. Esa diferencia está compuesta por todos aquellos elementos del sistema real no considerados en el modelo matemático, y por las aproximaciones realizadas para aquellos elementos que sí están incluidos. - Predicción de la matriz de covarianza: Una vez realizada la predicción de las variables del vector de estado, también es preciso predecir cuál será la matriz de covarianza, es decir, predecir cuál será el grado de dispersión que tendrá la estimación del filtro (segunda ecuación del cuadro 1 de la Figura 6.2). • Q: Matriz de ruido de la covarianza del proceso. Se suma a la predicción de la matriz de covarianza del vector de estado, basada en la matriz de covarianza de la estimación anterior. Modela los errores o dispersión esperados para el ruido del modelo dinámico. Su función es importante, cuando la matriz de covarianza de la salida del filtro, P(k) (cuadro 4 de la Figura 6.2), tiende a cero, su realimentación en la siguiente iteración en el cálculo de la predicción de la matriz de covarianza, P(kp) (cuadro 1 de la Figura 6.2), haría que esta fuese también próxima a cero. La suma de la matriz Q introduce un factor de error o dispersión asociado al ruido del proceso que permite al filtro seguir comparando su predicción con la medida. • P(kp): Matriz de covarianza del proceso. Predicción de la matriz de covarianza basada en el modelo dinámico y en la matriz de covarianza de la iteración anterior. - Entrada de la medida: Es la segunda ecuación que proporciona información sobre el sistema bajo estudio. Incluye los cambios que deben ser realizados sobre la medida obtenida de los sensores para adaptarla a la forma y estructura del vector de estado. • Y(k): Vector de la observación para la iteración actual (k). Vector que recoge el valor que proporciona la medida sobre cada una de las variables de estado (cuadro 2 de la Figura 6.2). • C: Matriz que adapta las medidas obtenidas directamente de los sensores a la estructura del vector de estado. Puede implicar un cambio de unidades, de ejes, de posición dentro del vector o, en el caso de que la misma variable sea medida por más de un sensor, una fusión de datos. 33 • X(km): Vector que contiene las medidas obtenidas directamente de los sensores. • Z(k): Vector de ruido de la medida. Al igual que ocurría con el vector W para la predicción, la medición puede tener unos pequeños errores asociados a los sensores, como pueden ser errores de calibración. - Matriz de covarianza de la medida: Determina el grado de dispersión que tienen los valores proporcionados por los sensores. Se utiliza para calcular la Ganancia de Kalman (cuadro 3 de la Figura 6.2). • R: Matriz de covarianza de la medida. Modela los errores o dispersión presentes en la medida, tanto por causa de los sensores como de los cálculos, al modelar el vector de ruido Z. - Cálculo de la Ganancia de Kalman: Utilizando las matrices de covarianza calculadas, se encarga de ponderar entre la medida y la predicción, para decidir cuál de las dos presenta mayor incertidumbre y, por tanto, es menos fiable (cuadro 3 de la Figura 6.2). • K: Ganancia de Kalman. Su valor oscila entre cero y uno. • H: Matriz que adapta la estructura de la matriz de covarianza del proceso para que coincida con la matriz de covarianza de la medida. También se encarga de adaptar la forma del vector de estado a la forma del vector de la medida en el cálculo de la estimación, ya que puede ser que las dimensiones de los vectores de estado de la medida y la predicción no coincidan. Normalmente, si las matrices tienen la misma dimensión, se toma como una matriz identidad. - Estimación del estado actual: Solución del filtrado. Una vez ponderada la medida y la predicción, el filtro estima una solución del vector de estado para la iteración actual (k) (cuadro 4 de la Figura 6.2). • X(k): Vector de estado que contiene la estimación del estado actual. - Matriz de covarianza de la estimación actual: Matriz que representa el grado de dispersión que tienen los valores estimados del vector de estado respecto a la media de los mismos. Cuantifica cómo de buena es la estimación (cuadro 4 de la Figura 6.2). • P(k): Matriz de la covarianza de la estimación del estado actual. • I: Matriz identidad de la misma dimensión de P(k). Tanto la matriz de la estimación del estado actual como la matriz de la covarianza de la estimación pasarán a llamarse estado anterior y realimentarán al filtro en la siguiente iteración. 34 7. APLICACIÓN DEL FILTRO DE KALMAN LINEAL Una vez estudiado qué es un filtro de Kalman y el álgebra detrás de su implementación, se detallará en este apartado una aplicación a un modelo real. Para ello, definiremos el modelo y las ecuaciones que van a regir su comportamiento. Posteriormente, se explicarán las pautas seguidas para la inicialización de los vectores y matrices, así como la manera de calcular las matrices de covarianza, tanto de la predicción como de la medida. Finalmente, se mostrarán las gráficas de los resultados obtenidos y se analizarán para determinar su grado de corrección y si se corresponden con los datos conocidos, obtenidos de la experiencia. 7.1 Modelo dinámico lineal El experimento realizado consiste, como ya fue explicado en el apartado 4.5, en un movimiento rectilíneo y uniforme realizado con un vehículo a lo largo de una autovía. El vehículo se desplazaba a velocidad constante (~75𝐾𝐾𝐾𝐾/ℎ), y presentaba algunas aceleraciones aleatorias, derivadas del estado del firme y de su pendiente, así como otros fenómenos no parametrizables, que no han sido tenidas en cuenta en la ecuación del modelo, sino que aparecerán incluidas de las ecuaciones de ruido. Por ello, las variables de estado del sistema serán la posición en los tres ejes coordenados (𝑥𝑥,𝑦𝑦,𝑧𝑧), así como las tres velocidades correspondientes a cada uno de estos ejes. Las ecuaciones que rigen la posición en el movimiento rectilíneo y uniforme del vehículo son: 𝑥𝑥(𝑘𝑘)=𝑥𝑥(𝑘𝑘−1)+𝑎𝑎𝑥𝑥∗𝐴𝐴 ; 𝑦𝑦(𝑘𝑘)=𝑦𝑦(𝑘𝑘−1)+𝑎𝑎𝑦𝑦∗𝐴𝐴 ; 𝑧𝑧(𝑘𝑘)=𝑧𝑧(𝑘𝑘−1)+𝑎𝑎𝑧𝑧∗𝐴𝐴 ; Mientras que las ecuaciones que rigen la velocidad son: 𝑎𝑎𝑥𝑥=𝑎𝑎𝑥𝑥 ; 𝑎𝑎𝑦𝑦=𝑎𝑎𝑦𝑦 ; 𝑎𝑎𝑧𝑧=𝑎𝑎𝑧𝑧 ; Expresando esto de forma matricial en función de las variables de estado: ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 𝑥𝑥𝑦𝑦𝑧𝑧 𝑎𝑎𝑥𝑥 𝑎𝑎𝑦𝑦 𝑎𝑎𝑧𝑧 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ � 𝑋𝑋(𝑘𝑘𝑘𝑘) = ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 100 010 001 𝐴𝐴0 0 0𝐴𝐴0 0 0 𝐴𝐴 000 000 000 100 010 001 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ � � � � � � � � � � � � � � � 𝑀𝑀 ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 𝑥𝑥𝑦𝑦𝑧𝑧 𝑎𝑎𝑥𝑥 𝑎𝑎𝑦𝑦 𝑎𝑎𝑧𝑧 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ � 𝑋𝑋(𝑘𝑘−1) Al no tener este sistema ninguna fuerza externa actuando, como puede ser el considerar la acción de la gravedad en un móvil volador, o una aceleración, que se añadirían como función de las variables a las ecuaciones del modelo, la matriz 𝑈𝑈(𝑘𝑘)= 0. 7.2 Inicialización de las matrices Una vez obtenido el modelo dinámico, falta definir cuáles serán los criterios para inicializar el resto de matrices que necesita el filtro para comenzar su funcionamiento. Algunas de ellas 35 permanecerán constantes durante todo el filtrado, mientras que otras serán inicializadas para la primera iteración siendo posteriormente actualizadas por el filtro. - Matrices constantes durante el funcionamiento del filtro: • H: utilizada para adaptar las dimensiones entre dos matrices. En el ejemplo lineal, no es necesaria ninguna modificación, por lo que: 𝐻𝐻= ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 100 010 001 000 000 000 000 000 000 100 010 001 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ • C: realiza las operaciones necesarias sobre las medidas directamente obtenidas de los sensores para adaptarlas a la forma, unidades y ejes en los que esté expresado el vector de estado. En este caso, las transformaciones fueron realizadas fuera del filtro, según el apartado 5, por lo que no es necesario ningún cambio adicional: 𝐶𝐶= ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 1 0 0 0 1 0 0 0 1 000 000 000 0 0 0 0 0 0 0 0 0 100 010 001 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ • W(k): Corrección de la predicción del vector de estado basada en el modelo. Se trata de una desviación constante aplicada directamente sobre el vector de estado que modela la contribución al movimiento de algún agente externo, como podría ser una componente de viento en la dirección 𝑥𝑥 de 5𝐾𝐾𝐾𝐾/ℎ. En este caso, al no ser medible esta contribución, se ha tomado una desviación simbólica: 𝑊𝑊= ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 0.01 0.01 0.01 0.01 0.01 0.01 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ • Z(k): Corrección de la medida, a partir de una desviación conocida del sensor, que se desea corregir. Hace que la estimación del filtro se desplace una constante. El sistema GPS introduce errores variables intencionadamente para uso comercial, a fin de que el sistema no pueda ser utilizado para aplicaciones militares. Para tales usos militares existe un modo denominado modo P o de “disponibilidad condicional”, en el cual se dispone de la máxima precisión del sistema (entorno y por debajo del metro). A fin de tener en cuenta este error intencionado introducido en el sistema se ha considerado una desviación simbólica: 36 𝑍𝑍= ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 0.01 0.01 0.01 0.01 0.01 0.01 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ - Matrices válidas para la primera iteración: • X(0): Considerando que se trata del inicio del movimiento, y que además el experimento está realizado a nivel del mar, todas las variables de estado serán inicializadas a cero. Se comprobará así la capacidad de convergencia del filtro a la estimación correcta. 𝑋𝑋(0) = ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 0 0 0 0 0 0 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ • P(0): La matriz de la covarianza del proceso debe ser cuadrada, de la misma dimensión que la mayor del vector de estado, e incluirá las varianzas y covarianzas entre las distintas variables. En muchos de los ejemplos de implementación del filtro consultados en la literatura toman la matriz inicial como diagonal, con un valor aleatorio y grande. Esto es porque se supone que no existe relación alguna entre las variables del vector de estado, por lo que sus covarianzas son cero y solo es necesario representar las varianzas. En el apartado 7.4 se realizará un estudio de la rapidez de convergencia del filtro según los valores iniciales de esta matriz. Un ejemplo sería: 𝑃𝑃(0) = ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 700 070 007 0 0 0 0 0 0 0 0 0 000 000 000 7 0 0 0 7 0 0 0 7 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ 7.3 Cálculo de las matrices de covarianza Q y R Existen diversos métodos para el cálculo de estas matrices. En el estudio realizado para su cálculo se ha observado que, a veces, por las condiciones del problema considerado y a la espera de unos resultados concretos, pueden tomarse como matrices diagonales constantes durante toda la implementación del filtro, donde cada elemento de dicha diagonal representa la posible varianza de la variable a la que está asociado. En otros casos, son calculadas en cada iteración mediante un método adaptado al problema. En este estudio, se ha decidido tomar como método para su cálculo el desarrollado en [10], que es detallado a continuación. El cálculo de las matrices Q y R va a ser análogo y está basado en la definición de varianza y covarianza vista en el apartado 6.3. Por ello, las matrices tomarán la forma: 𝑄𝑄=∑(𝑤𝑤𝑖𝑖−𝑤𝑤 �)(𝑤𝑤𝑖𝑖−𝑤𝑤 �)𝑇𝑇 𝑛𝑛−1 0𝑎𝑎−1 ; 𝑅𝑅=∑(𝑣𝑣𝑖𝑖−𝑣𝑣�)(𝑣𝑣𝑖𝑖−𝑣𝑣�)𝑇𝑇 𝑛𝑛−1 0𝑎𝑎−1 37 Figura 7.11: Velocidad en el eje y - filtro lineal - 50Hz Figura 7.12: Velocidad en el eje z - filtro lineal - 50Hz A pesar de las grandes oscilaciones que ocurren en estos casos al comienzo de las iteraciones, las velocidades en el caso de un muestreo más lento se estabilizan antes. El error entre la estimación del filtro de Kalman y la medida es del mismo orden que en caso de 100Hz. Ya que la gran variación de altura de la Figura 7.9 no se refleja en una variación de la velocidad, podemos concluir que se trata de un error en la recepción de las medidas. 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -20 -10 0 10 20 30 40 50 Velocidad según el eje y Tiempo v y Velocidad GPS Velocidad Fusión Velocidad Kalman 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -25 -20 -15 -10 -5 0 5Velocidad según el eje z Tiempo v z Velocidad GPS Velocidad Fusión Velocidad Kalman 44 7.4.3 Inicialización con distintas matrices de covarianza iniciales Se estudiará el comportamiento del filtro con una matriz de covarianza inicial diagonal de pequeño tamaño, como se definió en el apartado 7.2, para después continuar con cinco pruebas más. Por mayor claridad en el tiempo de convergencia de las matrices se realizará la implementación del filtro para los datos obtenidos a una frecuencia de 50Hz. Por otro lado, se ha decidido realizar el experimento con una única variable de estado, siendo esta representativa del comportamiento del filtro con el resto de variables. A la vista de los resultados obtenidos en el apartado 7.4.2, se ha decidido que esa variable sea la posición según el eje 𝑥𝑥. - Matriz diagonal de elevados valores: 𝑃𝑃2(0)=500 ∗ ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 100 010 001 000 000 000 000 000 000 100 010 001 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ Figura 7.13: Posición en el eje x – 𝑃𝑃2– 50Hz - Matriz completa con valores bajos: 𝑃𝑃3(0)= 7 ∗ ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 111 111 111 1 1 1 1 1 1 1 1 1 111 111 111 1 1 1 1 1 1 1 1 1 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 0 500 1000 1500 Posición según el eje x Tiempo x p Posición GPS Posición Fusión Posición Kalman con P 2 45 Figura 7.14: Posición en el eje x – 𝑃𝑃3– 50Hz - Matriz completa con valores elevados: 𝑃𝑃4(0)=500 ∗ ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 1 1 1 1 1 1 1 1 1 111 111 111 1 1 1 1 1 1 1 1 1 111 111 111 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ Figura 7.15: Posición en el eje x – 𝑃𝑃4– 50Hz 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -200 0 200 400 600 800 1000 1200 1400 1600 Posición según el eje x Tiempo x p Posición GPS Posición Fusión Posición Kalman con P 3 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -200 0 200 400 600 800 1000 1200 1400 1600 Posición según el eje x Tiempo x p Posición GPS Posición Fusión Posición Kalman con P 4 46 - Matriz de covarianzas negativas con valores pequeños: 𝑃𝑃5(0)= 7 ∗ ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 1−1−1 −1 1 −1 −1−1 1 −1−1−1 −1−1−1 −1−1−1 −1−1−1 −1−1−1 −1−1−1 1−1−1 −1 1 −1 −1−1 1 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ Figura 7.16: Posición en el eje x – 𝑃𝑃5– 50Hz Como se puede comprobar en estas gráficas, el valor de la matriz de covarianza inicial afecta a la rapidez con la que el filtro se establece en el entorno de la medida. Los mejores resultados se obtienen para una matriz diagonal de valores elevados (matriz 𝑃𝑃2) ya que, aunque cuando la matriz es completa de números elevados también se posiciona en los alrededores de la medida rápidamente, aparece una oscilación inicial brusca. Se deduce que valores pequeños para las matrices de covarianza no benefician la estabilización de las estimaciones, así como que las matrices con covarianzas negativas dan resultados muy parecidos con las matrices de covarianza diagonales. Por último, se muestra un ejemplo con una matriz de covarianzas negativas (recordar que la diagonal está formada por elementos al cuadrado que no pueden tomar valores negativos) con valores muy elevados, a la espera de obtener resultados parecidos a los de 𝑃𝑃2, como se muestra en la Figura 7.17. - Matriz de covarianzas negativas con valores grandes: 𝑃𝑃6(0)=500 ∗ ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 1−1−1 −11−1 −1−1 1 −1−1−1 −1−1−1 −1−1−1 −1−1−1 −1−1−1 −1−1−1 1−1−1 −1 1 −1 −1−1 1 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 0 500 1000 1500 Posición según el eje x Tiempo x p Posición GPS Posición Fusión Posición Kalman con P 5 47 Figura 7.17: Posición en el eje x – 𝑃𝑃6– 50Hz 7.4.4 Efecto de los vectores de ruido W y Z Por último, se mostrará en este apartado el efecto que tienen los vectores que modelan el ruido del sistema sobre la solución proporcionada por el filtro. Como se comentó en el apartado 7.2, existen desviaciones intencionadas en las medidas proporcionadas por los sensores. Por otro lado, el modelo dinámico realizado del sistema puede contener algún tipo de desviación por algún agente externo. Estas grandes desviaciones no se corresponden con el concepto de ruido blanco que maneja el filtro, por lo que no es capaz de eliminarlas ni de determinar su valor. En el caso de los sensores, tampoco puede ser solventada con ajustes en la calibración. Los vectores de ruido añaden al filtrado una desviación constante que contempla estos fallos. Por desconocer el valor de las desviaciones intencionadas del modelo y las medidas, para la implementación del filtro se ha supuesto que son de valor pequeño. Sin embargo, se realiza a continuación una prueba dando valores grandes a estos parámetros, para comprobar gráficamente el efecto que tienen. Por facilidad en la representación, se ha tomado también una única variable de estado para la observación del efecto, siendo esta representativa del resto de variables a las que se le aplica el filtrado Kalman. La variable de estado será, también en este caso, la posición según el eje 𝑥𝑥. Se observará el efecto de la modificación del valor de los vectores por separado, y a continuación el efecto de un aumento en ambos. Se utilizará la matriz 𝑃𝑃(0)=𝑃𝑃1 y se filtrarán los datos obtenidos a 50Hz. Siendo los nuevos valores: 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 0 500 1000 1500 Posición según el eje x Tiempo x p Posición GPS Posición Fusión Posición Kalman con P 6 48 𝑊𝑊= ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 3 3 3 3 3 3 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ ; 𝑍𝑍= ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 3 3 3 3 3 3 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ Figura 7.18: Posición en el eje x – Efecto de W – 50Hz Figura 7.19: Posición en el eje x – Efecto de Z – 50Hz 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -7000 -6000 -5000 -4000 -3000 -2000 -1000 0 1000 2000 Posición según el eje x - Efecto de W Tiempo x p Posición GPS Posición Fusión Posición Kalman 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 0 500 1000 1500 Posición según el eje x - Efecto de Z Tiempo x p Posición GPS Posición Fusión Posición Kalman 49 Figura 7.20: Posición en el eje x – Efecto de W y Z – 50Hz El efecto de W se hace notar sobre el modelo dinámico del sistema. Además de introducir una gran oscilación al comienzo de la estimación, cosa que puede seguir achacándose a errores en los valores iniciales, se puede observar que el resultado del filtro avanza casi paralelo a los datos proporcionados por las medidas. El efecto de Z en la estimación se hace notar menos, aunque atrasa la capacidad de estabilización del filtro. Sin embargo, cuando se representan ambos errores en conjunto, puede observase como la estimación del filtrado Kalman avanza paralelo a las medidas, incluso dando la impresión de divergir de ellas. Esto es porque el filtro asume esos errores como constantes, y por ello decide que su estimación debe estar desplazada sobre la medida. 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 0 500 1000 1500 2000 2500 3000 Posición según el eje x - Efecto de W y Z Tiempo x p Posición GPS Posición Fusión Posición Kalman 50 8. FILTRO DE KALMAN EXTENDIDO Esta sección se centrará en explicar de dónde surge y en qué consiste el filtro de Kalman Extendido, cuáles son sus diferencias respecto al filtro lineal y qué ventajas e inconvenientes presenta su utilización. 8.1 Introducción Cuando realizamos un filtrado hay dos ecuaciones que proporcionan información sobre el sistema bajo estudio: la ecuación de la medida u observación y la ecuación que modela el sistema dinámico. La ecuación de la observación describe cómo han sido tomadas las medidas por los sensores, mientras que la ecuación de modelado describe cómo se espera que el sistema evolucione con el tiempo. En el filtro de Kalman asumimos que estas ecuaciones son lineales, pero existen situaciones en las que no lo son. Reescribiéndolas, según [11], de un modo no lineal, las ecuaciones quedan de la forma: - Ecuación de la observación: si 𝑌𝑌𝑘𝑘 es la medida, 𝑋𝑋𝑘𝑘 las variables de estado, 𝑟𝑟𝑘𝑘 el ruido de las medidas y 𝑏𝑏 representa una matriz de ecuaciones: 𝑌𝑌𝑘𝑘=𝑏𝑏(𝑋𝑋𝑘𝑘,𝑟𝑟𝑘𝑘) - Ecuación del sistema dinámico: si 𝑋𝑋𝑘𝑘−1 es el estado estimado en el instante 𝑘𝑘−1, 𝑎𝑎𝑘𝑘𝑘𝑘 es un ruido dinámico aleatorio, 𝑋𝑋𝑘𝑘𝑘𝑘 es el estado predicho en el instante actual y 𝑓𝑓 es una matriz de ecuaciones del modelo dinámico: 𝑋𝑋𝑘𝑘𝑘𝑘=𝑓𝑓�𝑋𝑋𝑘𝑘−1,𝑎𝑎𝑘𝑘𝑘𝑘� El filtro de Kalman Extendido se aplica cuando la dinámica del sistema o la observación están modeladas por ecuaciones no lineales. Las ecuaciones pueden ser transformadas en lineales realizando una aproximación por serie de Taylor: 𝑥𝑥(𝐴𝐴+𝛥𝛥𝐴𝐴)=𝑥𝑥(𝐴𝐴)+𝛥𝛥𝐴𝐴𝑥𝑥󰇗(𝐴𝐴)+(𝛥𝛥𝐴𝐴)2 2! 𝑥𝑥󰇘(𝐴𝐴)+(𝛥𝛥𝐴𝐴)3 3! 𝑥𝑥(𝐴𝐴)+⋯ Cuando 𝛥𝛥𝐴𝐴 o 𝑥𝑥󰇘(𝐴𝐴) son muy pequeños, todos los términos de la serie, excepto los dos primeros, pueden despreciarse. Si se aplica esta aproximación a las ecuaciones descritas anteriormente, las ecuaciones en las que se basa un filtro de Kalman Extendido son: - Ecuación del sistema dinámico: La ecuación predice el estado actual del sistema dinámico en base a la siguiente ecuación [12]: 𝑋𝑋𝑘𝑘𝑘𝑘=𝑥𝑥�𝑘𝑘+𝜕𝜕𝑓𝑓 𝜕𝜕𝑥𝑥�𝑋𝑋𝑘𝑘−1−𝑋𝑋(𝑘𝑘−1)𝑘𝑘�+𝜕𝜕𝑓𝑓 𝜕𝜕𝑎𝑎𝑎𝑎𝑘𝑘𝑘𝑘 El término 𝑥𝑥�𝑘𝑘 representa la última estimación del estado, es decir, la salida del filtro en la iteración anterior. El segundo término está basado en la linealización del sistema dinámico entorno al valor conocido del estado correspondiente a la salida del filtro en la última 51 iteración. La derivada de la función en ese punto es multiplicada por el último incremento conocido del vector de estado, resultado de la diferencia entre el valor de la salida del filtro y la predicción en la iteración anterior, considerada esa diferencia como una señal de error. La derivada parcial por la que está multiplicada ese error se conoce como Jacobiano. El Jacobiano de un conjunto de funciones de varias variables es la derivada parcial de cada función respecto a cada una de las variables, expresando este resultado en forma matricial. Es importante señalar que, en general, el Jacobiano es dependiente del tiempo, esto es, la forma de la derivada cambia dependiendo de la posición en la que se encuentre en la curva no lineal. 𝜕𝜕𝑓𝑓 𝜕𝜕𝑥𝑥=𝜕𝜕(𝑓𝑓1,𝑓𝑓2, … , 𝑓𝑓𝑖𝑖) 𝜕𝜕�𝑥𝑥1,𝑥𝑥2,…,𝑥𝑥𝑗𝑗�= ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 𝜕𝜕𝑓𝑓1 𝜕𝜕𝑥𝑥1⋯𝜕𝜕𝑓𝑓1 𝜕𝜕𝑥𝑥𝑗𝑗 ⋮ ⋱ ⋮ 𝜕𝜕𝑓𝑓𝑖𝑖 𝜕𝜕𝑥𝑥1⋯𝜕𝜕𝑓𝑓𝑖𝑖 𝜕𝜕𝑥𝑥𝑗𝑗 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ El tercer término representa el efecto del ruido del sistema dinámico. El valor 𝑎𝑎𝑡𝑡 puede ser considerado como otro incremento del error, y está multiplicado por su respectivo Jacobiano. - Ecuación de la medida: 𝑌𝑌𝑘𝑘=𝑦𝑦�𝑘𝑘+𝜕𝜕𝑏𝑏 𝜕𝜕𝑥𝑥�𝑋𝑋𝑘𝑘−𝑋𝑋𝑘𝑘𝑘𝑘�+𝜕𝜕𝑏𝑏 𝜕𝜕𝑟𝑟𝑟𝑟𝑘𝑘 Donde el término 𝑦𝑦�𝑘𝑘 se calcula utilizando la predicción del estado de la iteración actual: 𝑦𝑦�𝑘𝑘=𝑏𝑏�𝑋𝑋𝑘𝑘𝑘𝑘, 0� El segundo y tercer término no pueden calcularse, ya que incluyen el vector de estado y el ruido de la medida, que son actualmente desconocidos, aunque sí pueden calcularse sus Jacobianos. El filtro de Kalman Extendido también se basa, como en el caso del filtro de Kalman lineal, en un método iterativo de corrección – predicción, pero con unas ecuaciones ligeramente modificadas. Las partes del filtro lineal donde se utilizan la matriz de la predicción y de la medida deben ser modificadas para usar matrices no lineales, que serán funciones de 𝑏𝑏 y 𝑓𝑓, así como de sus respectivos Jacobianos. Las ecuaciones mostradas sirven para ilustrar de dónde deriva el filtro Extendido, no siendo utilizadas directamente cuando se realiza una implementación del mismo. A continuación, se detallan las ecuaciones que sí serán implementadas, donde aparecerá el efecto que el Jacobiano tiene también sobre las matrices de covarianza [13]. - Predicción del estado: 𝑋𝑋𝑘𝑘𝑘𝑘=𝑓𝑓(𝑋𝑋𝑘𝑘−1, 0) - Predicción de la matriz de covarianza: 𝑃𝑃𝑘𝑘𝑘𝑘=�𝜕𝜕𝜕𝜕 𝜕𝜕𝑥𝑥�𝑃𝑃𝑘𝑘−1�𝜕𝜕𝜕𝜕 𝜕𝜕𝑥𝑥�𝐸𝐸+�𝜕𝜕𝜕𝜕 𝜕𝜕𝑎𝑎�𝑄𝑄�𝜕𝜕𝜕𝜕 𝜕𝜕𝑎𝑎�𝐸𝐸 52 - Obtención de la medida: 𝑌𝑌𝑘𝑘 - Cálculo de la Ganancia de Kalman: 𝐾𝐾=𝑃𝑃𝑘𝑘𝑘𝑘�𝜕𝜕𝑏𝑏 𝜕𝜕𝑥𝑥�𝐸𝐸��𝜕𝜕𝑏𝑏 𝜕𝜕𝑥𝑥�𝑃𝑃𝑘𝑘𝑘𝑘�𝜕𝜕𝑏𝑏 𝜕𝜕𝑥𝑥�𝐸𝐸+�𝜕𝜕𝑏𝑏 𝜕𝜕𝑟𝑟�𝑅𝑅�𝜕𝜕𝑏𝑏 𝜕𝜕𝑟𝑟�𝐸𝐸�−1 - Estimación del estado actual: 𝑋𝑋𝑘𝑘=𝑋𝑋𝑘𝑘𝑘𝑘+𝐾𝐾�𝑌𝑌𝑘𝑘−𝑏𝑏�𝑋𝑋𝑘𝑘𝑘𝑘, 0�� - Covarianza de la estimación actual: 𝑃𝑃𝑘𝑘=�𝐼𝐼−𝐾𝐾�𝜕𝜕𝑎𝑎 𝜕𝜕𝑥𝑥��𝑃𝑃𝑘𝑘𝑘𝑘 - Creación de un estado anterior para comenzar con la siguiente iteración. 8.2 Idoneidad y limitaciones A pesar de que el filtro de Kalman Extendido surgió como una alternativa al filtro de Kalman para sistemas no lineales, pronto aparecieron una serie de problemas derivados, en su mayoría, de la utilización y cálculo de la matriz Jacobiana. Ésta, al manejar términos correspondientes a derivadas parciales, es fuente de errores al provocar singularidades en el proceso de cálculo, por lo que los resultados obtenidos, aunque en apariencia puedan tener una forma o valores correctos, no lo son. Por otro lado, según [14], el hecho de aproximar las ecuaciones no lineales por unas que sí lo son mediante una aproximación por serie de Taylor induce también muchos errores, ya que en la mayoría de los casos prácticos se desprecian los términos de orden mayor o igual que dos de dicha serie por tomar valores muy pequeños. Es importante afirmar que el filtro Extendido no es óptimo. No obstante, es implementado en base a un conjunto de aproximaciones. Por ello, las matrices de covarianza no representan realmente las covarianzas de los estados estimados. Al contrario de lo que ocurre con el filtro de Kalman lineal, el Extendido puede divergir si las sucesivas linealizaciones no son una buena aproximación del modelo lineal a lo largo del dominio. 53 Figura 9.6: Velocidad en el eje z – filtro extendido – 100Hz En el caso de la velocidad los resultados no son tan favorables, mostrando mucha inestabilidad, presumiblemente por desajustes en las muestras tomadas, que hacen la estimación del filtro poco fiable, aunque su evolución tienda a acercarse a los valores medidos. 9.4.2 Entrada de datos a 50 Hz en tramo 2 Figura 9.7: Posición en el eje x – filtro extendido – 50Hz 0500 1000 1500 2000 2500 3000 3500 4000 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 Velocidad según el eje z Tiempo v z Velocidad GPS Velocidad Fusión Velocidad Kalman 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -200 0 200 400 600 800 1000 1200 1400 1600 Posición según el eje x Tiempo x p Posición GPS Posición Fusión Posición Kalman 60 Figura 9.8: Posición en el eje y – filtro extendido – 50Hz Observando los resultados, las muestras tomadas a 50Hz no ralentizan demasiado la estabilización de la estimación, la cual se ajusta de una manera más o menos correcta a las medidas. La componente 𝑦𝑦, al ser la que se encuentra afectada por el ángulo 𝜃𝜃, presenta mayor inestabilidad. La fusión presenta también grandes oscilaciones al principio provocadas por errores en la recogida de los datos. Figura 9.9: Posición en el eje z – filtro extendido – 50Hz 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -1000 -800 -600 -400 -200 0 200 400 Posición según el eje y Tiempo y p Posición GPS Posición Fusión Posición Kalman 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -0.5 0 0.5 1 1.5 2 2.5 3 3.5 4Posición según el eje z Tiempo z p Posición GPS Posición Fusión Posición Kalman 61 Figura 9.10: Velocidad en el eje x – filtro extendido – 50Hz Figura 9.11: Velocidad en el eje y – filtro extendido – 50Hz 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -300 -250 -200 -150 -100 -50 0 50 100 Velocidad según el eje x Tiempo v x Velocidad GPS Velocidad Fusión Velocidad Kalman 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -350 -300 -250 -200 -150 -100 -50 0 50 Velocidad según el eje y Tiempo v y Velocidad GPS Velocidad Fusión Velocidad Kalman 62 Figura 9.12: Velocidad en el eje z – filtro extendido – 50Hz Las velocidades, como en el caso de muestras a 100 Hz, dan peores resultados que las posiciones. Sin embargo, en este caso, a pesar de lo que pueda parecer en las gráficas, las escalan indican que las estimaciones difieren más de las medidas y con valores más grandes. Esto es porque se dispone de la mitad de información entre dos entradas de GPS, con lo que es más difícil interpolar el comportamiento del filtro entre ellas. Es importante remarcar aquí que, como se predijo, los resultados obtenidos con el filtro Extendido son peores que los del filtro lineal. Esto es porque el cálculo del Jacobiano y las aproximaciones tomadas tanto en la linealización como en las matrices de covarianza provocan la pérdida de mucha información. 9.4.3 Inicialización con distintas matrices de covarianza iniciales Se estudiará el comportamiento del filtro con una matriz de covarianza inicial diagonal de pequeño tamaño, como se definió en el apartado 9.3, para después estudiar el comportamiento tras realizar la inicialización con cinco matrices más. Estas matrices serán las mismas que las utilizadas en el estudio realizado en el apartado 7.4.3 para el filtrado lineal. Por mayor claridad en el tiempo de convergencia de las matrices se realizará la implementación del filtro para los datos obtenidos a una frecuencia de 50Hz. Por otro lado, se ha decidido realizar el experimento con una única variable de estado, siendo esta representativa del comportamiento del filtro con el resto de variables. A la vista de los resultados obtenidos en el apartado 9.4.2, por las oscilaciones que presenta al comienzo del filtrado, se ha decidido que esa variable sea la posición según el eje 𝑥𝑥. 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2Velocidad según el eje z Tiempo v z Velocidad GPS Velocidad Fusión Velocidad Kalman 63 - Matriz 𝑷𝑷𝟐𝟐(𝟎𝟎): Figura 9.13: Posición en el eje x – 𝑃𝑃2– 50Hz - Matriz 𝑷𝑷𝟑𝟑(𝟎𝟎): Figura 9.14: Posición en el eje x – 𝑃𝑃3– 50Hz 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -400 -200 0 200 400 600 800 1000 1200 1400 1600 Posición según el eje x Tiempo x p Posición GPS Posición Fusión Posición Kalman con P 2 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -200 0 200 400 600 800 1000 1200 1400 1600 Posición según el eje x Tiempo x p Posición GPS Posición Fusión Posición Kalman con P 3 64 - Matriz 𝑷𝑷𝟒𝟒(𝟎𝟎): Figura 9.15: Posición en el eje x – 𝑃𝑃4– 50Hz - Matriz 𝑷𝑷𝟓𝟓(𝟎𝟎): Figura 9.16: Posición en el eje x – 𝑃𝑃5– 50Hz 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -200 0 200 400 600 800 1000 1200 1400 1600 Posición según el eje x Tiempo x p Posición GPS Posición Fusión Posición Kalman con P 4 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 0 500 1000 1500 Posición según el eje x Tiempo x p Posición GPS Posición Fusión Posición Kalman con P 5 65 - Matriz 𝑷𝑷𝟔𝟔(𝟎𝟎): Figura 9.17: Posición en el eje x – 𝑃𝑃6– 50Hz Como se observa en las gráficas, en este caso el valor de la matriz de covarianza inicial casi no afecta a la rapidez con la que el filtro se establece en el entorno de la medida. Los resultados son bastante parecidos entre las distintas matrices que han sido probadas, llegando a concluir que es mejor una matriz de covarianza inicial con valores pequeños, porque minimiza la oscilación que se produce al comienzo de la estimación. Las matriz con covarianza negativa y valores elevados es en este caso la que peor resultado da, como se puede apreciar en la figura de arriba de este párrafo. Paradójicamente, es esa misma matriz completa de covarianzas negativas para valores pequeños la que proporciona mejores resultados (matriz 𝑃𝑃5). No se demuestra aquí por tanto que el hecho de que la matriz de covarianza inicial sea diagonal beneficie a la estabilización de la estimación. 9.4.4 Efecto de los vectores de ruido W y Z Se realizará en este apartado un estudio de las implicaciones que tienen los vectores de ruido W y Z en la solución del filtrado extendido, análogo al ya realizado en el apartado 7.4.4 para el caso de filtrado lineal. Se ha tomado como variable de estado la posición según el eje 𝑥𝑥. Se observará el efecto de la modificación del valor de los vectores por separado, y a continuación el efecto de un aumento en ambos. La matriz 𝑃𝑃(0)=𝑃𝑃1 y se filtrarán los datos obtenidos a 50Hz. Siendo los nuevos valores los ya utilizados en el caso lineal: 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -500 0 500 1000 1500 Posición según el eje x Tiempo x p Posición GPS Posición Fusión Posición Kalman con P 6 66 𝑊𝑊= ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 3 3 3 3 3 3 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ ; 𝑍𝑍= ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 3 3 3 3 3 3 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ Figura 9.18: Posición en el eje x – Efecto de W – 50Hz Figura 9.19: Posición en el eje x – Efecto de Z – 50Hz 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 0 500 1000 1500 Posición según el eje x - Efecto de W Tiempo xp Posición GPS Posición Fusión Posición Kalman 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -2000 -1500 -1000 -500 0 500 1000 1500 2000 Posición según el eje x - Efecto de Z Tiempo xp Posición GPS Posición Fusión Posición Kalman 67 Figura 9.20: Posición en el eje x – Efecto de W y Z – 50Hz Las matrices de ruido sí tienen, como se puede observar en las figuras, una gran importancia en la implementación de un filtro Extendido. Esto es debido a que el modelo de ruido adoptado para el problema que se esté estudiando se ve afectado por el cálculo de su Jacobiano, así como también es aproximado al caso lineal por una serie de Taylor. Todos estos factores hacen que cualquier variación de sus valores provoque una reacción del filtro a peor. La susceptibilidad del filtro extendido se ve aumentada en el caso de que el ruido únicamente afecte a la medida (Z). El filtro no es capaz de controlar esa desviación y diverge de un modo no amortiguado. Cuando el ruido afecta a la predicción (W), o a una combinación de ambos, la estimación tampoco es fiable debido a que no sigue en ningún momento lo que dictan las muestras obtenidas. Por ello, es muy importante a la hora de implementar un filtro Extendido que no solo el modelo sea acertado y la linealización lo más precisa posible, sino que el modelo de ruido con el que se trabaje sea también una buena estimación de la realidad, para evitar que el filtro no sea capaz de decidir a quién hacer más caso, si a unas medidas erróneas y con desviaciones o a un modelo que tras la linealización dista bastante de la realidad que se quiere modelar. 0500 1000 1500 2000 2500 3000 3500 4000 4500 5000 -200 0 200 400 600 800 1000 1200 1400 1600 Posición según el eje x - Efecto de W y Z Tiempo x p Posición GPS Posición Fusión Posición Kalman 68 10. COMPARATIVA DE LOS RESULTADOS OBTENIDOS En esta sección se realizará una comparación entre los resultados obtenidos mediante la implementación del Filtro de Kalman lineal y los proporcionados por la función que ofrece el programa Matlab, que realiza un filtrado Kalman equivalente a este. El código utilizado aparece descrito en el Apéndice E. La utilización, por tanto, de esta función predeterminada es posible debido a que el modelo del movimiento del experimento es lineal. En base a los estudios realizados en el apartado 7.4, se ha optado por utilizar aquí unas matrices de ruido W y Z de pequeño valor, debido a la imposibilidad de realizar en este caso un modelo acertado de ruido. Las matrices de covarianza Q y R serán las correspondientes a cada filtro, es decir, el filtro lineal funcionará con unas matrices de la covarianza que cambiarán en cada iteración, mientras que el filtro realizado mediante la función operará con matrices de covarianza constantes durante todo el filtrado, e iguales a matrices identidades de dimensión 6x6. Por otro lado, dado que se comprobó que la convergencia del filtro era más rápida en el caso de que la matriz P(0) fuese diagonal y tomara valores muy grandes, será la que se utilice en esta comparación, buscando así optimizar el filtrado. Por último, especificar que esta comparativa ha sido realizada en base a las muestras obtenidas cada 100Hz, dado que esta es una frecuencia normal a la que suele proporcionar valores la IMU. Se ha eliminado la representación de la componente de la medida obtenida del GPS por claridad en las figuras. Figura 10.1: Comparativa de la posición en dirección x 0 500 1000 1500 2000 2500 3000 3500 4000 Tiempo -100 0 100 200 300 400 500 600 700 800 x p Posición según el eje x Posición Fusión Posición Kalman Posición MATLAB 69 En cambio, la transformación Unscented genera una estimación Y �(UT) más próxima a 𝔼𝔼[y] y, además, consistente, al menos para el caso en que la distribución de 𝑋𝑋 es gaussiana. Figura 11.2: Medias ("+" MC, "◊" LIN y "◊" UT) y contornos 1σ estimados para [𝑥𝑥,𝑦𝑦]𝐸𝐸 Dadas estas propiedades de estimación superiores de la transformación Unscented sobre la linealización tradicionalmente empleada en el contexto del filtro de Kalman Extendido, y considerando la simplicidad con que se implementa, esto es, sin requerir el cálculo de la matriz Jacobiana, esta transformación se ha vuelto una alternativa atractiva a la linealización para aplicaciones de filtrado no lineal. 11.2. Filtro de Kalman Unscented Esta extensión del filtro de Kalman es el resultado de incorporar la transformación Unscented al filtro de Kalman Extendido, para mejorar las aproximaciones que se hacen de la 𝔼𝔼[y] y ℂov (y, y) que resulta de propagar una variable aleatoria, supuesta gaussiana, a través de una transformación no lineal. Al igual que el filtro Extendido, el filtro Unscented es recursivo, por lo que en realidad se aproxima la distribución de probabilidad real del estado 𝑥𝑥𝑘𝑘, en cada instante 𝑘𝑘, por una distribución gaussiana. Como el filtro Unscented no requiere del cálculo de matrices Jacobianas, presenta una complejidad de cálculo numérico comparable a la del filtro Extendido y, además, genera estimaciones de 𝔼𝔼[𝑥𝑥𝑘𝑘 |𝑦𝑦1:𝑘𝑘] de mayor orden. Es por ello que se ha vuelto una herramienta corriente para abordar el problema de estimación de estado, en tiempo real, de un sistema dinámico no-lineal. 76 11.3. El filtro de Kalman como ejemplo de filtro adaptativo. El objetivo de esta sección es introducir las nociones básicas relacionadas con el filtrado de Kalman como uno de los algoritmos de filtros adaptivos. Un filtro adaptativo es un sistema con un filtro lineal que tiene una función de transferencia controlada por parámetros variables y un medio para ajustar esos parámetros de acuerdo con un algoritmo de optimización. Debido a la complejidad de los algoritmos de optimización, casi todos los filtros adaptativos son filtros digitales. Los filtros adaptables son necesarios para algunas aplicaciones debido a que algunos parámetros de la operación de procesamiento deseada no se conocen de antemano o están cambiando. El filtro adaptativo de lazo cerrado utiliza la retroalimentación en forma de señal de error para refinar su función de transferencia. En términos generales, el proceso adaptativo de bucle cerrado implica el uso de una función de coste, que es un criterio para el rendimiento óptimo del filtro, para alimentar un algoritmo, que determina cómo modificar la función de transferencia del filtro para minimizar el coste en la siguiente iteración. La función de coste más común es el cuadrado medio de la señal de error. El filtrado Kalman, en este sentido, es un ejemplo claro de filtro adaptativo. En él, la solución es óptima por cuanto el filtro combina toda la información observada y el conocimiento previo acerca del comportamiento del sistema para producir una estimación del estado de tal manera que el error es minimizado estadísticamente. El término recursivo significa que el filtro recalcula la solución cada vez que una nueva observación o medida ruidosa es incorporada en el sistema. El filtro de Kalman es el principal algoritmo para estimar sistemas dinámicos representados en la forma de espacio estado. En esta representación, el sistema es descrito por un conjunto de variables denominadas de estado. El estado contiene toda la información relativa al sistema a un cierto punto en el tiempo. Esta información debe permitir la inferencia del comportamiento pasado del sistema, presente o futuro, dependiendo si la problemática a encarar por parte del filtro de Kalman es el alisado, el filtrado o la predicción respectivamente. Las ecuaciones que se utilizan para derivar el filtro de Kalman se pueden dividir en dos grupos: las que actualizan el tiempo o ecuaciones de predicción y las que actualizan los datos observados o ecuaciones de actualización. Las del primer grupo son responsables de la proyección del estado al momento 𝑟𝑟+ 1 tomando como referencia el estado en el momento 𝑟𝑟 y de la actualización intermedia de la matriz de covarianza del estado. El segundo grupo de ecuaciones son responsables de la retroalimentación, es decir, incorporan nueva información dentro de la estimación anterior con lo cual se llega a una estimación mejorada del estado. Las ecuaciones que actualizan el tiempo pueden también ser pensadas como ecuaciones de pronóstico, mientras que las ecuaciones que incorporan nueva información pueden considerarse como ecuaciones de corrección. Efectivamente, el algoritmo de estimación final puede definirse como un algoritmo de pronóstico-corrección para resolver numerosos problemas. Así, el filtro de Kalman funciona por medio de un mecanismo de proyección y 77 corrección al pronosticar el nuevo estado y su incertidumbre y corregir la proyección con la nueva medida. Este ciclo se muestra en la Figura 11.3. Figura 11.3: Esquema completo del concepto general del filtrado de Kalman. 11.4. Filtro Schmidt-Kalman. El Filtro Schmidt-Kalman es una modificación del filtro Kalman para reducir la dimensionalidad de la estimación del estado, mientras que todavía se consideran los efectos del estado adicional en el cálculo de la matriz de covarianza y las ganancias de Kalman. Una aplicación común es dar cuenta de los efectos de parámetros molestos tales como los sesgos de los sensores sin aumentar la dimensionalidad de la estimación del estado. Esto asegura que la matriz de covarianza representará con precisión la distribución de los errores. La ventaja principal de utilizar el filtro Schmidt-Kalman en lugar de aumentar la dimensionalidad del espacio de estado es la reducción de la complejidad computacional. Esto puede permitir el uso de filtros en sistemas en tiempo real. Otro uso de Schmidt-Kalman es cuando los sesgos residuales son inobservables, es decir, el efecto del sesgo no se puede separar de la medición. En este caso, Schmidt-Kalman es una forma robusta de no tratar de estimar el valor del sesgo, sino sólo de hacer un seguimiento del efecto del sesgo en la verdadera distribución de errores. Para su uso en sistemas no lineales, los modelos de observación y de transición de estado pueden linealizarse en torno a la media actual y la estimación de covarianza en un método análogo al filtro Kalman Extendido. 78 12. ECUACIONES PARA MODELOS UAV En este apartado se realiza el modelado del sistema de un UAV quadrotor al objeto de presentar una ejemplo de la extensibilidad del modelado presentado en este trabajo a sistemas dinámicos más complejos y susceptibles también de utilizar diferentes variantes del filtrado de Kalman para su implementación [19], [20] y [21]. 12.1. Descripción del UAV A tal fin se ha usado un helicóptero de pequeña escala con cuatro motores coplanarios, denominado quadrotor. El movimiento del quadrotor se debe a la diferencia de velocidad a la que giren los cuatro rotores, cuyos componentes son un motor eléctrico y unas hélices. Para lograr movimiento hacia adelante, la velocidad del rotor trasero debe ser aumentada y, simultáneamente, la velocidad del rotor delantero debe ser disminuida. El desplazamiento lateral se ejecuta con el mismo procedimiento, pero usando los rotores de la derecha y de la izquierda según lo antes expuesto. El movimiento de guiñada (yaw), se obtiene a partir de la diferencia en el par de torsión entre cada par de rotores, es decir, se aceleran los dos rotores con sentido horario mientras se desaceleran los rotores con sentido anti-horario, y viceversa. 12.2. Modelado del UAV Se desarrolla a continuación el modelado basado en leyes físicas que describan la posición y orientación del helicóptero quadrotor. El modelo dinámico del helicóptero se presenta bajo la formulación matemática de Newton-Euler. Se supondrá al vehículo como un cuerpo rígido en el espacio, sujeto a una fuerza principal (empuje) y tres momentos (pares). Figura 12.1: Modelo dinámico del UAV f f f f 𝔹𝔹 𝕎𝕎 𝜉𝜉 𝑥𝑥𝑊𝑊 � � � � �  𝑦𝑦𝑊𝑊 � � � � �  m 𝑧𝑧𝑊𝑊 � � � � �  𝑥𝑥𝐵𝐵 � � � �  𝑦𝑦𝐵𝐵 � � � �  𝑧𝑧𝐵𝐵 � � � �  𝜏𝜏𝜓𝜓 𝜏𝜏𝜙𝜙 𝜏𝜏𝜃𝜃 79 Analizando de forma más detallada lo comentado respecto al movimiento en la descripción de un UAV, el par para generar un movimiento de balanceo o de roll (ángulo φ) se realiza mediante un desequilibrio entre las fuerzas f2 y f4 (ver figura 12.1). Para el movimiento de cabeceo o de pitch (ángulo θ), el desequilibrio se realizará entre las fuerzas f1 y f3. El movimiento en el ángulo de guiñada o de yaw (ángulo ψ) se realizará por el desequilibrio entre los conjuntos de fuerzas (f1, f3) y (f2, f4). Este movimiento será posible ya que los rotores 1 y 3 giran en sentido contrario a los rotores 2 y 4. Finalmente, el empuje total, que hará que el helicóptero se desplace perpendicularmente al plano de los rotores, se obtendrá como suma de las cuatro fuerzas que ejercen los rotores. Se realizarán algunas hipótesis para la simplificación del modelo: - Se desprecia el efecto de los momentos causados por un cuerpo rígido sobre las dinámicas traslacionales y el efecto suelo. - Supondremos que el centro de masa es coincidente con el origen del sistema de coordenadas fijo del helicóptero y la estructura del helicóptero simétrica, obteniendo así una matriz de inercia diagonal. A continuación, y antes de obtener el modelo del helicóptero, se mostrará cómo se estima la posición y orientación del vehículo respecto a un sistema de coordenadas de referencia inercial denominado 𝕎𝕎. Se considera que sobre el vehículo se encuentra definido un sistema de coordenadas ligado con origen en su centro de masas, como puede observarse en la Figura 12.1. El sistema de referencia {𝔹𝔹} = {𝑥𝑥𝐵𝐵 � � � �  , 𝑦𝑦𝐵𝐵 � � � �  , 𝑧𝑧𝐵𝐵 � � � �  }, donde el eje 𝑥𝑥𝐵𝐵 � � � �  es la dirección normal de ataque del helicóptero, 𝑦𝑦𝐵𝐵 � � � �  es ortogonal a 𝑥𝑥𝐵𝐵 � � � �  y es positivo hacia babor en el plano horizontal, mientras que 𝑧𝑧𝐵𝐵 � � � �  está orientado en sentido ascendiente y ortogonal al plano 𝑥𝑥𝐵𝐵 � � � �  O 𝑦𝑦𝐵𝐵 � � � �  . El sistema de coordenadas inercial {𝕎𝕎} = {𝑥𝑥𝑊𝑊 � � � � �  , 𝑦𝑦𝑊𝑊 � � � � �  , 𝑧𝑧𝑊𝑊 � � � � �  } se considerará, en principio, fijo con respecto a la Tierra. Se designa 𝜉𝜉=[𝑥𝑥,𝑦𝑦,𝑧𝑧]𝐸𝐸 como la posición del centro de masas con respecto al sistema inercial 𝕎𝕎. Por otro lado, la rotación del helicóptero vendrá dada por una matriz de rotación 𝑊𝑊𝑅𝑅𝐵𝐵, que representa la matriz de rotación que rota los ejes del sistema {𝕎𝕎} para hacerlos coincidentes con los ejes del sistema {𝔹𝔹}. La orientación de un cuerpo rígido puede ser obtenida utilizando diversos métodos. Se utilizará la convención-xyz (giro alrededor de x, y’, z”) muy utilizada para aplicaciones de ingeniería aeroespacial y que se denomina de Tait-Bryan. Los ángulos de Tait-Bryan son tres ángulos, usados para describir una rotación general en el espacio Euclídeo tridimensional a través de tres rotaciones sucesivas en tornos de ejes del sistema móvil en el cual están definidos. Los ángulos de Tait-Bryan para describir la orientación de un helicóptero serán denominados: 𝜂𝜂= �𝜙𝜙 𝜃𝜃 𝜓𝜓� 80 La configuración de la rotación de un cuerpo rígido en el espacio se produce mediante tres rotaciones sucesivas: 1. Rotación según 𝑥𝑥 de 𝜙𝜙: el primer giro es el correspondiente al ángulo de roll o de balanceo, 𝜙𝜙, y se realiza alrededor del eje 𝑥𝑥. �𝑥𝑥1 𝑦𝑦1 𝑧𝑧1�= �1 0 0 0cos 𝜙𝜙 −sin 𝜙𝜙 0sin𝜙𝜙cos 𝜙𝜙��𝑥𝑥𝐵𝐵 𝑦𝑦𝐵𝐵 𝑧𝑧𝐵𝐵� 2. Rotación según 𝑦𝑦 de 𝜃𝜃: el segundo giro se realiza alrededor del eje 𝑦𝑦 a partir del nuevo eje 𝑦𝑦𝐵𝐵, con el ángulo pitch o ángulo de cabeceo, 𝜃𝜃 para dejar el ángulo 𝑧𝑧𝐵𝐵 en su posición final. �𝑥𝑥2 𝑦𝑦2 𝑧𝑧2�= �cos𝜃𝜃0sin 𝜃𝜃 0 1 0 −sin 𝜃𝜃0cos 𝜃𝜃��𝑥𝑥1 𝑦𝑦1 𝑧𝑧1� 3. Rotación según 𝑧𝑧 de 𝜓𝜓: el tercer giro y última rotación corresponde al ángulo de guiñada o yaw, 𝜓𝜓, se realiza alrededor del eje 𝑧𝑧 a partir del nuevo eje 𝑧𝑧𝐵𝐵, para llevar el helicóptero a su posición final. �𝑥𝑥𝑦𝑦𝑧𝑧�= �cos 𝜓𝜓 −sin 𝜓𝜓0 sin𝜓𝜓cos 𝜓𝜓0 0 0 1��𝑥𝑥2 𝑦𝑦2 𝑧𝑧2� El resultado global de estos giros se muestra en la siguiente matriz: 𝑊𝑊𝑅𝑅𝐵𝐵= �cos 𝜓𝜓cos 𝜃𝜃cos 𝜓𝜓sin 𝜃𝜃sin 𝜙𝜙− sin 𝜓𝜓cos 𝜃𝜃cos 𝜓𝜓sin 𝜃𝜃cos 𝜙𝜙+ sin 𝜓𝜓sin 𝜃𝜃 sin𝜓𝜓cos 𝜃𝜃sin 𝜓𝜓sin 𝜃𝜃sin𝜙𝜙+ cos 𝜓𝜓cos 𝜃𝜃sin 𝜓𝜓sin 𝜃𝜃cos 𝜙𝜙− cos 𝜓𝜓sin 𝜃𝜃 −sin 𝜃𝜃cos 𝜃𝜃sin 𝜙𝜙cos 𝜃𝜃cos 𝜙𝜙� La matriz de rotación inversa 𝐵𝐵𝑅𝑅𝑊𝑊 es la traspuesta de 𝑊𝑊𝑅𝑅𝐵𝐵, ya que dicha matriz es ortonormal. A partir de la relación entre la derivada de la matriz 𝑊𝑊𝑅𝑅𝐵𝐵 y una cierta matriz antisimétrica se podrán obtener las ecuaciones cinemáticas de rotación del vehículo que establecen las relaciones entre las velocidades angulares. La variación de los ángulos de Tait-Bryan es diferente a la velocidad angular del vehículo en el sistema de coordenadas de dicho cuerpo rígido, 𝜔𝜔=[𝑝𝑝,𝑞𝑞,𝑟𝑟]𝐸𝐸, las cuales pueden ser medidas directamente a través de una unidad de medida inercial, IMU. La relación entre la velocidad angular en el sistema fijado al cuerpo y la variación en el tiempo de los ángulos de Tait-Bryan, en función de la derivada de los ángulos: 𝜔𝜔=𝑊𝑊 (𝜂𝜂) 𝜂𝜂󰇗 𝜔𝜔=𝑊𝑊 (𝑅𝑅) 𝜂𝜂󰇗 Si por el contrario se tiene la velocidad y se quiere obtener la derivada de los ángulos: 𝜂𝜂󰇗=𝑊𝑊−1 (𝜂𝜂) 𝜔𝜔 𝜂𝜂󰇗=𝑊𝑊−1 (𝑅𝑅) 𝜔𝜔 81 La matriz 𝑊𝑊 se puede expresar tanto en función de los ángulos como en función de los elementos de la matriz de rotación cuyas definiciones son: 𝑊𝑊 (𝜂𝜂)= �1 0 sin 𝜃𝜃 0cos 𝜙𝜙sin 𝜙𝜙 cos 𝜃𝜃 0− sin 𝜙𝜙cos 𝜙𝜙sin 𝜃𝜃� , 𝑊𝑊 (𝜂𝜂)= �1sin 𝜙𝜙tan 𝜃𝜃cos 𝜙𝜙tan 𝜃𝜃 0cos 𝜙𝜙 − sin 𝜙𝜙 0− sin𝜙𝜙 𝑟𝑟𝑎𝑎𝐷𝐷 𝜃𝜃 cos 𝜙𝜙sec 𝜃𝜃� 𝑊𝑊 (𝑅𝑅)= ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 1 0 𝑟𝑟31 0𝑟𝑟33 �𝑟𝑟322+ 𝑟𝑟332𝑟𝑟32 0− 𝑟𝑟32 �𝑟𝑟322+ 𝑟𝑟332𝑟𝑟33 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ , 𝑊𝑊 (𝑅𝑅)= ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ 1 − 𝑟𝑟31 𝑟𝑟32 𝑟𝑟322+ 𝑟𝑟332 − 𝑟𝑟31 𝑟𝑟33 𝑟𝑟322+ 𝑟𝑟332 0𝑟𝑟33 �𝑟𝑟322+ 𝑟𝑟332− 𝑟𝑟32 �𝑟𝑟322+ 𝑟𝑟332 0 𝑟𝑟32 �𝑟𝑟322+ 𝑟𝑟332 𝑟𝑟33 �𝑟𝑟322+ 𝑟𝑟332 ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤ El movimiento rotacional del helicóptero viene dado por las componentes de las velocidades angulares en los tres ejes: velocidad angular de balanceo (p), velocidad angular de cabeceo (q), y velocidad angular de guiñada (r), sobre los ejes xB � � � �  , yB � � � �  y zB � � � �  , respectivamente. Estas velocidades rotacionales son debidas a los pares ejercidos sobre el sistema ligado al cuerpo del helicóptero producidas por las fuerzas externas, las cuales definen los diferentes momentos en los tres ejes: momento de balanceo (τϕ), momento de cabeceo (τθ), y momento de guiñada (τψ ), sobre los ejes xB � � � �  , yB � � � �  y zB � � � �  , respectivamente. El movimiento de traslación viene dado por las componentes de la velocidad 𝑊𝑊𝓋𝓋𝐵𝐵=𝑎𝑎 en los tres ejes inerciales con relación a la velocidad absoluta del helicóptero. Las velocidades v y V están relacionadas por la expresión 𝑊𝑊𝓋𝓋𝐵𝐵= 𝑊𝑊𝑅𝑅𝐵𝐵 𝑊𝑊𝓋𝓋𝐵𝐵 o en notificación simplificada: v = 𝑊𝑊𝑅𝑅𝐵𝐵 V 12.3. Formulación por Newton-Euler Se obtendrá a continuación de forma sucinta las ecuaciones dinámicas del helicóptero. A efectos de nomenclatura se usará v en lugar de 𝑊𝑊𝓋𝓋𝐵𝐵, V en lugar 𝑊𝑊𝓋𝓋𝐵𝐵(𝐵𝐵)y 𝜔𝜔 en lugar de 𝑊𝑊𝜔𝜔𝐵𝐵(𝐵𝐵). Las ecuaciones dinámicas de un cuerpo rígido sujeto a fuerzas externas aplicadas al centro de masa y expresadas en el sistema de coordenadas ligado al cuerpo se pueden obtener a través de la formulación de Newton-Euler como sigue: �𝐾𝐾I 0 0 J� �V󰇗ω󰇗�+ �ω x 𝐾𝐾 V ω x J ω�= �F + Fd τ+ τd� Donde 𝐽𝐽 es la matriz de inercia, WJB(B) , 𝐼𝐼 es la matriz identidad, 𝐾𝐾 es la masa total del helicóptero, 𝐹𝐹 son las fuerzas aplicadas, 𝐹𝐹𝑑𝑑 son fuerzas que actúan como perturbaciones, τ son los pares aplicados y τd son pares que actúan como perturbaciones. Según las suposiciones realizadas al inicio del capítulo, la matriz de inercia supuesta diagonal, tiene la siguiente forma: 82 J = �I𝑥𝑥𝑥𝑥 0 0 0 I𝑦𝑦𝑦𝑦 0 0 0 I𝑧𝑧𝑧𝑧� Si se considera el vector de estado [𝜉𝜉 𝓋𝓋 𝜂𝜂 𝜔𝜔 ]𝐸𝐸, donde 𝜉𝜉=[𝑥𝑥,𝑦𝑦,𝑧𝑧]𝐸𝐸y 𝓋𝓋 ∈ℝ2 representa la posición y la velocidad lineal en { 𝕎𝕎 }, 𝜂𝜂= [𝜙𝜙 𝜃𝜃 𝜓𝜓 ]𝐸𝐸 y 𝜔𝜔 ∈ℝ3 la orientación y la velocidad angular expresada en { 𝔹𝔹 }, pudiéndose escribir las ecuaciones de movimiento de un cuerpo rígido como sigue: 𝜉𝜉󰇗= v mv󰇗= 𝑊𝑊𝑅𝑅𝐵𝐵 Fb 𝑊𝑊𝑅𝑅𝐵𝐵󰇗= 𝑊𝑊𝑅𝑅𝐵𝐵S (ω) Jω󰇗= − ω x J ω+ τ b Donde S (ω)= ( 𝑊𝑊𝑅𝑅𝐵𝐵)𝐸𝐸 𝑊𝑊𝑅𝑅𝐵𝐵󰇗 El helicóptero es un sistema mecánico sub-actuado con 6 grados de libertad y sólo 4 actuadores, que corresponden a la fuerza principal y a los tres momentos actuantes producidos por las cuatro hélices. Las fuerzas y pares externos aplicados al cuerpo del helicóptero, FB ∈ 𝔹𝔹 y 𝜏𝜏𝑏𝑏 ∈ 𝔹𝔹 respectivamente, consisten en su propio peso, en el vector de fuerzas aerodinámicas, en el empuje y en los pares desarrollados por los cuatro motores. ⎩ ⎪ ⎨ ⎪ ⎧ 𝑊𝑊𝑅𝑅𝐵𝐵 FB= −𝐾𝐾𝑏𝑏· E3+ 𝑊𝑊𝑅𝑅𝐵𝐵 E3 ��𝑏𝑏 Ω𝑖𝑖2 4 𝑖𝑖=1 �+ AT 𝜏𝜏 𝐵𝐵= − �𝐽𝐽𝑅𝑅 4 𝑖𝑖=1 ( ω x E3) Ω𝑖𝑖 +𝜏𝜏 𝑎𝑎+ AR Donde AT= �𝐴𝐴𝑥𝑥 𝐴𝐴𝑦𝑦 𝐴𝐴𝑧𝑧�𝐸𝐸 y AR= �𝐴𝐴𝑘𝑘 𝐴𝐴𝑞𝑞 𝐴𝐴𝑟𝑟�𝐸𝐸son las fuerzas y pares aerodinámicos que actúan sobre el helicóptero, 𝑏𝑏 es el coeficiente de empuje aplicado por los rotores, 𝐽𝐽𝑅𝑅 es el momento de inercia rotacional del rotor alrededor de su eje, Ω𝑖𝑖 es la velocidad de giro del iésimo rotor, el par 𝜏𝜏𝑎𝑎 es el vector de pares de control aplicados al helicóptero. La fuerza principal o entrada de control, U1, se relaciona con los rotores mediante: 𝑈𝑈1= ��𝑓𝑓𝑖𝑖 4 𝑖𝑖=1 �= ��𝑏𝑏 Ω𝑖𝑖2 4 𝑖𝑖=1 � Siguiendo el proceso matemático con el siguiente vector de estados: 𝜁𝜁= �𝜉𝜉 𝑎𝑎 𝜂𝜂 𝜔𝜔 � Expresado según las componentes de cada uno como: 83 𝜁𝜁= ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ 𝑥𝑥 𝑦𝑦𝑧𝑧 𝐴𝐴0 𝑎𝑎0 𝑤𝑤0 𝜙𝜙 𝜃𝜃 𝜓𝜓 𝑝𝑝𝑞𝑞𝑟𝑟 ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤ Introduciendo además las expresiones de relación entre velocidad angular y derivadas de los ángulos, la relación entre v y V, la expresión de 𝜏𝜏𝑎𝑎 y la expresión que relaciona el empuje total con las velocidades de los rotores, se consigue expresar la ecuación diferencial no lineal de forma compacta con la siguiente expresión: 𝜁𝜁󰇗= f (𝜁𝜁)+ �g𝑖𝑖 4 𝑖𝑖=1 (𝜁𝜁) 𝑈𝑈𝑖𝑖 Siendo: 𝑈𝑈2= 𝜏𝜏 𝜙𝜙𝑎𝑎,𝑈𝑈3= 𝜏𝜏 𝜃𝜃𝑎𝑎,𝑈𝑈4= 𝜏𝜏 𝜓𝜓𝑎𝑎, Donde: 𝜁𝜁= ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ 𝐴𝐴0 𝑎𝑎0 𝑤𝑤0 𝐴𝐴𝑥𝑥 𝐾𝐾 𝐴𝐴𝑦𝑦 𝐾𝐾 −𝑏𝑏+ 𝐴𝐴𝑧𝑧 𝐾𝐾 𝑝𝑝+𝑞𝑞sin𝜙𝜙tan 𝜃𝜃+𝑟𝑟 cos 𝜙𝜙tan 𝜃𝜃 𝑞𝑞 cos 𝜙𝜙−𝑟𝑟 sin 𝜙𝜙 𝑞𝑞 sin 𝜙𝜙sec 𝜃𝜃+ 𝑟𝑟 cos 𝜙𝜙sec 𝜃𝜃 (𝐼𝐼𝑦𝑦𝑦𝑦− 𝐼𝐼𝑧𝑧𝑧𝑧) 𝐼𝐼𝑥𝑥𝑥𝑥 𝑞𝑞𝑟𝑟− 𝐽𝐽𝑅𝑅Ω 𝐼𝐼𝑥𝑥𝑥𝑥 𝑞𝑞+ 𝐴𝐴𝑘𝑘 𝐼𝐼𝑥𝑥𝑥𝑥 (𝐼𝐼𝑧𝑧𝑧𝑧− 𝐼𝐼𝑥𝑥𝑥𝑥) 𝐼𝐼𝑦𝑦𝑦𝑦 𝑝𝑝𝑟𝑟− 𝐽𝐽𝑅𝑅Ω 𝐼𝐼𝑦𝑦𝑦𝑦 𝑝𝑝+ 𝐴𝐴𝑞𝑞 𝐼𝐼𝑦𝑦𝑦𝑦 (𝐼𝐼𝑥𝑥𝑥𝑥− 𝐼𝐼𝑦𝑦𝑦𝑦) 𝐼𝐼𝑧𝑧𝑧𝑧 𝑝𝑝𝑞𝑞+ 𝐴𝐴𝑟𝑟 𝐼𝐼𝑧𝑧𝑧𝑧 ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤ 𝐠𝐠𝟏𝟏(𝜁𝜁)= [000𝑏𝑏11𝑏𝑏12𝑏𝑏1300 0 000]𝐸𝐸 𝐠𝐠𝟏𝟏(𝜁𝜁)= �000 0 00000 𝐴𝐴 𝐼𝐼𝑥𝑥𝑥𝑥 0 0�𝐸𝐸 84 𝐠𝐠𝟏𝟏(𝜁𝜁)= �0 0 00000000𝐴𝐴 𝐼𝐼𝑦𝑦𝑦𝑦 0�𝐸𝐸 𝐠𝐠𝟏𝟏(𝜁𝜁)= �0 0 00000000 0 1 𝐼𝐼𝑧𝑧𝑧𝑧�𝐸𝐸 Con: 𝑏𝑏11= 1 𝐾𝐾 (cos 𝜓𝜓sin 𝜃𝜃cos 𝜙𝜙+sin 𝜓𝜓sin 𝜙𝜙 𝑏𝑏21= 1 𝐾𝐾 (sin 𝜓𝜓sin 𝜃𝜃cos 𝜙𝜙−cos 𝜓𝜓sin𝜙𝜙) 𝑏𝑏31= 1 𝑚𝑚 ( cos 𝜃𝜃cos 𝜙𝜙) El modelo matemático obtenido puede asumirse suficientemente preciso en la presentación de todos los movimientos funcionales de un vehículo aéreo autónomo. Sin embargo, no es adecuado para el diseño de control, porque éste depende de fuerzas y momentos aerodinámicos, como puede verse en f (𝜁𝜁) , los cuales son desconocidos ante la presencia de vientos y turbulencias imprevisibles, y de efectos giroscópicos que se consideran desconocidos debido a que, en principio, no se tiene acceso a las velocidades de los motores. En consecuencia, estos términos serán despreciados durante la fase de diseño del control y serán considerados como perturbaciones externas. 85 Apéndice B: Tratamiento de medidas Este apéndice incluye las funciones programadas en Matlab que realizan el tratamiento de las medidas obtenidas de los sensores. 1 Latitud, longitud y altitud a coordenadas 𝒙𝒙,𝒚𝒚,𝒛𝒛. function [x,y,z] = Lat_long_alt_to_xyz (Lat,Long,Alt,lat_0,long_0) %Conversión de grados a radianes Latitud_GPS_rad = Lat*(pi/180); Longitud_GPS_rad = Long*(pi/180); %Condiciones iniciales R = 6371e3; %Radio de la Tierra en metros %CALCULO DE LA POSICIÓN RESPECTO A LA POSICIÓN INICIAL %Calculo de la diferencia entre la posición actual y el origen dif_lat = Latitud_GPS_rad-lat_0; dif_long = Longitud_GPS_rad-long_0; %Calculo de la distancia entre la posición actual y el origen a = sin(dif_lat/2)^2 +... cos(lat_0)*cos(Latitud_GPS_rad)*sin(dif_long/2)^2; c = 2*atan2(sqrt(a),sqrt(1-a)); d = R*c; %Calculo del bearing (ángulo en sentido horario desde la posición origen entre la dirección del vector distancia y el norte geográfico) theta = atan2(sin(dif_long)*cos(Latitud_GPS_rad),... cos(lat_0)*sin(Latitud_GPS_rad) - sin(lat_0)*cos(Latitud_GPS_rad)*cos(dif_long)); %Calculo de las posiciones en x,y,z x = d*sin(theta); y = d*cos(theta); z = Alt; end 2 Conversión de un vector en función para poder ser integrado function Y = integral(x, n) %Se forma un vector con los inputs de la función, el cual posteriormente sale definido como función x = n(1); y = n(2); z = n(3); Y = [x y z]; End 92 3 Rotación de las aceleraciones de la IMU function A = rotacion_acel_IMU (Pitch,Roll,Yaw,Acel_x,Acel_y,Acel_z) %Conversión a radianes pitch = Pitch*(pi/180); roll = Roll*(pi/180); yaw = Yaw*(pi/180); %Matrices de rotación R_z_yaw = [cos(yaw) -sin(yaw) 0; sin(yaw) cos(yaw) 0; 0 0 1]; R_y_roll = [cos(roll) 0 sin(roll); 0 1 0; -sin(roll) 0 cos(roll)]; R_x_pitch = [1 0 0; 0 cos(pitch) -sin(pitch); 0 sin(pitch) cos(pitch)]; %Matriz de rotación total R = R_z_yaw*R_y_roll*R_x_pitch; %Aceleraciones giradas A = R*[Acel_x; Acel_y; Acel_z]; end 93 Apéndice C: Filtro de Kalman Lineal Este apéndice incluye la función programada en Matlab que implementa un filtro de Kalman lineal. Utiliza para ello las funciones definidas en el Apéndice B. En el ejemplo aquí mostrado los datos se recibían a 100Hz, por lo que el intervalo de filtrado era de 0.01s. En los ejemplos realizados a 50 Hz, la variable t debe tomar el valor 0.02s. %FILTRO DE KALMAN LINEAL clear all; clc; format long; %Vamos a implementar el filtro para un movimiento MRU %El resultado será una aproximación óptima a la posición y velocidades reales del cuerpo en 3D (coordenadas x,y,z) %INICIALIZACIÓN DE VARIABLES Y OBTENCIÓN DE DATOS %Inicialización para la fusión de datos t = 0.01; %Intervalo de integración. Tiempo de duración de un ciclo Vz_GPS = 0; %Movimiento en el plano T_GPS_ant = 0; x_GPS_ant = 0; y_GPS_ant = 0; %Inicialización de las matrices del filtro X0 = zeros(6,1); %Posición inicial y velocidades en los ejes x, y, z para entrada al filtro Kalman P0 = 7*eye(6); %Matriz de la covarianza inicial del filtro Kalman A = [1 0 0 t 0 0; 0 1 0 0 t 0; 0 0 1 0 0 t; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1]; W = 0.01*ones(6,1); %Matriz de ruido del modelo dinámico. Viento y desperfectos de rodadura de la carretera H = eye(6); C = eye(6); Z = 0.01*ones(6,1); %Matriz de ruido del sensor. Aleatorio, "disponibilidad condicional" del uso militar I = eye(6); Sol = []; %Matriz que incluye las soluciones de posición y velocidad de GPS, de la fusión y de Kalman X_ant = X0; P_ant = P0; X_Kal = X0; P_Kal = P0; %Inicialización matrices covarianza ruido sum_Q = zeros(6); sum_R = zeros(6); sum_med_Q = zeros(6,1); sum_med_R = zeros(6,1); %Cargar el archivo de texto (separado con tabulación, no con ;) datos = load('1_v_100_1.txt'); [m,n] = size(datos); 94 for i = 1:m %OPERACIÓN DE FUSIÓN %Extraer los datos del archivo de texto Tiempo_GPS = datos(i,1); Latitud_GPS = datos(i,5); Longitud_GPS = datos(i,6); Altitud_GPS = datos(i,7); Velocidad_GPS = datos(i,8); Angulo_GPS = datos(i,9); Tiempo_IMU = datos(i,10); Heading_IMU = datos(i,11); Pitch_IMU = datos(i,12); Roll_IMU = datos(i,13); Acel_x_IMU = datos(i,14); Acel_y_IMU = datos(i,15); Acel_z_IMU = datos(i,16); %Convertir coordenadas geográficas a cartesianas de GPS if i == 1 Lat_0 = Latitud_GPS*(pi/180); Long_0 = Longitud_GPS*(pi/180); end [x_GPS,y_GPS,z_GPS] = Lat_long_alt_to_xyz (Latitud_GPS,Longitud_GPS,Altitud_GPS,Lat_0,Long_0); %Rotación de ejes de la IMU Yaw = Angulo_GPS*pi/180; %Ángulo de guiñada del móvil respecto al norte geográfico (radianes) Acel_rot_IMU = rotacion_acel_IMU (Pitch_IMU, Roll_IMU, Yaw, Acel_x_IMU, Acel_y_IMU, Acel_z_IMU); %Integrar aceleraciones y velocidades IMU Vel_xyz_IMU = quadv(@(x)integral(x, Acel_rot_IMU),0,t); Pos_xyz_IMU = quadv(@(x)integral(x, Vel_xyz_IMU),0,t); %Obtención de componentes de velocidad en los ejes del movimiento Vx_GPS = 0.514*Velocidad_GPS*sin(Angulo_GPS*pi/180); %m/s Vy_GPS = 0.514*Velocidad_GPS*cos(Angulo_GPS*pi/180); %m/s %Suma de velocidades de GPS e IMU (Fusión de datos) Vx_FUS = Vx_GPS + Vel_xyz_IMU(1); Vy_FUS = Vy_GPS + Vel_xyz_IMU(2); Vz_FUS = Vz_GPS + Vel_xyz_IMU(3); %Suma de posiciones de GPS e IMU (Fusión de datos) if x_GPS == x_GPS_ant && y_GPS == y_GPS_ant Tiempo_GPS = T_GPS_ant; end x_FUS = x_GPS + Pos_xyz_IMU(1) + Vx_FUS*(Tiempo_IMU - Tiempo_GPS)/1000; %Tiempo en segundos y_FUS = y_GPS + Pos_xyz_IMU(2) + Vy_FUS*(Tiempo_IMU - Tiempo_GPS)/1000; %Tiempo en segundos z_FUS = z_GPS + Pos_xyz_IMU(3); T_GPS_ant = Tiempo_GPS; x_GPS_ant = x_GPS; y_GPS_ant = y_GPS; %Vector de estado de la fusión Med_FUS = [x_FUS; y_FUS; z_FUS; Vx_FUS; Vy_FUS; Vz_FUS]; 95 %IMPLEMENTACIÓN DEL FILTRO KALMAN %Predicción de Kalman basada en modelo dinámico: matriz de estado X_p = A*X_ant + W; if i>=2; %Es necesaria una medida anterior y un estado anterior para calcular las matrices Q y R %Calculo de la matriz de covarianza de la medida sum_med_Q = sum_med_Q + (X_p - Pre_Kal_ant); media_Q = sum_med_Q./i; sum_Q = sum_Q + (X_p - Pre_Kal_ant - media_Q)*(X_p - Pre_Kal_ant - media_Q)'; Q = sum_Q./(i-1); %Predicción de Kalman basada en modelo dinámico: matriz de covarianza P_p = A*P_ant*A' + Q; %Calculo de la matriz de covarianza de la medida sum_med_R = sum_med_R + (Med_FUS - Med_FUS_ant); media_R = sum_med_R./i; sum_R = sum_R + (Med_FUS - Med_FUS_ant - media_R)*(Med_FUS - Med_FUS_ant - media_R)'; R = sum_R./(i-1); %Calculo de la ganancia de Kalman K = (P_p*H)/(H*P_p*H' + R); %Calculo de la medida Y = C*Med_FUS + Z; %Estimación de Kalman del estado actual X_Kal = X_p + K*(Y - H*X_p); P_Kal = (I - K*H)*P_p; end %Actualización de los estados anteriores para el siguiente bucle X_ant = X_Kal; P_ant = P_Kal; Pre_Kal_ant = X_p; Med_FUS_ant = Med_FUS; %Vector solución de posiciones de GPS, FUS y Kalman Sol = [Sol; x_GPS y_GPS z_GPS x_FUS y_FUS z_FUS X_Kal(1) X_Kal(2) X_Kal(3)... Vx_GPS Vy_GPS Vz_GPS Vx_FUS Vy_FUS Vz_FUS X_Kal(4) X_Kal(5) X_Kal(6)]; end 96 Apéndice D: Filtro de Kalman Extendido Este apéndice incluye la función programada en Matlab que implementa un filtro de Kalman Extendido. Utiliza para ello las funciones definidas en el Apéndice B. En el ejemplo aquí mostrado los datos se recibían a 50Hz, por lo que el intervalo de filtrado era de 0.02s. En los ejemplos realizados a 100 Hz, la variable t debe tomar el valor 0.01s. %FILTRO DE KALMAN EXTENDIDO clear all; clc; format long; %Vamos a implementar el filtro para un movimiento MRU %El resultado será una aproximación óptima a la posición y velocidades reales del cuerpo en 3D (coordenadas x,y,z) %INICIALIZACIÓN DE VARIABLES Y OBTENCIÓN DE DATOS %Inicialización fusión de datos t = 0.02; %Intervalo de integración. Tiempo de duración de un ciclo Vz_GPS = 0; %Movimiento en el plano T_GPS_ant = 0; x_GPS_ant = 0; y_GPS_ant = 0; %Inicialización matrices del filtro X0 = zeros(6,1); %Posición inicial y velocidades en los ejes x, y, z para entrada al filtro Kalman (6x6) [x_KAL y_KAL z_KAL Vx_KAL Vy_KAL Vz_KAL] P0 = 7*eye(6); %Matriz de la covarianza inicial del filtro Kalman (6x6) A = [1 0 0 t 0 0; 0 1 0 0 t 0; 0 0 1 0 0 t; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1]; W = 0.01*ones(6,1); %Matriz de ruido del modelo dinámico. Viento y desperfectos de rodadura de la carretera Z = 0.01*ones(3,1); %Matriz de ruido del sensor. Aleatorio, "disponibilidad condicional" uso militar I = eye(6); Sol = []; %Matriz que incluye las soluciones de posición en cartesianas de GPS, de la fusión y de Kalman X_ant = X0; P_ant = P0; X_Kal = X0; P_Kal = P0; %Inicialización matrices covarianza ruido Q = eye(6); Q(6,6) = 0.85; R = eye(3); %Cargar el archivo de texto (separado con tabulación, no con ;) datos = load('2_v_50_1.txt'); [m,n] = size(datos); 97 for i = 1:m %OPERACIÓN DE FUSIÓN %Extraer los datos del archivo de texto Tiempo_GPS = datos(i,1); Latitud_GPS = datos(i,5); Longitud_GPS = datos(i,6); Altitud_GPS = datos(i,7); Velocidad_GPS = datos(i,8); Angulo_GPS = datos(i,9); Tiempo_IMU = datos(i,10); Heading_IMU = datos(i,11); Pitch_IMU = datos(i,12); Roll_IMU = datos(i,13); Acel_x_IMU = datos(i,14); Acel_y_IMU = datos(i,15); Acel_z_IMU = datos(i,16); %Convertir coordenadas geográficas a cartesianas de GPS if i == 1 Lat_0 = Latitud_GPS*(pi/180); Long_0 = Longitud_GPS*(pi/180); end [x_GPS,y_GPS,z_GPS] = Lat_long_alt_to_xyz (Latitud_GPS,Longitud_GPS,Altitud_GPS,Lat_0,Long_0); %Rotación de ejes de la IMU para hacer coindicir direcciones GPS con IMU Yaw = Angulo_GPS*pi/180; %Ángulo de guiñada del móvil respecto al norte geográfico (radianes) Acel_rot_IMU = rotacion_acel_IMU (Pitch_IMU, Roll_IMU, Yaw, Acel_x_IMU, Acel_y_IMU, Acel_z_IMU); %Integrar aceleraciones y velocidades IMU Vel_xyz_IMU = quadv(@(x)integral(x, Acel_rot_IMU),0,t); Pos_xyz_IMU = quadv(@(x)integral(x, Vel_xyz_IMU),0,t); %Obtención de componentes de velocidad en los ejes del movimiento Vx_GPS = 0.514*Velocidad_GPS*sin(Angulo_GPS*pi/180); %m/s Vy_GPS = 0.514*Velocidad_GPS*cos(Angulo_GPS*pi/180); %m/s %Suma de velocidades de GPS e IMU (Fusión de datos) Vx_FUS = Vx_GPS + Vel_xyz_IMU(1); Vy_FUS = Vy_GPS + Vel_xyz_IMU(2); Vz_FUS = Vz_GPS + Vel_xyz_IMU(3); %Suma de posiciones de GPS e IMU (Fusión de datos) if x_GPS == x_GPS_ant && y_GPS == y_GPS_ant Tiempo_GPS = T_GPS_ant; end x_FUS = x_GPS + Pos_xyz_IMU(1) + Vx_FUS*(Tiempo_IMU - Tiempo_GPS)/1000; %Tiempo en segundos y_FUS = y_GPS + Pos_xyz_IMU(2) + Vy_FUS*(Tiempo_IMU - Tiempo_GPS)/1000; %Tiempo en segundos z_FUS = z_GPS + Pos_xyz_IMU(3); T_GPS_ant = Tiempo_GPS; x_GPS_ant = x_GPS; y_GPS_ant = y_GPS; 98 %Conversión de coordenadas cartesianas a cilíndricas r_FUS = sqrt(x_FUS^2 + y_FUS^2 + z_FUS^2); theta_FUS = atan2(x_FUS, y_FUS); %Vector de estado de la fusión Med_FUS = [r_FUS; theta_FUS; z_FUS]; %IMPLEMENTACIÓN DEL FILTRO KALMAN %Predicción de Kalman basada en modelo dinámico: matriz de estado X_p = A*X_ant + W; if i>=2 %Predicción de Kalman basada en modelo dinámico: matriz de covarianza P_p = A*P_ant*A' + Q; %Calculo de la ganancia de Kalman J = [X_p(1)/sqrt(X_p(1)^2+X_p(2)^2) X_p(2)/sqrt(X_p(1)^2+X_p(2)^2) 0 0 0 0;... X_p(2)/(X_p(1)^2+X_p(2)^2) -X_p(1)/(X_p(1)^2+X_p(2)^2) 0 0 0 0; 0 0 1 0 0 0]; K = (P_p*J')/(J*P_p*J' + R); %Calculo de la medida Y = Med_FUS + Z; %Estimación de Kalman del estado actual g = zeros(3,1); %El cálculo del arco tangente si el denominador es 0 puede dar problemas if X_p(1)==0 && X_p(2)==0 g(2) = 0; else g(2) = atan2(X_p(1), X_p(2)); end g(1) = sqrt(X_p(1)^2 + X_p(2)^2 + X_p(3)^2); g(3) = X_p(3); X_Kal = X_p + K*(Y - g); P_Kal = (I - K*J)*P_p; end %Actualización de los estados anteriores para el siguiente bucle X_ant = X_Kal; P_ant = P_Kal; Pre_Kal_ant = X_p; Med_FUS_ant = Med_FUS; %Vector solución de posiciones de GPS, FUS y Kalman Sol = [Sol; x_GPS y_GPS z_GPS x_FUS y_FUS z_FUS X_Kal(1) X_Kal(2) X_Kal(3)... Vx_GPS Vy_GPS Vz_GPS Vx_FUS Vy_FUS Vz_FUS X_Kal(4) X_Kal(5) X_Kal(6)]; end 99 Apéndice E: Filtro de Kalman Matlab En este apéndice está incluido el código que implementa la función de la que dispone el programa Matlab para aplicar un filtrado Kalman sobre la fusión de datos realizada en esta memoria. %FILTRO DE KALMAN LINEAL MATLAB clear all; clc; format long; %INICIALIZACIÓN DE VARIABLES Y OBTENCIÓN DE DATOS %Inicialización fusión de datos t = 0.01; %Intervalo de integración. Tiempo de duración de un ciclo Vz_GPS = 0; %Movimiento en el plano T_GPS_ant = 0; x_GPS_ant = 0; y_GPS_ant = 0; %Inicialización matrices del filtro X0 = zeros(6,1); P0 = 500*eye(6); A = [1 0 0 t 0 0; 0 1 0 0 t 0; 0 0 1 0 0 t; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1]; C = eye(6); Sol = []; %Inicialización matrices covarianza ruido Q = eye(6); R = eye(6); %Inicialización parámetros del filtro H = dsp.KalmanFilter('StateTransitionMatrix',A, 'ControlInputPort',false,... 'MeasurementMatrix',C, 'ProcessNoiseCovariance',Q, 'MeasurementNoiseCovariance',R,... 'InitialStateEstimate',X0, 'InitialErrorCovarianceEstimate',P0); %Cargar el archivo de texto datos = load('1_v_100_1.txt'); [m,n] = size(datos); for i = 1:m %OPERACIÓN DE FUSIÓN %Extraer los datos del archivo de texto Tiempo_GPS = datos(i,1); Latitud_GPS = datos(i,5); Longitud_GPS = datos(i,6); Altitud_GPS = datos(i,7); Velocidad_GPS = datos(i,8); Angulo_GPS = datos(i,9); Tiempo_IMU = datos(i,10); Heading_IMU = datos(i,11); Pitch_IMU = datos(i,12); Roll_IMU = datos(i,13); 100 Acel_x_IMU = datos(i,14); Acel_y_IMU = datos(i,15); Acel_z_IMU = datos(i,16); %Convertir coordenadas geográficas a cartesianas de GPS if i == 1 Lat_0 = Latitud_GPS*(pi/180); Long_0 = Longitud_GPS*(pi/180); end [x_GPS,y_GPS,z_GPS] = Lat_long_alt_to_xyz (Latitud_GPS,Longitud_GPS,Altitud_GPS,Lat_0,Long_0); %Rotación de ejes de la IMU para hacer coindicir direcciones GPS con IMU Yaw = Angulo_GPS*pi/180; %Ángulo de guiñada del móvil respecto al norte geográfico Acel_rot_IMU = rotacion_acel_IMU (Pitch_IMU, Roll_IMU, Yaw, Acel_x_IMU, Acel_y_IMU, Acel_z_IMU); %Integrar aceleraciones y velocidades IMU Vel_xyz_IMU = quadv(@(x)integral(x, Acel_rot_IMU),0,t); Pos_xyz_IMU = quadv(@(x)integral(x, Vel_xyz_IMU),0,t); %Obtención de componentes de velocidad en los ejes del movimiento Vx_GPS = 0.514*Velocidad_GPS*sin(Angulo_GPS*pi/180); Vy_GPS = 0.514*Velocidad_GPS*cos(Angulo_GPS*pi/180); %Suma de velocidades de GPS e IMU (Fusión de datos) Vx_FUS = Vx_GPS + Vel_xyz_IMU(1); Vy_FUS = Vy_GPS + Vel_xyz_IMU(2); Vz_FUS = Vz_GPS + Vel_xyz_IMU(3); %Suma de posiciones de GPS e IMU (Fusión de datos) if x_GPS == x_GPS_ant && y_GPS == y_GPS_ant Tiempo_GPS = T_GPS_ant; end x_FUS = x_GPS + Pos_xyz_IMU(1) + Vx_FUS*(Tiempo_IMU - Tiempo_GPS)/1000; y_FUS = y_GPS + Pos_xyz_IMU(2) + Vy_FUS*(Tiempo_IMU - Tiempo_GPS)/1000; z_FUS = z_GPS + Pos_xyz_IMU(3); T_GPS_ant = Tiempo_GPS; x_GPS_ant = x_GPS; y_GPS_ant = y_GPS; %Vector de estado de la fusión Med_FUS = [x_FUS; y_FUS; z_FUS; Vx_FUS; Vy_FUS; Vz_FUS]; %Filtro Kalman Matlab Kal = step(H, Med_FUS); Sol = [Sol; Kal(1) Kal(2) Kal(3) Kal(4) Kal(5) Kal(6)]; end 101