View
12
Download
0
Category
Preview:
Citation preview
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-3. 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 5-8. Capturas de la aplicación Sensor Fusion. 46
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
Figura 6-8. Histograma de medidas frente a distribución normal asociada para cada eje y sensor en una
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
Figura 7-2. Orientación del Smartphone en la posición de referencia inercial tras la simulación con el archivo
Evolucion_TRIAD.m. 63
Figura 7-3. Orientación del Smartphone en la posición de referencia inercial tras la simulación con el archivo
MEKF_comparacion.m. 65
Figura 7-4. Orientación del Smartphone en la posición de referencia inercial tras la simulación con el archivo
MEKF_giroscopos_EDC.m. 66
Figura 7-5. Orientación del Smartphone en la posición de referencia inercial tras la simulación con el archivo
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.
Giróscopos, acelerómetros y magnetómetros. Aplicación en smartphones
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
Giróscopos, acelerómetros y magnetómetros. Aplicación en smartphones
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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).
Cinemática de la Actitud
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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
Cinemática de la Actitud
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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)
Cinemática de la Actitud
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
Cinemática de la Actitud
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
Cinemática de la Actitud
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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
Notación Matlab Notación teoría
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.
Cinemática de la Actitud
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:
Notación Matlab Notación teoría
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
𝐾𝑘 = 𝑃𝑘−𝐻𝑇(𝐻𝑃𝑘
−𝐻𝑇 + 𝑅)−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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
A continuación se muestra el algoritmo de cálculo, el cual se esquematiza en el diagrama de flujo
mostrado en la Figura 4-2.
33
33
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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:
Estimación del estado hacia delante:
𝑥𝑘− = 𝑓(𝑥𝑘−1, 𝑢𝑘, 𝑤𝑘−1)(4.34)
Estimación de la covarianza hacia delante:
𝑃𝑘− = 𝐴𝑘𝑃𝑘−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:
Cálculo de la ganancia de Kalman:
𝐾𝑘 = 𝑃𝑘−𝐻𝑘
𝑇(𝐻𝑘𝑃𝑘−𝐻𝑘
𝑇 + 𝑉𝑘𝑅𝑘𝑉𝑘𝑇)
−1(4.36)
Actualización de la estimación con la medición 𝒛𝒌:
𝑥𝑘+ = 𝑥𝑘
− + 𝐾𝑘(𝑧𝑘 − ℎ(𝑥𝑘−, 0))(4.37)
Actualización de la covarianza del error:
𝑃𝑘+ = (𝐼𝑑 − 𝐾𝑘𝐻𝑘)𝑃𝑘
−(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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
A continuación se muestra el algoritmo de cálculo, el cual se esquematiza en el diagrama de flujo
mostrado en la Figura 4-3.
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):
Estimación del estado hacia delante:
𝑘− = 𝑘−1 +
1
2𝑆(𝑘)𝜔𝑘
∆𝑡𝑘(4.44)
𝑘− = 𝑘
−(|𝑘−|)−1(4.45)
Estimación de la covarianza hacia delante:
𝑃𝑘− = 𝐴𝑘𝑃𝑘−1𝐴𝑘
𝑇 + 𝐵𝑘𝑄𝐵𝑘𝑇𝑐𝑜𝑛𝐴𝑘 = 𝐼𝑑 − ∆𝑡𝑘 𝑘
×, 𝐵𝑘 = 𝐼𝑑∆𝑡𝑘,𝑄 = 𝐶𝑜𝑣(𝜗𝜔)(4.46)
𝑃 =1
2(𝑃 + 𝑃𝑇)(4.47)
37
37
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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)
Cálculo de la ganancia de Kalman:
𝐾𝑘 = 𝑃𝑘−𝐻𝑘
𝑇(𝐻𝑘𝑃𝑘−𝐻𝑘
𝑇 + 𝑅𝑘)−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)
Actualización de la covarianza del error:
𝑃𝑘+ = (𝐼𝑑 − 𝐾𝑘𝐻𝑘)𝑃𝑘
−(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.
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.
Figura 5-3. Capturas de la aplicación Sensor Fusion.
(c) Información de sensores.
41
41
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
Introducción a la metodología de la práctica
Figura 5-5. Diagrama de flujo del archivo FilterTemplate.m.
1
43
43
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
1
Figura 5-6. Subdiagrama de flujo del archivo FilterTemplate.m.
Introducción a la metodología de la práctica
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
Introducción a la metodología de la práctica
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.
Figura 5-8. Capturas de la aplicación Sensor Fusion.
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
Calibración de sensores
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
Calibración de sensores
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
Calibración de sensores
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:
Sensor Giróscopo Acelerómetro Magnetómetro
Dirección x y z x y z x y z
𝑬[𝑿(𝒕)] -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
Sensor Giróscopo Acelerómetro
Dirección x y z x y z
𝑹𝒖𝒊𝒅𝒐 -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.
Figura 6-8. Histograma de medidas frente a distribución normal asociada para cada eje y sensor en una
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
segunda simulación arbitraria estática.
Tabla 6-4. Ruido inducido por giróscopo y acelerómetro en una
segunda simulación arbitraria estática.
55
55
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
Sensor Giróscopo Acelerómetro Magnetómetro
Dirección x y z x y z x y z
𝑬[𝑿(𝒕)] -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
Sensor Giróscopo Acelerómetro
Dirección x y z x y z
𝑹𝒖𝒊𝒅𝒐 -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.
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.
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.
Tabla 6-6. Ruido inducido por giróscopo y acelerómetro en una
tercera simulación arbitraria estática.
Calibración de sensores
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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)
Estimación de la orientación del Smartphone en tiempo real
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)
Notación Matlab Notación teoría
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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
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 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.
Estimación de la orientación del Smartphone en tiempo real
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.
La correspondencia entre la notación aplicada en Matlab y en el desarrollo teórico en el presente proyecto
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.
En la Figura 7-2 se muestra un ejemplo de la orientación del Smartphone en una posición definida como
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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
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 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:
Figura 7-2. Orientación del Smartphone en la posición de referencia inercial tras la simulación
con el archivo Evolucion_TRIAD.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.
Estimación de la orientación del Smartphone en tiempo real
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.
La correspondencia entre la notación aplicada en Matlab y en el desarrollo teórico en el presente proyecto
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
En la Figura 7-3 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_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.
Notación Matlab Notación teoría
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
Figura 7-3. Orientación del Smartphone en la posición de referencia inercial tras la simulación
con el archivo MEKF_comparacion.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.
Tabla 7-2. Equivalencia de notación Matlab/Teoría para MEKF.m.
Estimación de la orientación del Smartphone en tiempo real
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)
Figura 7-4. Orientación del Smartphone en la posición de referencia inercial tras la simulación
con el archivo MEKF_giroscopos_EDC.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.
67
67
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
A continuación se establecen los pasos a seguir para implementar el algoritmo en Matlab que permita
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.
Estimación de la orientación del Smartphone en tiempo real
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.
Figura 7-5. Orientación del Smartphone en la posición de referencia inercial tras la simulación
con el archivo MEKF_outlier_rejection.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.
69
69
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
Estimación de la orientación del Smartphone en tiempo real
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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:
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.
Figura 3. Capturas de la aplicación Sensor Fusion.
(c) Información de sensores.
81
81
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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:
Boletín de prácticas
=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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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
Estimación de la covarianza hacia delante:
𝑃𝑘− = 𝐴𝑘𝑃𝑘−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.
Boletín de prácticas
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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.
Figura 4. Capturas de la aplicación Sensor Fusion.
Boletín de prácticas
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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?
Boletín de prácticas
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
Desarrollo de una práctica docente de estimación de la orientación basada en fusión de sensores de Smartphones
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?.
Boletín de prácticas
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.
Recommended