Trabajo Fin de Máster Máster en Ingeniería...

i

Equation Chapter 1 Section 1

Trabajo Fin de Máster

Máster en Ingeniería Aeronáutica

Desarrollo de una práctica docente de estimación de

la orientación basada en fusión de sensores de

Smartphones

Autor: Eva Tesón Muñoz

Tutor: Rafael Vázquez Valenzuela

Dep. Ingeniería Aeroespacial y Mecánica de Fluidos

Escuela Técnica Superior de Ingeniería

Universidad de Sevilla

Desarrollo de una práctica docente

de estimación de la orientación

basada en fusión de sensores de

Smartphones

Sevilla, 2017

Trabajo Fin de Máster

Máster en Ingeniería Aeronáutica

Desarrollo de una práctica docente de estimación de

la orientación basada en fusión de sensores de

Smartphones

Autor:

Eva Tesón Muñoz

Tutor:

Rafael Vázquez Valenzuela

Profesor titular

Dep. de Ingeniería Aeroespacial y Mecánica de Fluidos

Escuela Técnica Superior de Ingeniería

Universidad de Sevilla

Sevilla, 2017

Trabajo Fin de Grado: Desarrollo de una práctica docente de estimación de la orientación basada en fusión de

sensores de Smartphones

Autor: Eva Tesón Muñoz

Tutor: Rafael Vázquez Valenzuela

El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros:

Presidente:

Vocales:

Secretario:

Acuerdan otorgarle la calificación de:

Sevilla, 2017

El Secretario del Tribunal

A todos los que me han apoyado

en este camino

i

Agradecimientos

En primer lugar he de agradecer a mi tutor de proyecto Rafael Vázquez Valenzuela por ofrecerme

este atractivo proyecto y proporcionarme todas las facilidades para llevarlo a cabo.

Por otro lado agradecer a los alumnos Jason Condor Romero, Santiago Hoyos Martín, Juan Ramón Parra

Vilar e Inmaculada Soldado Serrano por su implicación en la realización de la práctica y sus posteriores

comentarios, siendo éstos la principal realimentación del presente proyecto.

Finalmente, dar las gracias a todos aquellos que me apoyaron en este camino.

Eva Tesón Muñoz

Sevilla, 2017

iii

Resumen

A la hora de estimar la orientación de un elemento se requiere la fusión de los datos recibidos por

sensores. La condición mínima para resolver este problema requiere de giróscopos que, junto con las condiciones

iniciales, permite resolver las ecuaciones diferenciales cinemáticas de la actitud de un vehículo o plataforma

móvil.

En teoría, una adecuada combinación de mediciones adquiridas por diversos sensores producirá siempre

un resultado mejor que el que se obtendría usando solamente el mejor de los sensores disponibles. Es por ello

que la estimación a partir de giróscopos se puede integrar con las medidas de otros sensores para mejorar la

precisión de la estimación.

El objeto de este proyecto se basa en partir de las medidas proporcionadas por los sensores de un

Smartphone (concretamente giróscopo, acelerómetro y magnetómetro) para estimar la orientación del mismo en

tiempo real mediante teoría de cuaterniones aplicando diferentes métodos de estimación de actitud en la

plataforma Matlab. Como objetivo final se diseñará y redactará una práctica numérica en la cual se tendrán que

implementar dicho métodos por parte de los alumnos.

Para ello, se parte inicialmente de conceptos básicos de la cinemática de la actitud y del Filtro de Kalman

que serán de gran utilidad para la comprensión y desarrollo del análisis tanto analítico como numérico para la

estimación de la actitud en tiempo real. La resolución de este problema se llevará a cabo para diferentes métodos,

comprobándose si verdaderamente la fusión de datos mejora la predicción por parte de un solo sensor.

Posteriormente, se desarrollará el boletín de la práctica para que los alumnos puedan implementar y visualizar

estos conceptos, suponiendo ésta una importante realimentación para una práctica real futura.

Finalmente, se propondrán posibles ampliaciones y mejoras de los estudios llevados a cabo a lo largo del

trabajo que podrían generar nuevas mejoras en el futuro y que resultarían de gran interés en numerosas

aplicaciones.

v

Abstract

When estimating the orientation of an element requires the fusion of the data received by sensors. The

minimum condition to solve this problem requires gyroscopes that, together with the initial conditions, allow

solving the kinematic differential equations of the attitude of a vehicle or the mobile platform.

In theory, a suitable combination of measurements acquired by various sensors produced always a better

result than what is obtained using only the best of the available sensors. That is why the estimation from

gyroscopes can be integrated with the measurements of other sensors to improve the precision of the estimation.

The object of this project is based on the measurements provided by Smartphone sensors (specifically

gyroscope, accelerometer and magnetometer) to estimate the orientation of the same in real time using the theory

of quaternions applying different methods of attitude estimation on the platform Matlab. The final objective is

to design a numerical practice in which they will have to implement these methods by the students.

To do this, we start with the basic concepts of the attitude kinematics and the Kalman filter, which are

useful for the understanding and development of both analytical and numerical analysis for the estimation of the

attitude in real time. The resolution of this problem is carried out for various methods, actually checking data

fusion improves prediction by the part of a single sensor. Subsequently, the bulletin of the practice was

developed so that the students apply and visualize these concepts, supposing an important feedback for a future

real practice.

Finally, it will propose possible extensions and improvements of the studies carried out in this work that

might generate new improvements in the future and that would be of great interest in many applications.

vii

Índice

Agradecimientos i

Resumen iii

Abstract v

Índice vii

Índice de Tablas ix

Índice de Figuras xi

Notación xiii

1 Introducción 1 1.1. Objetivos y alcance del proyecto 3 1.2. Contenido y estructura del documento 4

2 Giróscopos, acelerómetros y magnetómetros. Aplicación en Smartphones 7 2.1. Giróscopos 7 2.2. Acelerómetros 9 2.3. Magnetómetros 10 2.4. Disponibilidad de sensores en Smartphones 10

3 Cinemática de la Actitud 13 3.1. Sistemas de referencia 13

3.1.1. Sistema en ejes navegación (NED) 13 3.1.2. Sistema en ejes cuerpo 14

3.2. Métodos de representación de Actitud 14 3.2.1. Matriz de cosenos directores (DCM) 15 3.2.2. Ángulos de Euler 16 3.2.3. Ángulo y eje de Euler 17 3.2.4. Cuaterniones 18

3.3. Ecuaciones Diferenciales Cinemáticas de la Actitud 21 3.4. Discretización de las Ecuaciones Diferenciales Cinemáticas de la Actitud 22 3.5. Cuaternión de error 22 3.6. Cuaternión de rotación de Euler 23 3.7. Método TRIAD 24 3.8. Archivos de ayuda en Matlab 25

4 Filtro de Kalman 27 4.1. El Filtro de Kalman discreto (KF) 27

4.1.1. Origen computacional del filtro 28 4.1.2. Origen probabilístico del filtro 29

4.1.3. Algoritmo del Filtro de Kalman discreto 29 4.2. El Filtro de Kalman Extendido (EKF) 31 4.3. El Filtro de Kalman Extendido Multiplicativo (MEKF) 34

4.3.1. MEKF para las EDC 35 4.3.2. Consideraciones para el MEKF 36

5 Introducción a la metodología de la práctica 39 5.1. Aplicación Sensor Fusion 39

5.1.1. Select Sensor 39 5.1.2. Log Data 40 5.1.3. Sensor List 41 5.1.4. About/Help 41

5.2. Archivos para conexión entre Smartphone y PC 41 5.3. Método de conexión entre Smartphone y PC 45

6 Calibración de sensores 47 6.1. Análisis de la frecuencia de la toma de medidas de los sensores 47 6.2. Estadística del error. Distribución normal o Gaussiana 50

6.2.1. Variables aleatorias continuas unidimensionales 50 6.2.2. Variables aleatorias continuas multidimensionales 51

6.3. Tendencia normal de las medidas de los sensores 52 6.4. Archivos Matlab 56

7 Estimación de la orientación del Smartphone en tiempo real 57 7.1. Inicialización del estado 57 7.2. Posición de referencia inercial 57 7.3. Matrices de covarianza del ruido inducido 58 7.4. Paso de tiempo y frecuencia 59 7.5. Estimación de la Actitud mediante giróscopos 59 7.6. Estimación de la Actitud mediante el método TRIAD 61 7.7. Estimación de la Actitud mediante el MEKF 63

7.1.1. Outlier Rejection para el MEKF 67 7.8. Archivos Matlab 69

8 Práctica de Dinámica de Vehículos Espaciales 71 8.1. Boletín de la práctica 71 8.2. Cuestionario a los alumnos 72

8.2.1. Resultados de la valoración de los alumnos 72

9 Conclusiones y trabajo futuro 75 9.1. Conclusiones 75 9.2. Mejoras y trabajo futuro 76

Apéndice A. Boletín de prácticas 79

Apéndice B. Cuestionario para el alumno 91

Referencias 93

ix

ÍNDICE DE TABLAS

Tabla 2-1. Venta mundial de Smartphones a usuarios finales según SO. 10

Tabla 2-2. Sensores incluidos en las diferentes versiones de iPhone. 11

Tabla 2-3. Sensores incluidos en las diferentes versiones de Android. 11

Tabla 2-4. Smartphones válidos disponibles en el mercado. 12

Tabla 3-1. Equivalencia de notación Matlab/Teoría para quat2DCM.m. 25

Tabla 3-2. Equivalencia de notación Matlab/Teoría para Sq.m. 25

Tabla 3-3. Equivalencia de notación Matlab/Teoría para TRIAD.m. 26

Tabla 5-1. Descripción de funciones de Matlab aplicadas en los archivos FilterTemplate.m y

FilterTemplate_MEKF.m. 45

Tabla 6-1. Media y varianza de las medidas en una primera simulación arbitraria estática. 53

Tabla 6-2. Ruido inducido por giróscopo y acelerómetro en una primera simulación arbitraria estática. 53

Tabla 6-3. Media y varianza de las medidas en una segunda simulación arbitraria estática. 54

Tabla 6-4. Ruido inducido por giróscopo y acelerómetro en una segunda simulación arbitraria estática. 54

Tabla 6-5. Media y varianza de las medidas en una tercera simulación arbitraria estática. 55

Tabla 6-6. Ruido inducido por giróscopo y acelerómetro en una tercera simulación arbitraria estática. 55

Tabla 6-7. Definición de las salidas del archivo Distribucion_medidas.m. 56

Tabla 7-1. Equivalencia de notación Matlab/Teoría para Evolucion_giroscopos_EDC.m y

Evolucion_giroscopos_Euler.m. 60

Tabla 7-2. Equivalencia de notación Matlab/Teoría para MEKF.m. 65

Tabla 7-3. Resumen de archivos en Matlab aplicados en el Capítulo 7. 69

Tabla 8-1. Media y desviación típica de la evaluación por parte de los alumnos. 72

xi

ÍNDICE DE FIGURAS

Figura 1-1. Giróscopo de Leon Focault. 1

Figura 1-2. Distribución de sensores en Smartphone. 3

Figura 2-1. Sistema de referencia de los giróscopos MEMS en Smartphone. 7

Figura 2-2. Esquema de funcionamiento de un giróscopo MEMS. 8

Figura 2-3. Sistema de referencia de los acelerómetros en Smartphone. 9

Figura 2-4. Esquema de funcionamiento de un acelerómetro piezoeléctrico. 9

Figura 2-5. Magnetómetro MEMS. 10

Figura 3-1. Sistema de referencia NED. 14

Figura 3-2. Ángulos de Tait-Bryan/Euler (𝜑,𝜃,𝜓). 16

Figura 4-1. Diagrama de flujo del algoritmo KF discreto. 31

Figura 4-2. Diagrama de flujo del algoritmo EKF discreto. 34

Figura 4-3. Diagrama de flujo del algoritmo MEKF discreto para las EDC en cuaterniones. 38

Figura 5-1. Logotipo de la aplicación Sensor Fusion. 39

Figura 5-2. Capturas de la aplicación Sensor Fusion. 40


Figura 5-4. Botón de ajustes de la aplicación Sensor Fusion. 41

Figura 5-5. Diagrama de flujo del archivo FilterTemplate.m. 42

Figura 5-6. Subdiagrama de flujo del archivo FilterTemplate.m. 43

Figura 5-7. Modificación del subdiagrama de flujo del archivo FilterTemplate.m para el archivo

FilterTemplate_MEKF.m. 44


Figura 6-1. Medidas obtenidas en cada instante de tiempo para cada eje y sensor en una primera simulación

arbitraria. 47

Figura 6-2. Medidas obtenidas en cada instante para cada eje y sensor en una segunda simulación arbitraria 48

Figura 6-3. Medidas obtenidas en cada instante para cada eje y sensor en una tercera simulación arbitraria. 48

Figura 6-4. Medidas obtenidas en cada instante para cada eje y sensor frente al tiempo en una simulación

arbitraria. 49

Figura 6-5. Ejemplo de evolución caótica del ruido en una medida. 50

Figura 6-6. Representación de la distribución normal en torno a la media e intervalos de confianza. 51

Figura 6-7. Histograma de medidas frente a distribución normal asociada para cada eje y sensor en una

primera simulación arbitraria estática. 53


segunda simulación arbitraria estática. 54

Figura 6-9. Histograma de medidas frente a distribución normal asociada para cada eje y sensor en una tercera

simulación arbitraria estática. 55

Figura 7-1. Orientación del Smartphone en la posición de referencia inercial tras la simulación con el archivo

Evolucion_giroscopos_EDC_Euler.m. 61


Evolucion_TRIAD.m. 63


MEKF_comparacion.m. 65


MEKF_giroscopos_EDC.m. 66


MEKF_outlier_rejection.m. 68

xiii

Notación

𝐺𝑃𝑆

𝑆𝑂

𝑀𝐸𝑀𝑆

Global Positioning System

Sistema Operativo

Micro-Electro-Mechanical System

𝑁𝐸𝐷 North-East-Down

SR Sistema de Referencia

𝐷𝐶𝑀 Matriz de Cosenos Directores

𝜓 Ángulo de guiñada de Euler

𝜃 Ángulo de cabeceo de Euler

𝜑 Ángulo de alabeo de Euler

𝐸𝐷𝐶 Ecuaciones Diferenciales Cinemáticas de la Actitud

𝐼𝑑 Matriz identidad (3×3)

KF Filtro de Kalman

EKF Filtro Extendido de Kalman

MEKF Filtro Extendido Multiplicativo de Kalman

Capítulo 1

1

1

CAPÍTULO 1 INTRODUCCIÓN

Junto con la posición y la velocidad, la actitud (u orientación) relativa a un sistema de referencia completa

los parámetros de navegación de un vehículo en el espacio. La navegación es una de las primeras y más

importantes aplicaciones del Filtro de Kalman y de la fusión de medidas de sensores que se complementan para

resolver un problema no trivial. Al mismo tiempo, resulta un problema desafiante desde el punto de vista de la

complejidad y desde el punto de vista numérico.

Una visión moderna del problema de la navegación aérea es el conocimiento y monitorización para cada

instante de tiempo de la posición, orientación y velocidad de la platarforma (en este caso el Smartphone) para

actuar en consecuencia. Se podría definir este problema como crítico a lo largo de la historia de la humanidad.

No fue hasta el siglo XIX cuando llegó la invención del giróscopo gracias a un experimento fallido para explicar

la rotación de la Tierra desarrollado por Leon Focault.

Lo que hace especial a este aparato es que, por efecto giroscópico, cuando éste tiene una velocidad

angular, genera una oposición frente a perturbaciones. Dichas perturbaciones se reflejan como una nutación y

precesión, las cuales se pueden medir y procesar para obtener una medida de la velocidad angular a la que está

sometido el sistema. Los primeros sistemas de navegación se basaron en los denominados sistemas de

navegación inercial, los cuales basan su funcionamiento en la integración de las mediciones de giróscopos y

acelerómetros, lo que hizo posible el guiado de vehículos mediante la reconstrucción de su trayectoria. El primer

aparato en implementar este sistema fue el misil V2 alemán ya que, siendo el tiempo de vuelo tan corto, los

sistemas inerciales de la época acumulaban un error aceptable, y podían ser utilizarlos para alcanzar puntos

estratégicos como Londres o Bélgica.

Pasada la guerra, se dieron grandes avances en la introducción de plataformas giro-estabilizadas para

poder medir las aceleraciones y velocidades angulares respecto a un sistema de ejes inercial, facilitando así su

procesamiento y permitiendo abandonar los tediosos giróscopos mecánicos en favor de otros electrónicos, los

Figura 1-1. Giróscopo de Leon Focault.

Introducción

cuales no sólo abarataban el coste sino también el mantenimiento (no tenían partes móviles). Además, con el

avance de los ordenadores y el aumento de la potencia de cálculo se abandonó la idea de mesa giro-estabilizada

(debido a la presencia de numerosos problemas desde el punto de vista de inicialización y mantenimiento) y se

aplicaron sensores directamente al cuerpo, complicando así las ecuaciones que permiten obtener la trayectoria

pero que gracias a dicha nueva potencia de cálculo pasó a ser irrelevante.

Si bien la tecnología ha ido evolucionando, en la actualidad los sistemas de navegación en aplicaciones

aeroespaciales de precisión conservan una arquitectura basada en giróscopos y acelerómetros. Esto se debe a las

ventajas que estos sistemas presentan: su alta velocidad de muestreo (que los hace aptos para vehículos con

rápidas dinámicas) y su alta inmunidad a interferencias externas. Por otro lado, una de las principales desventajas

de los sistemas inerciales es que el error en la determinación de la posición, velocidad y orientación del vehículo

crece de manera no acotada con el tiempo. Esta desventaja es inherente a la arquitectura del sistema y su

severidad depende de la tecnología y calidad de los instrumentos inerciales que inciden en la velocidad de

crecimiento de los errores. Si bien es posible diseñar sistemas de navegación inercial que satisfagan los

requerimientos de las misiones aeroespaciales más exigentes, la elevada relación costo-precisión (y volumen)

que estos sistemas presentan resulta un factor determinante a ser tenido en cuenta en el diseño del sistema de

navegación.

Sin embargo, para aplicaciones en telefonía móvil la integración de la información inercial de giróscopos

y acelerómetros junto con medidas de magnetómetros y otros sensores que relacionan la orientación de la

plataforma respecto a un determinado sistema de referencia proporciona una solución aceptable para el cálculo

de la actitud del mismo.

El uso de teléfonos inteligentes ha experimentado un enorme incremento en los últimos años.

Actualmente, la mayoría de la gente usa un Smartphone competente diseñado para la interacción con el medio

que le rodea. Estos Smartphones están equipados con sensores inerciales, GPS, sensores de luz y de proximidad,

así como de micrófonos y cámaras. Los modelos más avanzados tienen mejores sensores haciendo que estos

dispositivos sean muy interesantes desde el punto de vista de la fusión de datos de sensores, proporcionando una

información sensorial multimodo y una gran potencia de computación en un pequeño, fácil y accesible

dispositivo móvil.

Generalmente, los dispositivos con el sistema operativo Android cuentan con sensores incorporados que

miden el movimiento, la orientación y diversas condiciones ambientales. Estos sensores son capaces de

proporcionar datos en bruto con alta precisión y son útiles si se desea monitorear el movimiento o

posicionamiento tridimensional del dispositivo.

La plataforma Android incluye tres grandes categorías de sensores:

Sensores de movimiento

Estos sensores miden las fuerzas de aceleración y las fuerzas de rotación a lo largo de tres ejes. Esta

categoría incluye acelerómetros, sensores de gravedad, giróscopos y sensores de vectores rotacionales.

Sensores ambientales

Estos sensores miden diversos parámetros ambientales, tales como temperatura y presión del aire

ambiente, iluminación y humedad. Esta categoría incluye barómetros, fotómetros y termómetros.

Sensores de posición

Estos sensores miden la posición física de un dispositivo. Esta categoría incluye sensores de orientación

y magnetómetros.

En la Figura 1-2 se puede ver la distribución de los sensores nombrados en el Smartphone.

3

3 Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones

El caso más típico a la hora de estimar la orientación de un elemento es la necesidad de integrar las

señales recibidar por tres giróscopos que, junto con las condiciones iniciales, resuelve las ecuaciones

diferenciales cinemáticas de la actitud del vehículo o plataforma móvil.

En teoría, una adecuada combinación de mediciones adquiridas por diversos sensores producirá siempre

un resultado mejor que el que se obtendría usando solamente el mejor de los sensores disponibles. Es por ello

que la estimación a partir de gisóscopos se puede integrar con las medidas de otros sensores para mejorar la

precisión de la estimación, siendo ésta integración con los acelerómetros y magnetómetros el objetivo de este

proyecto.

El algoritmo para llevar a cabo dicha integración más conocido es el Filtro de Kalman. Se trata de un

proceso iterativo apoyado en una serie de ecuaciones matemáticas que proporcionan una solución que disminuye

el error que existe en las dos señales recibidas de acelerómetros y giróscopos y del magnetómetro. Los principios

de operación del Filtro de Kalman están divididos en dos partes: predicción y actualización, tal y como se verá

en el apartado correspondiente. Dentro de los distintos posibles filtros que se pueden utilizar para este tipo de

problemas el Filtro de Kalman es el más atractivo puesto que es aquel que minimiza la varianza del error en la

estimación.

Finalmente, notar que el algoritmo que se va a desarrollar en este proyecto responde a la determinación

de actitud, problema que está incluido en la resolución de los Sistemas de Navegación.

1.1. Objetivos y alcance del proyecto

El objeto de este proyecto se basa en partir de las medidas proporcionadas por los sensores de un

Smartphone (concretamente giróscopo, acelerómetro y magnetómetro) para estimar la orientación del mismo en

tiempo real mediante teoría de cuaterniones aplicando diferentes métodos de estimación de actitud en la

plataforma Matlab. Como objetivo final se diseñará y redactará una práctica numérica en la cual se tendrán que

implementar dicho métodos por parte de los alumnos.

Figura 1-2. Distribución de sensores en Smartphone.

Introducción

El tratamiento de datos transmitidos en tiempor real desde un Smartphone permite a los estudiantes

adquirir experiencia en el diseño del Filtro de Kalman y en el uso de cuaterniones para determinar la orientación

del mismo, proporcionando un enfoque práctico y lúdico para la asignatura “Dinámica de vehículos espaciales”

del Máster en Ingeniería Aeronáutica. Esta experiencia también podrá ser aplicable para la asignatura de

“Navegación Aérea” del Grado en Ingeniería Aeroespacial.

Para desarrollar los algoritmos se han tomado como base los ficheros de Matlab de [1] y se han aplicado

como referencias principales [2] y [3], particularizando los métodos de estimación de actitud y la nomenclaura

de los códigos a los conocimientos adquiridos en las asignaturas citadas.

En mayor detalle, los objetivos del proyecto son:

Interpretar y adaptar los ficheros base de [1] para permitir la conexión entre Smartphone y la plataforma

Matlab.

Implementar algoritmo en Matlab partiendo de los ficheros proporcionados en [1] para la calibración de

giróscopo, acelerómetro y magnetómetro particularizando para cada dispositivo y analizando los datos

obtenidos de cada sensor.

Entender la representación y estimación de la orientación del dispositivo mediante las EDC, el cuaternión

de rotación de Euler y el método TRIAD e implementar el algoritmo correspondiente en tiempo real para

la estimación de la orientación del dispositivo en Matlab partiendo de los ficheros proporcionados en [1].

Entender el concepto de Filtro Extendido Multiplicativo de Kalman particularizándolo para el dispositivo

móvil (sistema discreto-discreto) e implementar el algoritmo correspondiente en tiempo real para la

estimación de la orientación del dispositivo en Matlab partiendo de los ficheros proporcionados en [1].

Comparar la eficiencia y deriva de la estimación de la actitud en cada método.

Redactar la memoria y preparar los archivos base en Matlab para la realización de la práctica numérica

por parte de los alumnos teniendo como realimentación para la misma la simulación real de una práctica

con alumnos de la asignatura “Dinámica de vehículos espaciales” del Máster en Ingeniería Aeronáutica

y una posterior encuesta.

1.2. Contenido y estructura del documento

Una vez definidos los objetivos y el alcance del proyecto, queda comentar cómo se estructurarán los

contenidos del mismo. Tras un primer capítulo de introducción, el resto de contenidos seguirán el siguiente

orden:

Capítulo 2: Giróscopos, acelerómetros y magnetómetros. Aplicación en Smartphones. En este

primer capítulo se presenta el estado del arte de la tecnología actual de los sensores presentes en

los Smartphones, dando una visión concreta del tipo de sensores con los que se trabajará en el resto

del proyecto. Además se ofrece un estudio de mercado sobre los sensores que éstos incluyen y

aquellos que son accesibles a compra actualmente con visión a su obtención para la realización de

la práctica con alumnos.

5

5 Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones

Capítulo 3: Cinemática de la Actitud. Para introducir la base teórica de la que se partirá para

establecer la estimación de la orientación del Smartphone, se muestra en este capítulo los diferentes

métodos de estimación de actitud particularizando para los sistemas de referencia que se

considerarán, así como una profundización en el álgebra de cuaterniones que facilitará el

planteamiento de las Ecuaciones Diferenciales Cinemáticas de la Actitud, el método TRIAD y el

cuaternión de rotación de Euler. Así mismo, se ofrecen archivos de ayuda que facilitarán la

programación en Matlab de estos métodos en capítulos posteriores.

Capítulo 4: Filtro de Kalman. En base al objetivo principal del proyecto, en este cuarto capítulo se

proporciona una visión somera de los Filtros de Kalman lineales y no lineales unidimensionales y

se profundiza en el caso del Filtro de Kalman Extendido Multiplicativo, cuya aplicabilidad al

problema de la estimación de la actitud se muestra para un sistema discreto-discreto, siendo éste el

tipo de sistema correspondiente al analizado en el presente proyecto.

Capítulo 5: Introducción a la metodología de la práctica. Para facilitar la comprensión del

tratamiento de datos y transmisión de los mismos entre Smartphone y PC, se presenta en este

capítulo de manera resumida la aplicación del Sitema Android que permitirá enlazar los datos de

los sensores del Smartphone en tiempo real con Matlab, los paquetes de trabajo que implementa y

los archivos base en Matlab que se proporcionan como partida a la estimación de la actitud para

los alumnos de la práctica.

Capítulo 6: Calibración de sensores. En este capítulo se estudia la frecuencia e instante de inicio

en la toma de medidas de los sensores que se usarán en la práctica así como un breve estudio sobre

la consideración del ruido que inducen las medidas de dichos sensores como ruido gaussiano.

Finalmente se presentan los archivos Matlab aplicados a lo largo del capítulo.

Capítulo 7: Estimación de la orientación del Smartphone en tiempo real. Este capítulo permite

poner en práctica los conocimientos teóricos comentados en los capítulos anteriores. En el mismo,

se presentan los pasos a seguir para la programación en Matlab de los diferentes métodos de

estimación de actitud propuestos, analizando y comparando los resultados obtenidos. Además, se

especifica el cálculo de la inicialización de la posición y referencias, así como el incremento de

tiempo entre medidas consecutivas y el rechazo de medidas no válidas que puedan perjudicar el

algoritmo de estimación de actitud. De nuevo, se presentan los archivos Matlab aplicados a lo largo

del capítulo.

Capítulo 8: Práctica de Dinámica de Vehículos Espaciales. Para cumplir el objetivo final del

presente proyecto, se dedica un capítulo completo a la presentación del boletín final de la práctica

a llevar a cabo por los alumnos, indicando las mejoras propuestas tras la realización de la misma

así como una valoración por parte los alumnos que supondrá una realimentación esencial con vistas

a la realización en un futuro de una práctica real para la asignatura de Dinámica de Vehículos

Espaciales y que puede extenderse a otras.

Capítulo 9: Conclusiones y trabajo futuro. Una vez desarrollados todos los contenidos de los

capítulos anteriores, se extraerán las debidas conclusiones en cuanto a las hipótesis preestablecidas

y los diferentes métodos de estimación de actitud. Para finalizar, se plantearán posibles mejoras o

modificaciones futuras.

Introducción

Capítulo 2

7

7

CAPÍTULO 2 GIRÓSCOPOS, ACELERÓMETROS Y

MAGNETÓMETROS. APLICACIÓN EN

SMARTPHONES

Los sensores utilizados en la determinación de la actitud se pueden agrupar en dos clases. La primera de

ellas está compuesta por los instrumentos de medición absoluta que, conociendo la posición de la plataforma,

calculan las direcciones vectoriales con respecto a un sistema de referencia inercial de los diferentes cuerpos que

encontramos en el espacio y de las líneas de fuerza del campo magnético terrestre. De esta forma, midiendo y

comparando estas direcciones respecto de las de un sistema de referencia fijo y conocido, se determina la

posición relativa entre el sistema de coordenadas del sensor (ejes cuerpo) y el inercial. Éste es el caso de los

magnetómetros. En la segunda clase se encuentran los sensores de medición relativa, los cuales determinan un

ratio de cambio de la actitud. Por ello en este caso es necesario conocer un punto de partida, o tener alguna

información externa, que permita usar las variaciones para calcular la actitud actual de la plataforma. Además,

muchos de estos sensores presentan ruido en las señales, lo que provocará ciertas imprecisiones en las medidas

tomadas si no se corrigen con información proveniente de los sensores absolutos de medición. Éste es el caso

de los giróscopos y acelerómetros.

A continuación se introduce el principio de funcionamiento de los sensores que se van a usar en la

práctica y su aplicación y evolución en los Smartphones.

2.1. Giróscopos

El giróscopo es un dispositivo mecánico formado esencialmente por un cuerpo con simetría de rotación

que gira alrededor de su eje de simetría. Cuando se somete el giróscopo a un momento de fuerza que tiende a

cambiar la orientación del eje de rotación, su comportamiento es aparentemente paradójico ya que el eje de

rotación, en lugar de cambiar de dirección como lo haría un cuerpo que no girase, cambia de orientación en una

dirección perpendicular a la dirección «intuitiva».

Los giróscopos se pueden clasificar de forma general en tres grandes tipos: los mecánicos, los ópticos y

los giróscopos MEMS. Sin embargo, son éstos últimos los más utilizados en los dispositivos móviles (ver Figura

2-1). A pesar de los años de desarrollo, los giróscopos mecánicos y los ópticos requieren procedimientos y

técnicas complicadas de construcción, lo que los convierte en dispositivos económicamente caros y no muy

accesibles. En cambio los sensores MEMS son construidos utilizando técnicas de micro-maquinado en silicio y

pueden llegar a ser muchos más económicos de construir. Los giróscopos MEMs hacen uso del efecto Coriolis

[4].

Figura 2-1. Sistema de referencia de los giróscopos MEMS en Smartphone.

Giróscopos, acelerómetros y magnetómetros. Aplicación en smartphones

El esquema del sensor, como se puede observar en la Figura 2-2, es bastante simple. Una masa m,

colocada en el extremo de una viga en voladizo que se encuentra vibrando.

Esta vibración provoca que la masa se mueva según una senoide de amplitud constante y, por tanto, con

una velocidad dada por la siguiente expresión:

𝑣 = 𝐴𝑤𝑛sin(𝑤𝑛𝑡)

donde 𝐴 es la amplitud del senoide y 𝑤𝑛 la frecuencia natural del sistema.

Además, por el hecho de encontrarse en un movimiento vibratorio, la masa experimentará una

aceleración de Coriolis que será medida por un elemento detector y convertida en un voltaje mediante un circuito

de acondicionamiento de señal, donde el voltaje será:

𝑉 = 𝑘𝑐|𝑎𝑐|

siendo 𝑘𝑐 una constante de proporcionalidad del sistema.

Por su parte, es sabido que la aceleración de Coriolis es proporcional a la velocidad de la masa y a su

velocidad de rotación, teniendo que:

𝑎𝑐 = 2Ω × 𝑣

donde Ω es la velocidad angular del sistema.

De esta manera, se puede establecer una relación entre el voltaje de salida y la velocidad angular que se

pretende determinar.

𝑉 = 𝑘𝑐|𝑎𝑐| = 𝑘𝑐|Ω × 𝑣|

Teniendo en cuenta que Ω y 𝑣 son perpendiculares, es decir, que se verifica|Ω × 𝑣| = Ω|𝑣|, queda la

siguiente expresión:

𝑉 = 2𝑘𝑐 Ω|𝐴𝑤𝑛sin(𝑤𝑛𝑡)| = 2𝑘𝑐 Ω𝐴𝑤𝑛 = 𝐾𝑐 Ω

siendo 𝐾𝑐 una constante de calibración del sistema.

En conclusión, el funcionamiento de este dispositivo se basará en proporcionar un voltaje de salida a

través de la medida de la aceleración de Coriolis para después determinar la velocidad angular con la que está

rotando el cuerpo.

Figura 2-2. Esquema de funcionamiento de un giróscopo MEMS.

9

9

Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones

2.2. Acelerómetros

Los acelerómetros son instrumentos destinados a medir las aceleraciones a las que está sometido un

cuerpo. Concretamente, son sensores inerciales que miden la fuerza de inercia generada cuando una masa es

afectada por un cambio de velocidad.

Actualmente es posible construir acelerómetros de tres ejes (X,Y,Z) (ver Figura 2-3) en un sólo chip de

silicio, incluyendo en el mismo la parte electrónica que se encarga de procesar las señales.

En los Smartphones, los acelerómetros más usados son los piezoeléctricos. Éstos se basan en el efecto

piezoeléctrico , más concretamente en las propiedades de los cristales piezo-eléctricos. Los cristales al sufrir una

variación en su estructura cristalina producen una corriente eléctrica. De esta forma, si se coloca un cristal de

este tipo entre una masa inercial y una base o carcasa que está unida al cuerpo cuya aceleración se desea medir,

se producirá una corriente cuando la masa ejerza una fuerza sobre el cristal o, lo que es lo mismo, cuando el

cuerpo esté sometido a una aceleración, que se podrá calcular midiendo la corriente creada (ver Figura 2-4).

Figura 2-3. Sistema de referencia de los acelerómetros en Smartphone.

Figura 2-4. Esquema de funcionamiento de un acelerómetro piezoeléctrico.


2.3. Magnetómetros

Los magnetómetros son dispositivos usados para cuantificar una señal magnética en fuerza o dirección.

Se usan, principalmente, para la medición del campo magnético terrestre y para la detección de anomalías

magnéticas de diferentes tipos.

En los Smartphones, los magnetómetros más usados son los microelectromecánicos al igual que los

giróscopos (ver Figura 2-5). El funcionamiento de éstos se basa en la detección del movimiento que se produce

en la estructura MEMS debido a la fuerza de Lorentz , que actúa sobre el conductor por el que circula la corriente.

La detección del movimiento puede realizarse electrónica u ópticamente mediante elementos piezorresistivos o

fuentes de luz láser o LED, respectivamente.

2.4. Disponibilidad de sensores en Smartphones

En la actualidad se puede encontrar cierta diversidad de sistemas operativos móviles en el mercado, cada

uno con niveles de implantación y madurez diferentes (ver Tabla 2-1).

Figura 2-5. Magnetómetro MEMS.

Tabla 2-1. Venta mundial de Smartphones a usuarios finales según SO.

11

11


Según se muestra en la Tabla 2-1, el SO más implantado a nivel mundial es con diferencia Android con

un 84.1% del mercado, además, en ciertos países como España las diferencias son más signfiicativas, donde

Android tiene el 90,8% de la cuota de mercado.

Aunque la disponibilidad de los sensores varía de un dispositivo a otro (entre Android y iOS), también

puede variar entre las mismas versiones de cada sistema operativo. Esto se debe a que los sensores se han

introducido a lo largo de varias versiones de plataformas.

En la Tabla 2-2 y 2-3 se muestran los sensores que incluyen las diferentes versiones de los sistemas

operativos iOS y Android respectivamente que han involucrado cambios en los sensores.

iPhone 7 Plus iPhone 7 iPhone 6 Plus iPhone 6 iPhone

Giróscopo 3 ejes Giróscopo 3 ejes Giróscopo 3 ejes Giróscopo 3 ejes Giróscopo 3 ejes

Acelerómetro Acelerómetro Acelerómetro Acelerómetro Acelerómetro

Sensor de

proximidad

Sensor de

proximidad

Sensor de

proximidad

Sensor de

proximidad

Sensor de

proximidad

Sensor de luz Sensor de luz Sensor de luz Sensor de luz Sensor de luz

Barómetro Barómetro Barómetro Barómetro -

Android 4.0 Android 2.3 Android 2.2 Android 1.5

Giróscopo 3 ejes Giróscopo 3 ejes - -

Acelerómetro Acelerómetro Acelerómetro Acelerómetro

Magentómetro Magentómetro Magentómetro Magentómetro

Sensor de proximidad Sensor de proximidad Sensor de proximidad Sensor de proximidad

Sensor de luz Sensor de luz Sensor de luz Sensor de luz

Barómetro Barómetro - -

Sensor de temperatura - - -

Sensor de gravedad Sensor de gravedad - -

Sensor de humedad - - -

Tabla 2-2. Sensores incluidos en las diferentes versiones de iPhone.

Tabla 2-3. Sensores incluidos en las diferentes versiones de Android.


Concretando en el caso de Android, a partir de la versión 4.0 se incluyen los sensores nombrados, sin

embargo, ello no implica que estos sensores siempre estén presentes en Smartphones con un sistema operativo

avanzado. Dependerá de si la propia plataforma lo contiene. A continuación, en la Tabla 2-4 se incluyen las

mejores posibilidades de adquirir un Smartphone con sistema operativo Android y a un precio asequible en el

mercado actual nombrando además las diferentes fuentes de compra. Este breve análisis del mercado de los

sensores presentes en los Smartphone permitirá adquirir suficientes dispositivos móviles para la realización de

la práctica con los alumnos.

BQ Aquarias 4.5 BQ Aquarias U Lite ZTE Blade S6

Sistema Operativo Android 6.0 Android 6.0 Android 5.0

Sensores

Giróscopo 3 ejes Giróscopo 3 ejes Giróscopo 3 ejes

Acelerómetro Acelerómetro Acelerómetro

Magentómetro Magentómetro Magentómetro

Sensor de proximidad Sensor de proximidad Sensor de proximidad

Sensor de luz Sensor de luz Sensor de luz

Precio fuente de

compra

113 € (Media Mark) 126 € (Media Mark) 149 € (El Corte Inglés)

Tabla 2-4. Smartphones válidos disponibles en el mercado.

Capítulo 3

CAPÍTULO 3 CINEMÁTICA DE LA ACTITUD

La navegación o posicionamiento a partir de sistemas de navegación inerciales se basa fundamentalmente

en la integración con respecto al tiempo de las magnitudes medidas por acelerómetros y giróscopos. Dichas

integraciones están regidas por un conjunto de ecuaciones diferenciales denominadas ecuaciones de la

navegación inercial, las cuales no son más que las ecuaciones del movimiento de un vehículo. En principio, el

conjunto de ecuaciones diferenciales se puede resolver en cualquier sistema de referencia, siendo el más sencillo

(matemáticamente hablando) un sistema de referencia inercial.

En conjunto, las ecuaciones de la navegación determinan nueve variables (3 de posición, 3 de velocidad

y 3 de actitud), por lo que serán necesarias 9 ecuaciones diferenciales para obtenerlas en función del tiempo.

Éstas se dividen en tres grupos:

Ecuaciones diferenciales cinemáticas de la actitud (EDC).

Ecuaciones diferenciales dinámicas.

Ecuaciones diferenciales cinemáticas.

Dado que el objetivo en este proyecto es conocer en cada instante de tiempo únicamente la orientación

del Smartphone a partir de las medidas de los sensores, sólo se necesitarán las ecuaciones diferenciales

cinemáticas de la actitud para su resolución.

3.1. Sistemas de referencia

Previo a tratar los métodos de representación de la actitud, se presentarán a continuación brevemente los

dos sistemas de referencia con los que se va a trabajar en este proyecto.

3.1.1. Sistema en ejes navegación (NED)

Se trata de un sistema de referencia inercial en el que el centro O es un punto cualquiera sobre la

superficie de la Tierra (elipsoide de referencia) o a una altitud h sobre ella. Los ejes conforman un sistema

cartesiano donde el eje OZ tiene la dirección de la perpendicular a la superficie del elipsoide con sentido

hacia abajo, y los ejes OX (apunta al norte-N) y OY (apunta al este-E) forman un plano perpendicular al eje

OZ por el punto O. Dicho sistema coincide con el definido por las coordenadas curvilíneas geodésicas de

forma que:

El eje N va en la dirección 𝑒𝜑.

El eje E va en la dirección 𝑒𝜆.

El eje D va en la dirección −𝑒ℎ.

El sistema NED es un sistema local, es decir, se mueve con el objeto mientras éste recorre la

superficie de la Tierra, por tanto el origen del SR no es fijo y no pasa por el centro de la Tierra. El sistema

representado en la Figura 3-1 es el sistema típico usado para navegación aérea.

Cinemática de la Actitud

Obsérvese que en general ninguno de los ejes del sistema NED pasa por el centro de la Tierra, por

tanto una transformación de coordenadas NED a cualquier otro sistema de referencia que pasa por el mismo

implica no sólo rotación sino una traslación que haga coincidir los centros de ambos.

3.1.2. Sistema en ejes cuerpo

En este caso, el sistema de ejes cuerpo está centrado en el Smartphone y se mueve con él.

Idealmente, este sistema estará centrado en el acelerómetro para permitir que éste mida aceleraciones puras.

Sin embargo , debido a que la localización exacta es desconocida en cada dispositivo móvil, se asumirá que

el origen del sistema de referencia se encuentra posicionado en el centro del mismo. Esta traslación del

centro del sistema de referencia no supondrá ningún inconveniente en el desarrollo del modelo matemático

ya que únicamente se va a estimar la orientación del Smartphone por lo que la traslación entre centros no

repercutirá en la estimación de la rotación.

En la Figura 2-3 se muestra el sistema de referencia en ejes cuerpo del Smartphone el cual es

aplicable tanto para sistemas operativos Android como iOS.

3.2. Métodos de representación de Actitud

Se entiende por actitud de un elemento móvil la orientación que éste posee respecto a un cierto sistema

de referencia (típicamente inercial o ejes órbita). Si el elemento móvil es un sólifo rígido (como es el caso del

Smartphone), es suficiente conocer la orientación de un sistema de referencia solidario al mismo (ejes cuerpo).

En concreto, trabajando en los sistemas de referencia comentados anteriormente (donde, en lo que sigue, se

designarán como n (sistema en Ejes Navegación), y b (sistema en ejes cuerpo del Smartphone)), se pueden

relacionar los mismos mediante una traslación más una rotación mediante la expresión:

𝑝𝐼 = 𝐶𝐵𝐼 𝑝𝐵 + 𝑡𝐵

𝐼 (3.1),

donde 𝑝𝐼 representa un punto expresado en el sistema de referencia inercial y 𝑝𝐵 en el sistema de ejes cuerpo.

La rotación y traslación relativa entre ambos sistemas de referencia vienen representadas por 𝐶𝐵𝐼 y 𝑡𝐵

𝐼

respectivamente. De nuevo, es importante recalcar que la traslación entre ambos centros de coordenadas no se

va a considerar ya que no supone ninguna modificación en el planteamiento matemático del problema, por lo

que se puede enfocar más específicamente el objeto del proyecto en la estimación de la matriz de rotación entre

ambos sistemas de referencia a partir de las medidas que se obtengan de los sensores del Smartphone. Dicho

movimiento puede interpretarse como la rotación que debe aplicarse al Smartphone para alinearse con el sistema

de ejes inercial, y la inversa de dicha matriz como la rotación que debe aplicarse al sistema de ejes inercial para

alinearse con los ejes cuerpo del Smartphone.

Figura 3-1. Sistema de referencia NED.

15

15


Para describir y caracterizar matemáticamente la orientación relativa entre ambos sistemas de referencia

se utilizan diversos métodos denominados parametrizaciones. Entre ellos se analizarán cuatro representaciones

clásicas:

Matriz de cosenos directores (DCM).

Ángulos de Euler.

Ángulo y eje de Euler.

Cuaterniones.

A continuación se va a dar una breve descripción de las tres primeras representaciones nombradas. Los

cuaterniones serán descritos más detalladamente puesto que será la representación utilizada para el presente

proyecto. Se ha optado por el uso de cuaterniones porque poseen ciertas ventajas numéricas que se describirán

en párrafos posteriores.

3.2.1. Matriz de cosenos directores (DCM)

Dado un sistema de referencia S (determinado por una base de vectores unitarios (𝑒𝑥 , 𝑒𝑦, 𝑒𝑧)) y otro

S' (determinado por una base de vectores unitarios (𝑒𝑥′, 𝑒𝑦′, 𝑒𝑧′)), mediante la teoría de Álgebra es bien

sabido que la orientación de S respecto a S' queda perfectamente determinada por la matriz de cambio de

base 𝐶𝑆𝑆′, de forma que es posible expresar un vector genérico (originalmente en base S) en base S' sin

más que palicar la operación:

𝑆 = 𝐶𝑆𝑆′𝑆′(3.2)

donde:

𝐶𝑆𝑆′ = [

𝑐11 𝑐12 𝑐13

𝑐21 𝑐22 𝑐23

𝑐31 𝑐32 𝑐33

](3.3)

siendo 𝐶𝑖𝑗 = 𝑒𝑖𝑆′

∙ 𝑒𝑗𝑆′

el coseno director entre 𝑒𝑖𝑆′

y 𝑒𝑗𝑆′

para 𝑖, 𝑗 = 𝑥, 𝑦, 𝑧.

Por tanto, la actitud queda definida a partir de nueve parámetros que no son independientes entre sí.

Ello implica que las entradas de la matriz no pueden ser cualesquiera, teniendo éstas que cumplir las

siguientes propiedades:

1. 𝐶𝑆𝑆′ es ortogonal, es decir, sus vectores son perpendiculares entre sí. Esta propiedad implica que:

𝐶𝑆𝑆′ = (𝐶𝑆′

𝑆 )−1 = (𝐶𝑆′𝑆 )−𝑇(3.4)

2. El determinante de 𝐶𝑆𝑆′ vale ± 1. Esta propiedad, teniendo en cuenta (3.4) se muestra de la siguiente

manera:

1 = det(𝐼𝑑) = det ((𝐶𝑆′𝑆 )(𝐶𝑆′

𝑆 )−1

) = det ((𝐶𝑆′𝑆 )(𝐶𝑆′

𝑆 )−𝑇

) = (det(𝐶𝑆′𝑆 ))

2(3.5)

El signo positivo se da cuando tanto S como S’ están definidos en mismo sentido (a derechas

o a izquierdas).


3. Supóngase que la actitud de 𝑆2 respecto a 𝑆1 viene dada por 𝐶𝑆1

𝑆2 y que la de 𝑆3 respecto a 𝑆2 viene

dada por 𝐶𝑆2

𝑆3. Entonces la actitud de 𝑆3 respecto a 𝑆1 viene dada por:

𝐶𝑆1

𝑆3 = 𝐶𝑆2

𝑆3𝐶𝑆1

𝑆2 (3.6),

es decir, la composición de actitudes viene dada por un simple producto matricial.

La matriz DCM siendo una parametrización tan redundante no resulta eficiente para los algoritmos

de estimación de actitud necesarios en este trabajo. Por otro lado, la integración numérica de las ecuaciones

formuladas en términos de la matriz de rotación introduce errores de redondeo que resultan en la falta de

ortogonalidad de la matriz. La corrección de la ortogonalidad de la matriz en cada paso de la integración

hace muy poco eficiente esta representación de la actitud. Es sin embargo útil para realizar cambios de

sistema de referencia.

3.2.2. Ángulos de Euler

Describen la actitud de un sistema de referencia respecto al sistema de referencia origen como una

secuencia de tres rotaciones en torno a los ejes cuerpo. Precisa de los tres ángulos de Euler más la secuencia

en que éstos han de aplicarse.

De la definición de DCM se puede obtener que una rotación alrededor de los ejes OX, OY o OZ

queda definida por las siguientes matrices respectivamente:

𝐶𝑋(𝛼) = [

1 0 00 cos(𝛼) 𝑠𝑒𝑛(𝛼)0 −𝑠𝑒𝑛(𝛼) cos(𝛼)

]𝐶𝑌(𝛼) = [cos(𝛼) 0 −𝑠𝑒𝑛(𝛼)

0 1 0𝑠𝑒𝑛(𝛼) 0 cos(𝛼)

]

𝐶𝑍(𝛼) = [cos(𝛼) 𝑠𝑒𝑛(𝛼) 0

−𝑠𝑒𝑛(𝛼) cos(𝛼) 00 0 1

](3.7)

En general, es posible usar 12 secuencias diferentes de tres ángulos para representar la actitud. Esto

implica que el orden en el que se realizan las rotaciones es importante, pues se puede expresar una misma

actitud con diferentes ternas de tres rotaciones. La elección de una secuencia u otra depende de la

aplicación. En particular, para el caso del sistema que aquí se estudia, lo que se busca es el cambio de

coordenadas de ejes cuerpo (sensores) a ejes NED (ecuaciones del movimiento). Clásicamente, se ha usado

la siguiente terna de rotaciones para tal tarea (ángulos de Tait-Bryan o de Euler):

Figura 3-2. Ángulos de Tait-Bryan/Euler (𝜑, 𝜃, 𝜓).

b (SR)

n (SR)

17

17


La secuencia mostrada en la Figura 3-2 se puede explicar de la siguiente manera:

Se parte del SR azul (ejes NED).

Giro respecto del eje OZ de 𝜓0, donde 𝜓 se denomina ángulo de guiñada (yaw en inglés).

Giro respecto del eje N(y) verde de𝜃0, donde 𝜃 se denomina ángulo de cabeceo (pitch en inglés).

Giro respecto del eje OX rojo de 𝜑0, donde 𝜑 se denomina ángulo de alabeo (roll en inglés).

Esta secuencia se puede esquematizar de la siguiente forma:

donde S y S’ son los sistemas de referencia generados tras el ángulo de guiñada y cabeceo respectivamente.

Por tanto, teniendo en cuenta la propiedad de la composición de rotaciones para la DCM (3.6), el paso de

ejes navegación a ejes cuerpo queda definido por:

𝐶𝑛𝑏(𝜑, 𝜃, 𝜓) = 𝐶𝑆′

𝑏 (𝜑)𝐶𝑆𝑆′

(𝜃)𝐶𝑛𝑆(𝜓)

cuya expresión resulta:

(3.8),

donde s(.) y c(.) son las abreviaturas de sen(.) y cos(.) respectivamente.

Analizando la expresión (3.9) se puede comprobar que se cumple que la actitud dada por

(180º + 𝜓, 180º – 𝜃, 180º + 𝜑) representa la misma orientación que (𝜓, 𝜃, 𝜑). Para evitar estas

ambigüedades se definen los ángulos tal que el ángulo de cabeceo cumpla 𝜃 ∈ (−90º ,90º) y los ángulos de

alabeo y guiñada estén en el intervalo 𝜑, 𝜓 ∈±180º .

La mayor ventaja de la representación de la actitud mediante los ángulos de Euler es su significado

físico. No obstante, hay que tener cuidado a la hora de componer dos actitudes, ya que pueden producirse

ciertas singularidades. Además, la composición de actitudes cuando se tienen rotaciones mixtas resulta una

matriz demasiado compleja para ciertas aplicaciones dificultando la operación con estos ángulos.

3.2.3. Ángulo y eje de Euler

El Teorema de Euler establece que: “el movimiento más general posible de un sólido con un punto

fijo es una rotación alrededor de un único eje”.

A partir de dicho Teorema es posible describir la actitud de un sistema S respecto de otro

denominado S’ a partir de un giro 𝜃 alrededor de un eje determinado 𝑒𝑆/𝑆′𝑆′ . Por tanto, es posible expresar

la actitud a partir de los siguientes cuatro parámetros:

Giro de ángulo 𝜃.

Tres componentes del vector unitario 𝑒𝑆/𝑆′𝑆′ = [𝑒𝑥, 𝑒𝑦, 𝑒𝑧]

𝑇.

b


Ello se traduce en que el eje de Euler es el vector en torno al cual ha de girarse el sistema de

referencia origen para obtener la actitud del sistema de referencia objeto de representación. El ángulo de

Euler será la cantidad a girar en torno al eje de Euler.

La relación de la DCM con el ángulo y eje de Euler se conoce como ecuación de Euler-Rodrigues

y viene dada por la siguiente expresión:

𝐶𝑆𝑆′ = cos(𝜃) 𝐼𝑑 + (1 − cos(𝜃))𝑒𝑆/𝑆′

𝑆′ (𝑒𝑆/𝑆′𝑆′ )

𝑇− 𝑠𝑒𝑛(𝜃)(𝑒𝑆/𝑆′

𝑆′ )×(3.9)

donde el operador (𝑒𝑆/𝑆′𝑆′ )

× representa el producto vectorial mediante la siguiente matriz:

(𝑒𝑆/𝑆′𝑆′ )

×= [

0 −𝑒𝑧 𝑒𝑦

𝑒𝑧 0 −𝑒𝑥

−𝑒𝑦 𝑒𝑥 0](3.10)

La ventaja de esta forma de representación de actitud es su significado físico siendo éste sencillo y

permitiendo entender de dónde se genera el concepto de cuaternión que se verá posteriormente. Sin

embargo, del mismo modo que ocurría con los ángulos de Euler, dicha representación presenta

ambigüedad, es decir la actitud determinada por la terna (𝑒𝑆/𝑆′𝑆′ ,𝜃) es equivalente a la dada por

− (𝑒𝑆/𝑆′𝑆′ , 360º – 𝜃) . Para evitarlo, se define 𝜃 en el intervalo [0º, 180º]. Además, en 𝜃 = 0º la representación

no está definida, siendo esta singularidad la que elimina su uso directo en navegación.

Esta parametrización es también redundante debido a la restricción sobre la norma del eje de Euler

(ha de ser unitario).

Los métodos de representación de actitud de un vehículo hasta ahora vistos no son computacionalmente

eficientes, bien por su complejidad, bien por la presencia de singularidades en las ecuaciones cinemáticas que

de su naturaleza se desprende. La forma de representación de actitud más eficiente computacionalmente para el

propósito de este trabajo es el cuaternión de actitud y en términos de cuaterniones se establecerán las Ecuaciones

Diferenciales Cinemáticas de la Actitud.

3.2.4. Cuaterniones

Los cuaterniones son una herramienta matemática inventada por Hamilton en el siglo XIX como

necesidad para la resolución de cierto tipo de problemas algebraicos de la época. Conceptualmente no son

más que una extensión de los números complejos a cuatro dimensiones.

Un número complejo z es un vector que está compuesto por una parte real y otra imaginaria, tal

que:

𝑧 = 𝑥 + 𝑖𝑦(3.11),

donde x e y son números reales e i representa la unidad imaginaria.

Un número complejo también se puede representar de la siguiente manera:

𝑧 = 𝜌𝑒𝑖𝜃 = 𝜌(cos(𝜃) + 𝑖𝑠𝑒𝑛(𝜃))(3.12)

Cuando 𝜌 = 1, dicha representación se puede ver como una rotación bidimensional de ángulo 𝜃

en un sistema cartesiano x-y donde el eje y sería el eje imaginario.

19

19


Un cuaternión genérico es una cuaterna de números 𝑞0, 𝑞1, 𝑞2, 𝑞3 que se puede escribir de la

siguiente forma (por analogía con los números complejos):

𝑞 = 𝑞0 + 𝑖𝑞1 + 𝑗𝑞2 + 𝑘𝑞3(3.13),

donde 𝑞0, 𝑞1, 𝑞2, 𝑞3 son números reales mientras que i, j, k son unidades imaginarias definidas en ejes

perpendiculares.

Normalmente se define = [𝑞1, 𝑞2, 𝑞3] como la parte vectorial del cuaternión y 𝑞0 como la parte

escalar, de forma que se puede escribir:

𝑞 = [𝑞0

](3.14)

Para el entendimiento de los cuaterniones es importante conocer su álgebra, es decir, cómo se opera

con los mismos. A continuación se describen propiedades algebraicas de utilidad para cálculos posteriores:

Suma: la suma de cuaterniones se da componente a componente, es decir, dados dos cuaterniones

𝑞 = 𝑞0 + 𝑖𝑞1 + 𝑗𝑞2 + 𝑘𝑞3 y 𝑞′ = 𝑞′0 + 𝑖𝑞′1 + 𝑗𝑞′2 + 𝑘𝑞′3, la suma de ambos viene dada por:

𝑞 + 𝑞′ = (𝑞0 + 𝑞′0) + 𝑖(𝑞1 + 𝑞′

1) + 𝑗(𝑞2 + 𝑞′2) + 𝑘(𝑞3 + 𝑞′

3)(3.15)

Producto: el producto entre dos cuaterniones se realiza componente a componente teniendo en cuenta

las siguientes reglas de multiplicación:

𝑖 ∗ 𝑖 = −1,𝑖 ∗ 𝑗 = 𝑘,𝑖 ∗ 𝑘 = −𝑗,𝑗 ∗ 𝑖 = −𝑘,𝑗 ∗ 𝑗 = −1,

𝑗 ∗ 𝑘 = 𝑖,𝑘 ∗ 𝑖 = 𝑗,𝑘 ∗ 𝑗 = −𝑖,𝑘 ∗ 𝑘 = −1(3.16)

De forma matricial el producto vectorial 𝑞′′ = 𝑞′ ∗ 𝑞 se puede escribir como:

[ 𝑞0

′′

𝑞1′′

𝑞2′′

𝑞3′′]

=

[ 𝑞0

′ −𝑞1′ −𝑞2

′ −𝑞3′

𝑞1′ 𝑞0

′ −𝑞3′ 𝑞2

′

𝑞2′

𝑞3′

𝑞3′

−𝑞2′

𝑞0′

𝑞1′

−𝑞1′

𝑞0′ ]

[

𝑞0

𝑞1𝑞2

𝑞3

](3.17)

En forma vectorial, la expresión (3.17) queda:

[𝑞0

′′

′′] = [

𝑞0′ 𝑞0 − ′𝑇

𝑞0′ + 𝑞0

′ + ′ × ](3.18)

Debe tenerse en cuenta que, en general, el producto de cuaterniones no es conmutativo, es

decir:

𝑞 ∗ 𝑞′ ≠ 𝑞′ ∗ 𝑞(3.19)

Conjugado: al igual que en el caso de números complejos de dos dimensiones, dado

𝑞 = 𝑞0 + 𝑖𝑞1 + 𝑗𝑞2 + 𝑘𝑞3 se define su conjugado como:

𝑞∗ = 𝑞0 − 𝑖𝑞1 − 𝑗𝑞2 − 𝑘𝑞3(3.20)


Módulo: se define el módulo de 𝑞 como:

|𝑞|2 = 𝑞 ∗ 𝑞∗ = 𝑞02 + 𝑞1

2 + 𝑞22 + 𝑞3

2(3.21)

División: la división entre dos cuaterniones se define haciendo uso del conjugado de modo que:

𝑞′/𝑞 = 𝑞′/𝑞 ∗ 𝑞∗/𝑞∗ = (𝑞′ ∗ 𝑞∗)/|𝑞|2(3.22)

Definida la herramienta matemática que permitirá plantear el problema de la estimación de actitud,

cabe preguntarse el sentido físico de la misma cuya interpretación resulta un tanto compleja.

Partiendo de una actitud definida mediante el eje y el ángulo de Euler, es decir, 𝑒y 𝜃, el cuaternión

que codifica dicha actitud se caracteriza por cumplir las siguientes relaciones:

𝑞 = [𝑐𝑜𝑠(𝜃/2)

𝑒𝑠𝑒𝑛(𝜃/2)](3.23)

|𝑞| = 1(3.24)

Se puede demostrar que la ecuación (3.24) es consecuencia de la definición en (3.23). Esto implica

que cualquier cuaternión que cumpla con (3.24) se puede descomponer de la forma definida por (3.23).

Debido a lo anterior, se puede afirmar que cualquier cuaternión de módulo unidad representa una actitud,

así como que cualquier actitud se puede representar mediante un cuaternión de módulo unidad. Sin

embargo, obsérvese que para los cuaterniones también existe una ambigüedad, la actitud representada por

𝑒y 𝜃 es equivalente a la representada por −𝑒y (360 − 𝜃)/2:

𝑞 = [𝑐𝑜𝑠(𝜃/2)

𝑒𝑠𝑒𝑛(𝜃/2)] = [

𝑐𝑜𝑠(360 − 𝜃)/2

−𝑒𝑠𝑒𝑛(360 − 𝜃)/2] = [

−𝑐𝑜𝑠(𝜃/2)

−𝑒𝑠𝑒𝑛(𝜃/2)] = −𝑞(3.25)

La igualdad (3.25) muestra que un giro y su opuesto representan la misma actitud. Para evitarlo se

impone que el término escalar del cuaternión sea positivo, esto es,𝑞0 ≥ 0. Esta ambigüedad no es

fácilmente resoluble, puesto que cuando 𝑞0 es cero tanto como − representan la misma actitud. Esta

ambigüedad puede ocasionar problemas para el control de actitud pero no para el propósito de este

proyecto, la estimación de actitud.

A la hora de tratar con los datos proporcionados por los sensores, éstos serán conocidos en forma

vectorial en ejes inerciales y será necesario girar dicho vector a ejes cuerpo. Para ello, es necesario conocer

la matriz de rotación (DCM) conocido el cuaternión que determina dicha rotación. Esta transformación

viene determinada por la fórmula de Euler-Rodrigues para cuaterniones:

𝐶 = (𝑞0 − 𝑇)𝐼𝑑 + 2𝑇 − 2𝑞0×(3.26),

donde el operador × representa el producto vectorial mediante la siguiente matriz:

× = [

0 −𝑞3 𝑞2

𝑞3 0 −𝑞1

−𝑞2 𝑞1 0](3.27)

Además, para la aplicación del método TRIAD que se verá posteriormente, será necesario conocer

cómo pasar de la matriz DCM al cuaternión asociado mediante las fórmulas:

𝑞0 =√1 + 𝑇𝑟(𝐶)

2, × =

1

4𝑞0

(𝐶𝑇 − 𝐶)(3.28)

donde 𝑇𝑟(𝐶) representa la traza de la matriz DCM.

21

21


3.3. Ecuaciones Diferenciales Cinemáticas de la Actitud (EDC)

Para el caso de la posición, las ecuaciones cinemáticas relacionan el vector posición con el vector

velocidad, mientras que las ecuaciones dinámicas relacionan el vector velocidad con el vector fuerza.

Para el caso de la actitud, las ecuaciones diferenciales cinemáticas de la actitud (EDC) relacionan la

representación de la actitud (DCM, ángulos de Euler, cuaterniones) con la velocidad angular . Típicamente

estas ecuaciones son no-lineales.

En el sistema de navegación inercial, los giróscopos proporcionan la velocidad angular por lo que será

necesario el uso de las EDC, es decir, integrar las ecuaciones para calcular la actitud. Por tanto es importante

conocer las EDC, en particular, para cuaterniones, ya que este método de representación se ha justificado

anteriormente. Una vez explicado el álgebra de cuaterniones básica que será usada para los cálculos, se va a

obtener la ecuación diferencial de actitud en cuaterniones. Para ello, dado que la definición de cuaternión de

actitud parte de la definición de ángulo y eje de Euler, es necesario expresar la ecuación diferencial que describe

la actitud en función del tiempo para el ángulo y eje de Euler, cuya formulación es la siguiente:

𝑏/𝑛𝑏 =

1

2[(𝑒𝑏/𝑛

𝑏 )×

+1

tan(𝜃/2)(𝐼𝑑 − 𝑒𝑏/𝑛

𝑏 (𝑒𝑏/𝑛𝑏 )𝑇)]𝑏/𝑛

𝑏 (3.29)

= (𝑒𝑏/𝑛𝑏 )𝑇 𝑏/𝑛

𝑏 (3.30)

Las ecuaciones (3.29) y (3.30) representan las cuatro ecuaciones diferenciales que determinan los cuatro

parámetros que definen el ángulo y eje de Euler para el caso de la rotación de los ejes cuerpo respecto a los NED.

Para obtener las EDC expresadas en cuaterniones, únicamente es necesario derivar la ecuación (3.25)

respecto del tiempo y sustituir 𝑏/𝑛𝑏 y por sus correspondientes expresiones, (3.29) y (3.30). Tras esto se llega

a:

= [

1

2𝑇 𝑏/𝑛

𝑏

1

2[𝑞× + 𝑞0𝐼𝑑]𝑏/𝑛

𝑏] =

1

2[

−𝑞1 −𝑞2 −𝑞3

𝑞0 −𝑞3 𝑞2𝑞3

−𝑞2

𝑞0

𝑞1

−𝑞1

𝑞0

] [

𝜔𝑥

𝜔𝑦

𝜔𝑧

] =1

2𝑆(𝑞) [

𝜔𝑥

𝜔𝑦

𝜔𝑧

](3.31),

donde 𝑏/𝑛𝑏 = [𝜔𝑥 𝜔𝑦𝜔𝑧]

𝑇.

Recordando la definición matricial del producto de cuaterniones, si se define un cuaternión 𝑞𝜔 cuya parte

escalar es nula y cuya parte vectorial es igual a las componentes de la velocidad angular, es decir:

𝑞𝜔 = [

0𝜔𝑥𝜔𝑦

𝜔𝑧

](3.32),

se puede expresar las EDC para cuaterniones de forma compacta como:

=1

2𝑆(𝑞) ∗ 𝑞𝜔(3.33)

Finalmente, la ecuación diferencial de actitud en cuaterniones queda definida por cuatro ecuaciones

diferenciales, bilineales y sin singularidades. La principal razón por la que se usa este método de cálculo es

porque no presenta singularidades ni funciones trigonométricas en la ecuación (3.33), introduciendo errores

de cálculo menores y disminuyendo la carga de trabajo a la hora de la integración.


Puesto que un cuaternión que representa actitud tiene que cumplir con la condición de módulo

unidad, será necesario normalizar cada cierto tiempo para evitar errores numéricos grandes. Este proceso

es conocido como renormalización y será una fuente de error, pero mucho menor que la que se produce en

la DCM, pues mientras que aquí hay que normalizar un vector de cuatro componentes, para la DCM hay

que ortogonalizar una matriz de tres vectores con tres componentes cada uno.

3.4. Discretización de las Ecuaciones Diferenciales Cinemáticas de la Actitud

Tal y como muestra la expresión (3.33), para estimar la evolución de la actitud de un elemento móvil,

únicamente es necesario conocer los valores de la velocidad angular proporcionados por el giróscopo. En este

caso, dado que se va a trabajar con una evolución discreta, ya que los valores de las medidas de los sensores se

tienen en ciertos instantes de tiempo y no de manera continua, es necesario conocer además el incremento de

tiempo entre la toma de medidas consecutivas. Como se verá en el Capítulo 7, este incremento (∆𝑡𝑘) es conocido,

por lo que se puede tener una primera estimación de la actitud del Smartphone.

Si se discretiza el cuaternión respecto al tiempo como:

≅𝑞𝑘

+ − 𝑞𝑘−

∆𝑡𝑘(3.34),

donde el subíndice k representa el instante de tiempo de trabajo, el superíndice " + " el instante posterior al de

trabajo y el superíndice " − " el instante apriori al de trabajo, entonces, teniendo en cuenta la definición de en

(3.33), se puede obtener la evolución discreta del cuaternión de actitud conocido su valor en el instante anterior

mediante:

𝑞𝑘+ = 𝑞𝑘

− +1

2𝑆(𝑞𝑘)𝑞𝜔𝑘

∆𝑡𝑘(3.35)

Es necesario en cada iteración normalizar el cuaternión de actitud para evitar errores numéricos además

de inicializar su valor.

3.5. Cuaternión de error

Un aspecto importante para el cálculo de la actitud es conocer el error que se comete en su estimación.

Al trabajar con cuaterniones, la formulación clásica aditiva de definición de error:

𝑞 = + 𝛿𝑞(3.36),

no puede ser aplicada ya que, aunque y 𝛿𝑞 tengan módulo unidad, la suma no tiene porqué tenerlo y 𝑞 no

tendría porqué ser un cuaternión. Es por esta razón por lo que se usa la formulación multiplicativa, en la que:

𝑞 = ∗ 𝛿𝑞(3.37),

donde 𝛿𝑞 es el llamado cuaternión de error y debe estar próximo al cuaternión unidad (𝑞 = [1000]𝑇), de

modo que este vector de error puede reescribirse en función de un vector de menor dimensión tal que:

𝛿𝑞() =1

√4 + ||2[2](3.38)

Esta expresión se puede linealizar obteniéndose la siguiente aproximación:

𝛿𝑞() ≈ [2

/2](3.39)

23

23


Ahora supóngase que se tiene una velocidad angular de referencia que genera un cuaternión de

referencia y que la velocidad angular puede expresarse como = + 𝜗𝑘, donde 𝜗𝑘 es el ruido inducido

por el giróscopo, el cual se considera pequeño.

Para estimar el 𝛿𝑞 basta con derivar la expresión (3.37) e igualarla a las EDC (3.33):

= ∗ 𝛿𝑞 + ∗ 𝛿 =1

2𝑞 ∗ 𝑞𝜔(3.40)

Teniendo en cuenta que =1

2 ∗ 𝑞, siendo

1

2 ∗ 𝑞 ∗ 𝛿𝑞 + ∗ 𝛿 =

1

2𝑞 ∗ 𝑞𝜔(3.41),

y multiplicando por el inverso de a la izquierda, se puede despejar 𝛿, obteniéndose:

𝛿 =1

2𝛿𝑞 ∗ 𝑞𝜔 −

1

2𝑞 ∗ 𝛿𝑞(3.42)

Sustituyendo la definición de cuaternión de error por su aproximación linealizada (3.39) en función del

parámetro (ver [7]) se obtiene la aproximación a la evolución de dicho parámetro con el tiempo:

≈ × +𝜗𝑘 (3.43)

Dado que se trabajará con un planteamiento discreto de las variables, conviene de nuevo discretizar la

expresión (3.43) como sigue:

𝑘+ = 𝑘

− + ∆𝑡𝑘−𝑘×𝑘

− + 𝜗𝑘(3.44),

la cual será últil escribirla de la siguiente forma:

𝑘+ = (𝐼𝑑 − ∆𝑡𝑘 𝑘

×)𝑘

− + ∆𝑡𝑘𝜗𝑘(3.45),

ya que se pueden identificar así las matrices (𝐴 y 𝐵) que definirán la dinámica del sistema como:

𝑘+ = 𝐴𝑘𝑘

− + 𝐵𝑘𝜗𝑘𝑑𝑜𝑛𝑑𝑒𝐴𝑘 = 𝐼𝑑 − ∆𝑡𝑘 𝑘×,𝐵𝑘 = ∆𝑡𝑘(3.46)

3.6. Cuaternión de rotación de Euler

Si se tiene un intervalo de tiempo lo suficientemente pequeño como para considerar la velocidad

angular constante en dicho intervalo, se puede definir otro método de estimación de actitud, el cuaternión

de rotación de Euler. Partiendo de la definición del eje y ángulo de Euler en un instante 𝑘,

𝑒𝑘 =𝑘

|𝑘|,𝜃𝑘 = |𝑘|∆𝑡𝑘(3.47),

se puede obtener la evolución discretizada del cuaternión de rotación como sigue:

𝑞𝑘 = [cos(𝜃𝑘/2)

𝑒𝑘 𝑠𝑒𝑛(𝜃𝑘/2)],𝑞𝑘

+ =𝑞𝑘− ∗ 𝑞𝑘 (3.48)

Del mismo modo que para el caso de las EDC, será necesario en cada iteración normalizar el cuaternión

de actitud para evitar errores numéricos además de inicializar su valor.


3.7. Método TRIAD

Otro método de estimación de actitud que se va a implementar es el método TRIAD aplicado a la

estimación de la actitud partiendo de los datos del acelerómetro y magnetómetro. Para ello se parte de las

medidas de los sensores en ejes cuerpo (𝑖𝐵) y las medidas de los mismos en ejes inerciales (𝑖

𝐼), que son

conocidas.

A partir de dichas medidas, el problema se reduce a despejar la matriz de rotación que permite relacionar

ambas:

𝑖𝐵 = 𝐶𝐼

𝐵𝑖𝐼(3.49)

Para simplificar la notación, la expresión (3.49) se puede reescribir como:

𝑖 = 𝐶𝑒𝑠𝑡𝑖(3.50),

donde 𝑖 = 𝑖𝐵, 𝑖 = 𝑖

𝐼 y 𝐶𝑒𝑠𝑡 = 𝐶𝐼𝐵.

Dado que se tienen dos medidas (y el método TRIAD no puede aplicarse para más de éstas), se definen

los siguientes vectores:

𝑟1 = 1

𝑟2 =1 × 2

|1 × 2|

𝑟3 =1 × 𝑟2

|1 × 𝑟2|

𝑠1 = 1

𝑠2 =1 × 2

|1 × 2|

𝑠3 =1 × 𝑠2

|1 × 𝑠2|(3.51)

Definiendo las matrices:

𝑀𝑟𝑒𝑓 = [𝑟1𝑟2𝑟3], 𝑀𝑜𝑏𝑠 = [𝑠1𝑠2𝑠3](3.52),

se obtiene la matriz que define la rotación como:

𝐶𝑒𝑠𝑡 = 𝑀𝑜𝑏𝑠𝑀𝑟𝑒𝑓𝑇 (3.53)

Obsérvese que el método TRIAD no es simétrico, sino que da más peso a la medida número 1. Por tanto

habrá que usar como primera medida la más fiable.

25

25


3.8. Archivos de ayuda en Matlab

Para la resolución de la estimación de la actitud en Matlab mediante cuaterniones, se proporcionan las

siguientes funciones ya programadas en Matlab como ayuda al alumno en la realización de la práctica:

Cálculo de matriz DCM conocido el cuaternión de rotación: mediante el archivo quat2DCM.m. En

este archivo se aplica la fórmula de Euler-Rodrigues para cuaterniones (3.26) para obtener la matriz de

rotación conocido el cuaternión. La correspondencia entre la notación en Matlab y en el desarrollo

teórico en el presente proyecto es la siguiente:

Cálculo del cuaternión de rotación conocida la matriz DCM: mediante el archivo DCM2quat.m. En

este archivo se aplica la fórmula (3.28) para obtener el cuaternión de rotación a partir de la matriz de

rotación. La correspondencia entre la notación en Matlab y en el desarrollo teórico en el presente

proyecto es la misma que la representada en la Tabla 3-1.

Cálculo del producto de cuaterniones: mediante el archivo qprod.m. Este archivo permite obtener el

cuaternión resultante de la multiplicación de otros dos mediante la forma vectorial representada en (3.18).

La correspondencia entre la notación en Matlab y en el desarrollo teórico en el presente proyecto es la

misma que la representada en la Tabla 3-1.

Cálculo de matriz de la EDC: mediante el archivo Sq.m. En este archivo se forma la matriz S(q) definida

en (3.30) a partir del cuaternión para facilitar la integración de las EDC. La correspondencia entre la

notación en Matlab y en el desarrollo teórico en el presente proyecto es la siguiente:

Notación Matlab Notación teoría

S 𝑆(𝑞)

q0, q1, q2, q3 𝑞0,𝑞1,𝑞2,𝑞3


C 𝐶

Q ×

q0, q1, q2, q3 𝑞0,𝑞1,𝑞2,𝑞3

q(2:4)

Tabla 3-1. Equivalencia de notación Matlab/Teoría para quat2DCM.m.

Tabla 3-2. Equivalencia de notación Matlab/Teoría para Sq.m.


Cálculo de matriz de rotación mediante el método TRIAD: mediante el archivo TRIAD.m. En este

archivo se calcula la matriz 𝐶𝑒𝑠𝑡 definida en (3.53) a partir de las medidas proporcionadas por los

sensores. La correspondencia entre la notación en Matlab y en el desarrollo teórico en el presente

proyecto es la siguiente:


C 𝐶𝑒𝑠𝑡

v1 1

v2 2

w1 1

w2 2

r1 𝑟1

r2 𝑟2

r3 𝑟3

s1 𝑠1

s2 𝑠2

s3 𝑠3

Tabla 3-3. Equivalencia de notación Matlab/Teoría para TRIAD.m.

Capítulo 4

27

27

CAPÍTULO 4 FILTRO DE KALMAN

El Filtro de Kalman es esencialmente un conjunto de ecuaciones matemáticas que implementan un

tipo de estimador Predictor-Corrector que es óptimo en el sentido de minimizar la covarianza de error de

estimación bajo el cumplimiento de ciertas condiciones.

El Filtro de Kalman ha sido objeto de extensas investigaciones y variadas aplicaciones, en particular

en el área de la navegación asistida o autónoma. La relativa simplicidad y la naturaleza robusta de este

filtro, junto con los grandes avances de los procesadores digitales han hecho que aumente su uso para una

gran variedad de aplicaciones.

La estimación de la actitud incluyendo medidas externas al giróscopo a priori proporciona un mejor

resultado en la predicción del movimiento. Para llevar a cabo esta fusión de datos, se va a recurrir a filtros

gaussianos, como lo es el Filtro de Kalman Extendido. Estos filtros permiten hacer correcciones a las señales a

partir de unos modelos matemáticos que relacionan los estados pasados y las actuaciones de control con una

predicción del estado actual, siendo ésta corregida a través de las observaciones externas.

Como punto de partida del problema, se considerará que se está trabajando con un sistema discreto-

discreto, esto es, un sistema cuya evolución es discreta en el tiempo (representada por ecuaciones

diferenciales ordinarias discretizadas en tiempo) y del cual se obtienen medidas cada cierto intervalo

temporal (por lo cual el proceso de medida se representa en tiempo discreto) ya que se tienen medidas con

ancho de banda limitado.

Inicialmente se desarrollarán los conceptos del Filtro de Kalman para casos unidimensionales y lineales

(KF) y para casos unidimensionales y no lineales (EKF). Dado que las ecuaciones diferenciales cinemáticas de

la actitud son siempre no lineales, será necesario linealizar las ecuaciones. Además, dado que en el caso de la

actitud se está trabajando tridimensionalmente, el Filtro de Kalman que aplicará en este proyecto será el Filtro

de Kalman Extendido Multiplicativo (MEKF), una extensión del mismo para 3-D. Destacar que se considerará

que el ruido que introducen las medidas de los sensores se podrá modelar como ruido blanco.

4.1. El Filtro de Kalman discreto (KF)

El Filtro de Kalman aborda el problema general de tratar de estimar el estado 𝑥 ∈ ℝ𝑛 de un proceso

de tiempo discreto que está gobernado por una ecuación diferencial lineal estocástica, cuyas ecuaciones del

proceso y la ecuación de medida son las siguientes:

𝑥𝑘 = 𝐴𝑥𝑥𝑘−1 + 𝐵𝑘𝑢𝑘 + 𝑤𝑘−1

𝑧𝑘 = 𝐻𝑘𝑥𝑘 + 𝑣𝑘(4.1),

donde

𝑥𝑘 es la variable de estado del proceso que se desea estimar,

𝑧 ∈ ℝ𝑚 es la variable de estado de observación o medición,

𝑢𝑘 es la entrada del sistema,

𝐴𝑘 , 𝐵𝑘 , 𝐻𝑘 son las matrices que definen la dinámica del sistema.

Filtro de Kalman

Las variables aleatorias 𝑤𝑘 y 𝑣𝑘 representan respectivamente el ruido del proceso y de la medición. Se

asumen que son independientes una de la otra, blancas y con distribución normal de probabilidad tal que:

𝑝(𝑤)~𝑁(0,𝑄)(4.2)

𝑝(𝑣)~𝑁(0, 𝑅)(4.3),

donde 𝑄𝑘 y 𝑅𝑘 son las matrices de covarianza del ruido del proceso y el de medición respectivamente.

Las condiciones comentadas se resumen en verificar las siguientes propiedades:

𝐸[𝑥0] = 𝑥0 ∶ Condición inicial de 𝑥𝑘,

𝐸[𝑥0, 𝑤𝑘] = 0∀𝑘 ∶ independencia entre condición inicial de estado y ruido de proceso,

𝐸[𝑥0, 𝑣𝑘] = 0∀𝑘 ∶ independencia entre condición inicial de estado y ruido de medición,

𝐸[𝑤𝑘 , 𝑣𝑗] = 0∀𝑘 ∶ independencia entre los ruidos de proceso y de medición,

𝐸[𝑥0, 𝑥0] = 𝑃0∀𝑘 ∶ condición inicial de P.

4.1.1. Origen computacional del filtro

Definiendo 𝑥𝑘− ∈ ℝ𝑛 como la estimación a priori del estado en el instante de tiempo 𝑡𝑘 y

𝑥𝑘+ ∈ ℝ𝑛 como la estimación a posteriori del estado estimado en el instante de tiempo 𝑡𝑘 dada la

medición 𝑧𝑘 , se definen los errores de estimación a priori y a posteriori respectivamente:

𝑒𝑘− ≡ 𝑥𝑘 − 𝑥𝑘

−(4.4)

𝑒𝑘+ ≡ 𝑥𝑘 − 𝑥𝑘

+(4.5)

Por tanto, la covarianza del error de estimación a priori y a posteriori se calculan como:

𝑃𝑘− ≡ 𝐸[𝑒𝑘

−𝑒𝑘−𝑇](4.6)

𝑃𝑘+ ≡ 𝐸[𝑒𝑘

+𝑒𝑘+𝑇](4.7)

Para la deducción de las ecuaciones para el Filtro de Kalman se comienza con una ecuación

que calcula la estimación a posteriori del vector de estado 𝑥𝑘+, como una combinación lineal de una

estimación a priori del estado 𝑥𝑘− y la diferencia ponderada entre una medición real 𝑧𝑘 y una predicción

de la medición 𝐻𝑥𝑘−como se muestra en la siguiente ecuación:

𝑥𝑘+ = 𝑥𝑘

− + 𝐾𝑘(𝑧𝑘 − 𝐻𝑥𝑘−)(4.8),

donde (𝑧𝑘 − 𝐻𝑥𝑘−) se denomina innovación de la medición o residuo, y es la que indica las

discrepancias entre el valor estimado de la medición (𝐻𝑥𝑘−) y el valor real medido para un instante 𝑡𝑘

(𝑧𝑘) .

La matriz 𝐾𝑘 ,de 𝑛 × 𝑚, en la ecuación anterior es elegida para ser la ganancia o el factor de

combinación que minimiza la ecuación de covarianza del error a posteriori (𝑃𝑘+). Esta minimización

puede ser lograda a partir de la definición de covarianza (ver [8]), obteniéndose:

29

29


𝐾𝑘 = 𝑃𝑘−𝐻𝑇(𝐻𝑃𝑘

−𝐻𝑇 + 𝑅)−1(4.9)

A partir de la expresión (4.9) se puede deducir que si la covarianza del error de medición R se

aproxima a cero, la ganancia K agrega peso al residuo más pronunciadamente:

lim𝑅𝑘→0

𝐾𝑘 =𝐻−1 (4.10)

Por otro lado, si la covarianza del error de estimación a priori 𝑃𝑘− se aproxima a cero, 𝐾𝑘 le da

menos peso al residuo:

lim𝑃𝑘

−→0𝐾𝑘 =0(4.11)

Otra manera de analizar los valores de 𝐾𝑘 es decir que a medida que la covarianza del error de

medición R tiende a cero, la medición actual 𝑧𝑘 es cada vez más confiable, mientras que la predicción

de la medición 𝐻𝑥𝑘− es cada vez menos confiable. En cambio cuando 𝑃𝑘

− se aproxima a cero la

medición 𝑧𝑘 es cada vez menos confiable y la predicción de la medición 𝐻𝑥𝑘− es cada vez más

confiable.

4.1.2. Origen probabilístico del filtro

La justificación de la ecuación (4.8) se basa en la probabilidad de la estimación a priori 𝑥𝑘−

condicionado en todas las mediciones anteriores 𝑧𝑘 (Regla de Baye). El tópico más importante hasta

aquí es que el Filtro de Kalman mantiene los dos primeros momentos de la distribución de los estados,

lo que implica que:

𝐸[𝑥𝑘] = 𝑥𝑘+(4.12)

𝐸[(𝑥𝑘 − 𝑥𝑘+)(𝑥𝑘 − 𝑥𝑘

+)𝑇] = 𝑃𝑘+(4.13)

La estimación a posteriori 𝑥𝑘+ refleja la media de la distribución de la variable de estado, primer

momento estadístico, y tiene una distribución normal cuando se cumple las hipótesis de estadísticas

de los ruidos de medición y de proceso simultáneamente. La covarianza del error de estimación a

posteriori 𝑃𝑘+ refleja la varianza de la distribución de la variable de estado, el segundo momento

estadístico, esto es:

𝑝(𝑥𝑘/𝑧𝑘) = 𝑁(𝐸[𝑥𝑘], 𝐸[(𝑥𝑘 − 𝑥𝑘+)(𝑥𝑘 − 𝑥𝑘

+)𝑇]) = 𝑁(𝑥𝑘+, 𝑃𝑘

+)(4.14)

4.1.3. Algoritmo del Filtro de Kalman Discreto

El Filtro de Kalman estima un proceso usando una forma de realimentación y control: el filtro

estima el estado del proceso en un tiempo y después obtiene la realimentación en forma de mediciones

(ruidosas). Las ecuaciones del Filtro de Kalman se pueden clasificar en dos grupos: ecuaciones de

actualización de tiempo y ecuaciones de actualización de mediciones.

Las ecuaciones de actualización en tiempo son responsables de la estimación hacia adelante

en tiempo del vector de estado actual y de la matriz de covarianza del error de estimación para obtener

sus estimaciones a priori, antes de las observaciones. Las ecuaciones de actualización de mediciones

son responsables de la realimentación incorporando las nuevas mediciones, las observaciones, en las

estimaciones a priori para obtener una estimación a posteriori mejorada, es decir, estas ecuaciones

pueden ser vistas como ecuaciones correctoras.

Filtro de Kalman

A continuación se muestra el algoritmo de cálculo, el cual se esquematiza en el diagrama de flujo

mostrado en la Figura 4-1.

Inicialización: en primer lugar se parte de unas condiciones iniciales estimadas:

𝑥(0) = 𝑥0, 𝑃(0) = 𝑃0(4.15)

Ecuaciones de actualización en tiempo (Predicción): se propaga en el tiempo el estado y su

covarianza:

Estimación del estado hacia delante:

𝑥𝑘− = 𝐴𝑥𝑘−1

− + 𝐵𝑢𝑘(4.16)

Estimación de la covarianza hacia delante:

𝑃𝑘− = 𝐴𝑃𝑘−1𝐴

𝑇 + 𝑄(4.17)

Ecuaciones de actualización de mediciones (Corrección): Suponiendo que en el instante de tiempo

𝑡𝑘 se obtiene una medida 𝑧𝑘 de acuerdo al modelo de medida del sistema:

Cálculo de la ganancia de Kalman:

𝐾𝑘 = 𝑃𝑘−𝐻𝑇(𝐻𝑃𝑘

−𝐻𝑇 + 𝑅)−1(4.18)

Actualización de la estimación con la medición 𝒛𝒌:


− + 𝐾𝑘(𝑧𝑘 − 𝐻𝑥𝑘−)(4.19)

Actualización de la covarianza del error:

𝑃𝑘+ = (𝐼𝑑 − 𝐾𝑘𝐻)𝑃𝑘

−(4.20)

Los pasos indicados (excepto la inicialización) se iteran. Mientras no haya medidas, se propaga la

estimación y la covarianza del error. Cuando haya alguna medida, se calcula la ganancia de Kalman y se

actualiza la estimación y la covarianza del error.

Este algoritmo garantiza (supuestas todas las hipótesis de partida verdaderas) que la covarianza del

error en cada instante, 𝑃𝑘 ,es la menor posible, por lo que la estimación 𝑥𝑘 es óptima.

31

31


4.2. El Filtro de Kalman Extendido (EKF)

El sistema planteado en la Figura 4-1 es un modelo lineal y no coincide con el modelo conformado

por las ecuaciones de mecanizado de actitud las cuales son no lineales. Por esta razón el Filtro de Kalman

no se implementará directamente para estimar el vector de estados, sino que se recurrirá a la variante del

Filtro de Kalman correspondiente, el Filtro de Kalman Extendido.

El Filtro de Kalman ataca el problema de estimar el vector de estado de un proceso discreto regido

por ecuaciones lineales estocásticas en diferencia. En el caso en el que el sistema esté definido por

ecuaciones no lineales se puede recurrir al Filtro de Kalman Extendido. Éste se basa en la linealización de

las ecuaciones del error. El procedimiento tiene su fundamento en la siguiente idea.

Dado un sistema de ecuaciones diferenciales estocásticas no lineales de un proceso, donde el vector

de estados es 𝑥𝑘 ∈ ℝ𝑛, entonces 𝑓 y ℎ se pueden definir como funciones no lineales tal que:

𝑥𝑘 = 𝑓(𝑥𝑘−1, 𝑢𝑘 , 𝑤𝑘−1)

𝑧𝑘 = ℎ(𝑥𝑘 , 𝑣𝑘)(4.21)

Aproximando el vector de estado y la medición sin tener en cuenta los ruidos de proceso ni el de

medición se obtiene:

𝑘 = 𝑓(𝑥𝑘−1, 𝑢𝑘 , 𝑤𝑘−1)

𝑘 = ℎ(𝑘 , 𝑣𝑘)(4.22)

Figura 4-1. Diagrama de flujo del algoritmo KF discreto.

Filtro de Kalman

Es importante señalar que en el EKF las distribuciones de las distintas variables aleatorias ya no son

normales después de someterse a sus respectivas transformaciones no lineales, es decir, no es posible

conocer el error. Para ello, el Filtro de Kalman Extendido lo que haze es estimar el error de la aproximación

sin ruido mediante un Filtro de Kalman aplicado a las ecuaciones linealizadas del error.

Así, replanteando el sistema de ecuaciones con la linealización respectiva, una aproximación válida

asumiendo que el error es lo suficientemente pequeño es la siguiente:

𝑥𝑘 ≈ 𝑘 + 𝐴(𝑥𝑘−1 − 𝑥𝑘−1) + 𝑊𝑤𝑘−1

𝑧𝑘 ≈ 𝑘 + 𝐻(𝑥𝑘 − 𝑘) + 𝑉𝑣𝑘(4.23),

donde 𝑥𝑘,𝑘 y 𝑥𝑘 son el valor actual, la aproximación y la estimación a priori del vector de estado

respectivamente. Las matrices 𝐴, 𝑊 y 𝐻, 𝑉 son las matrices de las derivadas parciales de 𝑓 y ℎ

respectivamente:

𝐴[𝑖,𝑗] =𝜕𝑓[𝑖]

𝜕𝑥[𝑗](𝑥𝑘−1, 𝑢𝑘, 0)(4.24)

𝑊[𝑖,𝑗] =𝜕𝑓[𝑖]

𝜕𝑤[𝑗](𝑥𝑘−1, 𝑢𝑘 , 0)(4.25)

𝐻[𝑖,𝑗] =𝜕ℎ[𝑖]

𝜕𝑥[𝑗](𝑘 , 0)(4.26)

𝑉[𝑖,𝑗] =𝜕ℎ[𝑖]

𝜕𝑣[𝑗](𝑘 , 0)(4.27),

siendo las expresiones del error de estimación (con 휀𝑘 el ruido del error de estimación del estado y 𝜂𝑘 el error

de estimación de la medida) y de las matrices de covarianza del proceso y de medición respectivamente:

𝑥𝑘≈ 𝐴(𝑥𝑘−1 − 𝑥𝑘−1

− ) + 휀𝑘 (4.28)

𝑧𝑘≈ 𝐻𝑥𝑘

+ 𝜂𝑘 (4.29)

𝑝(𝑥𝑘) = 𝑁(0, 𝐸[𝑥𝑘

𝑥𝑘𝑇 ])(4.30)

𝑝(휀𝑘) = 𝑁(0,𝑊𝑄𝑘𝑊𝑇)(4.31)

𝑝(𝜂𝑘) = 𝑁(0, 𝑉𝑅𝑘𝑉𝑇)(4.32)

Básicamente el funcionamiento del Filtro de Kalman Extendido es el mismo que el Filtro de

Kalman lineal. A continuación se presentan las ecuaciones de actualización del mismo, donde se ha

reemplazado 𝑥𝑘− = 𝑘 para ser consistente con la nomenclatura usada en el Filtro de Kalman.



33

33


Inicialización: en primer lugar se parte de unas condiciones iniciales estimadas:

𝑥(0) = 𝑥0,𝑥(0)= 0,𝑃(0) = 𝑃0(4.33)

Ecuaciones de actualización en tiempo (Predicción): se propaga en el tiempo el estado y su

covarianza:


𝑥𝑘− = 𝑓(𝑥𝑘−1, 𝑢𝑘, 𝑤𝑘−1)(4.34)


𝑃𝑘− = 𝐴𝑘𝑃𝑘−1𝐴𝑘

𝑇 + 𝑊𝑘𝑄𝑘−1𝑊𝑘𝑇(4.35)

Ecuaciones de actualización de mediciones (Corrección): Suponiendo que en el instante de tiempo

𝑡𝑘 se obtiene una medida 𝑧𝑘de acuerdo al modelo de medida del sistema:


𝐾𝑘 = 𝑃𝑘−𝐻𝑘

𝑇(𝐻𝑘𝑃𝑘−𝐻𝑘

𝑇 + 𝑉𝑘𝑅𝑘𝑉𝑘𝑇)

−1(4.36)

Actualización de la estimación con la medición 𝒛𝒌:


− + 𝐾𝑘(𝑧𝑘 − ℎ(𝑥𝑘−, 0))(4.37)


𝑃𝑘+ = (𝐼𝑑 − 𝐾𝑘𝐻𝑘)𝑃𝑘

−(4.38)

Al igual que en el caso lineal, los pasos indicados (excepto la inicialización) se iteran y mientras

no haya medidas, se propaga la estimación y la covarianza del error. Obsérseve que el error de estimación

(tanto para el estado ,𝑥𝑘, como para las medidas, 𝑧𝑘

) no juega ningún papel por lo que es eliminable del

algoritmo.

Una característica importante del Filtro de Kalman Extendido es que el Jacobiano (𝐻𝑘) en la

ecuación de la ganancia de Kalman (𝐾𝑘) sirve para corregir la propagación o destacar solamente las

componentes relevantes de información de la medición. En el caso de no existir un mapeo uno a uno entre

las mediciones 𝑧𝑘 y el vector de estado 𝑥𝑘 , el Jacobiano afecta a la ganancia de Kalman de manera que

destaca sólo la porción de la innovación (𝑧𝑘 − ℎ(𝑥𝑘−, 0)) que no afecta al vector de estado a estimar. En el

caso de que para todas las mediciones no haya un mapeo uno a uno entre las mediciones 𝑧𝑘 y el vector de

estado 𝑥𝑘 a través de h , como es de esperarse el filtro diverge rápidamente por tratarse de un proceso no

observable.

En este algoritmo hay que tener en cuenta que no existe garantía matemática de que la estimación del

EKF converja al valor real ni es ya óptimo. Sin embargo, es casi “lo mejor que se puede hacer” para el caso

no lineal. Si la estimación inicial es buena y el ruido introducido en el sistema no es excesivo, el

comportamiento es aceptablemente bueno.

Filtro de Kalman

4.3. El Filtro de Kalman Extendido Multiplicativo (MEKF)

En el caso de la estimación de la actitud, cuando dicha actitud está expresada mediante cuaterniones,

es necesario modificar el EKF resultando en el Filtro de Kalman Extendido Multiplicativo (MEKF). Éste

preserva el módulo unidad en el cuaternión estimado, por lo que, desde el punto de vista analítico, no es

necesario renormalizar. Sin embargo, durante el proceso de integración numérica, la acumulación de errores

puede romper la norma unitaria del cuaternión lo que hace imprescindible la renormalización del cuaternión

propagado.

El MEKF es la variación del Filtro de Kalman más adecuada para el problema objeto de este

proyecto. El MEKF representa la actitud real como el producto de cuaterniones, representado en (3.35),

donde es un cierto cuaternión de referencia unitario y 𝛿𝑞 es un cuaternión unitario que representa la

rotación desde al cuaternión real de actitud (nótese que este cuaternión se identificará con la estimación

del cuaternión de actitud). El error de actitud 𝛿𝑞 se parametriza según se explicó en el Capítulo 3 mediante

la expresión (3.38).

La idea en la que se basa este método es la siguiente:

Dos representaciones de actitud y , son claramente redundantes, ya que diferentes valores en

ambas representaciones pueden dar lugar a la misma actitud estimada. La idea básica del MEKF es calcular

una estimación sin restricciones del vector de error de actitud 𝛿𝑞() (de 3 componentes) mientras se utiliza

el cuaternión correctamente normalizado de manera que se obtenga una representación no singular de la

actitud.

Dada una estimación , la ecuación (3.35) indica que la estimación de la actitud correspondiente al

cuaternión de actitud real es ∗ 𝛿𝑞(). Se elimina la redundancia en la representación de actitud

escogiendo, cada vez que se tiene una actualización del error, el cuaternión de referencia de forma que es idénticamente cero. Puesto que 𝛿𝑞(0) es el cuaternión identidad, el cuaternión de referencia es la mejor

estimación del cuaternión real. Esto supone la principal ventaja conceptual del MEKF frente a otros

algoritmos de estimación de actitud.

Figura 4-2. Diagrama de flujo del algoritmo EKF discreto.

35

35


4.3.1. MEKF para las EDC

En las Ecuaciones Diferenciales Cinemáticas de la Actitud (3.33), el vector de velocidades

angulares representa a las velocidades angulares reales. Sin embargo, realmente se tendrá

conocimiento de las velocidades angulares medidas mediante giróscopos, cuya relación con la

velocidad angular real se modelará como el modelo más simple posible con ruido (sin tener en cuenta

el sesgo):

= 𝑔𝑦𝑟𝑜 + 𝜗𝜔(4.39)

donde 𝜗𝜔 representa el ruido blanco de la medida del giróscopo. Este ruido, a la hora de implementar el

MEKF no se tendrá en cuenta ya que es desconocido.

Por otro lado, el modelo de medida se basa en que en cada instante de medida 𝑡𝑘, se conocerán

m vectores (unitarios) en el sistema de referencia inercial, 𝑖,𝑘𝐼 , 𝑖 = 1,… , 𝑚 y se realizarán medidas

de dichos vectores en el sistema de referencia ejes cuerpo 𝑖,𝑘𝐵 (también supuestos unitarios) .

Dado que las medidas en el sistema de referencia inercial son conocidas (𝑖,𝑘𝐼 ), se puede obtener:

𝑖,𝑘𝐵′ = 𝐶𝐼

𝐵′(𝑘)𝑖,𝑘𝐼 𝑖 = 1,… ,𝑚(4.40),

donde 𝐶𝐼𝐵′(𝑘) representa la matriz de rotación del dispositivo desde el sistema de referencia inercial

a B’, que es el sistema de referencia al cual se refiere 𝑘 que no coincidirá con B debido a los errores. Esta

matriz se calcula a partir de la estimación de los cuaterniones en el instante 𝑡𝑘 mediante la fórmula de

Euler-Rodrigues definida en (3.26).

Para la formulación del Filtro de Kalman Extendido se deben tener en cuenta las siguientes

consideraciones:

Para calcular se aplicará la ecuación diferencial cinemática (3.35) discretizada empleando

las medidas de los giróscopos.

Las matrices de covarianza del ruido en las medidas (𝑄, 𝑅𝑖) se supondrán constantes y

conocidas en todo el proceso.

Se utilizará la representación de error de cuaterniones (3.37) explicada previamente.

En cuanto a la dinámica del cuaternión de error se empleará la definición en función del parámetro

de forma discretizada (3.45).

Ahora las matrices que definen la dinámica del sistema en el instante 𝑡𝑘 son:

𝐴𝑘 = 𝐼𝑑 − ∆𝑡𝑘 𝑘×𝑦𝐵𝑘 = 𝐼𝑑∆𝑡𝑘(4.41),

obtenidas a partir de la expresión (3.45), y:

𝐻𝑘(𝑘) =

[ (1,𝑘

𝐵′)×

(2,𝑘𝐵′

)×

⋮

(3,𝑘𝐵′

)×

]

=

[ (𝐶𝐼

𝐵′(𝑘)1,𝑘𝐼 )

×

(𝐶𝐼𝐵′(𝑘)2,𝑘

𝐼 )×

⋮

(𝐶𝐼𝐵′(𝑘)3,𝑘

𝐼 )×]

(4.42)

Filtro de Kalman

4.3.2. Consideraciones para el MEKF

El Filtro de Kalman es un método óptimo cuando se aplica a sistemas originalmente lineales,

sin embargo, cuando se aplica a linealizaciones de sistemas no lineales, no existe demostración

matemática de que el filtro sea convergente al valor real, y menos de que tal convergencia sea óptima.

Lo único que se ha comprobado con experimentación, es que si la estimación inicial es buena y el

ruido que entra al sistema no es excesivo, el filtro se comporta bien. Como curiosidad, el nombre

MEKF (Multiplicative Extended Kalman Filter) se toma de las propias características del filtro,

Extended porque extiende la teoría de filtros lineales a sistemas no lineales mediante una linealización

de las ecuaciones del error, y Multiplicative porque la ecuación del error para los cuaterniones ha de

ser definida de modo multiplicativo y no aditivo como el resto de variables.

Además de todo lo anterior, es necesario tener en cuenta ciertos aspectos matemáticos para

poder solucionar diversos problemas numéricos que surgen a la hora de implementar el filtro, como

son los siguientes:

Normalización del cuaternión: el cuaternión por definición tiene norma unidad, sin embargo,

debido a los errores numéricos esta condición no se va a cumplir al calcular q, acumulándose el

error con el tiempo. Por ello, es conveniente renormalizar cada vez que se calcule un cuaternión.

Esto va a provocar que un error que puede provenir del cálculo de una de las componentes del

cuaternión se extienda a las demás, pero al renormalizar en cada iteración el error va a ser

minúsculo.

Simetría de la matriz de covarianza: las matrices de covarianza tienen la característica de ser

simétricas y definidas positivas por definición, algo que no se cumple al propagar la covarianza

debido a la precisión finita de los ordenadores. Para solucionarlo basta con realizar el cálculo:

𝑃 =1

2(𝑃 + 𝑃𝑇)(4.43)

cada vez que hay un cálculo de la matriz P.



Inicialización: en primer lugar se fija (0) = 0 y 𝑃(0) = ∅(3 × 3) en función de los valores

iniciales estimados de la actitud. Las dimensiones de la matriz de covarianza son función del número

de medidas (direcciones) que obtenga el giróscopo, generalmente tres. Además, las matrices de

covarianza 𝑄 y 𝑅𝑖 se suponen constantes y conocidas para cada medida.

Ecuaciones de actualización en tiempo (Predicción):


𝑘− = 𝑘−1 +

1

2𝑆(𝑘)𝜔𝑘

∆𝑡𝑘(4.44)

𝑘− = 𝑘

−(|𝑘−|)−1(4.45)



𝑇 + 𝐵𝑘𝑄𝐵𝑘𝑇𝑐𝑜𝑛𝐴𝑘 = 𝐼𝑑 − ∆𝑡𝑘 𝑘

×, 𝐵𝑘 = 𝐼𝑑∆𝑡𝑘,𝑄 = 𝐶𝑜𝑣(𝜗𝜔)(4.46)

𝑃 =1

2(𝑃 + 𝑃𝑇)(4.47)

37

37


Ecuaciones de actualización de mediciones (Corrección): Para cada medida i se tiene 𝑖,𝑘𝐵 (la

medida) y 𝑖,𝑘𝐼 (el valor real). Se calcula para cada medida 𝑖,𝑘

𝐵′ = 𝐶𝐼𝐵′(𝑘)𝑖,𝑘

𝐼 ,obteniéndose:

𝐻𝑘(𝑘) =

[ (1,𝑘

𝐵′)×

(2,𝑘𝐵′

)×

⋮

(3,𝑘𝐵′

)×

]

(4.48)


𝐾𝑘 = 𝑃𝑘−𝐻𝑘


𝑇 + 𝑅𝑘)−1

(4.49)

donde 𝑅𝑘 es una matriz conteniendo las covarianzas de los ruidos de todas las medidas (por lo que

aunque las covarianzas de cada medida sean constantes, dependiendo de cuantas medidas se

tengan, en el instante de tiempo 𝑡𝑘 la matriz cambiará):

𝑅𝑘 = [

𝑅1 ∅ ⋯ ∅

∅ 𝑅2 ⋯ ∅

⋮

∅

⋮

∅

⋱

⋯

⋮

𝑅𝑚

](4.50)

Se calcula la matriz de los errores de medida:

𝛿𝑦𝑘 =

[ 1,𝑘

𝐵 − 1,𝑘𝐵′

2,𝑘𝐵 − 2,𝑘

𝐵′

…

𝑚,𝑘𝐵 − 𝑚,𝑘

𝐵′

]

(4.51)

Actualización de la estimación con la medición:

𝑘+ = 𝐾𝑘𝛿𝑦𝑘 (4.52)


𝑃𝑘+ = (𝐼𝑑 − 𝐾𝑘𝐻𝑘)𝑃𝑘

−(4.53)

Fase de reset: finalmente se actualiza 𝑘:

𝑘+ = 𝛿𝑞(𝑘

+) ∗ 𝑘− =

1

√4 + |𝑘+|

2[2𝑘

+] ∗ 𝑘−(4.54)

Filtro de Kalman

Figura 4-3. Diagrama de flujo del algoritmo MEKF discreto para las EDC en cuaterniones.

Capítulo 5

39

39

CAPÍTULO 5 INTRODUCCIÓN A LA METODOLOGÍA DE LA

PRÁCTICA

Uno de los objetivos fundamentales de este proyecto y el algoritmo en Matlab desarrollado es servir

como material de apoyo didáctico para el aprendizaje de los alumnos de la asignatura de Dinámica de Vehículos

Espaciales. La intención de la práctica informática es emplear los conceptos de representación y determinación

estática de actitud aprendidos en la asignatura así como la implementación del Filtro de Kalman para

proporcionar experiencia real en la estimación de la orientación de un elemento móvil a partir de medidas de

sensores. Para ello se utilizará Matlab y la aplicación (App) “Sensor Fusion”. Esta aplicación móvil permite

recopilar información acerca de los sensores presentes en el Smartphone y transmitirlos en tiempo real a un PC,

donde se accederá a dichos datos mediante Matlab, permitiendo así visualizar y demostrar la orientación

estimada del Smartphone.

A continuación se va a explicar en detalle el funcionamiento y alcance de la aplicación “Sensor Fusion”.

Posteriormete se analizarán los ejecutables en Matlab necesarios para la conexión entre la aplicación y el PC así

como el resto de programas esqueleto que se proporcionarán como base para el desarrollo posterior de la

práctica.

5.1. Aplicación Sensor Fusion

La aplicación Sensor Fusion es una aplicación gratuita de Android que

se puede descargar en Google Play (http://goo.gl/0qNyU). Escribiendo en el

buscador el nombre de la aplicación, hay que elegir aquella cuyo logotipo sea

el mostrado en la Figura 5-1.

A continuación se describen las funciones básicas que se encuentran al iniciar la aplicación. Éstas se

representan en la Figura 5-2 (a):

5.1.1. Select Sensor

En este apartado se muestran todos los sensores disponibles en el Smartphone (ver Figura 5-2 (b)).

Al seleccionar un sensor, las medidas que el sensor esté obteniendo se mostrarán en tiempo real en la

pantalla. En la Figura 5-2 (c) se muestra un ejemplo para el giróscopo.

Las medidas tanto del giróscopo como del acelerómetro y magnetómetro se visualizan en los tres ejes

del Smartphone, definidos éstos según la Figura 2-3. Además se muestra el valor medio de las medidas junto

con las unidades en las que se representa.

Figura 5-1. Logotipo de la

aplicación Sensor Fusion.

http://goo.gl/0qNyU

Introducción a la metodología de la práctica

5.1.2. Log Data

En este apartado se pueden elegir los sensores de los que se quiere guardar la información a través de

internet para ser guardada o procesada en tiempo real. Para elegir los sensores sólo es necesario clicar en el

recuadro anexo al nombre del mismo (ver Figura 5-3 (a)).

Las opciones “Stream” y “Log” están asociadas a si se quiere tratar las medidas a través de internet

o si sólo se van a guardar en un fichero respectivamente. La opción que se usará en esta práctica será a través

de internet.

Haciendo clic en el botón rojo se puede editar la configuración con el PC y la ubicación del archivo

de datos si se elige la función “Log” (ver Figura 5-3 (b)).

(a) Menú de inicio. (b) Selección de sensores.

Figura 5-2. Capturas de la aplicación Sensor Fusion.

(c) Medidas del giróscopo.

(a) Carga de datos. (b) Ajustes de carga de datos.


(c) Información de sensores.

41

41


5.1.3. Sensor List

En este apartado se abre una ventana donde se enumeran todas las medidas de sensores que están

disponibles en la aplicación (ver Figura 5-3 (c)). Algunos de los sensores mostrados son sensores físicos

(como acelerómetro o magnetómetro) y otros sensores son sensores virtuales que fusionan las medidas de

otros sensores (como el sensor de orientación). La toma de medidas comienza al seleccionar el botón que se

encuentra en la parte inferior de la Figura 5-3 (a). Inicialmente este botón señalará Start y, tras pulsarlo y

establecer conexión con el PC, se podrán en verde y cambiará a Stop, indicando que para parar la toma de

medidas es necesario volver a pulsarlo.

5.1.4. About/Help

En este apartado se muestra información y política de privacidad de la aplicación.

Además de todas las funciones mostradas, haciendo clic en el botón rojo, mostrado

en la Figura 5-4, en cualquier ventana, se abrirá de nuevo la opción de ajuste mostrada en

la Figura 5-3 (b) donde, además de la configuración de la carga de datos, se podrá cambiar

la frecuencia de las medidas, que como se verá posteriormente dependerá del sensor

elegido.

5.2. Archivos para conexión entre Smartphone y PC

Anexo a esta memoria, se adjunta código Matlab base para la programación de las tareas que se presentan

a continuación. La función de estos ficheros es la siguiente:

Archivo Startup.m: inicializa el entorno de trabajo para asociar los archivos proporcionados por los

sensores del Smartphone y que puedan integrarse las medidas en tiempo real en Matlab. Para ello, usa un

paquete java llamado sensordata.jar que también se incluye en los archivos descargables. Se puede

encontrar más información acerca de esta interfaz en [10].

Archivo ShowIP.m: determina la dirección IP del ordenador al que se va a conectar.

Archivo FilterTemplate.m y FilterTemplate_MEKF: estos archivos son los esqueletos que se van a

usar para realizar la estimación de la orientación en tiempo real a partir de los datos de los sensores del

Smartphone. Los datos de entrada de la función son los datos del acelerómetro (calAcc), giróscopo

(calGyr) y magnetómetro (calMag). Esta función proporciona como datos de salida una estructura (xhat)

compuesta por los tiempos de medida (t), la estimación de la orientación (x) para cada tiempo de medida

y la covarianza de la estimación de la orientación igualmente para cada tiempo de medida (P). Además,

proporciona una segunda estructura (meas) que guarda las medidas del acelerómetro (acc), giróscopo

(gyr), magnetómetro (mag), y orientación en cuaterniones (orientation) para cada tiempo de medida (t).

Figura 5-4. Botón de

ajustes de la aplicación

Sensor Fusion.


Figura 5-5. Diagrama de flujo del archivo FilterTemplate.m.

1

43

43


1

Figura 5-6. Subdiagrama de flujo del archivo FilterTemplate.m.


Figura 5-7. Modificación del subdiagrama de flujo del archivo

FilterTemplate.m para el archivo FilterTemplate_MEKF.m.

Modificación para MEKF

45

45


En las Figuras 5-5 y 5-6 se muestra un diagrama de flujo asociado al código en Matlab del archivo

FilterTemplate.m en el que se especifica la función de cada parte que conforma el código en cuestión. En la

Figura 5-7 se representa la modificación que se ha realizado sobre el archivo FilterTemplate.m (modificación

de las líneas de código contenidas entre las marcas verdes señaladas en la Figura 5-6 por las líneas representadas

en la Figura 5-7) para la obtención del archivo esqueleto para la programación del MEKF

(FilterTemplate_MEKF.m) especificando los pasos a seguir para ello. En ambos códigos aparecen funciones

propias de Matlab que se describen en la Tabla 5-1.

com.liu.sensordata Paquete Java para importar los datos recibidos de los

sensores del Smartphone.

Server = streamsensordatareader(Port) Establece la conexión con el puerto del Smartphone.

Server.start() Inicializa la toma de datos del Smartphone.

Server.status() Especifica si se sigue recibiendo medidas desde el

Smartphone.

Data=server.getNext(ms) Guarda las medidas indicando la frecuencia de la

toma en milisegundos (ms).

self=OrientationView(figname) Proporciona una figura para representar la

orientación del Smartphone asignándole un nombre.

setOrientation(self,x,P) Muestra la orientación estimada. Si se proporciona P,

se muestra además la covarianza de dicha estimación.

title(self,str) Muestra el título de la figura definida como self.

En la Figura 5-6 se muestra en rojo dónde se ha de colocar la estimación de la orientación para el archivo

FilterTemplate.m. Para el archivo FilterTemplate_MEKF.m. la estimación de la orientación se realiza a lo largo

de los pasos especificados en la Figura 5-7. Estas estimaciones son las que han de programar los alumnos durante

la práctica y que requerirá la inclusión de las funciones ayuda definidas en el Capítulo 3 y de la programación

de los diferentes métodos de estimación de actitud propuestos en el Capítulo 7.

5.3. Método de conexión entre Smartphone y PC

Para establecer la conexión entre el Smartphone y el PC se deben seguir los siguientes pasos, los cuales

serán necesarios antes de inicializar la toma de medidas mediante el Smartphone:

Ejecutar el archivo Startup.m. Este archivo deberá ser ejecutado cada vez que se inicie Matlab.

Ejecutar el archivo ShowIP.m. Este archivo proporciona la dirección IP que hay que introducir en la

opción IP Adress mostrada en la Figura 5-3 (b). En las opciones Frecuency y Port dejar las opciones por

defecto (100 Hz y 3400 respectivamente).

Tabla 5-1. Descripción de funciones de Matlab aplicadas en los archivos

FilterTemplate.m y FilterTemplate_MEKF.m.


Debe tenerse en cuenta que el Smartphone debe estar conectado a la misma red Wifi que el PC para

que sea posible la transferencia de datos.

Para verificar si se ha realizado correctamente la conexión:

Ejecutar el archivo FilterTemplate_prueba.m (la única diferencia con el archivo FilterTemplate.m

es que se incluye la inicialización del vector de estado, necesaria para la simulación) y al mismo tiempo

seleccionar los sensores giróscopo, acelerómetro y magnetómetro en la opción Log Data y pulsar el botón

Start que aparece en la parte inferior de la pantalla.

Si la conexión entre Smartphone y PC se ha realizado correctamente debe aparecer el mensaje mostrado

en la Figura 5-8 (a).

Cuando se quiera finalizar la toma de medidas, se pulsa la opción stop generando el mensaje mostrado en

la Figura 5-8 (b) y en Matlab se generarán las dos estructuras de salida del archivo FilterTemplate.m con

la estimación de la orientación del dispositivo y su covarianza, y las medidas del acelerómetro, giróscopo

y magnetómetro en cada instante de tiempo.

Si no se ha introducido la estimación de la orientación en el FilterTemplate.m, la representación de la

misma será la correspondiente a la condición inicial que se haya proporcionado ya que el estado (x) no se ha

propagado en el tiempo, es decir, no se ha estimado a partir de la condición inicial la orientación del Smartphone.

Esta tarea pendiente es la que los alumnos tendrán que desarrollar en la práctica, según se describirá en el

Capítulo 7

(a) Mensaje de inicio de conexión. (b) Mensaje de fin de conexión.


Capítulo 7

47

47

CAPÍTULO 6 CALIBRACIÓN DE SENSORES

Como se indicó en el Capítulo 4, para realizar la fusión de medidas mediante un Filtro de Kalman, es

necesario conocer la covarianza de los ruidos de medida (𝑄, 𝑅1, 𝑅2) para el cálculo de la ganancia de Kalman.

Es por ello que se dedica un capítulo completo al cálculo de dicha covarianza. Para ello, se tendrán que analizar

cómo evoluciona el ruido en las medidas de los sensores proporcionadas por el Smartphone, analizando la

frecuencia de la toma de medidas de cada uno y la condición inicial de la que parte la medida de cada sensor.

Finalmente se indican los archivos de código en Matlab adjuntos a esta memoria que permiten obtener las

salidas que se mencionarán.

6.1. Análisis de la frecuencia de la toma de medidas de los sensores

Un aspecto importante a la hora del tratamiento de datos es identificar la frecuencia con la que los

sensores toman las medidas así como el tiempo que tardan dichos sensores en iniciar la adquisición de datos.

Inicialmente en el bloque de Asociación del paquete de datos recibido a cada variable indicado en la

Figura 5-6, se especificó en la función Data=server.getNext(ms) y en la definición de la variable t una frecuencia

en la toma de medidas de 100 Hz, es decir, de 10 milisegundos. Esta frecuencia viene establecida como estándar

por la propia aplicación, sin embargo, para estimar con mayor precisión la orientación es necesario conocer el

incremento de tiempo entre medidas consecutivas.

Además, la estimación de la orientación comenzará cuando se tenga medidas del sensor giróscopo

esencialmente y para mejorar la estimación a priori mediante fusión de datos será necesario tener medidas del

resto de sensores considerados.

Para analizar los aspectos comentados, se han llevado a cabo tres simulaciones arbitrarias en las que se

muestran las medidas tomadas por cada sensor:

Figura 6-1. Medidas obtenidas en cada instante de tiempo para cada eje y sensor en

una primera simulación arbitraria.

Calibración de sensores

En la Figura 6-1 se aprecia cómo la frecuencia de la toma de medidas del acelerómetro y giróscopo

coincide aproximadamente con la establecida de manera estándar, es decir, con 100 Hz. Sin embargo, la

frecuencia de la toma de medidas del magnetómetro es menor. Dado que en los datos mostrados en la

Figura 6-1 entre cada toma de datos transcurre, en media, 0.01 segundos, la frecuencia de la toma de medidas

en este caso para el magnetómetro es en torno a 20 Hz.

Otro aspecto importante que se aprecia en los datos mostrados es que únicamente en el instante inicial se

tienen medidas del acelerómetro. La primera medida proporcionada por el giróscopo se da tras transcurrir 0.051

segundos desde el inicio de la simulación y la correspondiente al magnetómetro transcurridos 0.102 segundos.

Cabe preguntarse llegado a este punto, si esta frecuencia e instante de inicio de toma de medidas se

mantiene para cualquier simulación. Para su verificación se simulan otros dos movimientos arbitrarios

obteniéndose los resultados mostrados en las Figuras 6-2 y 6-3 respectivamente.

Figura 6-2. Medidas obtenidas en cada instante para cada eje y sensor en una

segunda simulación arbitraria.

Figura 6-3. Medidas obtenidas en cada instante para cada eje y sensor en una

tercera simulación arbitraria.

49

49


En base a los resultados obtenidos, se concluye que para el Smartphone con el que se ha realizado la

simulación la frecuencia de toma de datos en media es 100 Hz para el acelerómetro y giróscopo y 20 Hz para el

magnetómetro. Cabe indicar que estas frecuencias se establecen para el Smartphone que se ha usado para las

simulaciones. Será en la práctica física que se realizará con alumnos donde se verificará si estas frecuencias se

mantienen independientemente del tipo de Smartphone que se tenga.

Otra conclusión obtenida es que el instante de inicio de la toma de datos para los sensores giróscopo,

acelerómetro y magnetómetro es totalmente arbitrario y, a priori, impredecible. Desde el punto de vista de la

estimación de la orientación, el instante en el que se produce la primera medida del giróscopo indicará el

comienzo de la estimación de la misma ya que, el giróscopo, tal y como se indicó en el Capítulo 3, es la base

para la estimación de la actitud mediante las EDC (para el método TRIAD dependerá de qué dos sensores se

estén fusionando, requiriéndose datos de ambos sensores para poder llevar a cabo la estimación). A la hora de

aplicar el Filtro de Kalman, la fusión de las medidas del giróscopo junto con las del acelerómetro y

magnetómetro no supondrá problema alguno ya que, en el caso de tener únicamente las medidas del giróscopo,

el filtro no hará ninguna corrección sobre la orientación estimada mediante dicho sensor, pero seguirá siendo

imprescindible tener medidas del mismo.

Por otro lado, con el objetivo de tener una visión física de la variabilidad de las medidas de los sensores

que se están obteniendo, se representa en la Figura 6-4 las medidas obtenidas para cada eje y sensor de una

simulación arbitraria donde se ha tenido en cuenta las medidas no válidas (valores NaN) con la función

~any(isnan(meas.mag), 1) de Matlab. Esta representación se puede obtener ejecutando el archivo

Calibracion.m.

En dicha figuras se aprecia cómo el inicio de la toma de datos tanto del giróscopo como del magnetómetro

no coincide con el instante inicial. Además la evolución de la curva del magnetómetro presenta mayor

brusquedad lo que implica la menor frecuencia en la toma de medidas respecto al acelerómetro y giróscopo.

A continuación se estudiará la tendencia del ruido inducido por las medidas de los sensores giróscopo,

acelerómetro y magnetómetro comparando las mismas con un comportamiento gaussiano. Para ello, se hace

una breve introducción de las características del ruido en las medidas y del tratamiento de variables

unidimensionales y multidimensionales.

Figura 6-4. Medidas obtenidas en cada instante para cada eje y sensor frente al

tiempo en una simulación arbitraria.


6.2. Estadística del error. Distribución normal o Gaussiana.

Toda medida experimental está afectada de cierta imprecisión en sus valores debido a las imperfecciones

del aparato de medida. El valor de las magnitudes físicas se obtiene experimentalmente efectuando una medida.

Ésta puede ser directa sobre la magnitud en cuestión o indirecta, es decir, obtenida por medio de los valores

medidos de otras magnitudes ligadas con la magnitud problema mediante una fórmula física. Así pues, resulta

imposible llegar a conocer el valor exacto de ninguna magnitud, ya que los medios experimentales de

comparación con el patrón correspondiente en las medidas directas viene siempre afectado de imprecisiones

inevitables.

Este error se puede modelar como un sesgo (error constante) más un ruido blanco (aleatorio, con una

cierta covarianza), es decir, si se considera como ejemplo el caso del error de medida de un giróscopo tal que:

= + 𝛿(6.1)

donde es la medida del giróscopo, es la medida estimada y 𝛿 los errores de medida, la distribución de

una de las componentes de 𝛿 puede tener la siguiente evolución:

Dicha evolución implica que es imposible conocer el valor con exactitud de la medida del giróscopo ya

que ésta presenta alta variabilidad con el tiempo. Por tanto, la única solución ante este problema es recurrir a

representar las propiedades de dicha medida mediante la estadística. Concretamente, la teoría de procesos

estocásticos permite analizar cómo el ruido de los sensores se propaga como error de estimación.

A continuación se distinguen dos tipos de variables aleatorias continuas, haciendo hincapié en el uso que

se dará a cada una de ellas y las aplicaciones que se llevarán a cabo.

6.2.1. Variables aleatorias continuas unidimensionales

Una variable aleatoria continua (𝑋) es unidimensional cuando 𝑋 ∈ ℝ. En este caso, se definen las

siguientes propiedades:

𝐹(𝑋) = 𝑃(𝑋 ≤ 𝑥): la función de distribución 𝐹(𝑋) es la probabilidad de que 𝑋 ≤ 𝑥.

La función de distribución se calcula mediante la función de densidad 𝑓(𝑥): 𝐹(𝑋) = ∫ 𝑓(𝑦)𝑑𝑦𝑥

−∞.

Se define la esperanza matemática actuando sobre 𝑔(𝑥) como: 𝐸[𝑔(𝑋)] = ∫ 𝑔(𝑦)𝑓(𝑦)𝑑𝑦∞

−∞. A

partir de este parámetro se definen los siguientes:

1. Media: 𝑚(𝑋) = 𝐸[𝑋] = ∫ 𝑦𝑓(𝑦)𝑑𝑦∞

−∞.

2. Varianza: 𝑉(𝑋) = 𝐸 [(𝑋 − 𝑚(𝑋))2] = 𝐸[𝑋2] − (𝐸[𝑋])2.

3. Desviación típica 𝜎: 𝜎 = √𝑉(𝑋).

Figura 6-5. Ejemplo de evolución caótica del ruido en una medida.

51

51


6.2.2. Variables aleatorias continuas multidimensionales

Una variable aleatoria continua (𝑋) es multidimensional cuando ∈ ℝ𝑛. En este caso, se definen

las siguientes propiedades:

Cada componente de sigue una distribución unidimensional.

Al igual que en el caso unidimensional, se define una función de distribución conjunta, la cual se

calcula mediante la función de densidad 𝑓().

Se define la esperanza matemática actuando sobre 𝑔(𝑥) como: 𝐸[𝑔()] = ∫ 𝑔()𝑓()𝑑𝑦ℝ𝑛 . A

partir de este parámetro se definen los siguientes:

1. Media: () = 𝐸[] = ∫ 𝑓()𝑑𝑦ℝ𝑛 .

2. En este caso la varianza pierde sentido y se define la matriz de covarianza:

𝐶𝑜𝑣() = 𝐸 [( − ()) ( − ())𝑇] = 𝛴. Se trata de una matriz simétrica y

definida positiva cuyos valores en la diagonal representan la varianza de cada componente

de , y el resto de valores la correlación entre dos componentes de .

Para más información acerca de la descripción estadística del error consultar [6].

A continuación se va a analizar la semejanza del ruido inducido por las medidas tomadas con una

distribución normal o gaussiana. Esta semejanza se va a realizar en cada dirección de la toma de medidas de

cada sensor, por lo que se trabajará con variables unidimensionales, teniendo tres variables para el giróscopo,

acelerómetro y magnetómetro respectivamente.

Se dice que una variable unidimensional sigue una distribución normal cuando su función de densidad

verifica la siguiente expresión:

𝑓(𝑥) =1

𝜎√2𝜋exp(−

(𝑥 − 𝑚)2

2𝜎2 )(6.2)

En este caso, la variable se denota como una normal de media 𝑚 y desviación 𝜎 tal que:

𝑋~𝑁(𝑚, 𝜎2)(6.3)

Para una función que sigue una distribución normal se definen los intervalos de confianza (ver

Figura 6-6) en función de la probabilidad de que un valor se encuentre en dicho intervalo. De este modo se tienen

tres intervalos de confianza para la variable 𝑋:

Intervalo 1 − 𝜎:𝑃(𝑋 ∈ [𝑚 − 𝜎,𝑚 + 𝜎]) = 68,3%

Intervalo 2 − 𝜎:𝑃(𝑋 ∈ [𝑚 − 2𝜎,𝑚 + 2𝜎]) = 95,45%

Intervalo 3 − 𝜎:𝑃(𝑋 ∈ [𝑚 − 3𝜎,𝑚 + 3𝜎]) = 99,74%

Figura 6-6. Representación de la distribución normal en torno a la

media e intervalos de confianza.


Se define un proceso o variable estocástica como aquella variable aleatoria (𝑋(𝑡)) que cambia con el

tiempo. Las variables que se manejan en este proyecto (medidas de los sensores acelerómetro, giróscopo y

magnetómetro) serán este tipo de variable en cada dirección, es decir, si se tratan las tres medidas del

acelerómetro (el mismo ejemplo para las tres medidas del giróscopo y magnetómetro) por separado, cada medida

será una variable aleatoria continua estocástica y unidimensional. En caso de que se trate la medida cómo un

vector, ésta será una variable aleatoria continua estocástica y multidimensional.

Si la variable estocástica verifica una distribución normal, se dice que responde al comportamiento de un

proceso gaussiano. Si además dicho proceso verifica las siguientes propiedades:

𝐸[𝑋(𝑡)] = 0

𝐸[𝑋2] = 𝜎2

𝑅(𝑡, 𝜏) = 𝛿(𝑡 − 𝜏)𝜎2, donde 𝛿(𝑥) vale 1 si 𝑥 = 0 y 0 en cualquier otro caso. (Esta condición implica

que el valor del ruido blanco en un instante es independiente de su valor en cualquier instante anterior),

dicho proceso se denomina ruido blanco gaussiano y es el modelo más frecuente asumido para modelar las

fuentes de error de los sensores.

6.3. Tendencia normal de las medidas de los sensores

Dado que las fuentes de error de los sensores se suelen modelar como un ruido blanco gaussiano, a

continuación se va a verificar en Matlab que los resultados obtenidos a partir de los sensores del Smartphone se

asemejan a una distribución normal quedando como trabajo futuro la verificación de si además el ruido en las

medidas puede asumirse como ruido blanco.

Para obtener la distribución de las medidas de los sensores es necesario incluir tras el cierre de la función

definida en la Figura 5-5 los siguientes cálculos:

La función mean(meas.data(:, ~any(isnan(meas.data), 1)), 2) permite obtener el

vector con la media de cada componente de las tres medidas de cada sensor (especificando el sensor en

data), es decir, un vector columna de tres filas donde cada fila representa la media en x, en y y en z

respectivamente de las medidas tomadas en dichos ejes para cada sensor. La función isnan(x) busca los

valores NaN medidos por lo que es necesaria tenerla en cuenta para evitar errores. Esto se debe, como

ya se ha comentado, a que la frecuencia de la toma de medidas entre los sensores es distinta y puede

haber instantes de tiempo en los que no se esté recibiendo ninguna medida por parte de algún sensor.

La función cov(meas.data(:, ~any(isnan(meas.data), 1))') proporciona la matriz de

covarianza de cada sensor (especificando el sensor en data), de modo que serán las diagonales de dicha

matriz (cuadrado de la desviación típica en cada eje) las que interesarán para verificar la distribución de

las medidas en cada dirección.

La función histogram(meas.data(i,:),'Normalization','pdf') proporciona la

distribución normalizada de las medidas del sensor que se especifique en data en la dirección que se

especifique en i.

A la representación anterior se superpone la representación de la función de distribución para cada eje y

para cada sensor, teniendo en cuenta la media y desviación típica según la expresión (6.2). Los extremos

de la representación se deben ajustar para cada caso ya que las medidas irán variando para cada

simulación y para cada Smartphone.

Ejecutando el archivo Distribucion_medidas.m para tres simulaciones arbitrarias se obtienen los

siguientes resultados. Estas simulaciones se han realizado con el móvil estático de manera que sólo se está

midiendo el ruido que introducen los sensores.

53

53


Hay que tener en cuenta la medida del acelerómetro en el eje z, que indicará la gravedad, por lo que en

dicha dirección no se estará representando estrictamente el ruido inducido por el acelerómetro. En el

magnetómetro, del mismo modo, se estará midiendo el campo magnético terrestre por lo que sólo será de utilidad

el conocimiento de la desviación de sus medidas, no siendo posible el conocimiento del ruido que induce.

En la Figura 6-7 se muestra una comparativa de la función de distribución de las medidas tomadas en

cada eje y en cada sensor frente a la distribución normal asociada con misma media y desviación típica. La

similitud en la distribución permite asumir que el ruido en la medida de los sensores sigue aproximadamente

una distribución normal.

Para esta primera simulación se obtienen las siguientes propiedades:

La Tabla 6-2 muestra la media y desviación típica para cada sensor en cada eje y la Tabla 6-3 el ruido

inducido en el caso de giróscopo y acelerómetro. En el caso del giróscopo, la media representa directamente el

ruido, siendo éste bastante bajo. En el caso del acelerómetro, el eje z mide, además del ruido, la aceleración de

la gravedad, por lo que se puede obtener el ruido inducido simplemente eliminando esta medida constante,

Sensor Giróscopo Acelerómetro Magnetómetro

Dirección x y z x y z x y z

𝑬[𝑿(𝒕)] -1.4e-4 -3.2e-4 -4.2e-4 0.07 -0.08 9.87 20.6 -2.46 -28.6

𝝈𝟐 2.5e-5 8.2e-6 7.3e-4 1.93e-4 6.7e-4 3.4e-5 1.52 0.43 0.51

Sensor Giróscopo Acelerómetro

Dirección x y z x y z

𝑹𝒖𝒊𝒅𝒐 -1.4e-4 -3.2e-4 -4.2e-4 0.07 -0.08 0.07

Tabla 6-1. Media y varianza de las medidas en una primera simulación arbitraria estática.

Figura 6-7. Histograma de medidas frente a distribución normal asociada para cada eje y sensor

en una primera simulación arbitraria estática.

Tabla 6-2. Ruido inducido por giróscopo y acelerómetro en una

primera simulación arbitraria estática.


obteniéndose un ruido mayor que en el caso del giróscopo pero, aun así, considerablemente bajo. En el caso del

magnetómetro, es difícil conocer las medidas exactas del campo magnético en cada dirección, por lo que se

complica la identificación del ruido que induce. Sin embargo, la desviación típica proporciona una idea de la

variabilidad que presenta, siendo ésta bastante mayor que en el caso del giróscopo y acelerómetro. Una de las

consecuencias que pueden originar dicha desviación es la propia interacción entre el magnetómetro del móvil y

todos los instrumentos magnéticos que le rodean como puede ser el propio PC.

Para contrastar los datos obtenidos, se realizan dos simulaciones más obteniéndose las siguientes

representaciones y datos:

Para la segunda simulación se obtienen los siguientes resultados:



𝑬[𝑿(𝒕)] -1.6e-4 -4.1e-4 -4.7e-4 0.079 -0.075 9.763 19.53 -1.007 -29.7

𝝈𝟐 7e-5 2.3e-5 1.9e-5 2e-4 0.0021 3.5e-4 1.452 0.392 0.56



𝑹𝒖𝒊𝒅𝒐 -1.6e-4 -4.1e-4 -4.7e-4 0.079 -0.075 -0.037

Tabla 6-3. Media y varianza de las medidas en una segunda simulación arbitraria estática.


segunda simulación arbitraria estática.

Figura 6-8. Histograma de medidas frente a distribución normal asociada para cada eje y sensor de una




55

55


Una tercera simulación corrobora los resultados obtenidos:

Para la tercera simulación se obtienen los siguientes resultados

Finalmente, según la distribución del comportamiento mostrada, se concluye que el comportamiento del

ruido de los sensores aquí aplicados se puede modelar por un ruido gaussiano. La magnitud del valor de este

ruido es prácticamente despreciable para el giróscopo y baja para el acelerómetro, con una desviación pequeña

en ambos casos. Sin embargo, en el caso del magnetómetro, la desviación es bastante grande, debido esto a las

interferencias magnéticas que rodean al Smartphone, por ejemplo, el propio PC.

Por último, recalcar que los valores de ruido aquí obtenidos son para el Smartphone con el que se está

trabajando, verificándose estos valores tras realizar la práctica con los alumnos.



𝑬[𝑿(𝒕)] -1.37e-4 -3.05e-4 5.16e-4 0.079 -0.1087 9.79 19.86 -4.8 -28.1

𝝈𝟐 1.09e-5 6.25e-6 3.24e-4 1.74e-4 1.69e-4 3.3e-4 1.71 0.47 0.48



𝑹𝒖𝒊𝒅𝒐 -1.37e-4 -3.05e-4 5.16e-4 0.079 -0.1087 -0.021

Tabla 6-5. Media y varianza de las medidas en una tercera simulación arbitraria estática.


tercera simulación arbitraria estática.

Figura 6-9 Histograma de medidas frente a distribución normal asociada para cada eje y sensor de una





6.4. Archivos Matlab

Para la obtención de las representaciones y datos mostrados en este capítulo se presentan a continuación

los archivos aplicados:

Calibracion.m: Este archivo obtiene la representación de las medidas en cada eje de cada sensor que se

active frente al tiempo de simulación. En el mismo se tiene en cuenta la función isnan(x) para buscar los

valores NaN que se originen en las medidas recibidas por parte del Smartphone.

Distribucion_medidas.m: Este archivo obtiene las medias de las medidas obtenidas por parte de cada

sensor (vector medio y media de dicho vector), covarianzas medias de los errores inducidos por cada

vector y el histograma de cada medida frente a su distribución normal asociada. En el mismo se han

tenido en cuenta las funciones comentadas en el apartado 6.3. En la Tabla 6-7 se muestra la definición

de cada salida numérica de dicho programa:

Notación Matlab Definición

vector_giroscopo_media Vector medio del sensor giróscopo

vector_aceleracion_media Vector medio del sensor aceleración

vector_magnetico_media Vector medio del sensor magnético

media_aceleracion Media del vector medio del sensor aceleración

media_magnetico Media del vector medio del sensor magnetómetro

Q Matriz de covarianza del ruido inducido por el sensor

giróscopo

R1 Matriz de covarianza del ruido inducido por el sensor

acelerómetro

R2 Matriz de covarianza del ruido inducido por el sensor

magnetómetro

Tabla 6-7. Definición de las salidas del archivo Distribucion_medidas.m.

Capítulo 7

57

57

CAPÍTULO 7 ESTIMACIÓN DE LA ORIENTACIÓN DEL

SMARTPHONE EN TIEMPO REAL

Llegados a este capítulo, se tienen ya todas las herramientas necesarias para implementar en Matlab

los diferentes métodos de estimación de actitud comentados hasta ahora y obtener los resultados de

estimación de la orientación del Smartphone en tiempo real en cada caso, verificando si la fusión de datos

mejora la estimación de la actitud.

Para ello, es necesario inicialmente definir el estado del que parte la estimación así como determinar

la posición inercial de referencia del Smartphone que permita tener una referencia conocida. Además, se

tendrán en cuenta dos mejoras en la implementación del algoritmo, como son la especificación del

incremento de tiempo entre medidas consecutivas y el rechazo de picos de medida que se alejen del valor

medio estimado con anterioridad.

Finalmente, se muestran las conclusiones obtenidas en la implementación de cada método y una

comparación entre éstos basada en numerosas simulaciones realizadas. Estas conclusiones sí se verificarán

para cualquier Smartphone que se esté utilizando en la simulación, como se verá tras la realización de la

práctica y como era de esperar ya que, como norma general, la fusión de datos mejora la estimación de

actitud respecto al uso de un único sensor, aunque sea el mejor entre ellos.

7.1. Inicialización del estado

La inicialización de estado, es decir, el cuaternión que define la actitud del Smartphone en el instante

inicial de la simulación, puede elegirse cualquiera ya que éste será recalculado en el siguiente instante de tiempo

en el que se tengan medidas del giróscopo. En este proyecto se ha establecido como criterio la inicialización del

estado igual al cuaternión unidad, es decir:

𝑞 = [1000]𝑇(7.1)

7.2. Posición de referencia inercial

Para aplicar el MEKF y el método TRIAD es necesario conocer las medidas de los sensores en el

sistema de referencia inercial, en este caso, en el sistema NED.

Para la obtención de estas medidas de referencia, inicialmente se debe definir una posición de

referencia del Smartphone (por ejemplo, con el mismo sobre una mesa y con su eje y hacia al PC). Esta

posición representará la inicialización del estado anteriormente definida, es decir, en la posición de

referencia inercial el cuaternión que describe la orientación del Smartphone será, en este caso, el cuaternión

unidad (7.1).

Una vez definida la posición de referencia inercial, para obtener las medidas de los sensores en

dicha posición basta ejecutar el archivo Distribucion_medidas.m con el Smartphone inmóvil (en dicha

posición) ya que, como se vió en el Capítulo 6, con este programa se obtienen los valores medios de las

medidas de los tres sensores en cada eje. Como los resultados obtenidos son valores medios, es aconsejable

simular la toma de medidas durante un periodo largo de tiempo (por ejemplo 30 segundos) para obtener unos

resultados más fiables. La nomenclatura elegida para las salidas se ha definido en la Tabla 6-7.

Estimación de la orientación del Smartphone en tiempo real

En cuanto a los resultados que se obtienen, destacar que el vector de medidas inerciales medio del

acelerómetro apunta hacia abajo, es decir, hacia la dirección Down del sistema NED y el vector de medidas

inerciales medio del campo magnético no tiene componente en dirección este, solo tendrá dirección North y

Down en el sistema NED.

Para el caso del giróscopo, al estar el Smartphone sin movimiento, lo que se está midiendo es la media

del ruido inducido por el sensor, es decir, el bias del giróscopo. El resultado obtenido es que esta media es muy

pequeña (del orden de 𝑂(10−4)), lo que implica que el sensor giróscopo del Smartphone en cuestión es bastante

preciso. Es por ello que se considerará el bias del giróscopo despreciable en el algoritmo del MEKF analizado

en el Capítulo 4, es decir:

𝜔 ≈ 0(7.2),

facilitando así el tratamiento del algoritmo.

Destacar que la validez de esta aproximación se analizará en la realización de la práctica con los alumnos

verificando si es de aplicabilidad general independientemente del Smartphone utilizado.

7.3. Matrices de Covarianza del ruido inducido

Del mismo modo que en el apartado anterior, para aplicar el MEKF es necesario conocer las matrices

de covarianza del ruido inducido tanto del giróscopo (𝑄) como del acelerómetro (𝑅1) y magnetómetro (𝑅2):

Covarianza del error inducido por el giróscopo (𝜗𝜔): dado que se ha despreciado el bias

inducido por el giróscopo, la matriz de covarianza del mismo viene definida por el ruido gaussiano

que éste induce, es decir:

𝑄 = 𝐶𝑜𝑣(𝜗𝜔)(7.3)

Covarianza del error inducido por el acelerómetro (𝜗𝑔) y magnetómetro (𝜗𝑚): En este caso,

ambas matrices vienen definidas por el ruido gaussiano de cada medida respectivamente, tal que:

𝑅1 = 𝐶𝑜𝑣(𝜗𝑔)(7.4)

𝑅2 = 𝐶𝑜𝑣(𝜗𝑚)(7.5)

Estas matrices tienen dimensión (3 × 3), y su cálculo se consigue siguiendo el mismo procedimiento

que el definido para la obtención de la posición inercial de referencia. La nomenclatura elegida para las

matrices se ha definido en la Tabla 6-7.

Estas matrices se considerarán a priori constantes asociadas a los sensores de cada Smartphone.

59

59


7.4. Paso de tiempo y frecuencia

En el análisis de la frecuencia de las medidas de los sensores llevada a cabo en el Capítulo 6 se concluyó

que la frecuencia entre las medidas consecutivas de los sensores no siempre era la misma. Dado que para estimar

la orientación del Smartphone es necesario tener medidas del giróscopo en cada instante de tiempo (tanto para

implementar las EDC como el MEKF), la frecuencia elegida en la aplicación Sensor Fusion (Figura 5-3 (b)) no

deberá ser mayor que la frecuencia a la que es capaz de tomar medidas el sensor giróscopo del Smartphone que

se esté utilizando para la simulación.

Por otro lado, una vez establecida esta frecuencia, ésta no siempre se mantendrá constante por lo que el

intervalo de tiempo entre medidas consecutivas del giróscopo puede ir variando en torno al valor establecido.

Una vez establecidas las consideraciones a tener en cuenta a la hora de implementar los algoritmos de

estimación de actitud, se procede a determinar los pasos a seguir para programar en Matlab cada uno de los

diferentes métodos especificando mediante pantallazos la secuencia de programación.

7.5. Estimación de la Actitud mediante giróscopos

A continuación se establecen los pasos a seguir para implementar el algoritmo en Matlab que permita

obtener una estimación de la actitud mediante giróscopos y mediante dos métodos, implementando las EDC y

mediante el cuaternión de rotación de Euler. Para ello:

Se parte del FilterTemplate.m proporcionado, para ambos métodos.

Se incluye la inicialización del estado (cuaternión unidad)

y de su matriz de covarianza

Se determina el incremento de tiempo entre medidas consecutivas. Para ello:

1. Se inicializa el instante de tiempo previo (tprev)

2. Se calcula el instante de tiempo nuevo(tnuev)y el incremento de tiempo(dt)

3. Se actualiza el instante de tiempo previo(tprev=t-t0)


Se incluye el algoritmo de cálculo discreto correspondiente a cada método (EDC a la izquierda según

la expresión (3.35) y cuaternión de rotación de Euler a la derecha según las expresiones (3.47)-(3.48))

tras la toma de medidas del giróscopo. Se ha tenido en cuenta el incremento de tiempo calculado

previamente y que el cuaternión se debe normalizar en cada actualización. Además se hace uso de las

funciones Sq.m y qprod.m proporcionadas.

Implementando los pasos señalados, se obtienen los archivos Evolucion_giroscopos_EDC.m y

Evolucion_giroscopos_Euler.m adjuntos a la memoria respectivamente.

La correspondencia entre la notación aplicada en Matlab y en el desarrollo teórico en el presente proyecto

es la siguiente:

Simulando ambos métodos partiendo de la posición de referencia establecida con anterioridad para el

Smartphone se obtiene la representación que muestra el movimiento del Smartphone en tiempo real con el

cuaternión que describe su actitud en cada instante en la parte superior de la misma.

En la Figura 7-1 se muestra un ejemplo de la orientación del Smartphone en una posición definida como

inicial tras haber realizado un movimiento suave y constante al dispositivo y haber ejecutado el archivo

Evolucion_giroscopos_EDC_Euler.m en el cual se simulan los dos métodos comentados anteriormente de

manera simultánea. A priori, la diferencia entre ambos métodos parece escasa, sin embargo si se calcula el error

cometido comparando la posición final con la inicial se obtiene un error mayor para el caso de las EDC. Un

ejemplo de valores obtenidos son los siguientes:

𝑒𝑟𝑟𝑜𝑟𝐸𝐷𝐶 = 0.093447(7.6)

𝑒𝑟𝑟𝑜𝑟𝐸𝑢𝑙𝑒𝑟 = 0.09325(7.7)


x

xrot 𝑟𝑜𝑡

e 𝑒

theta 𝜃

dt ∆𝑡

Tabla 7-1. Equivalencia de notación Matlab/Teoría para

Evolucion_giroscopos_EDC.m y Evolucion_giroscopos_Euler.m.

61

61


Cabe preguntarse si el resultado obtenido es siempre el mismo, es decir, si el método del cuaternión de

rotación de Euler es mejor que las EDC para estimar la actitud del Smartphone. Realizando simulaciones

arbitrarias, es decir, sin intentar mantener una velocidad angular aproximadamente constante, se deduce que las

EDC obtienen una mejor estimación que el método de cuaternión de rotación de Euler, lo cual es lógico ya que

la aproximación de velocidad angular constante entre medidas consecutivas no representará un movimiento

brusco, por ejemplo. Por tanto, se concluye que de aquí en adelante, al estimar las EDC un movimiento genérico

con mayor precisión, será éste el método que se aplicará para estimar la actitud mediante la medida únicamente

del sensor giróscopo.

Destacar que esta conclusión se aplica independientemente del Smartphone con el que se realice la

comparación ya que en ambos algoritmos se parte de las mismas medidas.

7.6. Estimación de la Actitud mediante el método TRIAD


obtener una estimación de la actitud mediante el método TRIAD a partir de las medidas obtenidas de los sensores

acelerómetro y magnetómetro. Dado que este método da más peso a la medida número 1, se va a comparar las

dos soluciones posibles, una considerando el acelerómetro como medida más fiable y otra considerando el

magnetómetro como tal. Para ello:

Se parte del FilterTemplate.m proporcionado.

Se incluye la inicialización del estado (cuaternión unidad)

para ambos casos

Figura 7-1. Orientación del Smartphone en la posición de referencia inercial tras la simulación

con el archivo Evolucion_giroscopos_EDC_Euler.m.

Figura 6-9 Histograma de medidas frente a distribución normal asociada para cada eje y sensor

de una tercera simulación arbitraria estática.


Se incluye el algoritmo de cálculo discreto correspondiente al método TRIAD (según las expresiones

(3.49)-(3.50)).

Se incluye la llamada al archivo de ayuda TRIAD.m en el que se aplican las expresiones (3.51)-(3.53)

para el cálculo de la matriz de rotación y el uso de la función DCM2quat.m proporcionada.

Implementando los pasos señalados, se obtiene el archivo Evolucion_TRIAD.m adjunto a la memoria.


es la señalada en la Tabla 3-3.

Simulando ambos métodos partiendo de la posición de referencia establecida con anterioridad para el

Smartphone se obtiene la representación que muestra el movimiento del Smartphone en tiempo real con el

cuaternión que describe su actitud en cada instante en la parte superior de la misma.


inicial tras haber realizado una simulación arbitraria y haber ejecutado el archivo Evolucion_TRIAD.m en el cual

se compara la solución obtenida al considerar el acelerómetro como primera medida (acc + mag) o el

magnetómetro (mag + acc). Como se muestra en la misma, la diferencia entre ambos métodos es significativa,

ya que, si se calcula el error cometido comparando la posición final con la inicial, se obtiene un error mayor para

el caso de considerar la medida del magnetómetro como la primera. Un ejemplo de valores obtenidos son los

siguientes:

𝑒𝑟𝑟𝑜𝑟𝑎𝑐𝑐+𝑚𝑎𝑔 = 3.921 ∙ 10−5(7.8)

𝑒𝑟𝑟𝑜𝑟𝑚𝑎𝑔+𝑎𝑐𝑐 = 4.1447 ∙ 10−5(7.9)

63

63


Finalmente, se concluye que las medidas del magnetómetro son menos fiables que las del acelerómetro,

como era de esperar. Sin embargo, pueden darse situaciones en los que las características del magnetómetro sean

mejores que las del acelerómetro y se consiga una mejor estimación considerando estas medidas como primeras

en el método TRIAD. Esta posibilidad se analizará de nuevo en la práctica que se llevará a cabo con los alumnos.

7.7. Estimación de la Actitud mediante el MEKF


obtener una estimación de la actitud mediante giróscopos fusionado con las medidas obtenidas por el

acelerómetro y magnetómetro. Para ello:

Se parte del FilterTemplate_MEKF.m proporcionado.

Se incluye la inicialización del estado (cuaternión unidad) y de su matriz de covarianza y el incremento

de tiempo entre medidas consecutivas siguiendo los mismos pasos que en el apartado 7.5.

Se incluyen las matrices de covarianza comentadas en el apartado 7.3 (diferentes para cada Smartphone

y posición inicial).

Se incluye el algoritmo de cálculo discreto correspondiente al MEKF expuesto en el Capítulo 4 en las

líneas de código correspondientes. A continuación se especifica la correspondencia entre el algoritmo

y las ecuaciones discretas establecidas en dicho capítulo para facilitar la comprensión del mismo:


con el archivo Evolucion_TRIAD.m.




Destacar que se ha tenido en cuenta el incremento de tiempo calculado previamente y que el cuaternión

y la matriz de covarianzas del estado se deben normalizar en cada actualización. Además se hace uso de las

funciones Sq.m, qprod.m y quat2DCM.m proporcionadas.

Implementando los pasos señalados, se obtiene el archivo MEKF.m adjunto a la memoria.


es la siguiente:

(4.43)

(4.44)

(4.51)

(4.50)

(4.40)

(4.40)

(4.45)

(4.45)

(4.47)

(4.47)

(4.46)

(4.49)

(4.48)

(4.48)

(3.26)

65

65



inicial tras haber realizado un movimiento arbitrario sobre el dispositivo y haber ejecutado el archivo

MEKF_comparacion.m en el cual se simula la diferencia entre la estimación de la actitud al incluir el

magnetómetro respecto a la consideración del acelerómetro como única medida externa.


x

xrot 𝑟𝑜𝑡

e 𝑒

theta 𝜃

dt ∆𝑡

A 𝐴

B 𝐵

P 𝑃

CIB 𝐶𝐼𝐵′

VI1 1𝐼

VBprima1 1𝐵′

VBprima1_vect (1𝐵′

)×

VI2 2𝐼

VBprima2 2𝐵′

VBprima2_vect (2𝐵′

)×

H 𝐻

R 𝑅

Delta_y 𝛿

K 𝐾

a


con el archivo MEKF_comparacion.m.



Tabla 7-2. Equivalencia de notación Matlab/Teoría para MEKF.m.


La diferencia entre ambos métodos es pequeña, sin embargo en la propia figura se puede apreciar cómo

la inclusión del magnetómetro aproxima con mayor exactitud el cuaternión final al inicial (el cuaternión

identidad). Para este ejemplo los errores obtenidos en ambos casos son los siguientes:

𝑒𝑟𝑟𝑜𝑟𝑎𝑐𝑐 = 1.0747 ∙ 10−6(7.10)

𝑒𝑟𝑟𝑜𝑟𝑎𝑐𝑐+𝑚𝑎𝑔 = 2.2522 ∙ 10−6(7.11)

Como se deduce de los errores obtenidos, el incluir un sensor externo mejora la estimación de la

orientación, incluso puede dividir a la mitad el error obtenido en la predicción. En la práctica con los alumnos

se analizará si la inclusión del magnetómetro siempre mejora la estimación de la orientación del Smartphone ya

que ahora, a diferencia del caso analizado en el apartado 7.5, las medidas de partida son diferentes para los dos

códigos que se están comparando y por tanto el resultado variará entre diferentes Smartphones.

El hecho de que el magnetómetro mejore la estimación de la actitud, suponiendo que este sensor es

suficientemente bueno para ello, se basa en que el acelerómetro únicamente es capaz de corregir la estimación

realizada mediante los giróscopos en los planos (Z,X) y (Z,Y) ya que la componente del acelerómetro que

realmente corrige las medidas por su mayor magnitud es la componente z. Será el magnetómetro el que sea

capaz de corregir la orientación del Smartphone en el plano (X,Y) del mismo, es decir, estas medidas se

encargarán de rotar la estimación de la orientación en el eje z, por lo que la inclusión del magnetómetro se hace

necesaria para lograr una estimación correcta en todos los planos, siempre que sus medidas sean suficientemente

buenas.

Llegados a este punto, cabe preguntarse si la estimación de la orientación mediante el MEKF mejora la

predicha por las EDC. Para responder a esta pregunta, en la Figura 7-4 se muestra un ejemplo de la orientación

del Smartphone en una posición definida como inicial tras haber realizado un movimiento arbitrario sobre el

dispositivo y haber ejecutado el archivo MEKF_giroscopos_EDC.m en el cual se simula la diferencia entre la

estimación de la actitud mediante el MEKF (incluyendo el magnetómetro) y las EDC.

En dicha figura se aprecia cómo la fusión de datos mejora la estimación de la actitud partiendo únicamente

de los giróscopos (EDC). Para este ejemplo los errores obtenidos en ambos casos son los siguientes:

𝑒𝑟𝑟𝑜𝑟𝑀𝐸𝐾𝐹 = 3.329 ∙ 10−6(7.12)

𝑒𝑟𝑟𝑜𝑟𝐸𝐷𝐶 = 3.3376 ∙ 10−6(7.13)


con el archivo MEKF_giroscopos_EDC.m.



67

67


Finalmente, tras realizar numerosas simulaciones, se concluye que para el Smartphone aplicado en ellas

la fusión de datos mejora la predicción de la estimación de la actitud del mismo respecto al uso únicamente de

las medidas de los giróscopos como dato de partida. En la práctica con los alumnos se analizará si esta conclusión

es de aplicabilidad general ya que, de nuevo, las medidas de partida son diferentes para los dos códigos que se

están comparando y por tanto el resultado variará entre diferentes Smartphones.

7.7.1. Outlier Rejection para el MEKF

Un aspecto importante a la hora de tratar medidas de sensores es rechazar medidas no válidas, es

decir, picos de medida que estarían induciendo un error en la estimación de la actitud. Para eliminar estos

picos se propone tomar como error máximo admitido una desviación máxima de 3 veces la raíz cuadrada

de la traza de la matriz de covarianza de la misma. La diferencia entre la norma de la medida en el instante

k con la norma del vector referencia inercial medido inicialmente de la misma no deberá ser mayor a este

error máximo admitido para considerar dicha medida como válida.


eliminar estos picos en el algoritmo MEKF. Para ello:

Se parte del algoritmo MEKF programado en el apartado 7.7.

Se calcula la desviación máxima permitida en cada medida externa tras la definición de las matrices

de covarianza correspondientes.

Se incluye la norma del vector referencia inercial medido inicialmente de cada media.

Tras el condicional de medida disponible, se incluye un segundo comparando la norma de la medida

actual con el valor medio y se limita la diferencia a la desviación máxima permitida en cada medida.


Implementando los pasos señalados, se obtiene el archivo MEKF_outlier_rejection.m adjunto a la

memoria.

En la Figura 7-5 se muestra un ejemplo de la orientación del Smartphone en una posición definida

como inicial tras haber realizado un movimiento arbitrario sobre el dispositivo y haber ejecutado el archivo

MEKF_outlier_rejection.m.

Comparando los resultados sin eliminar los picos y eliminando los picos para una misma

simulación, se aprecia una cierta mejoría en la estimación. Más concretamente, para el ejemplo mostrado

en la Figura 7-5 se tienen los siguientes errores:

𝑒𝑟𝑟𝑜𝑟𝑀𝐸𝐾𝐹 = 2.1281 ∙ 10−6(7.14)

𝑒𝑟𝑟𝑜𝑟𝑀𝐸𝐾𝐹𝑜𝑢𝑡𝑙𝑖𝑒𝑟𝑟𝑒𝑗𝑒𝑐𝑡𝑖𝑜𝑛= 4.5555 ∙ 10−6(7.15)

En este caso, dado que la fuente de datos de ambos programas será la misma, la conclusión de que

eliminar los picos mejora la estimación es independiente del tipo de Smartphone. Sin embargo, un aspecto

importante a tener en cuenta es que se están eliminando medidas que se alejan de una media calculada para

un movimiento estático, es decir, la media de las medidas establecidas debería calcularse para un

movimiento semejante al movimiento que se quiera simular y del que se quieran eliminar los picos, ya que,

si la simulación se aleja del movimiento para el que se han calculado las medias, se podrían estar rechazando

medidas válidas.


con el archivo MEKF_outlier_rejection.m.



69

69


7.8. Archivos Matlab

En la Tabla 7-3 se muestra un resumen de los archivos de código en Matlab comentados en este capítulo

y la función de cada uno de ellos, obviando los archivos ayuda comentados en el apartado 3.8 del Capítulo 3.

Estos archivos son la solución a cada problema planteado, partiendo de los ficheros base (FilterTemplate.m y

FilterTemplate_MEKF.m) y haciendo uso de los archivos ayuda proporcionados.

Archivos solución Definición

Evolucion_giroscopos_EDC.m Representa en tiempo real la estimación de la orientación del

Smartphone aplicando las EDC

Evolucion_giroscopos_Euler.m Representa en tiempo real la estimación de la orientación del

Smartphone aplicando el cuaternión de rotación de Euler

Evolucion_giroscopos_EDC_Euler.m Representa simultáneamente en tiempo real la estimación de

la orientación del Smartphone aplicando las EDC y el

cuaternión de rotación de Euler

Evolucion_TRIAD.m Representa simultáneamente en tiempo real la estimación de

la orientación del Smartphone aplicando método TRIAD al

acelerómetro como primera medida y al magnetómetro como

segunda y viceversa

MEKF.m Representa en tiempo real la estimación de la orientación del

Smartphone aplicando el MEKF

MEKF_comparacion.m Representa simultáneamente en tiempo real la estimación de

la orientación del Smartphone aplicando el MEKF

incluyendo y sin incluir el magnetómetro

MEKF_giroscopos_EDC.m Representa simultáneamente en tiempo real la estimación de

la orientación del Smartphone aplicando las EDC y el MEKF

(con magnetómetro)

MEKF_outlier_rejection.m Representa en tiempo real la estimación de la orientación del

Smartphone eliminando picos de medida al acelerómetro y

magnetómetro en el MEKF

Tabla 7-3. Resumen de archivos en Matlab aplicados en el Capítulo 7.

Capítulo 8

71

71

CAPÍTULO 8 PRÁCTICA DE DINÁMICA DE VEHÍCULOS

ESPACIALES

Uno de los objetivos fundamentales de este proyecto y el algoritmo en Matlab desarrollado es servir

como material de apoyo didáctico para el aprendizaje de los alumnos de la asignatura Dinámica de Vehículos

Espaciales. Por ello, anexo a este proyecto se ha realizado un boletín para el desarrollo de una práctica numérica

en el que el alumno pueda interactuar directamente con la aplicación. La intención de este boletín es que sea

ameno, que ayude al alumno a programar en Matlab el código que permita la estimación de la actitud y que se

proporcione la solución al problema de manera que la representación de los resultados sea fácil de interpretar.

Con el fin de comprobar la aplicabilidad conceptual del algoritmo desarrollado, se ha llevado a cabo la

simulación de una práctica con los alumnos Jason Condor Romero, Santiago Hoyos Martín, Juan Ramón Parra

Vilar e Inmaculada Soldado Serrano de 1º de Máster en Ingeniería Aeronáutica en Junio de 2017.

En este capítulo se presenta la esquematización que se ha optado por dar al boletín de la práctica para

que la secuencia de tareas desarrollada tuviera coherencia. Inicialmente se presenta el boletín final, con las

modificaciones ya implementadas tras haber realizado la práctica con los alumnos. Posteriormente se especifican

las mejoras que se llevaron a cabo sobre el mismo tras la realización de dicha práctica y se presenta el

cuestionario realizado a los alumnos para valorar y proporner mejoras sobre el desarrollo de la misma.

8.1. Boletín de la práctica

Para guiar a los alumnos en la implementación de un algoritmo en Matlab capaz de estimar la actitud

en tiempo real de su Smartphone, se ha diseñado un boletín ceñido a los conocimientos básicos necesarios

para ello.

El boletín se esquematiza de la siguiente forma (se puede consultar en el Apéndice A).

Inicialmente se presenta una breve introducción al problema que se va a resolver. Posteriormente

se presenta la aplicación del Smartphone a descargar para la generación de la toma de datos de los sensores

para seguir con los conceptos básicos necesarios de la cinemática de la actitud en cuaterniones y del Filtro

de Kalman Multiplicativo Extendido. Consecutivamente se explican los archivos Matlab que se

proporcionan como ayuda para el desarrollo autónomo de la práctica por parte del alumno así como la

verificación de la correcta conexión entre Smartphone y PC y el análisis de los sensores (en términos de

frecuencia y ruido) de cada Smartphone. Los apartados que incluyen la asimilación de estos conceptos

(apartados del 1 al 5 del boletín) se proponen llevarlos a cabo de manera individual en casa para agilizar el

objetivo de la práctica.

De este modo, el apartado que se realizará en sí de manera conjunta será el sexto. Éste se inicia de

nuevo con la conexión entre Smartphone y PC y pide calcular las constantes requeridas para programar el

algoritmo en Matlab aplicando las EDC y el MEKF. Además se proporcionan programas comparativos ya

completos incluyendo el método TRIAD y el cuaternión de rotación de Euler además de la implementación

del Outlier Rejection.

Práctica de Dinámica de Vehículos Espaciales

8.2. Cuestionario a los alumnos

Para valorar la opinión de los alumnos, comprobar la facilidad de uso y eficacia del algoritmo

programado en Matlab y, por supuesto, recaudar propuestas de mejoras, recomendaciones, detección de

fallos, y cualquier otra valiosa aportación por parte de los alumnos (que son el usuario objetivo de la

práctica) se elaboró un cuestionario para que éstos lo rellenaran tras la realización de la práctica.

El formato a rellenar se puede consultar en el Apéndice B.

8.2.1. Resultados de la valoración de los alumnos

A continuación se van a analizar los resultados del cuestionario (en términos de valores medios

y desviación típica) y mostrar los comentarios más importantes de los alumnos.

Media Desviación Típica

Duración de la práctica 4 0

Claridad y secuenciación de ejercicios del boletín 4.25 0.957

Facilidad en la programación en Matlab 4 1.414

Contenido teórico autosuficiente 4 0

Interés didáctico encontrado 4.75 0.5

En general los resultados obtenidos son bastante optimistas. Los alumnos están satisfechos con

la claridad del boletín y el interés didáctico encontrado. Lo mismo sucede con la autosuficiencia del

contenido teórico encontrado y la duración de la práctica. El caso de la programación en Matlab es el

punto más dado a variedad de opiniones, como se ve en su mayor valor de la desviación, encontrando

mayor dificultad en este aspecto.

Los comentarios proporcionados por los alumnos como realimentación al boletín de la práctica

son los siguientes. Éstos proponen:

(1) La realización de una “pre-práctica” para facilitar los pasos iniciales de conexión.

(2) Explicación de los contenidos teóricos con antelación, especialmente los relacionados al MEKF.

(3) Comprobación previa de la disposición del software apropiado por parte de los alumnos.

(4) Corrección del código de comprobación de la conexión.

(5) Presentación de la práctica tipo “diagrama de flujo” previa a la realización de la misma.

Aparte de los comentarios mostrados, hacer hincapié en que tras la realización de la práctica

los alumnos valoraron positivamente ésta, considerándola como interesante ya que ofrece la

posibilidad de apreciar en primera persona el funcionamiento real de un sistema de determinación de

actitud y sus limitaciones. Sin embargo, todos ellos afirmaron que no les ha ayudado a comprender

bien el concepto del Filtro de Kalman visto en la asignatura y que sigue siendo aún un reto para ellos.

Tabla 8-1. Media y desviación típica de la evaluación por parte de los alumnos.

73

73


Tras la realización de la práctica se aplicaron las siguientes mejoras en cuanto al boletín de la misma

teniendo en cuenta los comentarios de los alumnos y los contratiempos encontrados durante la misma:

1) La primera parte de la práctica que incluye los apartados del 1 al 5 se propone hacer como trabajo

en casa para agilizar la conexión entre Smartphone y PC a la hora de la realización de la práctica

conjunta y para que el desarrollo de la misma sea más corto y ameno ya que inicialmente pueden

aparecer problemas de conexión hasta familiarizarse con la App.

2) La parte de calibración de sensores se propone hacer como parte guiada para acortar la duración de

la práctica ya que el objetivo de la misma no se centra en esta parte (aunque es importante entender

bien el concepto).

3) La programación del cuaternión de rotación de Euler y del Outlier Rejection se proporcionan como

programas dados para únicamente apreciar la diferencia en la representación respecto a otros

métodos de estimación de actitud.

4) Se incluye el programa comparativo del método TRIAD posteriormente a la práctica ya que, por

falta de tiempo, no se incluyó inicialmente.

5) Se añade el archivo FilterTemplate_prueba.m tras apreciar que para la comprobación de la

conexión entre Smartphone y PC es necesario inicializar el estado, paso que no se incluía en el

archivo FilterTmeplate.m indicado en su día en la memoria de la práctica.

6) Se deduce que es necesario una versión de Matlab posterior a 2015 (incluyendo ésta) y Windows

10 como sistema operativo para ejecutar sin problemas la función hist de Matlab y para visualizar

correctamente las representaciones de la estimación de actitud en tiempo real.

Aunque estas mejoras ya han sido implementadas, se espera que en el futuro se alcancen

mayores niveles de satisfacción por parte de los alumnos.

Práctica de Dinámica de Vehículos Espaciales

Capítulo 9

75

75

CAPÍTULO 9 CONCLUSIONES Y TRABAJO FUTURO

Tal y como se describió en la introducción, el objeto de este proyecto es generar un algoritmo en

Matlab para, a partir de las medidas proporcionadas por los sensores de un Smartphone, estimar la orientación

del mismo en tiempo real mediante teoría de cuaterniones aplicando diferentes métodos de estimación de actitud,

además del diseño de una práctica numérica en la cual se tendrán que implementar dicho métodos por parte de

los alumnos.

Después de lograr la implementación de los diferentes métodos de estimación de actitud en Matlab

y simular el algoritmo desarrollado en la práctica con los alumnos, se concluye este trabajo con un análisis

de los resultados obtenidos y las principales conclusiones que pueden extraerse de dichos resultados.

Posteriormente, se propondrán líneas de trabajo futuro que serían interesantes abordar para

completar y mejorar algunos de los resultados que se han obtenido en el trabajo.

9.1. Conclusiones

A continuación se presentan las conclusiones extraídas en cuanto a los resultados obtenidos en este

proyecto teniendo la realización de la práctica con los alumnos y el uso de otro Smartphone diferente al aplicado

en los resultados mostrados en el Capítulo 7 como referencia en cuanto a la validación de las hipótesis

establecidas a lo largo del trabajo. Estas conclusiones son las siguientes:

Se concluye que el error medio del ruido inducido por los giróscopos es pequeño (𝑂(10−4)), por lo

que la hipótesis de que el bias de este sensor se pueda considerar despreciable (7.2) es válida para una

primera aproximación independientemente del tipo de Smartphone. Además, se puso de manifiesto

que la frecuencia en la toma de medidas no es la misma para cada Smartphone, sin embargo, en media

se puede asumir que el giróscopo y acelerómetro tienen una frecuencia de muestreo entre 80 y 100 Hz

y el magnetómetro de 50 Hz. Finalmente, se dedujo que el instante de comienzo de la toma de medidas

de cada sensor era impredecible y aleatorio.

El método de estimación de actitud a partir únicamente de giróscopos (ya sea aplicando las EDC o el

cuaternión de rotación de Euler) proporcionará un mejor resultado cuanto mejor sea el sensor en

cuestión. Dado que ambos métodos (EDC y Euler) beben de la misma fuente de datos, se deduce que

la estimación de la actitud mediante las EDC proporciona un resultado más realista para un movimiento

que se aleje de verificar una velocidad angular aproximadamente constante entre medidas consecutivas,

siendo para este movimiento el cuaternión de rotación de Euler el mejor método de estimación de

actitud. Sin embargo, para no imponer restricciones al movimiento a la hora de conseguir un mejor

resultado, se elige el método de estimación de actitud mediante giróscopos aplicando las EDC como

predeterminado.

En cuanto al método de estimación de actitud TRIAD, si bien se demostró que en la práctica la

variabilidad de magnetómetros es alta y pueden llegar a ser tan buenos como los acelerómetros, en

general se concluye que las medidas de los mismos son menos fiables que la de los acelerómetros por

lo que la elección de las medidas de estos últimos como las más fiables es la opción más acertada.

Conclusiones y tabajo futuro

Tras la simulación del MEKF para distintos Smartphones se verifica uno de los principales objetivos

del presente proyecto, la inclusión de un sensor externo mejora la estimación de la orientación (ya sea

incluyendo únicamente el acelerómetro o magnetómetro o ambos como medidas externas). De nuevo,

en caso de que el magnetómetro fuera poco fiable, la inclusión de esta medida externa podría perjudicar

la estimación, pero por lo general se comprobó su eficacia.

La implementación del cálculo del incremento de tiempo entre instantes consecutivos de toma de

medidas por parte de los sensores y la eliminación de los picos de medida en el MEKF proporcionan

resultados favorables independientemente del Smartphone ya que estas modificaciones no se ven

alteradas por la fuente de datos recibida.

En base al contenido de la práctica realizada por los alumnos, el algoritmo aquí implementado puede

resultar muy aplicable y recomendable para visualizar el concepto teórico de la asignatura Dinámica

de Vehículos Espaciales, debido a la cercanía con la vida diaria del elemento al que se va a aplicar la

estimación de actitud (el propio Smartphone), lo que puede generar un mayor atractivo e implicación

por parte de los alumnos.

9.2. Mejoras y trabajo futuro

En el presente proyecto se han tratado conceptos básicos de estimación de actitud y de sus diferentes

métodos de predicción en tiempo real y se ha implementado código en Matlab que permita cumplir los objetivos

definidos en el inicio de este proyecto.

Dichos objetivos se han alcanzado satisfactoriamente, pero cabe plantearse nuevas posibilidades de

ampliación y mejoras tanto para el desarrollo del software como para la memoria del proyecto. Las propuestas

son las siguientes:

Una de las posibles mejoras en cuanto a completar las hipótesis preestablecidas sería la demostración

de que el ruido que inducen las medidas de los sensores puede ser considerado como ruido blanco

gaussiano ya que, en el Capítulo 6, únicamente se demostró la normalidad del mismo y no si puede

asumirse como un ruido blanco. Bajo esta hipótesis se establece el Filtro de Kalman Extendido

Multiplicativo por lo que es una mejora importante en cuanto a la justificación teórica del método de

estimación de actitud planteado en el Capítulo 4.

Desde el punto de vista de la implementación del MEFK se pueden establecer las siguientes mejoras:

1. Inclusión del bias del giróscopo: dado que se ha justificado que el bias del sensor giróscopo

puede despreciarse, la consideración del mismo en la implementación de la estimación de actitud

proporcionaría una mejora en los resultados obtenidos ya que, de este modo, se estaría teniendo en

cuenta que el ruido inducido por el giróscopo tiene un error medio que también se propaga con el

tiempo, al igual que el estado. Por tanto, la inclusión del bias del giróscopo se propone como tarea

futura.

2. Matrices de covarianza del ruido variables: a lo largo del proyecto se ha asumido que las

matrices de covarianza del ruido inducido por los sensores giróscopo, acelerómetro y

magnetómetro son constantes. Estas matrices pueden ser modificadas de forma que se obtengan

mejores características de los sensores correspondientes y se pueda obtener una mejor estimación

en tiempo real. Como trabajo futuro se propone establecer estas matrices variables en cada paso

de tiempo de modo que ofrezcan la mejor caracterización para cada sensor.

77

77


3. Eliminación de picos de medida: en la implemetación del Outlier Rejection se ha establecido un

criterio para la eliminación de medidas de forma que si ésta se desvía una cierta cantidad respecto

a una media se elimina. El problema de esta simplificación es que esta media, si se ha calculado

para un movimiento que se aleje del comportamiento que se está simulando en tiempo real, se

podrían estar rechazando medidas válidas. Por este motivo se propone calcular esta media a partir

de los datos previos al instante de simulación para la simulación que se está realizando en ese

momento, es decir, la media se debe basar en el histórico de datos que se tenga del movimiento

que se le esté dando al Smartphone en tiempo real partiendo del instante inicial, dando más peso

cuanto más reciente sea la medida para así tener en cuenta las posibles transiciones de las mismas.

Otro aspecto a mejorar desde el punto de vista del desarrollo de la práctica con los alumnos sería adaptar

los archivos Matlab generados para cualquier versión de este programa y para cualquier sistema

operativo ya que, durante la realización de la misma, surgió la necesidad de tener una versión igual o

más reciente a 2015 y un sistema operativo Windows 10.

Desde el punto de vista de la redacción del boletín de la práctica se propone implementar las mejoras

comentadas anteriormente además de una presentación previa a la práctica que explique los pasos que

se van a ir dando durante la misma y presentando el objetivo final. De esta forma se abordaría con

mayor expectación por parte de los alumnos y de forma más estructurada. Esta mejora se propone como

paso previo a la realización de la práctica de forma verbal por parte del profesor de la asignatura a modo

de resumen, ya sea en clase o en la misma práctica.

Conclusiones y tabajo futuro

Apéndice A

79

79

APÉNDICE A. BOLETÍN DE PRÁCTICAS

DINÁMICA DE VEHÍCULOS ESPACIALES

Práctica: Desarrollo de una práctica docente de estimación de la

orientación basada en fusión de sensores de Smartphones

En esta práctica se emplearán los conceptos de representación y determinación estática de actitud

aprendidos en la asignatura de Dinámica de Vehículos Espaciales así como la implementación del Filtro

de Kalman para proporcionar experiencia real en la estimación de la orientación de un elemento móvil a

partir de medidas de sensores. Para ello se utilizará la plataforma Matlab y la aplicación (App) “Sensor

Fusion”. Esta aplicación móvil permite recopilar información acerca de los sensores presentes en el

Smartphone y transmitirlos en tiempo real a un PC, donde se accederá a dichos datos mediante Matlab,

permitiendo así visualizar y demostrar la orientación estimada del Smartphone.

Como paso previo a la realización de la práctica y para facilitar la comprensión de la misma, se

propone hacer la primera parte de ésta en casa (apartados del 1 al 5). Para ello, será necesario disponer de

una versión de Matlab de 2015 en adelante y sistema operativo Windows 10. En enseñanza virtual se

encuentran los archivos necesarios para la realización de la práctica.

1. Descarga y uso de la aplicación para Android Sensor Fusion

Inicialmente, conecte su Smartphone a una red de internet particular y descargue la

aplicación disponible de manera gratuita en Google Play (http://goo.gl/0qNyU). Escribir

Sensor Fusion en el buscador y clicar en el logo de la figura

Al abrir la aplicación se muestran las siguientes funciones básicas (Figura 1 (a)):

Select Sensor: haciendo clic en este apartado se muestran los sensores disponibles en el Smartphone (ver

Figura 1 (b)). Al seleccionar un sensor, las medidas que el sensor esté obteniendo se mostrarán en

tiempo real en la pantalla. En la Figura 1 (c) se muestra un ejemplo para el giróscopo. Las medidas

tanto del giróscopo como del acelerómetro y magnetómetro se visualizan en los tres ejes del

Smartphone, definidos éstos según la Figura 2. Además se muestra el valor medio de las medidas

junto con las unidades en las que se representa.

(a) Menú de inicio. (b) Selección de sensor.

Figura 1. Capturas de la aplicación Sensor Fusion.

(c) Medidas del giróscopo.

Nombre del alumno:

Modelo de Smartphone:

http://goo.gl/0qNyU

Boletín de prácticas

Log Data: haciendo clic en este apartado se pueden elegir los sensores de los que se quiere almacenar la

información a través de internet para ser guardada o procesada en tiempo real. Para elegir los

sensores sólo es necesario clicar en el recuadro anexo al nombre del mismo (ver Figura 3 (a)). Las

opciones “Stream” y “Log” están asociadas a si se quiere tratar las medidas a través de internet o

si sólo se van a guardar en un fichero respectivamente. La opción que se usará en esta práctica será

a través de internet.

Haciendo clic en el botón rojo se puede editar la configuración con el PC (se verá en el

apartado 5.1) y la ubicación del archivo de datos si se elige la función “Log” (ver Figura 3 (b)).

Sensor List: haciendo clic en este apartado se abre una ventana donde se enumeran todas las medidas de

sensores que están disponibles en la aplicación (ver Figura 3 (c)). Algunos de los sensores

mostrados son sensores físicos (como acelerómetro o magnetómetro) y otros son sensores virtuales

que fusionan las medidas de otros sensores (como el sensor de orientación). La toma de medidas

comienza al seleccionar el botón que se encuentra en la parte inferior de la Figura 3 (a). Inicialmente

este botón marcará Start y, tras pulsarlo y establecer conexión con el PC, se podrán en verde y

cambiará a Stop, indicando que, para parar la toma de medidas, es necesario volver a pulsarlo.

About/Help: haciendo clic en este apartado se muestra información y política de privacidad de la App.

Haciendo clic en el botón rojo en cualquier ventana en la que aparezca, se abrirá de nuevo la

opción de ajuste mostrada en la Figura 3 (b) donde, además de la configuración de la carga de

datos, se podrá cambiar la frecuencia de las medidas.

Figura 2. Sistema de referencia del Smartphone.

(a) Carga de datos. (b) Ajustes de carga de datos.


(c) Información de sensores.

81

81


2. Conceptos relevantes de cinemática de actitud en cuaterniones

A la hora de estimar la actitud, se va a trabajar con cuaterniones. Los dos sistemas de referencia a

los que se va a recurrir para su representación son los siguientes:

Sistemas de ejes navegación (NED): sistema de referencia inercial con centro O en un punto

cualquiera sobre la superficie de la Tierra (elipsoide de referencia) o a una altitud h sobre ella. El

eje OZ tiene la dirección de la perpendicular a la superficie del elipsoide con sentido hacia abajo,

y los ejes OX (apunta al norte-N) y OY (apunta al este-E) forman un plano perpendicular al eje OZ

por el punto O.

Sistema ejes cuerpo del Smartphone: centrado en el Smartphone y se mueve con él. Idealmente,

este sistema estará centrado en el acelerómetro (Figura 2) para permitir que éste mida aceleraciones

puras. Sin embargo, debido a que la localización exacta es desconocida en cada dispositivo móvil,

se asumirá que el origen del sistema de referencia se encuentra posicionado en el centro del mismo.

Esta traslación del centro del sistema de referencia no supondrá ningún inconveniente en el

desarrollo del modelo matemático ya que únicamente se va a estimar la orientación del Smartphone.

En concreto, trabajando en los sistemas de referencia comentados anteriormente (donde, en lo que

sigue, se designarán como I (sistema en Ejes Navegación), y B (sistema en ejes cuerpo del Smartphone),

se pueden relacionar los mismos mediante una traslación más una rotación mediante la expresión:

𝑝𝐼 = 𝐶𝐵𝐼 𝑝𝐵 + 𝑡𝐵

𝐼

donde 𝑝𝐼 representa un punto expresado en el sistema de referencia inercial y 𝑝𝐵 en el sistema de ejes

cuerpo.

La rotación y traslación relativa entre ambos sistemas de referencia vienen representadas por 𝐶𝐵𝐼 y

𝑡𝐵𝐼 respectivamente. Como se ha comentado, la traslación entre ambos centros de coordenadas no se va a

considerar por lo que el problema planteado se puede resumir en la estimación de la matriz de rotación

entre ambos sistemas de referencia a partir de las medidas que se obtengan de los sensores del Smartphone

Como se ha visto en clase, partiendo de una actitud definida mediante el eje y el ángulo de Euler,

es decir, 𝑒y 𝜃, el cuaternión que codifica dicha actitud se caracteriza por cumplir las siguientes

relaciones:

𝑞 = [𝑐𝑜𝑠(𝜃/2)

𝑒𝑠𝑒𝑛(𝜃/2)]

|𝑞| = 1

Hay que tener en cuenta que esta formulación presenta una ambigüedad: la actitud representada por

𝑒y 𝜃 es equivalente a la representada por −𝑒y (360 − 𝜃)/2. Esta ambigüedad puede dar problemas para

el caso de control de actitud pero no para el aquí planteado, el de estimación de actitud.

A continuación se presentan tres conceptos importantes a la hora de estimar la actitud mediante

cuaterniones:

EDC: las EDC expresadas en cuaterniones son las siguientes:

=1

2[

−𝑞1 −𝑞2 −𝑞3

𝑞0 −𝑞3 𝑞2𝑞3

−𝑞2

𝑞0

𝑞1

−𝑞1

𝑞0

] [

𝜔𝑥

𝜔𝑦

𝜔𝑧

] =1

2𝑆(𝑞) [

𝜔𝑥

𝜔𝑦

𝜔𝑧

]

Definiendo :

𝑞𝜔 = [

0𝜔𝑥𝜔𝑦

𝜔𝑧

]

se puede expresar las EDC para cuaterniones de forma compacta como:


=1

2𝑆(𝑞) ∗ 𝑞𝜔

Dado que los datos recibidos por el móvil se obtendrán de manera discreta (con un paso de

tiempo ∆𝑡𝑘), conviene discretizar la expresión anterior para un instante de tiempo 𝑘 como:

𝑞𝑘+ = 𝑞𝑘

− +1

2𝑆(𝑞𝑘)𝑞𝜔𝑘

∆𝑡𝑘(1)

Paso de q a DCM: la matriz de rotación (DCM) conocido el cuaternión que determina dicha

rotación viene determinada por la fórmula de Euler-Rodrigues para cuaterniones:

𝐶 = (𝑞0 − 𝑇)𝐼𝑑 + 2𝑇 − 2𝑞0×(2)

donde el operador × representa el producto vectorial mediante la siguiente matriz:

× = [

0 −𝑞3 𝑞2

𝑞3 0 −𝑞1

−𝑞2 𝑞1 0]

Multiplicación de cuaterniones: la multiplicación de dos cuaterniones tal que 𝑞′′ = 𝑞′ ∗ 𝑞, puede

expresarse en forma vectorial del siguiente modo:

[𝑞0

′′

′′] = [

𝑞0′ 𝑞0 − ′𝑇

𝑞0′ + 𝑞0

′ + ′ × ](3)

3. Conceptos relevantes del Filtro de Kalman discreto

Dado que el problema de estimación es no lineal y multidimensional, el Filtro de Kalman a aplicar

al problema en cuestión será el MEKF (Filtro Extendido Multiplicativo de Kalman). Para ello se presenta

a continuación un resumen del algoritmo sin incluir el bias del giróscopo el cual se considerará a priori

despreciable. La designación de las variables que se presentan a continuación son las siguientes:

Notación Definición

𝒌 instantedemedida

∆𝒕 Tiempo entre medidas consecutivas

𝒊 medida(𝑖 = 1,… ,𝑚)

Estimación del cuaternión

𝑸 matrizdecovarianzadelruidodelgiróscopo

𝝎 ruidoinducidoporelgiróscopo

𝑷 Matriz de covarianza del estado

𝑨,𝑩𝒚𝑯 Matrices que definen la dinámica del sistema

𝑹 matrizdecovarianzadelosruidosdelasmedidasexternas

𝑲 Ganancia de Kalman

Ruido de la medida externa

𝜹𝒚 Error en las medidas externas

𝜹𝒒() Cuaternión de error

83

83


Inicialización: (0) = 0 y 𝑃(0) = ∅(3 × 3)

Ecuaciones de actualización en tiempo (Predicción):

Estimación del estado hacia delante:𝑘− =

𝑘−1+

1

2𝑆(𝑘)𝜔𝑘

∆𝑡𝑘,𝑘− = 𝑘

−(|𝑘−|)−1



𝑇 + 𝐵𝑘𝑄𝐵𝑘𝑇 ,𝑃 =

1

2(𝑃 + 𝑃𝑇)𝑐𝑜𝑛𝐴𝑘 = 𝐼𝑑 − ∆𝑡𝑘 𝑘

×, 𝐵𝑘 = 𝐼𝑑∆𝑡𝑘,𝑄 = 𝐶𝑜𝑣(𝜗𝜔)

Ecuaciones de actualización de mediciones (Corrección): Para cada medida i se tiene 𝑖,𝑘𝐵 (la

medida) y 𝑖,𝑘𝐼 (el valor real). Se calcula para cada medida 𝑖,𝑘

𝐵′ = 𝐶𝐼𝐵′(𝑘)𝑖,𝑘

𝐼 , donde B’ es el sistema

de referencia al cual se refiere 𝑘 y que no coincidirá con B debido a los errores. Con estas medidas

se calcula la siguiente matriz:

𝐻𝑘(𝑘) = [(1,𝑘𝐵′

)×, (2,𝑘

𝐵′)×, … , (𝑚,𝑘

𝐵′)×]

𝑇

Matriz R: 𝑅𝑘 = [

𝑅1 ∅ ⋯ ∅∅ 𝑅2 ⋯ ∅⋮∅

⋮∅

⋱⋯

⋮𝑅𝑚

]

Ganancia de Kalman:𝐾𝑘 = 𝑃𝑘−𝐻𝑘


𝑇 + 𝑅𝑘)−1

Matriz de errores de medida𝛿𝑦𝑘 = [1,𝑘𝐵 − 1,𝑘

𝐵′, 2,𝑘

𝐵 − 2,𝑘𝐵′

, … , 𝑚,𝑘𝐵 − 𝑚,𝑘

𝐵′]

𝑇

Actualización de la estimación con la medición:𝑘+ = 𝐾𝑘𝛿𝑦𝑘

Actualización de la covarianza del error:𝑃𝑘+ = (𝐼𝑑 − 𝐾𝑘𝐻𝑘)𝑃𝑘

−

Fase de Reset:𝑘+ = 𝛿𝑞(𝑘

+) ∗ 𝑘− =

1

√4+|𝑘+|

2[2𝑘

+] ∗ 𝑘−

4. Programas de Matlab proporcionados

Se proporciona código Matlab base para la programación de las tareas que se presentan a

continuación. Estos ficheros representan lo siguiente:

Archivo Startup.m: inicializa el entorno de trabajo en Matlab para asociar los archivos

proporcionados por los sensores del Smartphone y que puedan integrarse las medidas en tiempo real.

Para ello, usa un paquete java llamado sensordata.jar que también se incluye en los archivos

descargables. Se puede encontrar más información acerca de esta interfaz en http://goo.gl/I7KpfL.

http://goo.gl/I7KpfL


Archivo ShowIP.m: determina la dirección IP del ordenador al que se va a conectar.

Archivo FilterTemplate.m y FilterTemplate_MEKF.m: estos archivos son el esqueleto que se va

a usar para realizar la estimación de la orientación en tiempo real a partir de los datos de los sensores

del Smartphone. Los datos de entrada de la función son los datos del acelerómetro (calAcc), giróscopo

(calGyr) y magnetómetro (calMag). Estos archivos proporcionan como datos de salida una estructura

(xhat) compuesta por los tiempos de medida (t), la estimación de la orientación (x) para cada instante

de medida y la covarianza de la estimación de la orientación igualmente para cada instante de medida

(P). Además, proporciona una segunda estructura (meas) que guarda los datos del acelerómetro (acc),

giróscopo (gyr), magnetómetro (mag), y orientación en cuaterniones (orientation) para cada instante

de medida (t).

Archivo Sq.m: en este archivo se forma la matriz S (q) definida en (1) a partir del cuaternión para

facilitar la integración de la EDC. Tiene en cuenta que 𝑞0 es la primera componente del vector

cuaternión.

Archivo quat2DCM.m: en este archivo se aplica la fórmula de Euler-Rodrigues para cuaterniones

(2) para obtener la matriz de rotación conocido el cuaternión. De nuevo tiene en cuenta que 𝑞0 es la

primera componente del vector cuaternión.

Archivo qprod.m: este archivo permite obtener el cuaternión resultante de la multiplicación de otros

dos mediante la forma vectorial representada en (3).

El resto de archivos son la solución a los ejercicios propuestos y comparaciones entre resultados, que

se comentarán a lo largo del desarrollo de la práctica.

5. Ejercicios pre-práctica

5.1. Prueba de conexión entre Smartphone con PC

Para llevar a cabo la conexión entre Smartphone y PC se deben seguir los siguientes pasos:

Ejecutar el archivo Startup.m. Este archivo deberá ser ejecutado cada vez que se inicie Matlab.

Ejecutar el archivo ShowIP.m. Este archivo proporciona la dirección IP que hay que introducir en

la opción IP Adress mostrada en la Figura 3 (b). En las opciones Frecuency y Port dejar las opciones

por defecto (100 Hz y 3400 respectivamente).

Nota: el Smartphone debe estar conectado a la misma red Wifi que el PC para que sea posible la

transferencia de datos.

Nota: los pasos anteriores son necesarios antes de inicializar la toma de medidas mediante el Smartphone.

Para verificar si se ha realizado correctamente la conexión:

Ejecutar el archivo FilterTemplate_prueba.m (archivo FilterTemplate.m incluyendo

inicialización del estado) y al mismo tiempo seleccionar los sensores giróscopo, acelerómetro y

magnetómetro en la opción Log Data y pulsar el botón Start que aparece en la parte inferior de la

pantalla.

Si la conexión entre Smartphone y PC se ha realizado correctamente debe aparecer el mensaje

mostrado en la Figura 4 (a).

85

85


Cuando se quiera finalizar la toma de medidas, se pulsa la opción Stop generando el mensaje

mostrado en la Figura 4 (b) y en Matlab se generarán las dos estructuras de salida mencionadas en la

descripción del archivo FilterTemplate.m.

5.2. Calibración de sensores

5.2.1. Frecuencia de las medidas tomadas por los sensores

A continuación se pide identificar si la frecuencia de la toma de medidas es realmente la indicada

en la aplicación por defecto (100Hz) para el acelerómetro, magnetómetro y giróscopo del Smartphone e

identificar el instante en el que cada uno de ellos comienza su adquisición de datos.

Para ello ejecutar en Matlab el comando:

>> [xhat, meas] = Calibracion

y activar la transmisión de medidas de giróscopo, acelerómetro y magnetómetro en la App.

Posteriormente abrir el Workspace y las medidas obtenidas en la estructura meas. Analizar la

frecuencia con la que cada sensor obtiene medidas y el instante en el que comienza. Realizar 3 simulaciones

y analizar si la frecuencia y el instante de comienzo se mantienen constantes para cualquier simulación.

NOTA: la función isnan (x) se usa para encontrar los valores NaN ya que sólo se han de escoger los

valores del tiempo que tengan asignados dichas medidas.

Con el comando anterior también se representan las medidas obtenidas para cada sensor y para cada

eje frente al tiempo. Si se realiza una simulación para un periodo de tiempo pequeño (por ejemplo 30

segundos), se puede apreciar gráficamente en la distribución temporal de las medidas los saltos entre éstas

(debido a la presencia de valores no válidos (NaN)) y el instante en el que se inicia la toma de datos de cada

sensor.

5.2.2. Distribución del ruido en las medidas

A continuación se pide analizar la semejanza del ruido inducido por las medidas tomadas con una

distribución normal o gaussiana ya que el Filtro de Kalman asume que el ruido de inducido por los sensores

sigue dicha distribución.

Se dice que una variable unidimensional sigue una distribución normal cuando su función de

densidad verifica:

𝑓(𝑥) =1

𝜎√2𝜋exp(−

(𝑥 − 𝑚)2

2𝜎2 )(4)

(a) Mensaje de inicio de conexión. (b) Mensaje de fin de conexión.



En este caso, la variable se denota como una normal de media 𝑚 y desviación 𝜎:

𝑋~𝑁(𝑚, 𝜎2)

Se define un proceso o variable estocástica como aquella variable aleatoria (𝑋(𝑡)) que cambia con

el tiempo. Las medidas de los sensores serán este tipo de variable en cada dirección, es decir, si se tratan

las tres medidas del acelerómetro (el mismo ejemplo para las tres medidas del giróscopo y magnetómetro)

por separado, cada medida será una variable aleatoria continua estocástica y unidimensional. Si la variable

estocástica verifica una distribución normal, se dice que ésta responde al comportamiento de un proceso

gaussiano.

Si se ejecuta en Matlab el siguiente comando:

>> [xhat, meas] = Distribucion_medidas

y se activa la transmisión de medidas de giróscopo, acelerómetro y magnetómetro en la App, se obtiene

la representación de la distribución normalizada del ruido de cada sensor en cada eje.

NOTA: Para ello se ha aplicado la función

histogram(meas.data(i,:),'Normalization','pdf')

la cual proporciona la distribución normalizada de las medidas del sensor que se especifique en data

en la dirección que se especifique en i.

Superpuesta a la anterior se representa la función de distribución para cada eje y para cada sensor,

teniendo en cuenta la media y desviación típica y la función de densidad de una distribución normal

según la expresión (4).

NOTA: Los extremos de la representación se deben ajustar para cada caso ya que las medidas irán

variando para cada simulación y Smartphone.

En base a la representación obtenida, analizar si los datos de ruido siguen una distribución normal

y si se puede considerar como un ruido gaussiano. Por lo general, el ruido de los sensores se puede modelar

como tal. Si alguno de los sensores no cumple los requisitos, ¿a qué crees que puede ser debido?.

6. Ejercicios práctica

6.1. Prueba de conexión entre Smartphone con PC

Seguir los mismos pasos que en el apartado 5.1 para llevar a cabo la conexión entre Smartphone y

PC ahora para la red local wifi.

6.2. Cálculo de covarianza y posición de referencia

Tanto para identificar el ruido en las medidas como para programar un Filtro de Kalman, es necesario

conocer la covarianza del error en éstas y determinar una posición de referencia. Para ello se pide:

Colocar el Smartphone en una posición que se considerará la posición inicial de referencia, por

ejemplo, sobre la mesa y hacia el PC.

Ejecutar en Matlab el comando:

>> [xhat, meas] = Distribucion_medidas

y activar la transmisión de medidas de giróscopo, acelerómetro y magnetómetro en la App.

87

87


Tras 1 minuto aproximadamente con el móvil en la posición de referencia, parar la simulación. En

pantalla se muestran los siguientes datos:

El vector de aceleración media (vector_aceleracion_media), el vector de velocidad angular

media (vector_giroscopo_media) y el vector de campo magnético medio

(vector_magnetico_media).

NOTA: estos valores medios se obtienen mediante la siguiente función:

mean(meas.data(:, ~any(isnan(meas.data), 1)), 2)

la cual proporciona un vector columna que contiene la media de las medidas del sensor (data) en

cada eje.

La aceleración media (media_aceleracion) y el campo magnético medio

(media_magnetico).

Matriz de covarianza del ruido del giróscopo (Q), acelerómetro (R1) y magnetómetro (R2).

NOTA: Para el cálculo de las covarianzas se ha aplicado la siguiente función:

cov(meas.data(:, ~any(isnan(meas.data), 1))')

Esta función devuelve la matriz de covarianza cuyas diagonales son la desviación típica al

cuadrado de las medidas del sensor (data) en cada eje.

6.3. Estimación en tiempo real de la orientación mediante giróscopos

A continuación se pide implementar el algoritmo en Matlab que permita obtener una estimación de

la actitud mediante giróscopos implementando las EDC. Para ello:

Partir del FilterTemplate.m proporcionado.

Incluir el cuaternión inicial (se tomará el cuaternión unidad por convenio) tras la línea de código % Current filter state.

Determinar el incremento de tiempo entre medidas consecutivas. Para ello:

1. Inicializar el instante de tiempo previo (tprev=0) antes de la línea de código %% Filter loop.

2. Calcular el instante de tiempo nuevo (tnuev=t-t0) y el incremento de tiempo

(dt=tnuev-tprev)justo antes de identificar la variable gyr (gyr = data(1, 5:7)').

3. Actualizar el instante de tiempo previo(tprev=t-t0)tras la línea de código %Save estimates.

Incluir el algoritmo de cálculo discreto correspondiente según (1) tras la toma de medidas del

giróscopo (línea de código if ~any(isnan(gyr))).

NOTA: el incremento de tiempo será el dt calculado.

NOTA: el cuaternión se debe normalizar en cada actualización.

1. Anotar los vectores y valores medios y matrices de covarianza generados. Comenta los resultados

obtenidos. ¿Tiene sentido el orden de magnitud de las matrices de covarianza de cada sensor?


NOTA: hacer uso de los archivos Sq.m y qprod.m proporcionados.

Simular el algoritmo partiendo de la posición de referencia establecida con anterioridad para el

Smartphone. En la representación se muestra el movimiento del Smartphone en tiempo real y el

cuaternión que describe su actitud en cada instante. Tras realizar sobre el Smartphone un

movimiento aleatorio, colocarlo sobre su posición de referencia y apreciar la deriva generada

respecto a la representación de partida.

El archivo Evolucion_giroscopos_EDC_euler.m proporciona una comparación entre la

estimación de actitud mediante las EDC y mediante el cuaternión de rotación de Euler el cual asume

que la velocidad angular es constante entre instantes consecutivos de toma de medidas.

El archivo Evolucion_TRIAD.m proporciona una comparación entre la estimación de actitud

mediante el método TRIAD asumiendo como medida más fiable el acelerómetro (acc + mag) y

por otro lado el magnetómetro (mag + acc).

6.4. Estimación en tiempo real de la orientación mediante MEKF

A continuación se pide implementar el algoritmo en Matlab del MEKF que permita obtener una

estimación de la actitud mediante giróscopos fusionado con las medidas obtenidas por el acelerómetro y

magnetómetro. Para ello:

Partir del FilterTemplate_MEKF.m proporcionado.

Incluir el cuaternión inicial (cuaternión unidad) e inicializar la matriz de covarianza del estado tras

la línea de código % Current filter state.

Determinar el incremento de tiempo entre medidas consecutivas siguiendo los mismos pasos que

en el apartado anterior.

2. Si se ejecuta el archivo Evolucion_giroscopos_EDC_euler.m aplicando al Smartphone un movimiento

suave y con velocidad angular aproximadamente constante, tras colocar el Smartphone en la posición

inicial se aprecia una desviación respecto a ésta (cuaternión unidad). ¿En qué caso se obtiene una mejor

estimación de la actitud?. Si se le aplica un movimiento aleatorio, ¿cuándo se obtiene una mejor

estimación?.

3. ¿Qué método usarías para estimar el estado en el MEKF?

4. Si se ejecuta el archivo Evolucion_TRIAD.m aplicando al Smartphone un movimiento arbitrario, tras

colocar el Smartphone en la posición inicial se aprecia una desviación respecto a la misma (cuaternión

unidad). ¿En qué caso se obtiene una mejor estimación de la actitud?.

89

89


Incluir las matrices de covarianza pedidas en la pregunta 1 en %DESIGNACION DE VARIABLES Y

COVARIANZAS.

Incluir el algoritmo de cálculo discreto correspondiente al MEKF explicado en el apartado 3 en las

líneas de código correspondientes.

NOTA: en %medicion acelerómetro y %medicion magnetómetro incluir los vectores

medios pedidos en la pregunta 1.

NOTA: el cuaternión se debe normalizar en cada actualización y la matriz de covarianza del estado

debe ser siempre simétrica.

NOTA: hacer uso de los archivos Sq.m y qprod.m proporcionados.

Simular partiendo de la posición de referencia establecida con anterioridad para el Smartphone y

activando los sensores acelerómetro y campo magnético en la App. En la representación se muestra

el movimiento del Smartphone en tiempo real y el cuaternión que describe su actitud en cada

instante.

El archivo MEKF_comparacion.m proporciona una comparación entre la inclusión o no inclusión

del magnetómetro en el algoritmo. Verificar los resultados obtenidos (adaptar las matrices de

covarianzas y valores de referencia a cada Smartphone).

Ejecutar el archivo MEKF_girosc.m que proporciona una comparación entre el uso del MEKF y

la estimación de actitud únicamente mediante giróscopos (aplicando las EDC) (de nuevo adaptar

las matrices de covarianzas y valores de referencia a cada Smartphone).

6.5. Consideración de Outlier Rejection

Durante la simulación, es posible que se obtengan medidas de los sensores no válidas, es decir,

picos de medida que estarían induciendo un error en la estimación de la actitud. Para eliminar estos picos

se puede tomar como referencia una desviación máxima de 3 veces la raíz cuadrada de la traza de la matriz

de covarianza respectivas a acelerómetro y magnetómetro y comparar la norma de la medida en el instante

k con la norma del vector referencia inercial medido inicialmente. Para ello:

Ejecutar el archivo MEKF_outlier_rejection.m.

5. Tras colocar el Smartphone en la posición inicial, si se le aplica una rotación en el mismo plano en el que se

encuentra y se vuelve a colocar en la posición inicial, ¿qué fenómeno se aprecia?. Simular con y sin

magnetómetro. ¿El magnetómetro mejora o empeora la estimación?

7. Tras la simulación del archivo, ¿se aprecia alguna mejoría?.

6. Simulando este último archivo igualmente partiendo de la posición inicial y acabando en ella, ¿ qué método

estima mejor la actitud?.

Apéndice B

91

91

APÉNDICE B. CUESTIONARIO PARA EL ALUMNO

CUESTIONARIO DE VALORACIÓN PARA EL

ALUMNO

Marque con una X la puntuación que considere más acorde para los siguientes aspectos

(1 muy deficiente, 5 excelente)

1 2 3 4 5

Duración de la práctica

Claridad y secuenciación de ejercicios del boletín

Facilidad en la programación en Matlab

Contenido teórico autosuficiente

Interés didáctico encontrado

Indique si ha detectado algún aspecto a mejorar en la realización de la práctica

Comentarios (tanto positivos como negativos) y sugerencias

¿Cómo calificaría la aplicabilidad de la práctica a la hora de exponer los conocimientos

adquiridos en clase?

Cuestionario para el alumno

Referencias

93

93

REFERENCIAS

[1] Linköping University. Department of electrical engineering.

http://www.control.isy.liu.se/en/student/tsrt14/index.html

[2] Gustaf Hendeby, Fredrik Gustafsson, Niklas Wahlström. Teaching Sensor Fusion and Kalman

Filterin using a Smartphone. Linköping University post print. 2014. http://liu.diva-

portal.org/smash/get/diva2:741780/FULLTEXT01.pdf

[3] Statistical Sensor Fusion – Lab 2. Orientation Estimation using a Smartphone. Linköping University. May

2014. https://www.control.isy.liu.se/student/tsrt14/file/orientation.pdf

[4] Gartner, «Gartner,» 1Q 2016. [En línea]. Available:

http://www.gartner.com/technology/topics/dataanalytics.jsp.

[5] A. Ollero, «Estimación y Sensores de Navegación,» de Vehículos Aéreos No Tripulados, Sevilla, 2012.

[6] Rafael Vázquez Valenzuela. Tema 3: Control y determinación de la actitud. Apuntes Vehículos Espaciales

y Misiles 2014-2015. Departamento de Ingeniería Aeroespacial. Universidad de Sevilla.

[7] Rafael Vázquez Valenzuela. Tema 2: Cinemática y Dinámica de la Actitud. Apuntes Vehículos Espaciales

y Misiles 2014-2015. Departamento de Ingeniería Aeroespacial. Universidad de Sevilla.

[8] G. Welch, G. Bishop, An Introduction to the Kalman Filter, SIGGRAPH, 2001.

[9] Rafael Vázquez Valenzuela. Tema 5: Notas Filtro de Kalman para sistemas continuos-discretos. Apuntes

Vehículos Espaciales y Misiles 2014-2015. Departamento de Ingeniería Aeroespacial. Universidad de

Sevilla.

[10] http://goo.gl/I7KpfL.

Trabajo Fin de Máster Máster en Ingeniería...

Documents

Transcript of Trabajo Fin de Máster Máster en Ingeniería...