Bernadi
Cargado por
caoap3847Bernadi
Cargado por
caoap3847UNIVERSIDAD DE BUENOS AIRES
Facultad de Ingeniería
Tesis de Grado de Ingeniería Electrónica
SISTEMA DE NAVEGACIÓN INS/GPS PARA UN
COHETE SUBORBITAL CONTROLADO
Tesista: Pablo Tomás Bernadí
Director: Dr. Ing. Juan Ignacio Giribet
Marzo de 2013
Índice general
1. Introducción 6
1.1. Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.2. Lanzadores Satelitales . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
1.3. Navegación Integrada . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.4. Desarrollo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
1.5. Sistemas de Referencia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.5.1. Terna Inercial (ECI) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.5.2. Terna Terrestre (ECEF) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
1.5.3. Terna Local Geodésica (LGV) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
1.6. Representaciones de Actitud . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.6.1. Matriz de Cosenos Directores . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
1.6.2. Ángulo Vectorial . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
1.6.3. Ángulos de Euler . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
1.6.4. Cuaterniones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
1.6.5. Discusión . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2. Navegación Inercial Strapdown 18
2.1. Sensores Inerciales MEMS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.1.1. Giróscopos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.1.2. Acelerómetros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.1.3. Modelo de Error de la IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
2.2. Trayectoria Nominal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.3. Ecuaciones de Navegación en ECI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
2.4. Ecuaciones de Navegación en ECEF . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
2.5. Navegador Inercial Simple . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3
4 Índice general
2.6. Navegador Inercial de Precisión . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
2.6.1. Integración de la Ecuación Diferencial de Actitud . . . . . . . . . . . . . . . . . 31
2.6.2. Integración de las Ecuaciones de Posición y Velocidad . . . . . . . . . . . . . . 33
[Link]. Integración del término de gravedad . . . . . . . . . . . . . . . . . . . 34
[Link]. Integración del Término de Fuerza Especí ca . . . . . . . . . . . . . . 35
2.6.3. Algoritmo de Navegación Inercial de Precisión . . . . . . . . . . . . . . . . . . . 38
[Link]. Simpli cación del Algoritmo . . . . . . . . . . . . . . . . . . . . . . . 38
[Link]. Determinación de Tl , Tm y Ts . . . . . . . . . . . . . . . . . . . . . . . 39
2.7. Validación del Algoritmo Inercial . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
2.8. Desventajas de la Navegación Inercial . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
3. Estimación de Errores 46
3.1. Filtro de Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
3.2. Modelo Dinámico del Error . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
3.2.1. Error en Actitud . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
3.2.2. Error en Posición y Velocidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
3.2.3. Errores Instrumentales . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
[Link]. Términos de Error Ampliados . . . . . . . . . . . . . . . . . . . . . . 51
[Link]. Modelo de Desvíos de los Parámetros Instrumentales . . . . . . . . . . 52
3.2.4. Modelo Dinámico Completo para Pequeñas Perturbaciones . . . . . . . . . . . . 52
3.2.5. Modelo de Observaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
3.2.6. Algoritmo del Filtro de Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
3.3. Seguimiento del Error del Navegador Inercial . . . . . . . . . . . . . . . . . . . . . . . 56
3.4. Presupuesto de Errores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
3.4.1. Desarrollo de las Ecuaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
[Link]. Covarianza a priori . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
[Link]. Covarianza a posteriori . . . . . . . . . . . . . . . . . . . . . . . . . . 60
3.4.2. Implementación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
[Link]. Errores en Actitud . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
[Link]. Errores en Velocidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
[Link]. Errores en Posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
3.4.3. Reducción de la In uencia del Ruido . . . . . . . . . . . . . . . . . . . . . . . . 67
Índice general 5
3.4.4. Eliminación de Estados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
4. Navegación Integrada 69
4.1. Corrección de Variables de Navegación . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
4.1.1. Actitud . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
4.1.2. Posición y Velocidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
4.1.3. Parámetros Instrumentales . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
4.2. Algoritmo de Navegación Integrado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
4.2.1. Navegación de la Trayectoria . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
4.2.2. Navegación Integrada con INS Euler . . . . . . . . . . . . . . . . . . . . . . . . 79
4.2.3. Alineación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
4.2.4. Pérdida de Datos GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
4.3. Simulación de Monte Carlo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
4.3.1. Análisis de los Datos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
4.3.2. Modi cación del Paso de Predicción del Filtro de Kalman . . . . . . . . . . . . 92
4.4. Consideración de un Retardo en el Dato GPS . . . . . . . . . . . . . . . . . . . . . . . 94
4.4.1. Propagación de la Innovación . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97
4.4.2. Simulaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
5. Incorporación del Sistema de Navegación Integrado en un Simulador 102
5.1. Estructura del Simulador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
5.1.1. Errores del Navegador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
5.1.2. Errores del Vehículo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
5.2. Consideración del Retardo en el Dato GPS . . . . . . . . . . . . . . . . . . . . . . . . . 109
5.3. Tiempos de Ejecución . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115
5.4. Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117
5.5. Futuro Trabajo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
A. Modelo de Gravedad J2 121
B. Presupuesto de Errores con Valores de Ruido Actualizados 123
B.1. Errores en Actitud . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
B.2. Errores en Velocidad . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125
B.3. Errores en Posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
Capítulo 1
Introducción
1.1. Objetivos
El objeto de esta tesis es diseñar un sistema de navegación para el vuelo de un cohete suborbital
que cumpla con los requerimientos impuestos por los sistemas de guiado y control. Un sistema de
navegación provee información de la posición, velocidad y orientación del vehículo, datos que en este
caso deben ser calculados en tiempo real. La calidad de la información de navegación debe ser tal
que, en conjunto con los sistemas de guiado y control, garantice un buen seguimiento de la trayectoria
prevista.
Debido a la naturaleza experimental del vehículo, no está prevista su recuperación ni de ninguna
de sus partes. Es por esto que otro de los requerimientos impuestos es el del bajo costo de los sistemas.
En este contexto surge el problema de lograr una calidad de navegación aceptable con sensores de
performance moderada.
1.2. Lanzadores Satelitales
El 4 de octubre de 1957 una adaptación del misil intercontinental de dos etapas R-7 puso en órbita
el Sputnik-1, primer satélite arti cial, dando inicio a la Era Espacial. Este vehículo constituye el primer
lanzador satelital, y, con la carrera espacial como motor principal, se han desarrollado muchos desde
entonces. El lanzador Soyuz desciende de esta familia de cohetes rusos, aunque con mayor capacidad
y tamaño, ya que era capaz de llevar cargas de hasta 6.400kg.
Los Estados Unidos respondieron a este evento, entre otras cosas con el desarrollo de cohetes basados
en el misil Thor, que se adaptó para hacer pruebas con cargas satelitales. Se realizaron desarrollos
con distintas etapas de propulsión, llamando a la cuarta Thor-Delta. Eventualmente, esta serie de
vehículos llegó a conocerse simplemente como Delta. El vehículo Delta II fue diseñado especí camente
6
1.3. Navegación Integrada 7
para poner en órbita los satélites GPS del bloque II, utilizándose luego para diversas misiones. En la
segunda etapa de este vehículo se encuentra el sistema de navegación inercial, capaz de calcular su
posición, velocidad y orientación en todo momento.
En la década del 70, Europa tomó la decisión de ingresar en el mercado de los lanzadores satelitales,
dando nacimiento al programa Ariane por medio de la ESA (European Space Agency). Entre 1988 y
2003 el Ariane-4 capturó el 50 % del mercado de lanzamientos comerciales, llevando cargas de hasta
7.500kg a órbitas bajas y de hasta 4.300kg a órbitas GTO (Geostationary Transfer Orbit).
En latinoamérica, Brasil es el país más avanzado en materia de lanzadores. Se han realizado varias
pruebas desde 1984, siendo el VLS-1 el actual foco de atención. Es un cohete de tres etapas, que
será capaz de llevar cargas de hasta 340kg a órbitas bajas. En la Argentina, el Plan Espacial Nacional
llevado a cabo por la Comisión Nacional de Actividades Espaciales (CoNAE) contempla diversos cursos
de acción para su concreción. Uno de estos es el Acceso al Espacio, que comprende las acciones que se
realicen con el objetivo de permitir la colocación en órbita de los satélites incluídos en el Plan Espacial
y la inserción de la Argentina en el mercado de provisión de los servicios de lanzamiento.
Existen actividades que tienen el objetivo de incrementar el conocimiento en el área de los lanzadores
satelitales. En particular se menciona el proyecto Tronador, cuya segunda fase (vehículo Tronador II)
es un lanzador satelital multietapa de cargas livianas. En el marco del desarrollo de tecnología con
miras a la construcción de dicho lanzador, se están contruyendo una serie de vehículos experimentales
controlados de menor complejidad. El sistema de navegación diseñado formará parte de la electrónica
del primero de ellos, un vehículo de una sola etapa.
El VEX-1 es un cohete experimental desarrollado por la CoNAE que utiliza combustible líquido y
mide 13m de altura y 1m de diámetro. Para corregir la trayectoria, se controla la dirección de la tobera,
técnica conocida como control del vector de empuje (TVC, por sus siglas en inglés). Para la navegación,
se cuenta con una unidad de mediciones inerciales (IMU) que provee información de velocidad angular
y fuerza especí ca, y con un receptor GPS que provee datos de posición y velocidad.
1.3. Navegación Integrada
En 1960, R. E. Kalman publicó un artículo en el que proponía un algortimo para la estimación
óptima de sistemas dinámicos lineales en presencia de ruido blanco, lo que dio en llamarse Filtro de
Kalman. El enfoque recursivo y desde la óptica del espacio de estados llamó la atención de ingenieros
alrededor del mundo, y las aplicaciones crecieron exponencialmente, llegando a los campos de teoría
de sistemas lineales, estadística, procesamiento de señales e identi cación de sistemas, entre muchos
8 Capítulo 1. Introducción
otros.
Luego de la publicación de su trabajo, Kalman fue contactado por el jefe de la División de Análisis
Dinámico de la NASA en California, cuyo equipo estaba trabajando en la estimación y control de
trayectorias para enviar astronautas a la Luna y de regreso [11]. Estos trabajos pasaron luego a formar
parte de las misiones Apollo. Se habían vislumbrado las ventajas que ofrecía el ltro de Kalman
para el problema de seguimiento, en particular la posibilidad de separar la predicción del estado y
la toma de mediciones, ya que podían pasar horas antes de que un determinado sensor suministrar
información. Otras contribuciones de este equipo de trabajo consisten en el desarrollo del Filtro de
Kalman Extendido, junto con análisis de Monte Carlo que demostraban que las alinealidades del modelo
no contribuían más allá de los márgenes de error considerados. Muchos de los resultados obtenidos
fueron rápidamente compartidos, entre otros, con universidades como el MIT, que en 1961 ganó el
concurso para diseñar los sistemas de guiado y navegación para las misiones Apollo. A partir de ese
momento, el ltro de Kalman se incorporó integralmente a las mismas.
No pasó mucho tiempo hasta que se notó que el ltro de Kalman era útil en sistemas de navegación
inerciales donde existen errores instrumentales variables en el tiempo que deben ser identi cados,
así como incertidumbres en el estado inicial. Además, la matriz de covarianza del ltro de Kalman
relacionaba la performance de los sensores con la del navegador naturalmente. En este contexto y con
el ltro de Kalman como base nació la fusión de datos de diversos sensores, principalmente inerciales
y de radar. El avión C-5A fue el primero en contar con un sistema de navegación integrado. Desde
entonces, diversas fuentes de datos se han utilizado para corregir los errores cometidos por el sistema
de navegación inercial (INS), tales como barómetros para estabilizar el error vertical, o magnetómetros
o star trackers para corregir la orientación o actitud del vehículo. Las combinaciones son enormes, lo
que demuestra la versatilidad de esta técnica.
Con el surgimiento del Sistema de Posicionamiento Global (GPS) en la década del 90, se tuvo
disponibilidad de datos de la posición y la velocidad de un receptor, y además de su movimiento
relativo a los satélites GPS en vista. Esto dio origen a sistemas de navegación INS/GPS, clasi cados
generalmente como débilmente acoplados y fuertemente acoplados. Los primeros utilizan la información
de posición y velocidad calculados por el receptor, mientras que los segundos utilizan directamente las
mediciones independientes de cada satélite.
La navegación integrada puede aprovecharse para explotar características complementarias de diver-
sos tipos de sensores, incrementando la robustez y precisión de la información provista. Las mediciones
de una Unidad de Mediciones Inerciales son independientes de las condiciones ambientales, poseen una
1.4. Desarrollo 9
alta tasa de muestreo, pero debido a errores instrumentales el error que surge de navegar sus datos
crece inde nidamente. Por otro lado, los sistemas de navegación satelitales tienen la ventaja de poseer
errores acotados, pero con una fuerte dependencia de las condiciones externas al vehículo y una baja
frecuencia de actualización. La combinación de ambas fuentes de información potencialmente provee
datos con un error acotado y a una alta frecuencia, con tolerancia a condiciones ambientales adversas.
Estas características han motivado el diseño de sistemas de navegación de buena calidad a un costo
mucho menor debido a la relajación en los requerimientos de los sensores. Sin embargo, la desventaja
de esta técnica reside en el aumento de la complejidad del algoritmo de navegación y de la carga
computacional.
1.4. Desarrollo
Esta tesis se desarrolló en el marco de un convenio entre la CoNAE y el Grupo de Procesamiento de
Señales, Identi cación y Control de la Facultad de Ingeniería de la Universidad de Buenos Aires. En el
mismo se han realizado otras tesis relacionadas con la navegación integrada INS/GPS con aplicaciones
aeroespaciales y aeronáuticas. Se expone en [10] el diseño de un algoritmo de navegación integrada que
fusiona información de una IMU, un receptor GPS, un sensor solar grueso y un sensor de horizonte.
Por otro lado, en dicho trabajo se desarrolla un sistema de navegación inercial que fue el utilizado en la
navegación del cohete sonda VS30, lanzado el 16 de Diciembre de 2007. En [4] se propone un sistema
de navegación integrado INS/GPS para la navegación del VS30, utilizando datos reales del vehículo
y comparando el desempeño de los sistemas inercial e integrado. En [6] se implementa un sistema de
navegación integrado INS/GPS/Magnetómetro para resolver la navegación de un avión que transporta
un radar de apertura sintética. Se compara el desempeño de distintos sensores inerciales, en particular
sensores de tecnologías MEMS con sensores IFOG.
En este trabajo, además de exponer las bases teóricas del algoritmo de navegación integrado, se
incluyen análisis de la in uencia de errores instrumentales sobre la navegación, y consideraciones acerca
del retardo en los datos del receptor GPS. Un aspecto importante de este trabajo es el de la presentación
de resultados obtenidos de un simulador de vuelo que utiliza el navegador integrado como base para el
sistema de guiado. Asimismo, se midieron los tiempos de ejecución del algoritmo de navegación sobre
el hardware previsto para el vuelo.
10 Capítulo 1. Introducción
1.5. Sistemas de Referencia
Existe una variedad de sistemas de referencia utilizados en navegación. Dependiendo de la aplicación
y el tipo de datos de salida que se necesiten, se puede elegir uno u otro, cada uno con sus respectivas
ventajas y desventajas. Se describen en esta sección los sistemas de referencia más comunes, de los que
surgirán las respetivas ecuaciones a resolver para poder realizar la navegación del vehículo.
Se de nen a continuación algunos parámetros que resultan útiles a la hora de construir los sistemas
de referencia:
Elipsoide de referencia: En el sistema WGS84 [2], se modela la forma de la tierra mediante
un esferoide achatado en los polos, con medidas de 6378137m para el eje ecuatorial y de aproxi-
madamente 6356752, 31m para el eje polar.
Longitud (λ): coordenada geográ ca que especi ca la posición de un punto sobre la super cie
del elipsoide de referencia respecto de la dirección Este-Oeste. Es una medida angular, y por
convención, la posición de longitud igual a cero grados es el meridiano de Greenwich.
Vertical Local Geodésica: vector normal al plano tangente en un punto del elipsoide de refer-
encia. Si se extiende una recta con esta dirección desde la super cie del elipsoide hasta el plano
del ecuador, no pasará por el centro de masa de la tierra, a diferencia de la vertical geocéntrica,
que si lo hace.
Latitud Geodésica (Φ): similarmente a la longitud, de ne la posición de un punto sobre la
super cie del elipsoide de referencia en la dirección Norte-Sur. El ángulo Φ se mide respecto del
plano del ecuador, utilizando la dirección de la vertical geodésica local.
Altura Geodésica (h): Altura medida respecto de un punto del elipsoide de referencia en la
dirección de la vertical geodésica.
1.5.1. Terna Inercial (ECI)
Los sistemas de referencia inerciales no están acelerados, y se mueven en forma rectilínea y uniforme
entre ellos. La terna ECI (Earth-Centered Inertial) se encuentra centrada en la tierra y no está sometida
a rotación alguna. El eje x tiene la dirección del equinoccio vernal y el eje z coincide con el eje de rotación
de la tierra, mientras que el eje y completa la terna dextrógira, de niendo un sistema de coordenadas
ortogonal. La posición de un punto se expresa como un vector de coordenadas [ xi y i z i ], donde el
superíndice i indica la terna ECI. Asimismo, la velocidad se expresa como [ vxi vyi vzi ].
1.5. Sistemas de Referencia 11
Figura 1.1: Sistemas de referencia ECI y ECEF
1.5.2. Terna Terrestre (ECEF)
El sistema de referencia ECEF (Earth-Centered Earth-Fixed) está centrado en la tierra y jo a ella,
donde el eje z coincide con el eje de rotación de la tierra, el eje x coincide siempre con el meridiano
de Greenwich a la altura del ecuador, y el eje y completa la terna dextrógira. Debido a la rotación
que experimenta este sistema, no es inercial, lo que dará origen a fuerzas aparentes actuando sobre
un movimiento dado. Similarmente a la terna ECI, la posición y velocidad de un punto relativo al
sistema de coordenadas ECEF se expresan como los vectores [ xe y e z e ] y [ vxe vye vze ]. La terna
de referencia ECEF tiene un interés particular, ya que el Sistema de Posicionamiento Global provee
sus datos de posición y velocidad en estas coordenadas.
1.5.3. Terna Local Geodésica (LGV)
Dado un punto de la tierra, la terna LGV (Local Geodetic Vertical) consiste en dos ejes xg e yg
formando un plano tangente al elipsoide de referencia, y el tercero paralelo a la vertical geodésica
local. Los ejes horizontales xg e yg apuntan en las direcciones Este y Norte, respectivamente, mientras
que el eje vertical zg apunta en la dirección de la vertical geodésica (Up), por lo que se lo denomina
ENU (East-North-Up). La posición de un punto viene dada por su latitud geodésica, longitud y altura
geodésica: [ Φ λ h ]. Una variante para esta terna es de nir los ejes en las direcciones NED (North-
East-Down).
12 Capítulo 1. Introducción
Figura 1.2: Sistema de referencia LGV
1.6. Representaciones de Actitud
Existen diferentes descripciones para la orientación de un cuerpo respecto de un sistema de refer-
encia. En esta sección se da un breve resumen de estos métodos de representación, y luego se discuten
sus ventajas y desventajas, con miras a su utilización en un algoritmo de navegación.
1.6.1. Matriz de Cosenos Directores
Se denota Cnb a la matriz de rotación de la terna b del cuerpo a la terna n de navegación, que
por ahora se considerará genérica. La matriz de cosenos directores es una matriz de dimensión 3 × 3,
cuyas columnas representan versores del cuerpo proyectados sobre la terna de navegación. El elemento
cij de la matriz es el coseno del ángulo entre el eje i de la terna n y el eje j de la terna b. La
transformación de un vector de una terna a otra se realiza según la transformación lineal
vn = Cnb vb (1.1)
donde vb y vn son vectores en las ternas b y n respectivamente [8].
Una propiedad interesante de las matrices de rotación es que son matrices unitarias y determinante
igual a 1. Por lo tanto, cumplen con la propiedad
CT = C−1 (1.2)
1.6. Representaciones de Actitud 13
Por lo tanto, la obtención de la matriz de transformación inversa es directa:
vb = Cbn vn (1.3)
= (Cnb )−1 vn (1.4)
(Cnb )−1 vn = (Cnb )T vn (1.5)
La evolución en el tiempo de una matriz de rotación se describe mediante la ecuación diferencial
Ċnb = Cnb S(ω bnb ) (1.6)
donde ω bnb es la velocidad angular de b respecto de n, expresada en coordenadas b. S() es una
función de R3 en el conjunto de las matrices antisimétricas tal que
0 −a3 a2 b1
a × b = S(a)b = a
3 0 −a 1 b2
(1.7)
−a2 a1 0 b3
y que permite convertir el producto vectorial en el producto de una matriz y un vector. Cumple además
con la propiedad
S(a)b = a × b = −b × a = −S(b)a (1.8)
La composición de rotaciones es simple, ya que consiste en multiplicar sucesivamente las matrices
de rotación
0
Cnb = Cnn0 Cnb (1.9)
1.6.2. Ángulo Vectorial
Toda matriz de rotación Cnb (salvo la identidad) tiene un autovalor real cuyo autovector asociado
de ne el eje de rotación. Este ángulo vectorial es invariante ante la rotación entre las ternas b y n,
y su módulo de ne el ángulo rotado. Si θ bn es el ángulo vectorial entre las ternas b y n, su relación
con la matriz de rotación es
Cnb = exp [S(θ nb )] (1.10)
El ángulo rotado puede obtenerse mediante la relación
tr(Cnb ) − 1
kθ nb k = arc cos (1.11)
2
lo que permite encontrar la relación del eje de Euler con la matriz de cosenos directores según [8]
c32 − c23
θ̆nbx = (1.12)
2 sin kθ nb k
14 Capítulo 1. Introducción
c13 − c31
θ̆bny = (1.13)
2 sin kθ nb k
c21 − c12
θ̆bnz = (1.14)
2 sin kθ nb k
De estas ecuaciones se desprende que existe una singularidad para sin kθ nb k = 0. Aunque es posible
encontrar el eje a pesar de esto, su cálculo no resulta sujeto a estas ecuaciones, lo que di culta su uso
en un algoritmo.
La ecuación que describe la variación del ángulo vectorial es la llamada ecuación del coneo, derivada
por Bortz en [3]
1 1 kθ bn (t)k sin (kθ bn (t)k)
θ̇ bn (t) = ω bnb (t) + b
θ bn (t) × ω nb (t) + 1 − θ bn (t) × θ bn (t) × ω b
nb (t)
2 kθ bn (t)k2 2 (1 − cos(kθ bn (t)k))
(1.15)
y reproducida en [8], que al ser integrada da el ángulo vectorial rotado en el intervalo de tiempo
considerado.
1.6.3. Ángulos de Euler
Toda transformación de una terna de referencia a otra puede realizarse haciendo tres rotaciones
sucesivas alrededor de diferentes ejes:
1. Rotación de un ángulo ψ alrededor del eje z original
2. Rotación de un ángulo θ alrededor del nuevo eje y
3. Rotación de un ángulo φ alrededor del nuevo eje x
donde ψ, θ y φ son los llamados ángulos de Euler, y θ es distinto al ángulo vectorial θ nb . Generalmente,
se asocia los ejes x, y y z con ejes del vehículo, por lo que los ángulos de Euler describen la orientación
del mismo. Por esta razón, se denomina a los ángulos ψ Yaw (rumbo), θ Pitch (elevación o cabeceo),
y φ Roll (rolido). Estas rotaciones pueden expresarse independientemente mediante sus matrices de
cosenos directores:
cos ψ sin ψ 0
1. Rotar un ángulo ψ alrededor del eje z : C1 = − sin ψ cos ψ 0
0 0 1
cos θ 0 sin θ
2. Rotar un ángulo θ alrededor del eje y : C2 = 0 1 0
− sin θ 0 cos θ
1 0 0
3. Rotar un ángulo φ alrededor del eje x: C3 = 0 cos φ sin φ
0 − sin φ cos φ
1.6. Representaciones de Actitud 15
Componiendo estas rotaciones se obtiene la matriz de cosenos directores expresada en función de los
ángulos de Euler:
1 0 0 cos θ 0 sin θ cos ψ sin ψ 0
C3 C2 C1 =
0 cos φ sin φ
0 1 0 − sin ψ cos ψ 0
(1.16)
0 − sin φ cos φ − sin θ 0 cos θ 0 0 1
cos θ cos ψ cos θ sin ψ − sin θ
=
− cos φ sin ψ + sin φ sin θ cos ψ cos φ cos ψ + sin φ sin θ sin ψ sin φ cos θ (1.17)
sin φ sin ψ + cos φ sin θ cos ψ − sin φ cos ψ + cos φ sin θ sin ψ cos φ cos θ
de donde se deducen las relaciones entre ambas representaciones:
c23
φ = arctan (1.18)
c33
c12
ψ = arctan (1.19)
c11
θ = arcsin (−c13 ) (1.20)
Similarmente al caso del ángulo vectorial, existen singularidades para c13 = 0 y c11 = 0. También
es destacable que esta matriz de rotación es igual a la que representa la rotación en orden inverso
alrededor de los ejes de la terna original [7].
Cuando los ángulos son pequeños, se pueden realizar las aproximaciones cos α w 1 y sin α w
α, y despreciando productos entre ángulos, la matriz de cosenos directores se aproxima a la matriz
antisimétrica
1 −ψ θ
Cw
ψ 1 −φ
(1.21)
−θ φ 1
1.6.4. Cuaterniones
Las rotaciones mediante cuaterniones surgen de la noción del ángulo vectorial descripto antes. Son
vectores de cuatro elementos construidos a partir del ángulo vectorial θ nb = θ̆ nb kθ nb k:
θx kθ nb k
kθ nb k sin 2
kθ nb k θy kθ nb k
θ̆ nb sin 2
kθ nb k sin 2
qnb = = (1.22)
cos kθ2nb k θz
kθ nb k sin
kθ nb k
2
cos kθ2nb k
16 Capítulo 1. Introducción
De la de nición puede comprobarse rápidamente que este cuaternión tiene norma unitaria:
s
1 kθ nb k kθ nb k
kqnb k 2
= 2 θx2
+ + θy2
sin θz2 + cos2
kθ nb k 2 2
s
kθ nb k kθ nb k
= sin2 + cos2
2 2
= 1
El cuaternión también admite una representación con tres elementos complejos y uno real: q = q1 i +
q2 j + q3 k + q4 , llamada representación hipercompleja. Esta notación permite de nir el conjugado de
un cuaternión, análogamente al conjugado de un número complejo:
q∗ = q1 (−i) + q2 (−j) + q3 (−k) + q4 (1.23)
El producto de dos cuaterniones se puede derivar teniendo en cuenta los productos entre ejes complejos
ii = jj = kk = −1, ij = k y ji = −k. Expresada matricialmente:
q −q3 q2 q1 p
4 1
q3 q4 −q1 q2
p2
q◦p= (1.24)
−q2 q1 q4 q3 p3
−q1 −q2 −q3 q4 p4
También es directa la demostración que el producto entre un cuaternión y su conjugado es igual a su
norma al cuadrado: q 2
q ◦ q∗ = q12 + q22 + q32 + q42 = kqk2 = 1 (1.25)
Para realizar la rotación de un vector de R3 mediante cuanterniones es necesario agregar una
componente al mismo. Se convierte entonces al vector en su representación hipercompleja, y se agrega
0 como componente real, construyendo en este caso un cuaternión de norma distinta a cero. La rotación
se realiza entonces mediante la composición
rn n
rb n
= qb ◦ ◦ qb ∗ (1.26)
0 0
donde se ha rotado el vector rb hacia rn , y la componente real no es de interés.
Similarmente a las matrices de cosenos directores, es posible combinar diferentes rotaciones en un
sólo cuaternión
0
qnb = qnn0 ◦ qnb (1.27)
1.6. Representaciones de Actitud 17
Finalmente, la relación entre cuaterniones y matriz de cosenos directores viene dada por:
q42 + q12 − q22 − q32 2 (q1 q2 − q3 q4 ) 2 (q1 q3 + q2 q4 )
n n
Cb (qb ) =
2 (q1 q2 + q3 q4 ) q42 + q22 − q12 − q32 2 (q2 q3 − q1 q4 )
(1.28)
2 (q1 q3 − q2 q4 ) 2 (q2 q3 + q1 q4 ) q42 + q32 − q12 − q22
Notar que si se reemplaza qnb por −qnb se llega a la misma matriz de cosenos directores, por lo que
existen dos cuaterniones que representan la misma rotación.
La variación de un cuaternión en el tiempo viene dada [8, 21] por la ecuación
1 ω bnb 1 ω nnb
q̇nb = qnb ◦ =
n
◦ qb (1.29)
2 0 2 0
1.6.5. Discusión
A pesar de que todos los métodos de representación son equivalentes y permiten realizar ulterior-
mente la misma operación, debe realizarse un análisis desde el punto de vista de su implementación en
un algoritmo. Los criterios a considerar son por lo tanto el tamaño de la variable y la complejidad del
cáclulo.
La matriz de cosenos directores tiene una interpretación simple, así también es su uso y su com-
posición. Sin embargo, cuenta con las desventajas de requerir 6 parámetros redundantes, y lo que es
CT = C−1
peor, se debe veri car que la matriz de rotación sea unitaria . Esta comprobación no es
directa, por lo que un algoritmo que requiera actualizar la información de actitud se verá limitado por
tal motivo.
El ángulo vectorial cuenta con la ventaja de que no contiene parámetros redundantes. Sin embargo,
existe una singularidad cuando sin kθ nb k = 0, lo que tampoco es deseable.
Al igual que el ángulo vectorial, los ángulos de Euler no contienen datos redundantes y tienen una
interpretación geométrica muy clara. Como contrapartida, las rotaciones deben efectuarse una por vez
y en un orden determinado. Es necesario calcular la matriz de cosenos directores para realizar una
rotación, con lo que esto conlleva.
Finalmente, los cuaterniones cuentan con las ventajas de tener pocos parámetros redundantes,
admitir una composición de rotaciones directa, y de no tener singularidades salvo para θ nb = 0.
Además, es muy fácil normalizarlos luego de realizar operaciones entre ellos. Estas características
hacen del cuaternión el elemento adecuado para la representación de rotaciones, a pesar de contener
un parámetro redundante.
Capítulo 2
Navegación Inercial Strapdown
La navegación inercial es una técnica de navegación autocontenida que utiliza mediciones de ve-
locidad angular y aceleración, junto con condiciones iniciales de un vehículo para calcular su actitud,
velocidad y posición respecto de un determinado sistema de referencia en todo momento. Existen dos
técnicas de navegación inercial ampliamente difundidas [21]:
Gimbaled o de Plataforma: los instrumentos inerciales permanecen en la misma posición iner-
cial a lo largo de la trayectoria. Las mediciones en este caso vienen dadas por las acciones tomadas
por la plataforma para mantenerse en la misma posición, dando como resultado ecuaciones más
simples de resolver, instrumentos más precisos, pero con la contrapartida de un aumento en su
complejidad y costo.
Strapdown: la plataforma es rígida y está ja al cuerpo del vehículo, por lo que se suele consid-
erar la terna de los sensores equivalente a la del cuerpo y se la denota con la letra b (body). Los
datos obtenidos mediante los sensores deben ser luego transformados a la terna de navegación de
interés.
La gura 2.1 muestra un esquema básico del funcionamiento de un sistema de navegación inercial strap-
down. Las mediciones de velocidad angular son integradas en el tiempo para obtener la orientación del
vehículo respecto de un determinado sistema de referencia. Por otro lado, la aceleración es proyectada
según la orientación para obtener su valor en el sistema de referencia elegido. Una vez logrado esto, se
integra una vez para obtener la velocidad, que a su vez es integrada para obtener la posición, siempre
que se conozcan los valores iniciales de todas estas variables.
18
2.1. Sensores Inerciales MEMS 19
Figura 2.1: Navegación Inercial Strapdown
2.1. Sensores Inerciales MEMS
Los sistemas microelectromecánicos (MEMS) han sido objeto de creciente interés en los últimos
años. Son sistemas mecánicos construidos sobre el mismo sustrato que la electrónica, ofreciendo muchas
ventajas respecto de otros tipos de tecnologías. Al adaptar la tecnología utilizada para la construcción
de circuitos integrados es posible lograr sistemas mecánicos de precisión, permitiendo reducir signi ca-
tivamente su costo al producirlos en masa. Frente a otras tecnologías, los sensores MEMS cuentan con
virtudes cada vez más valorables, como por ejemplo tamaño reducido, bajo peso y consumo, resistencia
a shocks, baja cantidad de piezas y operación en ambientes hostiles.
Por otro lado, la reducción en el tamaño por lo general degrada la sensibilidad y aumenta el
ruido de medición, y existen grandes dependencias de la temperatura [21]. Sin embargo, el creciente
conocimiento de las características fundamentales y los mecanismos de error de cada sensor permiten
compensarlos en gran medida, logrando una precisión cada vez mayor.
2.1.1. Giróscopos
Los giróscopos MEMS miden el efecto Coriolis sobre una masa testigo en vibración. Dicha masa
testigo es forzada a vibrar en un eje con un alto Q (ancho de banda pequeño respecto de la frecuencia
de resonancia). Al aplicar una rotación perpendicular al eje de vibración, se induce una aceleración
de Coriolis perpendicular a los otros dos ejes. Esta aceleración es la que se mide y guarda relación
con la magnitud de la velocidad de rotación. A pesar de que muchas con guraciones están basadas en
este principio, por lo general se pueden dividir en los siguientes tipos: osciladores simples, osciladores
balanceados y resonadores cilíndricos, como se muestra en la gura 2.2.
20 Capítulo 2. Navegación Inercial Strapdown
Figura 2.2: Clasi cación de Giróscopos
Se considerará aquí el funcionamiento de los giróscopos balanceados, y en particular de un tipo de
ellos similar a un diapasón (Tuning Fork). En la gura 2.3 se muestra un esquema de estos giróscopos.
Consisten en un par de masas testigo suspendidas, que oscilan con un desfasaje de 180°. La aplicación
de una velocidad angular perpendicular a la velocidad lineal de las masas produce una aceleración de
Coriolis que las empuja fuera del plano de oscilación. Este movimiento es sensado por placas capacitivas
arriba y abajo de las masas, dando como salida una señal proporcional a la velocidad angular aplicada.
El tamaño de las masas testigo es de alrededor de 1000 × 1000 × 20 micrómetros, oscilando a una
frecuencia de decenas de KHz , con un desplazamiento máximo de 10µm. La precisión del sistema
depende enormemente de la calibración de la electrónica, en particular del oscilador y de las placas
sensoras.
Figura 2.3: Giróscopo MEMS Tuning Fork
2.1. Sensores Inerciales MEMS 21
2.1.2. Acelerómetros
Los acelerómetros fueron los primeros sensores construidos con tecnología MEMS, con fuerte in-
centivo por parte de la industria automotriz principalmente . Con esta tecnología, la aceleración puede
sensarse esencialmente por medio de dos efectos:
Desplazamiento de una masa testigo sostenida por un exor
Variación en la frecuencia de vibración de un elemento
En el primer caso, los sensores son análogos a una masa suspendida entre resortes, que se desplaza al
ser acelerada. Su sensibilidad es menor que en el segundo caso, aunque cuenta con la ventaja de un
fácil y versátil encapsulamiento. En el segundo, en cambio, la aceleración es sensada por medio de la
variación en la frecuencia de resonancia de vigas en contacto con una masa testigo, y no a través del
desplazamiento de la misma.
Figura 2.4: Acelerómetro de Masa Vibratoria
La estructura de los acelerómetros es similar a la de un tuning fork, con una masa testigo cuadrada
relativamente pesada, sostenida por cuatro exores. Adheridos a estos, se encuentran unas vigas que
se hacen oscilar electrostáticamente, induciendo la vibración sobre la masa. Las vigas sufren una carga
axial adicional en presencia de una aceleración en la dirección de la vibración, que se mani esta como
una variación en su frecuencia de resonancia. La escala de la medición queda expresada en una variación
de Hz/g . En la gura 2.4 se puede ver que las vigas opuestas se colocan una del lado interior de la
22 Capítulo 2. Navegación Inercial Strapdown
estructura y la otra del lado exterior. Esta con guración produce variaciones opuestas en su frecuencia
de vibración, que pueden ser comparadas con vigas no conectadas a la masa, que sirven para medir la
temperatura y proveer señales de compensación.
2.1.3. Modelo de Error de la IMU
Idealmente, una Unidad de Mediciones Inerciales (IMU, por sus siglas en inglés) se conforma por
una terna de sensores independientes que miden magnitudes físicas. En la realidad, su construcción no
es perfecta y aparecen errores que modi can la medición. Por un lado, errores sistemáticos tales como
sesgos, factores de escala, no ortogonalidad entre los ejes, variabilidad dependiente de la temperatura,
que pueden ser medidos e identi cados. Por otro, existen incertidumbres de caracter estocástico como
ruido blanco, inestabilidad del sesgo, o movimiento Browniano, que sólo pueden caracterizarse mediante
su estadística.
Tanto los acelerómetros como los giróscopos se ven afectados por errores debidos a su construcción
y a la tecnología empleada para la medición. El sesgo [20] (bias en inglés) es la observación de una
medición en ausencia de una magnitud física que la produzca. El factor de escala es la relación entre el
cambio en la medición y el de la magnitud que produce tal medición. Los errores de ortogonalidad entre
los ejes producen mediciones en todos los ejes cuando en realidad sólo deberían producirlos en uno,
quedando todas las mediciones acopladas entre sí. La no linealidad en la medición se mani esta como
una variación en el factor de escala. Todos estos parámetros pueden variar con la temperatura, lo que
requiere una caracterización en ese sentido. Existen también errores tales como sesgo dependiente de
la aceleración (en giróscopos), o errores debidos a anisoelasticidad [16, 13] (en giróscopos vibratorios).
En este trabajo se propone un modelo de error que incluye los factores de error más comunes:
sesgos, factores de escala y de no ortogonalidad entre los ejes.
σx σxy σxz bx ξx
m̂(t) =
I + σ
yx σy σyz
m(t) + b + ξ
y y (2.1)
σzx σzy σz bz ξz
= (I + Σ(σ m )) m(t) + bm + ξ m (t) (2.2)
donde el vector bm es el sesgo, los σi en la diagonal de Σ(σ m ) modelan los factores de escala, y los σij
los factores de no ortogonalidad que proyectan mediciones sobre los demás ejes, mientras que el vector
ξm representa el ruido de medición. Se usa el subíndice m para denotar genéricamente un instrumento,
que puede ser un giróscopo o un acelerómetro. Se requiere un juego de parámetros independientes para
2.2. Trayectoria Nominal 23
cada uno de ellos, ya que los errores son distintos y las ternas no comparten ejes. Despejando de este
modelo el vector de mediciones m(t), es posible corregir las mediciones de la IMU en función de los
errores instrumentales:
m(t) = (I + Σ(σ m ))−1 (m̂(t) − bm ) (2.3)
En la tabla 2.1 se muestran los parámetros de error especi cados [1] para la IMU prevista para el
proyecto. Debido a que los parámetros de error pueden ser identi cados e incorporados en el software
para corregir las mediciones, se muestran únicamente las inestabilidades en los mismos, que serán
los factores que afectarán las mediciones. En [14] se describen procesos para identi car los errores
sistemáticos y determinar y cuanti car la existencia de procesos estocásticos, ensayos indispensables
para reducir el impacto de los mismos en la navegación.
Giróscopo Acelerómetro
Ruido 0,4 °/s 5 mg
Inestabilidad en Sesgo 0,007 °/s 0,1 mg
Inestabilidad en Factor de Escala 0,01 % 0,01 %
Inestabilidad en Factor de No Ortogonalidad 0,087 % 0,087 %
Cuadro 2.1: Valores de Parámetros de Error de la IMU (1σ )
2.2. Trayectoria Nominal
El vehículo realizará un vuelo controlado de 60 segundos, luego de los cuales naliza el empuje.
Durante los primeros 5 segundos, se realiza un ascenso vertical, para luego realizar una maniobra
denominada gravity turn que es utilizada para per lar el vehículo en una trayectoria que mantenga
un ángulo de ataque
1 pequeño. Esto permite reducir el impacto del factor aerodinámico sobre el cuerpo
del vehículo, lo que a su vez reduce el consumo de combustible. En la gura 2.5 se muestra el per l
de altura para la trayectoria prevista, en que se observa la altura máxima alcanzada, o el apogeo del
vuelo es de aproximadamente 9km.
El lanzamiento se realiza desde una rampa que de ne la posición inicial del vehículo, así como su
actitud dentro de un determinado margen de error. El eje z de la IMU coincidirá con el de Roll en
el vehículo, mientras que los ejes x e y coincidirán con los ejes Yaw y Pitch, respectivamente. En el
instante inicial, el vehículo está orientado de forma tal que el eje de Roll coincide con la vertical local,
mientras que el eje de Yaw apunta en la dirección de la velocidad horizontal.
1
Ángulo entre el vector velocidad del vehículo y el del viento
24 Capítulo 2. Navegación Inercial Strapdown
Figura 2.5: Per l de Altura de la Trayectoria
Debido a que el vehículo realiza un vuelo controlado, se espera que su trayectoria no se aparte de la
prevista dentro de ciertos márgenes. Resulta útil de nir algunos parámetros de error que se utilizarán
para evaluar el error de guiado, lo que da una medida de cuánto se aparta la trayectoria efectuada por
el vehículo de la nominal.
Attitude Tracking Error: error de guiado en actitud, representa el ángulo entre la dirección del
eje axial real del vehículo y la dirección deseada en ese instante. Se espera que para los ejes Pitch
y Yaw estos errores sean menores a 3 grados, mientras que para Roll no hay una especi cación
debido a que no se realiza control sobre este eje.
Ascent Altitude Error: Diferencia entre la altitud deseada y la real en ese instante. Se espera
que este error esté dentro de los 100m al nalizar el vuelo controlado.
Range Error: Diferencia entre la distancia horizontal real recorrida y la deseada. Se espera que
esté dentro de los 300m.
Lateral Tracking Error: Error lateral (perpendicular a la dirección de movimiento horizontal)
respecto de la trayectoria deseada. Este error puede llegar a los 500m.
2.3. Ecuaciones de Navegación en ECI 25
2.3. Ecuaciones de Navegación en ECI
En un sistema de referencia inercial, la posición Pi (t) de un vehículo se obtiene a partir de la
ecuación de Newton: h i
Fi (t) = [Link] (t) = m P̈i (t) − gi (t) (2.4)
donde Fi (t) es la fuerza que actúa en el punto Pi (t), m es la masa del móvil, ai (t) es la aceleración a
la que está sometido el mismo, y gi (t) es la fuerza de gravedad en coordenadas inerciales. Se de ne la
fuerza especí ca en coordenadas inerciales como
Fi (t)
f i (t) = (2.5)
m
que, reemplazada en la ecuación 2.4 permite llegar a la expresión
f i (t) = P̈i (t) − gi (t) (2.6)
Debido a que en un sistema de navegación strapdown se mide la fuerza especí ca en terna del cuerpo
f b (t), se necesita expresar este vector en terna inercial, lo que se logra mediante la rotación f i (t) =
Cib (t)f b (t). Introduciendo esta relación en 2.6, y despejando P̈i (t), se llega a la ecuación fundamental
de navegación en coordenadas inerciales:
P̈i (t) = gi (t) + Cib (t)f b (t) (2.7)
Con el propósito de simpli car la lectura de las ecuaciones, se eliminará la dependencia de las
variables respecto del tiempo t. Dado que la fuerza especí ca fb se mide en coordenadas del cuerpo del
vehículo, es necesario conocer en todo momento la matriz de rotación Cib , que depende de la velocidad
angular ω bib que afecta al vehículo. Considerando esta nueva variable y además la velocidad en terna
inercial Vi , otra de las variables que se desean conocer, queda determinado el sistema de ecuaciones a
resolver para obtener las variables de navegación
Ċib = Cib S(ω bib ) (2.8)
V̇i = gi + Cib f b (2.9)
i i
Ṗ = V (2.10)
Estas ecuaciones describen la evolución de las variables Pi (t), Vi (t) y Cib (t) a partir de sus condiciones
iniciales Pi (0), Vi (0) y Cib (0), utilizando los datos f b (t) y ω bib (t) proporcionados por la IMU. En la
práctica, en lugar de integrar la ecuación diferencial matricial 2.8 que requiere actualizar 9 parámetros
redundantes, se integra únicamente la ecuación del coneo, de dimensión 3, que da el ángulo vectorial
26 Capítulo 2. Navegación Inercial Strapdown
de rotación instantáneo entre ambas ternas. Conociendo este ángulo es posible luego calcular la matriz
de rotación o el cuaternión correspondiente.
2.4. Ecuaciones de Navegación en ECEF
Derivando la relación Pi = Cie Pe dos veces respecto del tiempo, utilizando 1.6 y teniendo en cuenta
que la velocidad angular de la tierra ω eie es constante:
Ṗi = Ċie Pe + Cie Ṗe = Cie S(ω eie )Pe + Cie Ṗe (2.11)
P̈i = Cie S(ω eie )S(ω eie )Pe + Cie S(ω eie )Ṗe + Cie S(ω eie )Ṗe + Cie P̈e (2.12)
Reordenando, usando la ecuación 2.7 y expresando el producto vectorial,
h i
P̈i = Cie ω eie × (ω eie × Pe ) + 2(ω eie × Ṗe ) + P̈e = gi + Cib f b (2.13)
ω eie × (ω eie × Pe ) + 2(ω eie × Ṗe ) + P̈e = Cei gi + Cei Cib f b (2.14)
P̈e = Cei gi + Cei Cib f b − ω eie × (ω eie × Pe ) − 2(ω eie × Ṗe ) (2.15)
que lleva a la ecuación fundamental de la navegación en coordenadas ECEF:
P̈e = V̇e = Ceb f b + [ge − ω eie × (ω eie × Pe )] − 2(ω eie × Ve ) (2.16)
donde Ve es la velocidad en terna terrestre.
El término ge es la fuerza de gravedad en el punto Pe , ω eie × (ω eie × Pe ) es la aceleración centrífuga
debida a la rotación de la tierra y 2(ω eie × Ve ) es el término de Coriolis. Para resolver esta ecuación es
necesario conocer también la evolución de la matriz de rotación Ceb :
Ċeb = Ceb S(ω beb ) (2.17)
= Ceb S(ω bib − ω bie ) (2.18)
= Ceb S(ω bib ) − Ceb S(ω bie ) (2.19)
= Ceb S(ω bib ) − S(ω eie )Ceb (2.20)
Las ecuaciones que determinan la dinámica de un vehículo en coordenadas ECEF a partir de las
mediciones inerciales ω bib y fb entonces son
Ċeb = Ceb S(ω bib ) − S(ω eie )Ceb (2.21)
V̇e = Ceb f b + [ge − ω eie × (ω eie × Pe )] − 2(ω eie × Ve ) (2.22)
e e
Ṗ = V (2.23)
La terna de referencia ECEF resulta muy atractiva debido la relativa simplicidad de sus ecuaciones
2.5. Navegador Inercial Simple 27
y al hecho de que la resolución de las mismas provee datos que pueden ser comparados con los provistos
por un GPS sin ninguna transformación adicional. Por estas razones, el sistema de referencia ECEF
será el utilizado para el algoritmo de navegación inercial.
2.5. Navegador Inercial Simple
Las ecuaciones de navegación en diferentes ternas de referencia describen la variación de los
parámetros de navegación en el tiempo en función de las magnitudes físicas que se pueden medir,
como velocidad angular y fuerza especí ca. El cálculo de los parámetros de navegación viene dado en-
tonces por la integración en el tiempo de las ecuaciones de navegación, operación que admite diversas
aproximaciones numéricas que dan origen a diferentes grados de precisión.
Dado el modelo x(t) = ḟ (t), con x(t) diferenciable, el método más simple de integración consiste
en utilizar el método de Euler, aproximando ḟ (t) en tk+1 según
f (tk+1 ) − f (tk )
ḟ (tk+1 ) '
tk+1 − tk
por lo que la función en ese instante puede aproximarse como
f (tk+1 ) ' f (tk ) + (tk+1 − tk ) · ḟ (tk+1 ) (2.24)
Las ecuaciones de navegación pueden integrarse usando este método, siendo tk+1 − tk el intervalo
de tiempo en que se toman las mediciones inerciales, y que, para simpli car la notación, se denominará
T . Para incorporar la medición de los giróscopos en la integración de la actitud, es conveniente utilizar
la siguiente expresión:
qib (tk+1 ) ' qib (tk ) + T q̇ib (tk+1 )
1 ω bib (tk+1 )
' qib (tk ) + T qib (tk ) ◦
2
0
T b
2 ω ib (tk+1 )
' qib (tk ) ◦ (2.25)
1
El cuaternión resultante (ecuación 2.25) contiene la actitud actual en terna inercial. Para convertir
este dato a la terna ECEF, se lo rota mediante el cuaternión qei (tk+1 ):
qeb (tk+1 ) = qei (tk+1 ) ◦ qib (tk+1 )
28 Capítulo 2. Navegación Inercial Strapdown
(k + 1)T ω eie i
= ◦ qb (tk+1 ) (2.26)
0
Luego se puede utilizar este cuaternión para hallar la matriz de rotación Ceb (tk+1 ) e integrar las
ecuaciones de velocidad y posición en la misma terna
h i
Ve (tk+1 ) ' Ve (tk ) + T Ceb f b + [ge − ω eie × (ω eie × Pe )] − 2(ω eie × Ve ) (2.27)
Pe (tk+1 ) ' Pe (tk ) + T Ve (tk+1 ) (2.28)
donde se ha suprimido el término tk+1 para simpli car la lectura.
Las ecuaciones desarrolladas conforman un rudimentario algoritmo de navegación inercial. Se toman
los datos de velocidad angular y fuerza especí ca provistos por los sensores y se integran con el método
propuesto para producir datos de actitud, velocidad y posición en terna terrestre.
Algoritmo 2.1 Algoritmo de Navegación (Euler)
leer(f b , ω bib )
T b
2 ω ib (tk+1 )
qib (tk+1 ) = qib ◦
1
(k + 1)T ω eie
qeb (tk+1 ) = ◦ qib
0
Ve (tk+1 ) ' Ve (tk ) + T Ceb f b + [ge − ω eie × (ω eie × Pe )] − 2(ω eie × Ve )
Pe (tk+1 ) ' Pe (tk ) + T Ve (tk+1 )
Si bien la implementación de este método de integración de las ecuaciones de navegación es muy
simple, su performance en general no es buena. El error depende del paso de integración T, que está
limitado por la tasa a la que se reciben los datos de la IMU.
En los grá cos (2.6) a (2.8) se representan los errores en el ángulo vectorial de actitud, velocidad y
posición, en cada uno de sus ejes. Se han usado mediciones ideales, es decir que no están afectadas por
ruidos, sesgos, o errores en condiciones iniciales, de forma de evaluar el error numérico del algoritmo.
Los errores se evalúan utilizando las ecuaciones
ϕb = S−1 Cbe Ceb(IN S) − I (2.29)
δVe = VIN
e
S −V
e
(2.30)
δPe = PeIN S − Pe (2.31)
2.5. Navegador Inercial Simple 29
donde ϕb es el ángulo vectorial de error entre la terna del cuerpo navegada y la real, y las matrices de
rotación se obtienen de la trayectoria nominal y del algoritmo inercial, respectivamente.
Figura 2.6: Error en Actitud
Figura 2.7: Error en Velocidad
Se observa un error en actitud que oscila en los ejes x e y, llegando a aproximadamente 0,8° de
error. En el eje z el error es apenas menor, pero igualmente creciente. El error en velocidad en el
30 Capítulo 2. Navegación Inercial Strapdown
eje x crece hasta alcanzar aproximadamente 7m/s, siendo los errores en otros ejes bastante menores.
Para el caso de la posición, el error en el eje x (asociado al error en velocidad del eje x) crece hasta
aproximadamente 270m.
Evidentemente los niveles de error alcanzados en ausencia de cualquier tipo de perturbación hacen
que este algoritmo por sí solo no sea útil para una aplicación. Sin embargo se mostrará más adelante
que la navegación integrada permite reducir los errores causados por un navegador inercial de ciente,
logrando una buena performance.
Figura 2.8: Error en Posición
2.6. Navegador Inercial de Precisión
En esta sección, siguiendo el desarrollo expuesto en [5] y basado en los trabajos de Savage [17, 18],
se presentará un algoritmo de integración de las ecuaciones de navegación en tiempo real que se efectúa
en tres cadencias, como se muestra en la gura 2.9.
Figura 2.9: Cadencias
2.6. Navegador Inercial de Precisión 31
Esto permite separar cálculos de diferentes complejidades según la necesidad de actualizarlos, de
forma de optimizar el tiempo y la capacidad de cálculo de la computadora, mientras que la precisión no
se ve afectada en forma signi cativa. Los intervalos de tiempo están relacionados mediante Ts = [Link]
y Tm = [Link] . La notación para indicar un determinado instante de tiempo es la siguiente:
tk+1 = tk + [Link]
tk,j = tk + [Link]
tk,j,i = tk + [Link] + [Link]
2.6.1. Integración de la Ecuación Diferencial de Actitud
Como se mencionó antes, en la práctica se evita resolver la ecuación 2.8. En su lugar, se busca
resolver la ecuación que describe la variación en el tiempo de la terna del cuerpo respecto de la inercial.
Si ψ b (λ, tk,j ) es la solución a la ecuación (1.15), representa el ángulo vectorial rotado en el intervalo
[tk,j , λ] (λ ∈ [tk,j , tk,j+1 ]) a partir de la condición inicial ψ b (tk,j ) = 0. El ángulo rotado por la terna
terrestre en el intervalo t ∈ [tk , tk+1 ] es θ e (t, tk ) = Ωeie (t − tk ), y al ser Ωeie constante, resulta colineal
en todo tiempo con el ángulo rotado.
Se consideran las rotaciones mediante cuaterniones. La rotación de la terna del cuerpo a la terrestre
en el instante tk+1 viene dada por la composición de cuaterniones:
e(t ) e(t ) b(t )
qeb (tk+1 ) = qe(tk+1
k)
◦ qb(tkk) ◦ qb(tk+1
k
) (2.32)
donde además se acumula
b(t ) b(t ) b(t ) k,M −1b(t )
qb(tkk+1 ) = qb(tkk,1 ) ◦ qb(tk,1
k,2 )
◦ · · · ◦ qb(tk+1 ) (2.33)
Los cuaterniones pueden calcularse como
θ(t ,tk ) kθ(tk+1 ,tk )k
e(t ) − kθ(tk+1
k+1 ,tk )k
sin 2
qe(tkk+1 = (2.34)
)
kθ(tk+1 ,tk )k
cos 2
Se consideran las siguiente aproximaciones para la integración de las ecuaciones de navegación:
1. Ts su cientemente pequeño para considerar
e(t )
Ce(λ)k = exp(S(θ e (λ, tk ))) ∼
= I + S(θ e (λ, tk ))
con λ ∈ [tk , tk+1 ], y θ e (λ, tk ) = (λ − tk )Ωeie
32 Capítulo 2. Navegación Inercial Strapdown
2. Ts su cientemente pequeño para considerar
γ e (Re (t)) ∼
= γ e [Re (tk ) + Ve (tk )(t − tk )] ∼
= γ e [Re (tk )] + γR
e
[Re (tk )] Ve (tk )(t − tk )
donde γ eR [Re (tk )] es el Jacobiano de γ e [Re (tk )] en la posición Re (tk ).
3. Tm su cientemente pequeño como para despreciar los términos de segundo orden en la ecuación
del coneo
b 1
ψ̇ ∼= ω bib + S(ψ b )ω bib
2
4. Tm su cientemente pequeño para realizar la aproximación
b(t )
Cb(λ)k,j = exp(S(ψ b (λ, tk,j ))) ∼
= I + S(ψ b (λ, tk,j ))
con λ ∈ [tk,j , tk,j+1 ]
5. Tl su cientemente pequeño para considerar una variación lineal de la rotación de la terna b en
el tiempo
1 b
ψ b (λ, tk,j,i ) ∼
= ψ bk,j,i + ψ k,j,i+1 − ψ bk,j,i
2
con λ ∈ [tk,j,i , tk,j,i+1 ]
6. fb es constante en el Intervalo Tl
Para obtener el ángulo rotado por la terna del cuerpo debemos integrar la ecuación del coneo, donde
aplicamos la hipótesis 3:
ˆ
tk,j+1
1 b
ψ (tk,j , tk,j+1 ) ∼
b
= ω bib (λ) b
+ S ψ (λ, tk,j ) ω ib (λ) dλ (2.35)
2
tk,j
Subdividiendo el intervalo Tm en L intervalos Tl ,
L−1 ˆ
tk,j,i+1
1 b
ψ (tk,j , tk,j+1 ) ∼
X
b
= ω bib (λ) b
+ S ψ (λ, tk,j,i ) ω ib (λ) dλ (2.36)
2
i=0 t
k,j,i
´ tk,j,i+1
Llamando ∆ψ bk,j,i = tk,j,i ω bib (λ)dλ, que es la integración en un tiempo Tl de la velocidad angular
medida por los giróscopos, considerando la linealidad de ψb en Tl y el resultado presentado por Savage
en [17] se llega a que
L−1
X
1 1
ψ (tk,j , tk,j+1 ) ∼
b
= ∆ψ bk,j,i+1 + S(ψ bk,j,i )∆ψ bk,j,i+1 + S(∆ψ bk,j,i )∆ψ bk,j,i+1 (2.37)
2 12
i=0
Para el cálculo de esta ecuación se consideran las condiciones iniciales ψ bk,j,i = 0 y ∆ψ bk,j,i = 0. Con el
b(t )
ángulo ψ b (tk,j , tk,j+1 ) calculado es posible construir el cuaternión qb(tk,j
k,j+1 )
.
2.6. Navegador Inercial de Precisión 33
2.6.2. Integración de las Ecuaciones de Posición y Velocidad
Las ecuaciones de traslación Ṙe = Ve y V̇e = f e − 2S(Ωeie )Ve + γ e (Re ) pueden representarse
agrupando las variables en un único vector de la siguiente forma:
Ṙ(t) 0 I 0
Ẋ(t) = = · X(t) + (2.38)
e
V̇(t) 0 −2S(Ωie ) γ e (Re (t)) + Ceb f b (t)
0
= A · X(t) +
u(t)
El equivalentee discreto de este sistema coincide con el continuo para los instantes tk :
tˆk+1
0
X(tk+1 ) = e ATs
· X(tk ) + eA(tk+1 −ξ) dξ (2.39)
tk u(ξ)
donde
(sI + 2S(Ωeie ))−1
I
I Q(t)
−1 −1 −1
eAt
s
= =L (sI − A) =L s =
(2.40)
0 (sI + 2S(Ωeie ))−1 0 P(t)
En este caso, se ha llamado
n o
P(t) = L−1 (sI + 2S(Ωeie ))−1
cos(2Ωe t) sin(2Ωe t) 0
−2S(Ωeie )t
= e =
− sin(2Ωe t) cos(2Ωe t) 0
; P(0) = I3
(2.41)
0 0 1
( )
(sI + 2S(Ωeie ))−1
−1
Q(t) = L
s
ˆt sin(2Ωe t) 1 − cos(2Ωe t) 0
1
= P(λ)dλ = cos(2Ω t) − 1
e sin(2Ωe t) 0 ; Q(0) = 03 (2.42)
2Ωe
0
0 0 2Ωe t
Reemplazando (2.40) en (2.39), se obtiene
tˆk+1
Q(tk+1 − ξ)
X(tk+1 ) = eATs · X(tk ) + u(ξ)dξ (2.43)
tk P(tk+1 − ξ)
34 Capítulo 2. Navegación Inercial Strapdown
y desarrollando,
tˆk+1
Q(tk+1 − ξ)
X(tk+1 ) = eATs · X(tk ) +
e e e b
γ (R (ξ)) + Cb f (ξ) dξ
tk P(tk+1 − ξ)
tˆk+1 tˆk+1
Q(tk+1 − ξ) Q(tk+1 − ξ) e b
= eATs
e e
· X(tk ) + γ (R (ξ))dξ + Cb f (ξ)dξ
tk P(tk+1 − ξ) tk P(tk+1 − ξ)
ATs
= e · X(tk ) + Xgk+1 + Xfk+1
El término Xgk+1 corresponde a la contribución debida a la gravedad, mientras que Xfk+1 depende
de la dinámica del vehículo. Se considerará la integración de estos términos por separado.
[Link]. Integración del término de gravedad
Utilizando la hipótesis 2 y haciendo el cambio de variables λ = tk+1 − ξ , se desarrolla la expresión
del término de gravedad:
tˆk+1
Q(t k+1 − ξ) e e
Xgk+1 = γ (R (ξ))dξ
tk P(tk+1 − ξ)
tˆk+1
∼ Q(tk+1 − ξ) e e e e e
= (γ [R (tk )] + γ R [R (tk )] V (tk )(ξ − tk )) dξ
tk P(tk+1 − ξ)
ˆTs
∼ Q(λ) e e e e e ∆Rgk+1
= (γ [R (tk )] + γ R [R (tk )] V (tk )(Ts − λ)) dλ , (2.44)
0 P(λ) ∆Vgk+1
ˆTs
∆Rgk+1 = Q(λ) (γ e [Re (tk )] + γ eR [Re (tk )] Ve (tk )(Ts − λ)) dλ
0
ˆTs ˆ Ts
= Q(λ)dλ (γ e [Re (tk )] + γ eR [Re (tk )] Ve (tk )Ts ) − Q(λ)λdλγ eR [Re (tk )] Ve (tk )
0
0
y, de la misma forma,
ˆTs
∆Vgk+1 = P(λ) (γ e [Re (tk )] + γ eR [Re (tk )] Ve (tk )(Ts − λ)) dλ
0
ˆTs ˆTs
e e
= P(λ)dλ (γ [R (tk )] + γ eR [Re (tk )] Ve (tk )Ts ) − P(λ)λdλγ eR [Re (tk )] Ve (tk )
0 0
2.6. Navegador Inercial de Precisión 35
´ Ts
Por lo tanto, para obtener la expresión exacta de Xgk+1 basta con resolver las integrales
o Q(λ)dλ,
´ Ts ´ Ts ´ Ts
o λQ(λ)dλ, o P(λ)dλ y
o λP(λ)dλ.
Otra forma de resolver el término de gravedad consiste en considerar que las matrices Q(t) y P(t)
se mantienen constantes en el intervalo Ts y considerar las aproximaciones
sin(2Ωe Ts ) 1 − cos(2Ωe Ts ) 0
1 1
Q(t) ∼
= Q = [Q(0) + Q(Ts )] = cos(2Ω T ) − 1
e s sin(2Ωe Ts ) 0 (2.45)
2 4Ωe
0 0 1
1 + cos(2Ωe Ts ) sin(2Ωe Ts ) 0
1 1
P(t) ∼
= P = [P(0) + P(Ts )] = − sin(2Ω T
e s ) 1 + cos(2Ω T
e s ) 0 (2.46)
2 2
0 0 1
Debido a la lenta rotación de la tierra y la longitud del intervalo de tiempo Ts , esta aproximación es
aplicable en la mayoría de los casos.
Reemplazando estas aproximaciones, se obtiene
ˆTs
Q e e
Xgk+1 ∼
=
e e e
(γ [R (tk )] + γ R [R (tk )] V (tk )(Ts − λ)) dλ
0 P
ˆTs ˆTs
Q e e Q e
∼
=
e e e
(γ [R (tk )] + γ R [R (tk )] V (tk )Ts ) dλ −
e e
γ R [R (tk )] V (tk ) λdλ
P 0 P 0
Q e e Q e Ts2
∼
=
e e e
(γ [R (tk )] + γ R [R (tk )] V (tk )Ts ) Ts −
e e
γ R [R (tk )] V (tk )
P P 2
Q Ts2
∼
=
γ e
[R e
(t k )] Ts − γ e
[R e
(t k )] V e
(t k ) (2.47)
R
2
P
El modelo de gravedad utilizado en el algoritmo es el J2 , que se detalla en el apéndice, junto con
su jacobiano.
[Link]. Integración del Término de Fuerza Especí ca
Para resolver este término se considerará nuevamente la aproximación realizada sobre las matrices
Q(t) y P(t):
tˆk+1
Q(tk+1 − ξ) e b
Xfk+1 = Cb f (ξ)dξ
tk P(tk+1 − ξ)
36 Capítulo 2. Navegación Inercial Strapdown
tˆk+1
Q
∼
=
Ceb f b (ξ)dξ
P tk
tˆk+1
∼ Q e(ξ) e(t ) b(t )
= Ce(tk ) C b(tkk) Cb(ξ)k f b (ξ)dξ
P tk
Dividiendo el intervalo de tiempo Ts en M intervalos Tm y aplicando la hipótesis hipótesis 1:
tˆ
M −1 k,j+1
∼ Q X e(tk,j ) b(tk,j ) b
Xfk+1 = (I − (ξ − tk,j )S(Ωeie )) Cb(tk,j ) Cb(ξ) f (ξ)dξ
P j=0 t
k,j
∼ Q
= (Ic − Im ) (2.48)
P
donde se ha llamado
M −1 ˆ
tk,j+1
e(t ) b(t )
X k,j k,j b
Ic = Cb(tk,j ) Cb(ξ) f (ξ)dξ (2.49)
j=0 t
k,j
M −1 ˆ
tk,j+1
e(t ) b(t )
X
Im = S(Ωeie ) k,j
(ξ − tk,j )Cb(tk,j k,j b
) Cb(ξ) f (ξ)dξ (2.50)
j=0 t
k,j
Resolución del Término Ic Para resolver este término se divide el intervalo Tm en L intervalos Tl
y se aplica la hipótesis 4:
M −1 L−1 ˆ
tk,j,i+1
e(tk,j )
X X
Ic = Cb(tk,j I + S(ψ b (ξ, tk,j,i )) f b (ξ)dξ
)
j=0 i=0 t
k,j,i
M −1 L−1 ˆ
tk,j,i+1 ˆ tk,j,i+1
e(tk,j )
X X
= Cb(tk,j f b (ξ)dξ + S(ψ b (ξ, tk,j,i ))f b (ξ)dξ
)
j=0 i=0 tk,j,i
tk,j,i
Utilizando las hipótesis 5 y 6, se resuelven las integrales:
M −1
"L−1 #
X e(tk,j )
X
b 1 b b
Ic = Cb(tk,j ) ∆Vk,j,i+1 + S ψ k,j,i + (ψ k,j,i+1 − ψ k,j,i ) ∆Vk,j,i+1
2
j=0 i=0
M −1
e(t )
X k,j
= Cb(tk,j ) I ci
j=0
donde L−1
X
1 b
b b
I ci = ∆Vk,j,i+1 + S ψ k,j,i + (ψ k,j,i+1 − ψ k,j,i ) ∆Vk,j,i+1 (2.51)
2
i=0
2.6. Navegador Inercial de Precisión 37
e(t
k,j ) k e(t ) e k e(t )
Utilizando la hipótesis 1, es posible hacer el reemplazo Cb(tk,j ) = Cb(tk,j ) − jTm S(Ωie )Cb(tk,j ) :
M −1
e(tk ) e(tk )
X
e
Ic = Cb(tk,j ) − jT m S(Ωie )Cb(tk,j ) Ici
j=0
M −1
e(t ) e(t )
X
k e k
= Cb(tk,j ) Ici − jTm S(Ωie )Cb(tk,j ) Ici (2.52)
j=0
Resolución del Término Im Realizando el cambio de variables λ = ξ − tk,j , obtenemos
M −1 ˆ
Tm
e(t ) b(t )
X
Im = S(Ωeie ) k,j
λCb(tk,j k,j b
) Cb(λ+tk,j ) f (λ + tk,j )dλ
j=0 0
Descomponiendo la integral sobre Tm en L intervalos Tl y aplicando la hipótesis 4,
M −1 L−1 ˆ
tk,j,i+1
e(tk,j )
X X
Im = S(Ωeie ) Cb(tk,j λ I + S(ψ b (λ, tk,j,i )) f b (λ + tk,j,i )dλ
)
j=0 i=0 t
k,j,i
M −1 L−1 ˆ
tk,j,i+1 ˆ
tk,j,i+1
e(tk,j )
X X
= S(Ωeie ) Cb(tk,j λf b (λ + tk,j,i )dλ + λS(ψ b (λ, tk,j,i ))f b (λ + tk,j,i )dλ
)
j=0 i=0 tk,j,i tk,j,i
Llamando al término de las integrales
L−1
X ˆ
tk,j,i+1 ˆ
tk,j,i+1
Imi = λf b (λ + tk,j,i )dλ + λS(ψ b (λ, tk,j,i ))f b (λ + tk,j,i )dλ (2.53)
i=0 tk,j,i tk,j,i
e(t
k,j )
y desarrollando el término Cb(tk,j ) de la misma forma que antes:
M −1
e(t )
X
Im = S(Ωeie ) k,j
Cb(tk,j ) Imi
j=0
M −1
e(tk ) e(tk )
X
= S(Ωeie ) Cb(tk,j ) − jT m S(Ωe
ie )Cb(tk,j ) Imi (2.54)
j=0
M −1
e(t ) e(t )
X
= S(Ωeie )Cb(tk,j
k 2 e k
) Imi − jTm S (Ωie )Cb(tk,j ) Imi (2.55)
j=0
En [18] se desarrollan las integrales en (2.53) para llegar a
L−1
X 2i + 1 3i + 2 b b 2i + 1
Imi = Tl ∆Vk,j,i+1 + S (ψ k,j,i+1 − ψ k,j,i ) + ψ k,j,i ∆Vk,j,i+1 (2.56)
2 6 2
i=0
38 Capítulo 2. Navegación Inercial Strapdown
2.6.3. Algoritmo de Navegación Inercial de Precisión
A partir de las ecuaciones obtenidas anteriormente, se construye el algoritmo de navegación inercial,
utilizando como entradas los datos suministrados por la Unidad de Mediciones Inerciales.
Algoritmo 2.2 Algoritmo de Navegación Inercial
Inicialización(qeb inicial , e
Vinicial , Peinicial )
f or(k = 0, k + +, F IN )
Ic = 0, Im = 0
f or(j = 0, j + +, , j < M )
ψ bk,j,0 = 0, ∆ψ bk,j,0 = 0, Ici = 0, Imi = 0
f or(i = 0, i + +, i < L)
b
leer(∆Vk,j,i+1 , ∆ψ bk,j,i+1 )
ψ bk,j,i+1 = ψ bk,j,i + ∆ψ bk,j,i+1 + 12 S(ψ bk,j,i )∆ψ bk,j,i+1 + 12
1
S(∆ψ bk,j,i )∆ψ bk,j,i+1
1
Ici = Ici + ∆Vk,j,i+1 + S ψ bk,j,i + (ψ bk,j,i+1 − ψ bk,j,i ) ∆Vk,j,i+1
2
b b 2i+1 b
Imi = Imi + Tl 2 ∆Vk,j,i+1 + S 3i+2
2i+1
6 (ψ k,j,i+1 − ψ k,j,i ) + 2 ψ k,j,i ∆Vk,j,i+1
end
h i
e(tk ) e e(tk )
Ic = Ic + Cb(tk,j Ic
) i − jT m S(Ω ie )C I
b(tk,j ) ic
h i
e(tk ) 2 (Ωe )Ce(tk ) I
Im = Im + S(Ωeie )Cb(tk,j ) Im i − jT m S ie b(tk,j ) m i
b(t )
qb(tk,j
k,j+1 )
= q(ψ bk,j+1 )
e(t )
k ke(t ) k,j b(t )
qb(tk,j+1 ) = qb(tk,j ) ◦ qb(tk,j+1 )
end
Ts2
Q e e e e e
Xgk+1 = γ [R (tk )] Ts − γ R [R (tk )] V (tk )
P 2
Q
Xfk+1 = (Ic − Im )
P
X(tk+1 ) = eATs · X(tk ) + Xgk+1 + Xfk+1
e(t ) e(t )
qeb (tk+1 ) = qe(tkk+1
)
k
◦ qb(tk+1 )
end
[Link]. Simpli cación del Algoritmo
En el caso en que Tl = Tm , se pierde la distinción entre los ciclos correspondientes a i y j. Podría
decirse que las funciones contenidas en el ciclo i pasan a formar parte del ciclo j. Esto permite elim-
inar algunos términos del ciclo, ya que las variables i, ψ bk,j,i , ∆ψ bk,j,i y ∆Vk,j,i permanecen en cero
constantemente.
2.7. Validación del Algoritmo Inercial 39
Algoritmo 2.3 Ciclo Rápido Simpli cado
b
leer(∆Vk,j,i+1 , ∆ψ bk,j,i+1 )
ψ bk,j,i+1 = ∆ψ bk,j,i+1
Ici = ∆Vk,j,i+1 + S 12 ψ bk,j,i+1 ∆Vk,j,i+1
Imi = Tl 21 ∆Vk,j,i+1 + S 13 ψ k,j,i+1 ∆Vk,j,i+1
Lo mismo ocurre cuando Tm = Ts , ya que los términos donde aparece j se mantienen en cero.
Algoritmo 2.4 Ciclo Medio Simpli cado
ins rapido()
k e(t )
Ic = Cb(tk,j ) Ici
e(t )
Im = S(Ωeie )Cb(tk,j
k
) Imi
b(t )
qb(tk,j
k,j+1 )
= q(ψ bk,j+1 )
e(t )
k k e(t ) b(t
k,j )
qb(tk,j+1 ) = qb(tk,j ) ◦ qb(tk,j+1 )
Estas ecuaciones son mucho más directas, intervienen menos términos y eliminan la necesidad de
utilizar ciclos for anidados. En caso de que Tl = Tm = Ts , todas las funciones formarían parte de un
mismo ciclo, que toma datos y devuelve resultados a una misma frecuencia.
Surge, a partir de las simpli caciones derivadas de considerar Tl = Tm = Ts , una disyuntiva: utilizar
un algoritmo con ciclos anidados que realiza una determinada cantidad de cálculos a una determinada
tasa, o usar una simpli cación del algoritmo que funciona a una tasa mucho mayor, pero que realiza
menos cálculos y tiene una precisión mayor. El estudio de qué versión conviene utilizar dependerá de
las características de la computadora donde se lo implementará y de los valores de Tl , Tm y Ts .
[Link]. Determinación de Tl , Tm y Ts
El requerimiento de frecuencia de actualización de las variables de navegación impuesto por el
sistema de control es de 10Hz. Por otro lado, la frecuencia de actualización de la IMU está prevista en
100Hz. Estas condiciones jan los tiempos para el ciclo lento (Ts =100ms) y el ciclo rápido (Tl =10ms),
quedando por de nir Tm . Para ganar precisión, lo más conveniente es actualizar el cuaternión de actitud
con el ángulo rotado en el ciclo rápido cada vez que se lo calcula, lo que ja Tm = Tl =10ms.
2.7. Validación del Algoritmo Inercial
Para evaluar el algoritmo, al igual que en la sección 2.5 se utilizaron datos sintéticos generados a
partir de la trayectoria ideal del cohete. Esto permite prácticamente eliminar las incertidumbres debidas
40 Capítulo 2. Navegación Inercial Strapdown
a la generación de las mediciones inerciales, por lo que se está analizando la precisión del algoritmo
inercial únicamente. Con el mismo objetivo, se inyectaron datos inerciales y condiciones iniciales libres
de ruido o cualquier otra perturbación.
Figura 2.10: Error en Actitud
Figura 2.11: Error en Velocidad
Los errores para este navegador resultan varios órdenes de magnitud menores que para el navegador
2.7. Validación del Algoritmo Inercial 41
basado en el método de Euler. El error en actitud se mantiene dentro de los 0, 0001° durante el vuelo
( gura 2.10), mientras que los errores en velocidad y posición son del orden de los 0, 0001m/s y 0, 02m,
respectivamente. En la gura 2.13 se puede ver que la navegación de la trayectoria coincide con la
nominal.
Figura 2.12: Error en Posición
Figura 2.13: Trayectoria
42 Capítulo 2. Navegación Inercial Strapdown
2.8. Desventajas de la Navegación Inercial
Cuando se incluyen incertidumbres en las mediciones y en las condiciones inerciales, como sucede
en la práctica, se propagan a través de las ecuaciones de navegación. Los sesgos se integran constan-
temente, los ruidos afectan la precisión de la medición, y las condiciones iniciales erróneas no pueden
ser detectadas. En la gura (2.14) se observan las mediciones de una IMU afectada por errores como
los dados en la tabla 2.1, esperados para la IMU que se utilizó en este trabajo.
Figura 2.14: Mediciones IMU
Figura 2.15: Error en Actitud
2.8. Desventajas de la Navegación Inercial 43
Figura 2.16: Error en Velocidad
Figura 2.17: Error en Posición
El resultado de navegar datos ruidosos es un error creciente con el tiempo, en particular en posición
y velocidad. El módulo del error en posición llega a los 500m en tan sólo 60 segundos, mientras que el
error en velocidad alcanza un pico de 17m/s. Estos valores son inaceptables en un sistema de navegación
para un vehículo controlado que debe seguir una trayectoria predeterminada. Surge de este resultado
44 Capítulo 2. Navegación Inercial Strapdown
la necesidad de implementar un sistema que sea robusto ante incertudumbres y perturbaciones, y en
lo posible que sea capaz de identi car errores instrumentales.
Figura 2.18: Trayectoria
En las guras 2.19 a 2.21 se muestran los errores resultantes de navegar esta misma secuencia de
datos con el navegador basado en el método de Euler. Se puede ver que las magnitudes de los errores
son similares en ambos casos.
Figura 2.19: Error en Actitud (Euler)
Esto indica que el error numérico debido a la integración de las ecuaciones de navegación pasa
a un segundo plano en presencia de los ruidos y errores instrumentales de la IMU, lo que opaca la
2.8. Desventajas de la Navegación Inercial 45
performance del algoritmo de precisión, pasando a estar ambos métodos en un nivel de performance
similar.
Figura 2.20: Error en Velocidad (Euler)
Figura 2.21: Error en Posición (Euler)
Capítulo 3
Estimación de Errores
Se ha mostrado la necesidad de implementar un sistema capaz de identi car errores en las condi-
ciones iniciales, y que sea robusto ante ruidos presentes en las mediciones, así como a errores instrumen-
tales. En el campo de la navegación es común utilizar el Filtro de Kalman como recurso para estimar
estos errores e incorporar su naturaleza aleatoria en el proceso. En este capítulo se describe el Filtro
de Kalman, su estructura, y se deduce el modelo dinámico que utiliza como base en la estimación.
3.1. Filtro de Kalman
El Filtro de Kalman clásico se plantea en el contexto de sistemas dinámicos lineales perturbados
por ruido. El objetivo es estimar el estado dinámico incorporando también las características del ruido
y observaciones ruidosas independientes. El modelo lineal variante en el tiempo para tal sistema viene
dado por
ẋ(t) = F(t)x(t) + B(t)ξ(t)
(3.1)
y(t) = C(t)x(t) + η(t)
donde x(t) es el vector de estados, F(t) es la matriz que contiene la información acerca de la dinámica
y(t) es el vector de observaciones del estado. Los vectores ξ(t) y η(t) son ruidos supuestos
del sistema e
T T
blancos Gaussianos de media nula, con matrices de covarianza E ξ(t)ξ (t) = Qδ(t) y E η(t)η (t) =
Rδ(t), donde δ(t) es la función Delta de Dirac.
Para efectuar los cálculos digitalmente, es necesario de nir el equivalente en tiempo discreto de este
sistema:
xk+1 = Φk xk + Bk ξ k
(3.2)
yk+1 = Ck+1 xk+1 + η k+1
donde Φk es la llamada matriz de transición de estados entre el instante k y el instante siguiente k + 1.
46
3.1. Filtro de Kalman 47
Esta matriz se de ne como
Φk = exp [Fk T ] (3.3)
donde T es el período de discretización.
La discretización del ruido no es directa, sino que media la matriz de transición de estados según
la ecuació ˆT
ξk = Φ(T, τ )B(τ )ξ(τ )dτ (3.4)
0
a partir de la que se puede calcular la covarianza del ruido discretizado
ˆT
E ξ k ξ Tk Φ(T, τ )B(τ )Q(τ )BT (τ )ΦT (T, τ )dτ
= (3.5)
0
∼
= Bk QBTk T (3.6)
Esta es una solución aproximada bastante común, que es válida cuando los autovalores de F(t) son
mucho menores que el período de discretización T [9].
Es necesario conocer también las condiciones iniciales del vector de estados, construyendo además
una matriz de covarianza del error inicial que será propagada en el tiempo y será una medida de la
incertidumbre en la estimación de cada variable. Siendo el valor inicial del vector de estados x̂0/0 =
E[x0 ], se de ne la matriz de covarianza inicial como
P0/0 = E (x0 − x̂0/0 )(x0 − x̂0/0 )T
(3.7)
El primer paso del algoritmo consiste en predecir la evolución del sistema a partir de su dinámica.
Tomando la esperanza [8, 9] de la ecuación 3.2 se obtien
x̂k+1/k = Φk x̂k/k (3.8)
donde x̂k+1/k es el valor esperado para el vector de estados en el instante k+1 calculado a partir de
la dinámica del proceso.
Además de propagar el estado, se propaga la matriz de covarianza del error en la estimación mediant
Pk+1/k = Φk Pk/k ΦTk + Bk QBTk T (3.9)
El término Φk Pk/k ΦTk corresonde a la propagación en el tiempo de la matriz de covarianza del estado,
mientras que Bk Qk BTk T da cuenta del ruido de proceso en un intervalo de tiempo T. De esta forma,
a la propagación de estas matrices se le llama Predicción.
El segundo paso consiste en actualizar estas estimaciones cuando se dispone de una observación
48 Capítulo 3. Estimación de Errores
del estado, relacionados mediante yk+1 = Ck+1 xk+1 . Se calcula primero la ganancia de Kalman, que
puede interpretarse como una medida de la con abilidad de la observación
−1
Kk+1 = Pk+1/k CTk+1 Ck+1 Pk+1/k CTk+1 + Rk+1 (3.10)
Luego se actualiza o corrige la estimación del estado utilizando la ganancia Kk+1 y la observación,
mediant
x̂k+1/k+1 = x̂k+1/k + Kk+1 yk+1 − Ck+1 x̂k+1/k (3.11)
El término yk+1 − Ck+1 x̂k+1/k recibe el nombre de innovación, y representa la información independi-
ente introducida en la estimación. La secuencia de innovaciones debe estar descorrelacionada, es decir
que es análoga a un ruido blanco con la potencia del ruido de las observaciones.
Finalmente, se actualiza la estimación de la matriz de covarianza del error incorporando la ganancia
de Kalma
Pk+1/k+1 = (I − Kk+1 Ck+1 ) Pk+1/k (3.12)
Esta segunda fase del Filtro de Kalman recibe el nombre de Actualización, ya que se corrigen los datos
predichos mediante la observación de datos adicionales.
3.2. Modelo Dinámico del Error
Debido a que las ecuaciones de navegación son marcadamente alineales, no es posible aplicar di-
rectamente el Filtro de Kalman al problema considerado. A pesar de esto, es posible desarrollar un
modelo para la dinámica de un error δx propagándose por las ecuaciones de navegación. Para el caso de
perturbaciones su cientemente pequeñas, esta dinámica de error se comporta linealmente, permitiendo
utilizar el Filtro de Kalman para estimarlo. Se estudia en esta sección la propagación del error a través
de las ecuaciones de navegación 2.21, 2.22 y 2.23.
3.2.1. Error en Actitud
Se de ne el error en actitud como
δCeb = Ĉeb − Ceb (3.13)
donde Ĉeb es la matriz de rotación calculada por el INS y Ceb es la matriz de rotación real. A su vez, se
suele utilizar la siguiente relación entre ambas matrices:
Ĉeb = Ceb (I + S(ϕb )) (3.14)
donde ϕb es el ángulo vectorial de error en terna del cuerpo. Para ϕb pequeño, se puede interpretar el
término I + S(ϕb ) como una aproximación de la matriz de rotación entre la terna del cuerpo sin error
3.2. Modelo Dinámico del Error 49
y la calculada por el INS, es decir
Ĉeb = Ceb (I + S(ϕb )) (3.15)
∼ b
= Cebreal Cbreal
IN S
Se desarrollan las ecuaciones para obtener el modelo dinámico del error en actitud.
˙ e = Cˆ˙e − Ċe
δC b b b
e
h i h i
= Ĉeb S(ω̂ bib ) − S(Ω̂ie )Ĉeb − Ceb S(ω bib ) − S(Ωeie )Ceb
e
Dado que la velocidad angular de la tierra es conocida, Ω̂ie = Ωeie :
˙ e = Ĉe S(ω̂ b ) − Ce S(ω b ) − S(Ωe )Ĉe − S(Ωe )Ce
δC b b ib b ib ie b ie b
= Ĉeb S(ω̂ bib ) − Ceb S(ω bib ) − S(Ωeie )δCeb
Ahora, reemplazando Ĉeb = Ceb +δCeb y ω̂ bib = ω bib +δω bib , desarrollando y despreciando los productos
entre errores:
˙ e = (Ce + δCe ) S(ω b + δω b ) − Ce S(ω b ) − S(Ωe )δCe
δC b b b ib ib b ib ie b
= Ceb S(ω bib ) + Ceb S(δω bib ) + δCeb S(ω bib ) + δCeb S(δω bib ) − Ceb S(ω bib ) − S(Ωeie )δCeb
= Ceb S(δω bib ) + δCeb S(ω bib ) − S(Ωeie )δCeb (3.16)
Desarrollando el término de error como se de nió en (3.14) obtenemos la relación:
Ĉeb = Ceb (I + S(ϕb ))
Ĉeb − Ceb = Ceb S(ϕb )
δCeb = Ceb S(ϕb ) (3.17)
que reemplazada en (3.16) permite llegar a
˙ e = Ce S(δω b ) + Ce S(ϕb )S(ω b ) − S(Ωe )Ce S(ϕb )
δC (3.18)
b b ib b ib ie b
Por otro lado, diferenciando (3.17) se llega a otra expresión para ˙ e:
δC b
˙ e = Ċe S(ϕb ) + Ce S(ϕ̇b )
δC (3.19)
b b b
h i
= Ceb S(ω bib ) − S(Ωeie )Ceb S(ϕb ) + Ceb S(ϕ̇b )
= Ceb S(ω bib )S(ϕb ) − S(Ωeie )Ceb S(ϕb ) + Ceb S(ϕ̇b ) (3.20)
50 Capítulo 3. Estimación de Errores
Igualando (3.18) y (3.20):
Ceb S(δω bib ) + Ceb S(ϕb )S(ω bib ) − S(Ωeie )Ceb S(ϕb ) = Ceb S(ω bib )S(ϕb ) − S(Ωeie )Ceb S(ϕb ) + Ceb S(ϕ̇b )
S(δω bib ) + S(ϕb )S(ω bib ) = S(ω bib )S(ϕb ) + S(ϕ̇b )
S(δω bib ) + S(ϕb )S(ω bib ) − S(ω bib )S(ϕb ) = S(ϕ̇b )
S(ϕb )S(ω bib )−S(ω bib )S(ϕb ) = S S(ϕb )ω bib
Analizando elemento por elemento, es posible demostrar que ,
por lo que se llega a
S(δω bib ) + S S(ϕb )ω bib = S(ϕ̇b ) (3.21)
y, nalmente
ϕ̇b = −S(ω bib )ϕb + δω bib (3.22)
3.2.2. Error en Posición y Velocidad
Como en la sección anterior, el error en velocidad se de ne como δVe = V̂e − Ve . Diferenciando
esta ecuación,
˙ e = V̂˙ e − V̇e
δV
h i h i
= Ĉeb f̂ b − 2S(Ωeie )V̂e + ĝe (P̂e , Ωeie ) − Ceb f b − 2S(Ωeie )Ve + ge (Pe , Ωeie )
= Ĉeb f̂ b − Ceb f b − 2S(Ωeie )V̂e − 2S(Ωeie )Ve + ĝe (P̂e , Ωeie ) − ge (Pe , Ωeie )
e
e b e b
e e ∂g
= Ĉb f̂ − Cb f − 2S(Ωie )δV + δPe
∂Pe Pe
e
e e
b b
e b e e ∂g
= (Cb + δCb ) f + δf − Cb f − 2S(Ωie )δV + δPe
∂Pe Pe
Desarrollando y despreciando nuevamente los productos entre errores:
e
˙ e b e b e b e b e b e e ∂g
e
δV = Cb f + Cb δf + δCb f + δCb δf − Cb f − 2S(Ωie )δV + δPe
∂Pe Pe
e
e b e b e e ∂g
= Cb δf + δCb f − 2S(Ωie )δV + δPe
∂Pe Pe
y haciendo el reemplazo δCeb = Ceb S(ϕb ),
e
˙ e b b e e ∂g
e
δV = Cb S(ϕ )f − 2S(Ωie )δV + δPe + Ceb δf b
∂Pe Pe
e
e b b e e ∂g
= −Cb S(f )ϕ − 2S(Ωie )δV + δPe + Ceb δf b (3.23)
∂Pe Pe
La ecuación para la propagación del error en posición es simplemente
˙ e = δVe
δP (3.24)
3.2. Modelo Dinámico del Error 51
3.2.3. Errores Instrumentales
Se recuerda el modelo para las mediciones inerciales perturbadas por ruido y errores en la construc-
ción del instrumento:
σx σxy σxz b ξ
x x
m̂(t) =
I + σyx σy σyz
m(t)+ b + ξ = (I + Σ(σ m )) m(t)+bm +ξ m (t)
y y (3.25)
σzx σzy σz bz ξz
El término de error instrumental puede ampliarse de la siguiente forma
δm = δbm + L(m̂)δσ m + ξ m (3.26)
donde δbm es la inestabilidad en el sesgo, δσ m contiene la inestabilidad en los factores de escala y no
ortogonalidad entre los ejes del instrumento, y L(m̂) es tal que
L(m̂)σ m = Σ(σ m )m̂ (3.27)
T
Si σm = σx σy σz σxy σxz σyz σyx σzx σzy entonces
mx 0 0 my mz 0 0 0 0
L(m̂) =
0 my 0 0 0 mz mx 0 0
(3.28)
0 0 mz 0 0 0 0 mx my
[Link]. Términos de Error Ampliados
Se determinan las ecuaciones que contemplan la inclusión de los errores instrumentales en las
ecuaciones dinámicas del error. Para los giróscopos
ϕ̇b = −S(ω bib )ϕb + δω bib
= −S(ω bib )ϕb + δbω + L(ω bib )σ ω + ξ ω (3.29)
y para los acelerómetros
e
˙ e b e e ∂g
e
δV = −Cb S(f )ϕ − 2S(Ωie )δV + δPe + Ceb δf b
∂Pe Pe
e
e b e e ∂g e e
b
= −Cb S(f )ϕ − 2S(Ωie )δV + δP + Cb δb f + L(f )δσ f + ξ f
∂Pe e
e P
∂g
= −Ceb S(f b )ϕ − 2S(Ωeie )δVe + δPe + Ceb δbf + Ceb L(f b )δσ f + Ceb ξ f (3.30)
∂Pe Pe
De esta forma pueden separarse los términos que se busca estimar de los que son puramente ruido,
52 Capítulo 3. Estimación de Errores
lo que quedará en evidencia cuando se construya el modelo completo de la dinámica del error.
[Link]. Modelo de Desvíos de los Parámetros Instrumentales
En este caso, siguiendo la de nición del error instrumental dada antes y agregando una inestabilidad
en el mismo
m = m̂ + δm (3.31)
la inestabilidad en los parámetros de error de los sensores queda de nida como:
δm = m − m̂ (3.32)
donde m puede representar el sesgo, factor de escala o factor de no ortogonalidad de los instrumentos
inerciales.
A pesar de que la inestabilidad δm se considera constante durante el vuelo, es común describirla
como un Random Walk, o movimiento Browniano (el resultado de integrar ruido blanco en el tiempo),
lo que mejora la observabilidad del estado. Por lo tanto, el modelo dinámico para esta inestabilidad
resulta
˙ =ξ
δm (3.33)
m
donde ξm es ruido blanco Gaussiano.
3.2.4. Modelo Dinámico Completo para Pequeñas Perturbaciones
Las ecuaciones 3.29, 3.30 y 3.33 deducidas en las secciones anteriores pueden agruparse en un modelo
contínuo similar al 3.1. Sin embargo, sólo se dispone de estimaciones de las variables que intervienen en
la dinámica del error, que en este caso son la velocidad angular ω̂ bib , la fuerza especí ca f̂ b y la matriz
de rotación de la terna del cuerpo a la de la tierra Ĉeb . Estos parámetros son medidos por la IMU y
calculados por el navegador inercial:
δ ẋ(t) = F̂(t)δx(t) + B̂(t)ξ(t) (3.34)
3.2. Modelo Dinámico del Error 53
ϕ̇b −S(ω̂ bib ) 0 0 I L(ω̂ bib ) 0 0 ϕb
˙
δV e −Ĉe S(f̂ b ) −2S(Ωe ) γe 0 0 Ĉeb Ĉeb L(f̂ b ) δVe
b ie
δP˙e
0 I 0 0 0 0 0
δPe
δb˙ ω =
0 0 0 0 0 0 0
δbω
δσ˙ ω
0 0 0 0 0 0 0
δσ ω
δb˙ f 0 0 0 0 0 0 0 δbf
δσ˙ f 0 0 0 0 0 0 0 δσ f
I 0 0 0 0 0 0 ξω
0 Ĉe I 0 0 0 0 ξf
b
0 0 0 0 0 0 0 ξg
+ 0 0 0 I 0 0 (3.35)
0
ξ bω
0 0 0 0 I 0 0 ξ σω
0 0 0 0 0 I 0 ξ bf
0 0 0 0 0 0 I ξ σf
siendo los ruidos Gaussianos que afectan el sistema:
ξω ruido en giróscopos
ξf ruido en acelerómetros
ξg ruido en parámetros de gravedad
ξ bω ruido en sesgo de giróscopos
ξ σω ruido en factores de no ortogonalidad y escala en giróscopos
ξ bf ruido en sesgo de acelerómetros
ξ σf ruido en factores de no ortogonalidad y escala en acelerómetros
3.2.5. Modelo de Observaciones
Las mediciones y el vector de estados están relacionados mediante
y(t) = C(t)δx(t) + η(t) (3.36)
54 Capítulo 3. Estimación de Errores
ϕ
δVe
e
δP
yP os 0 0 I 0 0 0 0 η P os
=
δbω + (3.37)
yV el 0 I 0 0 0 0 0 η V el
δσ ω
δbf
δσ f
donde y = yIN S − yGP S es la observación y los η son los ruidos que afectan las mediciones de posición
y velocidad. La matriz C(t) es constante, por lo que no es necesario recalcularla cada vez que aparece
una nueva medición.
3.2.6. Algoritmo del Filtro de Kalman
Teniendo las expresiones para cada una de las variables necesarias para implementar el Filtro de
Kalman, se da el algoritmo para el mismo. Las mediciones inerciales y salidas del INS se incorporan
en la propagación del estado, mientras que las mediciones del GPS actúan como observaciones del
mismo. Debe destacarse que no hay necesidad de que los ciclos de predicción y actualización del iltro
de Kalman tengan el mismo período, sino que, mientras no se tenga una nueva observación del estado,
se lo puede estimar mediante su dinámica. Por lo tanto, cada vez que el navegador inercial da un
dato como salida, se ejecuta el paso de predicción, y cuando llega un dato del receptor GPS, el de
actualización. Mientras la salida del navegador se actualiza cada 0, 1s, el dato del receptor GPS lo hace
cada 1 segundo o incluso más, si algún dato GPS no es válido; por este motivo, el paso de predicción
se realizará 10 ó más veces antes de que se realice el de actualización. Se indican estos ciclos internos
mediante los subíndices k y n, donde el primero corresponde a la recepción de un dato GPS y el
segundo a la actualización de la salida del navegador inercial.
1. Inicialización
Una vez de nido el vector de estados, se necesitan las condiciones iniciales para el mismo. Estas
vienen dadas p
δx0/0 = E [δx0 ]
que es el error esperado en el instante inicial,
h i
P0/0 = E δx0/0 δxT0/0
que es la covarianza del error inicial.
3.2. Modelo Dinámico del Error 55
2. Estimación a priori
Se calculan las matrices del modelo Bk,n y Fk,n en el instante tk,n . También es necesario calcular
la matriz de transición de estado
1
Φk,n = exp (Fk,n Ts ) w I + Fk,n Ts + F2k,n Ts2
2
donde Ts es el período entre datos del navegador inercial. Se comprobó que la aproximación de
primer orden para la matriz de transición de estados, comúnmente utilizada en la bibliografía,
no era lo su cientemente precisa. Por lo tanto, se utilizó una aproximación de segundo orden.
Con estas matrices se propagan el vector de estados y la matriz de covarianza para obtener sus
estimaciones a priori
δxk,n+1 = Φk,n δxk,n
Pk,n+1 = Φk,n Pk,n ΦTk,n + Bk,n QBTk,n Ts
Este paso se repite a una frecuencia 1/Ts , hasta que se reciba una nueva medición externa. En
ese instante se tendrá δxk+1/k y Pk+1/k .
3. Innovación
En el momento en que se recibe una observación independiente del estado yk+1 (medición GPS),
se calcula el valor predicho para la misma mediante
ŷk+1 = Ck+1 δxk+1
y luego se calcula la innovación δyk+1 = yk+1 − ŷk+1 .
4. Ganancia de Kalman
Se calcula Ck+1 para obtener la ganancia de Kalman
−1
Kk+1 = Pk+1/k CTk+1 Ck+1 Pk+1/k CTk+1 + Rk+1
donde Rk+1 es la covarianza del ruido de medición.
5. Actualización
Se corrige la estimación a priori mediante
δxk+1/k+1 = δxk+1/k + Kk+1 δyk+1
y se calcula la matriz de covarianza a posteriori
Pk+1/k+1 = (I − Kk+1 Ck+1 ) Pk+1/k
56 Capítulo 3. Estimación de Errores
3.3. Seguimiento del Error del Navegador Inercial
La función del Filtro de Kalman en el contexto de la navegación es estimar errores de forma óptima.
Recordando lo mostrado en el Capítulo 2, al utilizar instrumentos ruidosos el error en las variables de
navegación computadas inercialmente se hace muy importante. Se muestra en las guras el resultado
de ejecutar el Filtro de Kalman en comparación con el error cometido por el navegador inercial. En la
tabla 3.1 se dan los valores utilizados para la simulación, consistentes con los instrumentos disponibles
para el vehículo.
Giróscopo Acelerómetro Actitud Velocidad Posición
Incertidumbre Inicial - - 1 0,5 m/s 20 m
°
Ruido 0,4 /s° 5 mg - 0,5 m/s 20 m
Inestabilidad en Sesgo 0,007 /s ° 0,1 mg - - -
Inestabilidad en Factor de Escala 0,01 % 0,01 % - - -
Inestabilidad en Factor de No Ortogonalidad 0,087 % 0,087 % - - -
Cuadro 3.1: Valores de parámetros (1σ )
Se considera a la matriz de covarianza de la observación Rk constante, salvo en el caso de que se
disponga de información más precisa provista por el receptor GPS. En este caso, Rk guardará relación
con la matriz de Dilución de la Precisión (DOP) calculada en el receptor, que depende fuertemente de
la geometría de la constelación de satélites GPS en vista [12, 15].
Figura 3.1: Error en Actitud (ángulo vectorial)
3.3. Seguimiento del Error del Navegador Inercial 57
Figura 3.2: Error en Velocidad
Figura 3.3: Error en Posición
A pesar de inicializar el vector de estados en cero, el Filtro de Kalman identi ca rápidamente los
errores en posición, velocidad y actitud, siguiéndolos a lo largo de la trayectoria del vehículo. Se puede
ver que en el caso del eje z del ángulo vectorial de error, no es posible identi carlo con precisión. El
error en actitud es observado mediante el error en el vector velocidad, que por lo general es invariante
58 Capítulo 3. Estimación de Errores
ante una rotación axial, o en la dirección del movimiento del mismo.
Se espera que la estimación de los errores en las variables de navegación por medio de su modelo
linealizado sea coincidente con los errores cometidos por el algoritmo de navegación inercial, siempre
que éste no incorpore errores signi cativos debidos a la integración de las ecuaciones. Teniendo esto en
cuenta, basta con corregir las variables para obtener una mejor calidad en la navegación.
3.4. Presupuesto de Errores
El análisis de la covarianza de cada incertidumbre del modelo permite identi car factores domi-
nantes y su comportamiento a lo largo de una determinada trayectoria. Se desarrolla en esta sección
un sistema de ecuaciones lineal respecto de las incertidumbres del proceso descripto en las secciones
anteriores. De esta forma, se analiza el comportamiento de las diversas fuentes de error por separado,
obteniendo su contribución individual a la incertidumbre total en la estimación del vector de estados.
A partir de esta información es posible determinar los pasos a seguir para mejorar la performance
del sistema, como por ejemplo eliminar estados que no in uyan signi cativamente, o alternativamente
obtener sensores de mejor calidad que provean datos con menor ruido.
3.4.1. Desarrollo de las Ecuaciones
Siguiendo el desarrollo presentado en [9], el modelo de tiempo discreto del proceso a estimar es
xk+1 = Φk xk + Bk ξ k
(3.38)
yk+1 = Ck+1 xk+1 + η k+1
Lo que se utiliza en realidad es una estimación de cada una de las variables presentes en el modelo, que
no son conocidas a la perfección. Esto da lugar a un estimador, que no incluye necesariamente todos
los estados originales
x̂k+1 = Φ̂k x̂k + B̂k ξ̂ k
(3.39)
ŷk+1 = Ĉk+1 x̂k+1 + η̂ k+1
T
P̂k+1/k = Φ̂k P̂k/k Φ̂k + B̂k Q̂B̂Tk
−1
K̂k+1 = P̂k+1/k ĈTk+1 Ĉk+1 P̂k+1/k ĈTk+1 + R̂k+1 (3.40)
P̂k+1/k+1 = I − K̂k+1 Ĉk+1 P̂k+1/k
Se considera el cálculo de la varianza del error δx = Vx − x̂. Por lo general, se da que dim(x) >
dim(x̂), como resultado de la imposibilidad de incluir todos los estados que dan lugar a la dinámica
real del sistema. La matriz V elige los estados que se incluyen en el estimador.
3.4. Presupuesto de Errores 59
Se puede conformar el sistema completo de la siguiente forma:
xk+1/k Φk 0 xk/k Bk
= + ξk (3.41)
x̂k+1/k 0 Φ̂k x̂k/k 0
xk+1/k
yk+1 = Ck+1 0 + η k+1 (3.42)
x̂k+1/k
xk+1/k
ŷk+1 = 0 Ĉk+1 (3.43)
x̂ k+1/k
xk+1/k+1 xk+1/k 0
= + (yk+1 − ŷk+1 ) (3.44)
x̂k+1/k+1 x̂k+1/k K̂k+1
xk+1
δxk+1 = V −I (3.45)
x̂k+1
Estas expresiones representan la propagación en el tiempo del sistema real y del estimado. Debido
a que la estimación se calcula de forma exacta no se incluyen términos de ruido en las las correspon-
dientes en las ecuaciones 3.41 y 3.43. Además, los ceros en la matriz de ganancia demuestra que las
correcciones no tienen efecto sobre el estado real del sistema.
La covarianza del error en la estimación para el caso general viene dada por
Cov(δx) = M = E δxδxT
(3.46)
T
x x
= E V −I V −I (3.47)
x̂ x̂
T
x V
= V −I E xT x̂T (3.48)
x̂ −I
T T T
xx xx̂ V
= V −I E (3.49)
x̂xT x̂x̂T −I
Desarrollando la esperanza utilizando las ecuaciones 3.41 y 3.44 se calculan las covarianzas del error a
priori y a posteriori, respectivamente.
60 Capítulo 3. Estimación de Errores
[Link]. Covarianza a priori
Reemplazando la ecuación 3.41 y considerando procesos descorrelacionados, se desarrolla el término
de la esperanza
xk+1/k P11k+1/k P12k+1/k
Mk+1/k = = E xTk+1/k x̂Tk+1/k = (3.50)
x̂k+1/k P21k+1/k P22k+1/k
Separando los elementos de la matriz, se obtienen las ecuaciones que propagan la covarianza del error
en cada caso:
P11k+1/k = Φk P11k/k ΦTk + Bk Qk BTk (3.51)
T
P12k+1/k = Φk P12k/k Φ̂k (3.52)
T
P22k+1/k = Φ̂k P22k/k Φ̂k (3.53)
[Link]. Covarianza a posteriori
Reemplazando la ecuación 3.44 y considerando procesos descorrelacionados, se desarrolla el término
de la esperanza
xk+1/k+1 P11k+1/k+1 P12k+1/k+1
Mk+1/k+1 = E xTk+1/k+1 x̂Tk+1/k+1 = (3.54)
x̂k+1/k+1 P21k+1/k+1 P22k+1/k+1
Desarrollando cada término se obtienen las ecuaciones que propagan la covarianza del error a posteriori
en cada caso:
P11k+1/k+1 = P11k+1/k (3.55)
T
P12k+1/k+1 = P12k+1/k I − K̂k+1 Ĉk+1 + P11k+1/k CTk+1 K̂Tk+1 (3.56)
T
P22k+1/k+1 = I − K̂k+1 Ĉk+1 P22k+1/k I − K̂k+1 Ĉk+1 + K̂k+1 Ck+1 P11k+1/k CTk+1 K̂Tk+1
T
+K̂k+1 Rk+1 K̂Tk+1 + K̂k+1 Ck+1 P11k+1/k I − K̂k+1 Ĉk+1
+ I − K̂k+1 Ĉk+1 P21k+1/k CTk+1 K̂Tk+1 (3.57)
3.4.2. Implementación
Cuando sea razonable suponer que Φ̂k = Φk , Ĉk+1 = Ck+1 (la estimación de estas matrices sea
su cientemente precisa), y en el caso en que el modelo contenga los estados necesarios para modelar
adecuadamente el proceso,entonces V = I, es decir dim(x) = dim(x̂). Por lo tanto, el estado de error
queda de nido como δx = x− x̂, y las ecuaciones que propagan la covarianza de este error se simpl can
3.4. Presupuesto de Errores 61
notablemente
Mk+1/k = Φk Mk/k ΦTk + Bk Qk BTk (3.58)
Mk+1/k+1 = (I − Kk+1 Ck+1 ) Mk+1/k (I − Kk+1 Ck+1 )T + Kk+1 Rk+1 Kk+1 (3.59)
M = E δxδxT .
donde Ya que estas ecuaciones son lineales en los términos de error (M, Q y R), es
válido el principio de superposición [9], lo que permite analizar la contribución de cada incertidumbre
por separado. Este análisis requiere conocer el valor de la matriz de ganancias K para cada observación,
que se obtiene de una simulación que incorpore todas las incertidumbres.
Considerando que las matrices que involucran ruidos o incertidumbres son diagonales, las podemos
PNq PNr PNm
escribir como Q= i=1 Qi , R= i=1 Ri y M(0) = i=1 Mi (0), donde cada matriz Ai contiene un
sólo término de incertidumbre. Se realizará una simulación por cada una de estas matrices (Nq +Nr +Nm
en total), igualando las otras a cero. Esto produce como salida Nq + Nr + Nm secuencias de matrices
de covarianza, cada una dependiente de una única fuente de error.
Para mayor claridad, en los grá cos se han separado las fuentes de error en 4 categorías
1. Ruido blanco en giróscopos y acelerómetros
2. Ruido blanco en mediciones de posición y velocidad GPS
3. Incertidumbre en la orientación, velocidad y posición iniciales del vehículo
4. Incertidumbre en los sesgos, factores de escala y factores de no ortogonalidad de giróscopos y
acelerómetros
Los valores para los parámetros de error instrumentales, de GPS e incertidumbres en el estado inicial
son iguales a los dados en la tabla 3.1.
[Link]. Errores en Actitud
Para los ejes xey del ángulo vectorial de error ϕb ( guras 3.4 y 3.5 respectivamente), la in uencia
de las diversas fuentes de incertidumbre es similar. El error en actitud inicial es el dominante, pero se
extingue rápidamente luego del inicio del vuelo. También decrecen los errores en velocidad incial, que,
a pesar de no tener una in uencia tan importante, son corregidos. El error inicial en el eje z ( gura
3.6) del ángulo vectorial de error es el único remanente, que tiene una pequeña in uencia en el error
en actitud a lo largo de la trayectoria (menor a 0,1°).
62 Capítulo 3. Estimación de Errores
Figura 3.4: Errores en ϕx
Figura 3.5: Errores en ϕy
Una vez que desaparece la in uencia de la incertidumbre inicial sobre las componentes x e y del
error en actitud, este queda dominado por el ruido blanco en los giróscopos en primer lugar, y por
el ruido en la medición de velocidad del GPS en segundo. El ruido en los acelerómetros tiene menor
in uencia.
3.4. Presupuesto de Errores 63
El ruido en la medición de posición del GPS y los sesgos en los giróscopos tienen una incidencia un
orden de magnitud menor al de las variables analizadas anteriormente, y similar entre sí. El sesgo en
los acelerómetros es el segundo factor instrumental con in uencia notable. Se observa que los demás
parámetros instrumentales, tales como factores de escala y no ortogonalidad tienen un efecto apenas
apreciable en el error en actitud.
Figura 3.6: Errores en ϕz
El análisis es similar para el eje z del ángulo vectorial de error ϕb , aunque el error inicial en actitud
en este eje no es directamente observable, y por lo tanto decrece lentamente, comparado con los ejes xe
y . El ruido en los giróscopos sigue siendo igualmente dominante, pero con una magnitud mayor. El error
debido al ruido en la medición de velocidad GPS tiene una incidencia un poco mayor, aunque similar a
los otros ejes. El mayor cambio, sin embargo, se produce en la in uencia de los errores instrumentales,
siendo los sesgos y factores de escala de los giróscopos los más importantes. Esto se debe a que, al no
poder observar el error en actitud en el eje z mediante la medición de velocidad, este queda fuertemente
in uenciado por la calidad de las mediciones inerciales.
[Link]. Errores en Velocidad
Para el análisis del error en velocidad y de la in uencia de los distintos factores sobre éste, es impor-
tante tener en cuenta que la medición de la fuerza especí ca debe ser compensada con la estimación del
vector gravedad en ese punto. Es por esto que factores como el error en la estimación de la orientación
64 Capítulo 3. Estimación de Errores
del vehículo respecto de la tierra y el error en su posición tendrán un efecto notable en la calidad de
dicha compensación.
Figura 3.7: Errores en Vx
Figura 3.8: Errores en Vy
Se puede observar nuevamente que los errores en condiciones iniciales se extinguen rápidamente,
quedando como remanente el error inicial en actitud en el eje z de ϕb ( gura 3.9). El error en velocidad
3.4. Presupuesto de Errores 65
inicial es el factor dominante, pero se hace despreciable dos segundos después de iniciado el vuelo
( guras 3.7 y 3.8). Los errores iniciales en actitud son los factores dominantes secundarios.
Figura 3.9: Errores en Vz
Durante el vuelo, una vez eliminado el efecto de los errores inciales, los factores más importantes
resultan ser el ruido en la medición de velocidad GPS y el ruido en los giróscopos, siendo sus magnitudes
similares. Resulta poco intuitivo en este caso que el ruido en los giróscopos tenga mayor incidencia que
el de los acelerómetros, resultado que adquiere mayor sentido cuando se considera la compensación del
vector gravedad, que depende de la información de actitud.
El tercer factor determinante del error en velocidad es el ruido en los acelerómetros, y detrás de
este factor, se encuentran el ruido en la medición de posición GPS y las inestabilidades en parámetros
instrumentales, de los cuales los sesgos en los giróscopos y el factor de escala de los acelerómetros son
los más importantes, seguidos por los sesgos en acelerómetros.
[Link]. Errores en Posición
El caso más claro para analizar resulta ser el de los errores en posición, debido al claro dominio de
unos pocos factores sobre esta variable. Como era de esperarse, el error inicial en posición se extingue
en el tiempo, dando paso al dominio del ruido en la medición de posición del GPS, al que se suma el
error en la medición de velocidad.
66 Capítulo 3. Estimación de Errores
Figura 3.10: Errores en Px
Figura 3.11: Errores en Py
En un grado mucho menor, hay una in uencia del ruido en los acelerómetros, seguido por el de los
giróscopos. Los parámetros instrumentales tienen un efecto prácticamente despreciable sobre el error
en posición, siendo el factor de escala y el sesgo en los acelerómetros los factores destacables, seguidos
con una mangitud mucho menor los sesgos en los giróscopos.
3.4. Presupuesto de Errores 67
Figura 3.12: Errores en Pz
3.4.3. Reducción de la In uencia del Ruido
El análisis de los resultados arrojados por el presupuesto de errores indica consistentemente que los
factores con mayor in uencia en el error en la navegación de la trayectoria prevista son el ruido en los
instrumentos inerciales y en el GPS. Debido a que el efecto de la incertidumbre en los valores iniciales
de las variables de navegación se extingue en el tiempo, y que, en todo caso pueden ser mejoradas
mediante una mejor alineación, resulta interesante notar que una mayor calidad en las variables de
navegación en el largo plazo está estrechamente ligada a la reducción de los niveles de ruido en los
instrumentos.
Considerando que la IMU de la que se dispone tiene la capacidad de proveer datos hasta una
frecuencia máxima de 819,2Hz cuando el algoritmo de navegación inercial los requiere a 100Hz, y
tomando el resultado anterior, surge la posibilidad de promediar los datos inerciales provistos pos la
misma. De esta forma, se promedia asimismo el ruido en la medición, reduciendo su impacto en la
calidad de las variables de navegación.
El promedio de un número N de muestras sucesivas xi de un proceso de ruido blanco Gaussiano
de media nula y potencia σ2 da como resultado un proceso de potencia
N N
" #
1 X 1 X 1 1
σ 2avg = Var xi = 2 Var [xi ] = 2 N σ 2 = σ 2
N N N N
i=1 i=1
√
Por lo tanto, el desvío del ruido queda reducido en un factor 1/ N . Aplicando este promediado a
68 Capítulo 3. Estimación de Errores
los datos de la IMU con una frecuencia de 800Hz, se tiene N = 8, lo que da una reducción en el
desvío del ruido de 0,3536. Los nuevos valores para los desvíos del ruido de giróscopos y acelerómetros
son de 0,1414°/s y 1,7678mg respectivamente, y son los valores que se utilizarán para el resto de las
simulaciones.
Lamentablemente este promediado no puede realizarse sobre los datos provistos por el receptor
GPS, debido a su baja tasa de actualización. Por esta razón, los valores indicados para los ruidos de
medición de posición y velocidad permanecen iguales. Se muestran los nuevos resultados del presupuesto
de error con los valores de ruido actualizados en el Apéndice B, donde se observa un comportamiento
similar para las fuentes de error, pero con una magnitud menor.
3.4.4. Eliminación de Estados
Otro resultado interesante del análisis de los resultados del presupuesto de errores es que la in-
uencia de la mayoría de las inestabilidades en parámetros instrumentales son despreciables frente a
la de los demás factores. Los parámetros instrumentales más in uyentes resultan ser el sesgo en los
giróscopos y el factor de escala en los acelerómetros, particularmente sobre el eje z, seguido por el
sesgo.
Teniendo en cuenta la capacidad de cómputo limitada disponible durante el vuelo para realizar
cálculos en tiempo real, parece natural efectuar una reducción en la cantidad de estados considerados,
particularmente los correspondientes a los parámetros instrumentales con menor in uencia en la calidad
de los datos de navegación. Los estados eliminados corresponderían en principio a los factores de no
ortogonalidad de giróscopos y acelerómetros, y adicionalmente los factores de escala de los giróscopos.
Esta idea será tenida en cuenta en los análisis subsiguientes, evaluando la posibilidad de quitar estos
estados y otros que no sean observables. Sin embargo debe recordarse que los efectos de los errores
instrumentales son despreciables frente a los ruidos de medición, por lo que la decisión no tendrá un
gran impacto cuantitativo.
Capítulo 4
Navegación Integrada
Incorporando las técnicas descriptas en los capítulos anteriores, se está en condiciones de construir
el algoritmo de navegación integrado. El objetivo nal es acotar el crecimiento inde nido de los errores
cometidos por el navegador inercial por su cuenta. Para esto, se construyó en el capítulo anterior un
modelo lineal para la dinámica del error, que describe la propagación del mismo a través del algoritmo
de navegación inercial. Mediante la observación de datos adicionales, como en este caso de un recptor
GPS, es posible además identi car incertidumbres en el estado inicial, así como errores instrumentales.
Se describe en este capítulo la forma de combinar la estimación del error efectuada por el Filtro de
Kalman con los datos de salida del navegador inercial para cumplir con el objetivo.
Se reserva una sección al estudio del caso, más cercano a la realidad, en que el dato provisto por el
receptor GPS llega con un retardo que puede ser signi cativo. Se estudia el impacto de este retardo y
se analiza y simula una solución a este problema. Finalmente, se estudian los tiempos de ejecución del
algoritmo de navegación corriendo sobre el hardware previsto para el vuelo.
4.1. Corrección de Variables de Navegación
El resultado de ejecutar el Filtro de Kalman es una estimación del error cometido en el cálculo
de las variables de navegación, y adicionalmente, en los parámetros de error de los instrumentos. El
siguiente paso es utilizar esta estimación para corregir los datos. La forma de hacerlo dependerá de la
de nición de error que se haya utilizado.
4.1.1. Actitud
Para el caso de la actitud, se recuerda que una de las de niciones utilizadas es
Ĉeb = Ceb (I + S(ϕb ))
69
70 Capítulo 4. Navegación Integrada
∼ b
= Cebreal Cbreal
IN S
siendo la aproximación válida siempre que el ángulo vectorial de error ϕb sea un ángulo pequeño.
Asumiendo que la condición anterior se da, el equivalente de esta de nición para cuaterniones es
q̂ebIN S = qebreal ◦ qbbreal
IN S
Se recuerda que q̂ebIN S es el cuaternión que da el INS como salida, qbbreal
IN S
= q(ϕb ) es la pequeña
rotación que se estima mediante el Filtro de Kalman y qebreal es el cuaternión real. Con esto en cuenta,
sólo resta despejar de la ecuación la expresión para el cuaternión real:
∗
qebreal = q̂ebIN S ◦ qbbreal
IN S
Esta es la variable que se usa de aquí en adelante, hasta que se corrija el estado nuevamente. Después
de hacer esto es necesario establecer en el algoritmo que el error cometido es ahora nulo (ϕ
b = 0), ya
que ha sido compensado.
4.1.2. Posición y Velocidad
Para las variables de posición y velocidad, la de nición de error es:
δu = û − u (4.1)
siendo û la última estimación disponible de un determinado estado y δu el valor estimado mediante el
Filtro de Kalman para el error en ese estado. En este caso, la corrección es directa:
u = û − δu (4.2)
4.1.3. Parámetros Instrumentales
Se han de nido los errores en parámetros instrumentales (sesgos, factores de escala y de no ortog-
onalidad) según:
δu = u − û (4.3)
Por lo tanto, la corrección de estas variables se realiza mediante
u = û + δu (4.4)
4.2. Algoritmo de Navegación Integrado 71
4.2. Algoritmo de Navegación Integrado
Ya se dieron todos los elementos necesarios para la implementación del navegador integrado. Debe
mecionarse que, dado que el vector de estados contiene la estimación del error que comete el INS,
luego de corregir las variables de navegación, debe forzarse δx = 0. Esto se debe a que 0 es la mejor
estimación del error en el INS inmediatamente después de ser corregido. En caso de que la corrección
se realice cada vez que hay una observación disponible, no hay razón para realizar la propagación del
vector de estados mediante la matriz de transición, ya que el primero siempre permanecerá en cero.
Además, el paso de actualización del vector de estados queda simpli cado de la siguiente form
δxk+1/k+1 = δxk+1/k + Kk+1 yk+1 − Ck+1 δxk+1/k (4.5)
= Kk+1 yk+1 (4.6)
Sin embargo, no se elimina este paso del algoritmo debido a que representa el caso más general, siendo
la corrección de variables de navegación un paso que no necesariamente se ejecute periódicamente.
Algoritmo 4.1 Navegador Integrado INS/GPS
Inicializar Variables del Navegador (Pe , Ve , qeb )
Inicializar Variables del Kalman (ϕb , δVe , δPe , δbω , δσ ω , δbf , δσ f , P0/0 )
While(1)
{
Ejecutar Navegador Inercial (calcula Pe , Ve , qeb )
Calcular Fk,n , Bk,n y Φk,n
Propagar el estado δxk,n+1 = Φk,n δxk,n
Propagar la matriz de covarianza Pk,n+1 = Φk,n Pk,n ΦTk,n + Bk,n QBTk,n Ts
if(Datos PGP S y VGP S disponibles)
{
Calcular la Observación del error yk+1 = yk+1
IN S − yGP S
k+1
−1
Calcular la ganancia de Kalman Kk+1 = Pk+1/k CTk+1 Ck+1 Pk+1/k CTk+1 + Rk+1
Corregir la estimación del error δxk+1/k+1 = δxk+1/k + Kk+1 yk+1 − Ck+1 δxk+1/k
Calcular la covarianza de la nueva estimación Pk+1/k+1 = (I − Kk+1 Ck+1 ) Pk+1/k
Corregir las variables de navegación y parámetros instrumentales
(Pe ,Ve ,qeb ,bω ,σ ω ,bf ,σ f )
y resetear el vector de estados (δx = 0)
}
}
72 Capítulo 4. Navegación Integrada
4.2.1. Navegación de la Trayectoria
El efecto de corregir los datos con la estimación del error es una navegación mucho más precisa en
presencia de ruidos e incertidumbres iniciales. Utilizando el navegador inercial de precisión y corrigiendo
las variables de navegación con la estimación del error provista por el Filtro de Kalman, se implementa
un navegador integrado INS/GPS.
Figura 4.1: Error en Actitud
Figura 4.2: Error en Velocidad
4.2. Algoritmo de Navegación Integrado 73
Las guras 4.1, 4.2 y 4.3 muestran los errores en actitud, velocidad y posición del navegador
integrado. Estos errores resultan de navegar el mismo juego de datos utilizado en la sección 2.7 para
ilustrar el efecto de ruidos y errores instrumentales en una IMU. Se observa una marcada mejoría en
la calidad de las variables de navegación, oscilando en torno a cero con una determinada con abilidad,
dada por la covarianza de la estimación del Filtro de Kalman. Adicionalmente, se corrigen rápidamente
los errores en actitud y posición inicial, algo que no era posible utilizando simplemente el navegador
inercial.
Figura 4.3: Error en Posición
Las innovaciones correspondientes a la observación de los errores en velocidad y posición deben
estar descorrelacionadas, ya que representan una sucesión de vectores ortogonales entre sí. Se muestra
del lado izquierdo de las guras 4.4 y 4.5 los procesos de innovaciones, mientras que del lado derecho se
gra can las funciones de autocorrelación de los mismos. Se observa que las funciones de autocorrelación
son similares a una función delta, típica de un proceso de ruido blanco descorrelacionado.
El análisis de las innovaciones es una herramienta importante para saber si el Filtro de Kalman
está funcionando correctamente. Cualquier inconsistencia en los valores elegidos para los procesos de
ruido y observación puede manifestarse modi cando el esquema anterior, obteniendo una función de
autocorrelación muy diferente.
74 Capítulo 4. Navegación Integrada
Figura 4.4: Innovaciones en Velocidad
Figura 4.5: Innovaciones en Posición
En las guras 4.6 y 4.7 se muestran las covarianzas teóricas de los errores instrumentales calculadas
por el Filtro de Kalman. Debe notarse que los parámetros para los que es posible observar el error ven
su desvío reducido cada vez que se efectúa una actualización del Filtro de Kalman. Los parámetros
que no es posible observar mediante la medición del error en posición y velocidad tienen un desvío
4.2. Algoritmo de Navegación Integrado 75
que crece en el tiempo debido al ruido de proceso. La observabilidad de los parámetros depende de
la matriz de la dinámica del proceso y de la de su observación. Como se mencionó en la Sección 3.3,
la matriz de observaciones se considera constante en el tiempo, por lo tanto la observabilidad de los
parámetros dependerá fuertemente de la trayectoria elegida.
Figura 4.6: Desvío del Error en la Estimación del Bias de los Giróscopos (°/s)
Figura 4.7: Desvío del Error en la Estimación del Bias de los Acelerómetros (m/s )
2
76 Capítulo 4. Navegación Integrada
El sesgo en los giróscopos es observable en todos sus ejes, y por ende el desvío en su estimación se
reduce con el tiempo. Por otro lado, el sesgo en los acelerómetros es observable solamente en el eje z,
mientras que en los ejes x e y el desvío crece.
Figura 4.8: Desvío del Error en la Estimación del Factor de Escala de los Giróscopos
Figura 4.9: Desvío del Error en la Estimación del Factor de Escala de los Acelerómetros
El factor de escala de los giróscopos ( gura 4.8) no es observable en niguno de sus ejes durante
4.2. Algoritmo de Navegación Integrado 77
el vuelo impulsado. Sin embargo, a partir de los 60 segundos, el desvío de la estimación en el eje z
comienza a decrecer. Esto apoya la idea de eliminar estos estados del vector de estados, ya que no
aportan información útil durante el vuelo (de hecho, se naliza con una incertidumbre aún mayor que
en los instantes iniciales). El factor de escala de los acelerómetros ( gura 4.9) es observable únicamente
en el eje z.
Figura 4.10: Desvío del Error en la Estimación del Factor de No Ortogonalidad de los Giróscopos
Figura 4.11: Desvío del Error en la Estimación del Factor de No Ortogonalidad de los Acelerómetros
78 Capítulo 4. Navegación Integrada
En el caso de los factores de no ortogonalidad de giróscopos y acelerómetros, mostrados en las
guras 4.10 y 4.11 respectivamente, se evidencia un crecimiento lineal del desvío en la estimación,
debido a la no observabilidad de estos parámetros.
El análisis de los datos obtenidos mediante la navegación integrada con rma la idea de elminar
estados no observables y con poca in uencia en el error nal. Se ha con rmado que los factores de no
ortogonalidad en giróscopos y acelerómetros no son observables, y que, además, no implican un factor
determinante sobre el error nal. De este modo, resulta natural desecharlos del vector de estados,
ganando simplicidad en el Filtro de Kalman y tiempo de cálculo. Por otro lado, los factores de escala
de los giróscopos no son observables durante el vuelo, razón por la cual conviene también descartarlos.
El factor de escala de los acelerómetros tiene mayor in uencia sobre el error que el sesgo, y por
ende resultaría evidente la necesidad de incluir el primero en el vector de estados. Sin embargo, la
dependencia del sesgo con la temperatura, voltaje de operación, y otros factores hace del mismo un
parámetro mucho más variable que el factor de escala [1]. Es por esto que resulta más seguro incluirlo
en el vector de estados, dejando fuera el factor de escala, que tiene un valor más estable. A partir de
este momento, el nuevo modelo dinámico del error viene dado por las siguientes matrices:
ϕ̇b −S(ω̂ bib ) 0 0 I 0 ϕb
˙e −Ĉeb S(f̂ b ) −2S(Ωeie ) γe Ĉeb
δVe
δV 0
˙e =
δP 0 I 0 0 e
0 δP
˙
δbω 0 0 0 0 0 δbω
δb˙ f 0 0 0 0 0 δbf
I 0 0 0 0 ξω
e
0 Ĉb I 0 0 ξf
+ 0 0 0 0 0
ξg
(4.7)
0 0 0 I 0 ξ bω
0 0 0 0 I ξ bf
quedando modi cada la matriz de observaciones de la siguiente forma:
4.2. Algoritmo de Navegación Integrado 79
ϕb
e
δV
yP os 0 0 I 0 0 η P os
= e
δP +
(4.8)
yV el 0 I 0 0 0 η V el
δbω
δbf
4.2.2. Navegación Integrada con INS Euler
En la sección 2.5 se implementó un navegador inercial de simple. Debido a la magnitud del error
cometido, no resultaba atractivo para una aplicación real. Sin embargo, al utilizarlo dentro del contexto
de la navegación integrada, su performance mejora notablemente. Aplicando el Filtro de Kalman para
estimar el error cometido por el navegador inercial y utilizando esta información para corregirlo, es
posible lograr una performance prácticamente idéntica a la del navegador integrado basado en un
navegador inercial de precisión.
En las guras 4.12 a 4.14 se comparan los errores en actitud, velocidad y posición de navegadores
integrados que utilizan como base los algoritmos inerciales mencionados. Se ha navegado exactamente
el mismo conjunto de datos para la comparación. Los resultados son casi idénticos para los tres tipos
de error, mostrando la robustez del sistema implementado.
Figura 4.12: Error en Actitud
80 Capítulo 4. Navegación Integrada
Figura 4.13: Error en Velocidad
Figura 4.14: Error en Posición
4.2.3. Alineación
A pesar de que en la aplicación en consideración la posición y actitud del vehículo antes del vuelo
pueden ser conocidas, inicializando el algoritmo de navegación con esos valores, existen pequeños
desvíos que es conveniente identi car. La alineación es el proceso por el cual se determina la posición
4.2. Algoritmo de Navegación Integrado 81
y orientación de los ejes del sistema de navegación respecto de la terna de referencia. Resulta crucial
una correcta alineación si se ha de navegar inercialmente, ya que los errores iniciales se propagan en
el tiempo, degradando la calidad de la información. En este caso, es común utilizar la técnica de gyro-
compassing, dependiente de las mediciones de fuerza especí ca y velocidad angular en estado de reposo
sobre la super cie terrestre. Sin embargo, esto requiere de giróscopos de muy buena calidad, capaces
de medir la velocidad angular de la tierra, que no están disponibles en esta aplicación.
Considerando el sistema de navegación integrada de bajo costo propuesto, se pueden incorporar
las mediciones del receptor GPS para identi car y corregir errores iniciales en posición y actitud.
Adicionalmente, es posible identi car parcialmente los sesgos en la IMU, lo que reduce su impacto
durante el vuelo.
En la gura 4.15 se ve que el error en actitud (inicialmente de 1° en cada eje) es identi cado y
corregido inmediatamente en los ejes x e y , mientras que en z este error no es observable y por lo tanto
se propaga en el tiempo. Esto se debe a que la actitud es determinada mediante la acción del vector
gravedad, que es invariante ante una rotación respecto del eje vertical.
Las incertidumbres iniciales en velocidad y posición ( guras 4.16 y 4.17) son también identi cadas
rápidamente, mientras que la covarianza del error en la estimación de estas variables se estabiliza en
valores menores a los iniciales.
Figura 4.15: Error en Actitud
82 Capítulo 4. Navegación Integrada
Figura 4.16: Error en Velocidad
Figura 4.17: Error en Posición
Por otro lado, las desviaciones en los sesgos del giróscopo en los ejes xey son identi cados, aunque
en un tiempo considerable ( gura 4.18). El sesgo en el eje relacionado con la medición del ángulo de
rolido tampoco es observable, por lo que su valor no se ve modi cado signi cativamente.
4.2. Algoritmo de Navegación Integrado 83
Figura 4.18: Bias de Giróscopos
Figura 4.19: Desvío del Error en la Estimación de Bias de Giróscopos
En el caso de los acelerómetros ( gura 4.19), el único sesgo observable es el del eje z, que en la
orientación inicial mide el efecto del vector gravedad (o más precisamente, la fuerza normal del suelo
en ese punto de la tierra).
84 Capítulo 4. Navegación Integrada
Figura 4.20: Bias de Acelerómetros (m/s )
2
Figura 4.21: Desvío del Error en la Estimación de Bias de Acelerómetros (m/s )
2
En el cuadro 4.1 se muestran los desvíos nales en la estimación del error alcanzados por el Filtro
de Kalman. Se han considerado únicamente las variables observables. Puede verse que se obtiene un
desvío del error signi cativamente menor a los valores de error iniciales. El caso más notable es el de
la posición, en que se logra un desvío del 15 % del valor original, mientras que en los sesgos el desvío
4.2. Algoritmo de Navegación Integrado 85
ronda el 50 % del valor inicial.
Parámetro Desvío Inicial Desvío Final
Actitud (ejes x e y) 1,0 ° 0,35 °
Velocidad 0,5 m/s 0,21 m/s
Posición 20,0 m 3,1 m
Sesgo de Giróscopo (ejes x e y) 0,007 °/s 0,0042 °/s
Sesgo de Acelerómetros (eje z) 0,1 mg 0,0556 mg
Cuadro 4.1: Desvíos Finales del Error en la Estimación
4.2.4. Pérdida de Datos GPS
Para analizar el comportamiento del navegador en circunstancias adversas, se contempla la situación
en que el receptor GPS no provee datos de salida a la frecuencia especi cada. Para simular este
escenario, se incorpora una probabilidad de pérdida de datos del GPS del 50 %, lo que da origen a
una secuencia no periódica. El navegador integrado actualiza la estimación del error en el momento en
que se obtiene una medición GPS válida, mientras que, cuando no hay datos disponibles, se propaga
la matriz de covarianza incorporando el ruido de proceso. Esto puede observarse en las guras 4.22 a
4.24, en que la covarianza del error aumenta en los períodos en que no hay datos GPS disponibles,
efecto particularmente notable para el error en velocidad ( gura 4.23).
Figura 4.22: Error en Actitud
A pesar de contar con la mitad de las mediciones disponibles durante el vuelo, se observa que el
error se mantiene dentro de su cota de 2σ , excepto para el caso del eje x de la posición. Los niveles de
86 Capítulo 4. Navegación Integrada
error son similares a los obtenidos sin pérdida de datos.
Figura 4.23: Error en Velocidad
Figura 4.24: Error en Posición
Adicionalmente, el análisis de la autocorrelación de los procesos de innovaciones ( guras 4.25 y 4.26)
demuestra que la información incorporada es independiente. Esto indica que el sistema de navegación es
robusto ante pérdidas signi cativas de información externa, e incorpora adecuadamente la información
4.2. Algoritmo de Navegación Integrado 87
a la estimación cuando esta información está disponible.
Figura 4.25: Innovaciones en Velocidad
Figura 4.26: Innovaciones en Posición
88 Capítulo 4. Navegación Integrada
4.3. Simulación de Monte Carlo
Debido a que los errores en los datos de la IMU y GPS son aleatorios, la simulación de la navegación
de una determinada trayectoria dependerá de cada incerteza y proceso de ruido presentes. Al comparar
la trayectoria navegada con la ideal, se obtiene un grá co del error cometido en la navegación. Una sola
realización de esta simulación no es su ciente para establecer cotas para los errores cometidos, por lo
que se recurre a una simulación de Monte Carlo para obtener resultados con signi cancia estadística.
Se realiza un número alto de simulaciones, utilizando datos de IMU, GPS, y condiciones iniciales
alterados aleatoriamente, buscando cubrir todo el rango de posibilidades y combinaciones. El análisis
del conjunto de trayectorias obtenido permite obtener datos importantes acerca de la con abilidad del
sistema de navegación.
4.3.1. Análisis de los Datos
Se simuló un total de 300 trayectorias, analizando luego la media del error cometido y su desvío,
y comparándolos con los estimados por el Filtro de Kalman. Los estados analizados mediante este
método son los errores en actitud, velocidad y posición.
El análisis se realiza estimando la media según
N
1 X
µx = xi (4.9)
N
i=1
y el desvío usando el estimador insesgado
v
u N
u 1 X
σx = t (xi − µx )2 (4.10)
N −1
i=1
Se gra ca la media en azul y la cota de 2 desvíos en rojo para el resultado de las simulaciones.
4.3. Simulación de Monte Carlo 89
Figura 4.27: Errores en Actitud
Figura 4.28: Errores en Velocidad
Se puede ver que las medias de los diferentes errores se mantienen en cero, que es el comportamiento
esperado para el navegador. La gura 4.27 muestra un error en actitud que se mantiene dentro de los
0,3° para los ejes x e y, con un intervalo de con anza del 95 %. El error en velocidad tiene una cota de
2σ que ronda los 0,5m/s, y el de posición converge lentamente a 8m ( guras 4.28 y 4.29).
90 Capítulo 4. Navegación Integrada
Figura 4.29: Errores en Posición
Se comparó la covarianza calculada por el Filtro de Kalman con la covarianza del error obtenida en
las simulaciones. Hay buena concordancia entre ambas para posición y velocidad ( guras 4.31 y 4.32,
respectivamente). Como se muestra en la gura 4.30, aparece una diferencia en el caso de la covarianza
del error en actitud, a pesar de que sigue la forma de la curva.
Figura 4.30: Desvío del Error en Actitud
4.3. Simulación de Monte Carlo 91
Figura 4.31: Desvío del Error en Velocidad
Los picos observados más notoriamente en el error en velocidad con una frecuencia de un segundo
corresponden a los períodos en que se navega inercialmente. En este período de tiempo, los ruidos
instrumentales se propagan por las ecuaciones de navegación incrementando el error.
Figura 4.32: Desvío del Error en Posición
92 Capítulo 4. Navegación Integrada
4.3.2. Modi cación del Paso de Predicción del Filtro de Kalman
La propagación de las matrices del Filtro de Kalman es una tarea muy costosa computacionalmente,
así como su actualización cuando llega una observación. Es por eso que realizar la propagación de la
matriz de covarianza a priori cada 100ms puede resultar comprometedor. Debido a que la dinámica
del vehículo bajo las condiciones de la trayectoria en consideración no es muy rápida, puede asumirse
con cierto grado de error que las mediciones inerciales se mantienen aproximadamente constantes en
intervalos de 1 segundo. En este caso, es posible calcular la matriz de transición de estados Φk para
cada segundo, utilizándola para propagar el vector de estados y la matriz de covarianza a priori en este
intervalo de tiempo. De esta forma, el paso T en las ecuaciones 3.3 y 3.9 pasa a valer 1 segundo, y el
Filtro de Kalman se ejecuta en su totalidad con una frecuencia de 1Hz. Esta estructura del algoritmo
resultará además útil cuando se considere el retardo en el dato del GPS, por lo que se estudia mediante
simulaciones el impacto de esta aproximación sobre la performance del navegador.
Figura 4.33: Comparación de Media y Desvío para el Error en Actitud
Se han simulado nuevamente 300 trayectorias utilizando el nuevo método de propagación de ma-
trices y se muestran las medias y cotas de 2 desvíos para el error en cada variable de navegación.
Se gra can las medias y desvíos para la propagación cada 100ms en negro, mientras que los nuevos
resultados se muestran en azul y rojo. Los grá cos muestran que realizar la propagación de la matriz
de covarianza cada un segundo no afecta en ninguna forma la performance del navegador. Las medias
y desvíos conciden perfectamente, indicando que, para esta aplicación en particular es posible evitar
4.3. Simulación de Monte Carlo 93
propagar las matrices cada 100ms, sin degradar en forma alguna la calidad de los datos.
Figura 4.34: Comparación de Media y Desvío para el Error en Velocidad
Figura 4.35: Comparación de Media y Desvío para el Error en Posición
94 Capítulo 4. Navegación Integrada
4.4. Consideración de un Retardo en el Dato GPS
Se ha asumido que la sincronización entre las mediciones inerciales y las del receptor GPS es
perfecta. En la práctica, esto se puede aproximar utilizando el PPS (Pulse per Second) generado por el
receptor como marca de tiempo, a partir de lo que se puede generar un clock que gobierne la toma de
datos de IMU y GPS. A pesar de que se puede asumir que las mediciones son simultáneas, el receptor
entrega el dato con un cierto retardo debido al procesamiento de la información para obtener posición
y velocidad. Este retardo puede ser de cientos de milisegundos, lo que tiene una importante in uencia
en el algoritmo de navegación si no se lo tiene en cuenta.
Figura 4.36: Error en Actitud
En las guras 4.36 a 4.38 se muestran los errores resultantes de una simulación de Monte Carlo
de una trayectoria con datos GPS retrasados aleatoriamente entre 450ms y 750ms, que son valores
esperables para este proyecto. Se actualizó el vector de estados en el instante en que se recibió el dato
GPS como si correspondiese a ese momento. La performance del sistema se ve gravemente degradada,
en particular la media del error de navegación, a pesar de que su varianza se mantiene acotada debido
que depende de la dinámica del proceso, que no cambia.
4.4. Consideración de un Retardo en el Dato GPS 95
Figura 4.37: Error en Velocidad
Figura 4.38: Error en Posición
Por otro lado, en las guras 4.39 y 4.40 se observan los procesos de innovaciones. Se mencionó que
cuando el Filtro de Kalman funciona correctamente, deben ser secuencias de datos descorrelacionados.
Sin embargo, al no tener en cuenta el retraso del dato GPS, aparece una correlación temporal. Resulta
evidente la necesidad de considerar este retardo en la estimación de los errores y la corrección de las
96 Capítulo 4. Navegación Integrada
variables de navegación, con el objetivo de volver a un comportamiento predecible.
Figura 4.39: Innovaciones de Velocidad
Figura 4.40: Innovaciones de Posición
Una posible consideración del retardo consiste en propagar las mediciones de posición y velocidad
en el tiempo. Para el caso de la posición, esto es muy simple ya que se utiliza la medición de velocidad
del GPS para calcular una posición posterior. La contrapartida es que se reduce la con abilidad del
4.4. Consideración de un Retardo en el Dato GPS 97
dato, ya que se incorpora el ruido de la medición de velocidad. Para los valores considerados esto no
es tan malo, ya que resulta despreciable frente al ruido en posición.
La propagación de la velocidad, por otro lado, considera la aceleración, que no es provista por el
recpetor GPS y debe ser estimada. Si se hace a partir de mediciones sucesivas de velocidad, también
se correlaciona el ruido de medición, que deja de ser blanco. Este resulta un problema mayor para el
navegador, que asume esta naturaleza para todos los procesos de ruido.
Un proceso que salvaguarda la naturaleza de los ruidos y mantiene la estructura del navegador
consiste en propagar la innovación en el tiempo. Se describe a continuación esta modi cación y se
presentan resultados de simulaciones.
4.4.1. Propagación de la Innovación
El Filtro de Kalman estima en forma óptima el vector de estados xk/k y luego lo propaga en el
tiempo mediante la matriz de transición de estados Φk . En el navegador propuesto en este trabajo este
paso no ocurre, ya que una vez disponible la estimación del error, se procede a corregir las variables
de navegación, después de lo cual se fuerza el vector estados a cero (ya que se asume se han eliminado
los errores).
Si en lugar de realizar la corrección de variables de navegación en el instante de la medición, se
sigue propagando el vector de estados, la estimación sigue siendo óptima con la información disponible
(aunque degradada respecto del momento de la actualización, ya que el ruido de proceso incrementa
la varianza en la estimación). Aplicando esta característica al problema del retraso en el dato GPS, es
posible calcular la actualización del vector de estados de un instante previo, (siempre que se conozca
la ganancia de Kalman correspondiente a ese instante) y luego propagarlo hasta un instante posterior
de interés.
Cuando el vector de estados se propaga utilizando la dinámica del proceso, se aplica la matriz de
transición de estados Φk a la estimación más reciente δxk/k :
δxk+1/k = Φk δxk/k−1 + Kk yk − Ck δxk/k−1 (4.11)
= Φk δxk/k−1 + Φk Kk yk − Ck δxk/k−1 (4.12)
donde el primer término depende de la dinámica del sistema y el segundo es la propagación de la
innovación. Este último término es el de interés para el problema en cuestión, ya que indica que es
posible utilizar la solución óptima incluso con un retardo.
De la estructura del Filtro de Kalman puede verse que el cálculo de la ganancia de Kalman 3.10
98 Capítulo 4. Navegación Integrada
y de la matriz de covarianza a posteriori 3.12 no dependen de la medición en sí, sino de su varianza.
Esto permite calcularlos independientemente de la disponibilidad de una medición. En el contexto
del problema del retardo, es posible conocer el momento de medición gracias al PPS generado por
el receptor. En este momento se guardan los datos de posición y velocidad del INS con los que se
compararán los datos del GPS, y se calculan la ganancia Kk+1 y la matriz de covarianza a posteriori
Pk+1/k+1 . Cuando el dato correspondiente a ese instante de tiempo está disponible para su lectura,
se actualiza el vector de estados y se lo propaga utilizando la matriz de transición de estados entre el
momento de la medición y la llegada del dato.
Teóricamente, la forma más conveniente de utilizar este método es actualizar el vector de estados y la
matriz de covarianza en el momento que se recibe el dato (que será variable con el delay). Esto garantiza
que la información incorporada posee la menor varianza posible. Sin embargo, di culta notablemente
los cálculos que deben realizarse, ya que es necesario propagar la matriz de covarianza entre el momento
de la toma de la medición hasta el de la actualización, y luego desde este instante hasta el de la toma de
la medición siguiente. Esto implica calcular dos veces por segundo la matriz de transición de estados
y la matriz de covarianza a priori, lo que a primera vista resulta poco práctico, sobre todo en una
aplicación de tiempo real.
Figura 4.41: Diagrama de Tiempos
Si se decide mantener los instantes de actualización jos cada 1 segundo, entonces el problema
queda simpli cado, ya que las matrices se actualizan (en el sentido numérico) cada 1 segundo. De esta
forma, sólo es necesario cambiar el orden de la actualización, calculando primero el vector de estados
a posteriori (en función de las matrices del instante previo) y luego calcular la matriz de transición de
estados para propagarlo, así como la matriz de covarianza. En la gura 4.41 se representa la secuencia
de pasos a seguir para actualziar el vector de estados con mediciones del instante previo.
Manteniendo la la actualización ja en un segundo, la estructura del algoritmo de navegación inte-
grado queda modi cada como se muestra en el algoritmo 4.2. Los datos del GPS recibidos corresponden
4.4. Consideración de un Retardo en el Dato GPS 99
al instante tk , mientras que se incorporan en el Filtro de Kalman en el instante tk+1 . Se realizaron
simulaciones implementando ambos métodos, y se observó que la performance no es sustancialmente
distinta; más considerando la complejidad de un método sobre otro. Por este motivo, se ha optado por
propagar la innovación hasta el segundo siguiente, resultando en la misma cantidad de operaciones que
en el Filtro de Kalman con datos simultáneos.
Algoritmo 4.2 Navegador Integrado INS/GPS Considerando Retardos en los Datos GPS
Inicializar Variables del Navegador (Pe , Ve , qeb )
Inicializar Variables del Kalman (ϕb , δVe , δPe , δbω , δbf , P0/0 )
While(1)
{
Ejecutar Navegador Inercial (calcula Pe , Ve , qeb )
if(Datos PGP
k
S y VGP S disponibles)
k
{
Calcular la covarianza de la estimación pasada Pk/k = (I − Kk Ck ) Pk/k−1
Calcular la Observación del error pasado yk = ykIN S − ykGP S
Corregir la estimación del error pasado δxk/k = δxk/k−1 + Kk yk − Ck δxk/k−1
Calcular Fk , Bk y Φk
Propagar el estado al instante actual δxk+1/k = Φk δxk/k
Corregir las variables de navegación y parámetros instrumentales
(Pe ,Ve ,qeb ,bω ,bf )
y resetear el vector de estados δxk+1/k = 0
Propagar la matriz de covarianza actual Pk+1 = Φk Pk/k ΦTk + Bk QBTk Tk
−1
Calcular la ganancia de Kalman actual Kk+1 = Pk+1/k CTk+1 Ck+1 Pk+1/k CTk+1 + Rk+1
Guardar las salidas actuales del INS yk+1
IN S
}
}
Nuevamente, si se decide corregir las variables de navegación cada vez que se obtiene una nueva
actualización en la estimación del error, el paso de actualización pasa a ser simplemente
δxk/k = Kk yk (4.13)
mientras que incorporando la propagación del estado hasta el instante actual, se obtiene
δxk+1/k = Φk Kk yk (4.14)
100 Capítulo 4. Navegación Integrada
4.4.2. Simulaciones
Simulando nuevamente 300 trayectorias, se obtuvieron las medias y varianzas de los errores cometi-
dos en los parámetros de navegación, y se las comparó con las obtenidas en el caso sin retardo. Se
gra ca en negro las medias y cotas de 2σ del desvío para el caso sin retardo, expuesto en la sección
anterior, mientras que para las medias y desvíos de la nueva simulación se siguen utilizando el azul y
el rojo, respectivamente.
Figura 4.42: Error en Actitud
Se observa que el desvío del error en actitud ( gura 4.42) converge prácticamente al mismo valor
que en el caso sin retardo, aunque desfasado un segundo en el tiempo. Sin embargo, como se mostró
antes, este parámetro no había sufrido grandes cambios al incorporar el retardo en las mediciones. La
media del error, por otro lado, se mantiene en cero, indicando que la estimación del error es correcta.
Por otro lado, el desvío del error en velocidad mostrado en la gura 4.43 sufre una alteración más
notoria, particularmente en los primeros 10 segundos de navegación. Luego de este transitorio, el desvío
sigue una curva similar al caso sin delay, aunque ligeramente mayor. El desvío del error en posición
( gura 4.44) se comporta idénticamente al caso sin retardo, siempre con un desfasaje de un segundo.
4.4. Consideración de un Retardo en el Dato GPS 101
Figura 4.43: Error en Velocidad
Figura 4.44: Error en Posición
Estos resultados sugieren que la propagación de la innovación mantiene la calidad de la navegación
lo más cercana posible al caso sin retardo. De esta forma, se resuelve el problema del retardo en el
dato del GPS, manteniendo la performance del sistema de navegación, y más importante, con la misma
cantidad de operaciones por segundo.
Capítulo 5
Incorporación del Sistema de Navegación
Integrado en un Simulador
Hasta ahora se han explicado las bases teóricas que llevan al diseño e implementación de un sistema
de navegación integrada. Se ha visto que la mejora en la calidad de los datos provistos, así como en la
robustez del sistema es notoria. Sin embargo, las simulaciones dadas constituyen una prueba parcial
de estas cualidades.
En este capítulo se discute la inclusión del navegador integrado en un simulador que contempla
diversas variables físicas y ambientales, e incorporando los sistemas de guiado y control. Se pretende
evaluar el desempeño del algoritmo de navegación en un entorno similar al de la aplicación para el
que fue diseñado, utilizando los datos navegados para alimentar otros sistemas que a su vez toman
decisiones en base a los mismos. En este caso, las simulaciones se realizan a lazo cerrado (on-line),
distinguiendolas de las efectuadas a lazo abierto únicamente sobre la trayectoria nominal (o-line).
5.1. Estructura del Simulador
Para comprobar la validez de los datos provistos por el sistema de navegación se lo incluyó en un
simulador, que integra los sistemas de navegación, guiado y control del cohete. El sistema de guiado,
basado en el trabajo desarrollado en [19], toma la posición, velocidad, actitud y velocidad angular
corregida y calcula las acciones que debe tomar el sistema de control para seguir la trayectoria deseada.
Además incluye perturbaciones sobre el vehículo debidas a vientos, modela los actuadores, tanques de
combustible, y modos de vibración, por lo que constituye un resultado importante desde el punto de
vista de la validación del algoritmo de navegación en una aplicación que exige un correcto desempeño
del sistema.
102
5.1. Estructura del Simulador 103
Figura 5.1: Estructura del Simulador
5.1.1. Errores del Navegador
Se introdujeron ruidos blancos en las mediciones tanto inerciales como de GPS, pero uniformes
en vez de Gaussianos, con el objetivo de exigir al navegador. Se consideró un ancho de 6σ para una
distribución Gaussiana, siendo reemplazada por una uniforme. Desde el punto de vista teórico, esto
implica una degradación en la estimación, ya que el estimador lineal óptimo deja de serlo en todo sentido
si el ruido no es Gaussiano. Las potencias de los ruidos se mantienen como en los ejemplos anteriores
y se gra can las medias y errores de 2σ para una simulación de Monte Carlo de 100 trayectorias.
Se gra ca en esta sección el error en actitud del navegador utilizando los ángulos de Euler: Roll,
Pitch y Yaw, que tienen relación directa con los ejes del vehículo. Estos ángulos se pueden obtener a
partir del cuaternión de error qbbreal
IN S
= q(ϕb ) combinando las ecuaciones 1.18 a 1.20 con la 1.28 para
obtener
2(q2 q3 − q1 q4 ) 2(q2 q3 − q1 14 )
φ = arctan = arctan (5.1)
q42 + q32 − q22 − q12 1 − 2(q22 + q12 )
θ = arcsin (−2(q1 q3 + q2 q4 )) (5.2)
2(q1 q2 − q3 q4 ) 2(q1 q2 − q3 q4 )
ψ = arctan 2 = arctan (5.3)
q4 + q12 − q22 − q32 1 − 2(q22 + q32 )
donde se ha utilizado la propiedad de norma unitaria de los cuaterniones, kqk2 = q12 + q22 + q32 + q4 = 1.
Los errores en velocidad y posición se evalúan como en las secciones anteriores.
104 Capítulo 5. Incorporación del Sistema de Navegación Integrado en un Simulador
Figura 5.2: Error en Actitud
Figura 5.3: Error en Velocidad
Debido a que el vehículo no realiza control de rolido, el error de actitud inicial en este eje no causa
mayor impacto en la práctica. Por esta razón se consideran errores en actitud del orden de 1 grado
para Pitch y Yaw, mientras que en Roll el error es de 6 grados. Se observa en la gura 5.2, sin embargo,
una degradación de la performance del sistema de navegación respecto de las simulaciones o-line con
5.1. Estructura del Simulador 105
ruido Gaussiano. Luego de un transitorio, el error en actitud en los ejes de Pitch y Yaw se mantiene
dentro de los 0,8°, mientras que el error en Roll tarda en converger a un desvío mayor, que llega en su
punto mínimo a 1,2°.
El error en velocidad ( gura 5.3) también se ve incrementado respecto del caso o-line. De igual
forma, luego de un transitorio, se mantiene dentro de 0,8m/seg para los ejes x y y, mientras que para
el eje z converge a 0,4m/seg. El error en posición, mostrado en la gura 5.4, se mantiene dentro de los
12m, valor mayor al obtenido en las simulaciones o-line.
Figura 5.4: Error en Posición
Si bien los valores para los desvíos obtenidos para el navegador en presencia de ruido no Gaussiano
son mayores, su valor es acotado y menor que el de las mediciones disponibles. En la sección siguiente
se observará que estos niveles de con ablidad son su cientes para asegurar un correcto guiado del
vehículo.
5.1.2. Errores del Vehículo
El error de guiado del vehículo para los ejes Yaw y Pitch resulta menor a 1° después del transitorio
inicial. Puede observarse que los picos en el desvío tienen una frecuencia de 10Hz , coincidente con la
frecuencia de las acciones de control de actitud. Debido a que no hay control de rolido, el error en este
eje aumenta en el tiempo, aunque esto no in uye en el error en otros ejes.
106 Capítulo 5. Incorporación del Sistema de Navegación Integrado en un Simulador
Figura 5.5: Attitude Tracking Error (cotas de 2σ )
Figura 5.6: Attitude Tracking Error (medias y desvíos)
En la gura 5.6 se observan con mayor claridad las medias y desvíos del error de guiado durante
el vuelo controlado. La naturaleza ondulatoria del error concuerda con un vehículo oscilando alrede-
dor de la trayectoria deseada, y el desvío del error en Roll es acotado, mientras que su media crece
inde nidamente con el tiempo.
5.1. Estructura del Simulador 107
Otro de los parámetros de interés en el guiado es el error en altitud (Ascent Altitude Error). El
error se mantiene dentro de límites establecidos para este vehículo, con una media que llega a 10m de
error, y un desvío que llega en su máximo a los 11m. Debe notarse que en este caso el crecimiento del
desvío no guarda relación con la oscilación en torno a la media, sino que el error en las trayectorias
efectuadas resulta aproximadamente paralelo a la media.
Figura 5.7: Ascent Altitude Error (cotas de 2σ )
Figura 5.8: Range Error (cotas de 2σ )
108 Capítulo 5. Incorporación del Sistema de Navegación Integrado en un Simulador
La media del error de rango (Range Error) llega a los 60m, mientras que su desvío aumenta hasta
alcanzar los 11m. Finalmente, el error de seguimiento lateral (Lateral tracking Error) es el que más se
desvía de la trayectoria nominal, llegando su media a valer 40m, y con un desvío de 130m.
Figura 5.9: Lateral Tracking Error (cotas de 2σ )
A pesar de que las magnitudes de estos errores resultan mucho mayores que los cometidos por
el navegador integrado, se encuentran dentro de valores esperados para el vuelo en las condiciones
dispuestas. Vale aclarar que los errores resultantes para el vehículo no son únicamente dependientes
del error en el navegador, sino que se contemplan los errores en los sistemas de guiado y control.
5.2. Consideración del Retardo en el Dato GPS 109
5.2. Consideración del Retardo en el Dato GPS
Tal como se hiciera en la Sección 4.4, se analiza la performance del navegador integrado cuando
se toma en consideración un retardo en el dato GPS. En esta sección se analizan nuevamente los
resultados del simulador mediante el método de Monte Carlo. Incorporando la actualización retrasada
en un segundo en el navegador, se simularon 100 trayectorias y se compararon las medias y desvíos
con las obtenidas sin retardo en la recepción del dato GPS.
Figura 5.10: Error de Navegador en Actitud
Figura 5.11: Error del Navegador en Velocidad
110 Capítulo 5. Incorporación del Sistema de Navegación Integrado en un Simulador
Figura 5.12: Error del Navegador en Posición
Los resultados son consistentes con los mostrados anteriormente. Como se ve en la gura 5.10,
la media del error en actitud permanece en cero mientras que la varianza converge prácticamente a
la obtenida sin retardo en el dato GPS. Es notable el hecho de que este método reduce el efecto del
transitorio inicial en actitud, debido al segundo adicional que tarda en comenzar la corrección. En las
guras 5.11 y 5.12, se muestra que la media y la varianza del error en posición coinciden, mientras que
la diferencia es nuevamente más notoria en el caso de la velocidad.
5.2. Consideración del Retardo en el Dato GPS 111
En el error de guiado ( guras 5.13 y 5.14) se ve re ejada la reducción del efecto del transitorio
en actitud, siendo menor que en el caso anterior. Puede observarse además una reducción inicial en el
desvío del error en Roll, aunque después de los 10 segundos, pasa a ser apenas mayor como consecuencia
de la ligeramente degradada calidad del cálculo de actitud del navegador.
Figura 5.13: Attitude Tracking Error (cotas de 2σ )
Figura 5.14: Attitude Tracking Error (medias y desvíos)
112 Capítulo 5. Incorporación del Sistema de Navegación Integrado en un Simulador
La media y desvío en el error de altura ( guras 5.15 y 5.16) son inicialmente idénticos, y comienzan
a apartarse a partir de los 30 segundos. A pesar de esto, sus valores resultan muy similares.
Figura 5.15: Ascent Altitude Error (cotas de 2σ )
Figura 5.16: Ascent Altitude Error (media y desvío)
En el caso del error de rango, gra cado en las guras 5.17 y 5.18, las medias son prácticamente
coincidentes durante todo el vuelo, mientras que el desvío del navegador con delay llega a ser 1m mayor
5.2. Consideración del Retardo en el Dato GPS 113
al caso sin delay.
Figura 5.17: Range Error (cotas de 2σ )
Figura 5.18: Range Error (media y desvío)
Finalmente, en las guras 5.19 y 5.20 se observa que la media del error de seguimiento lateral
se aparta notoriamente, especialmente a partir de los 40 segundos. El desvío, por otro lado, llega a
aumentar en 10m, aunque porcentualmente no sea un cambio signi cativo.
114 Capítulo 5. Incorporación del Sistema de Navegación Integrado en un Simulador
Figura 5.19: Lateral Tracking Error (cotas de 2σ )
Figura 5.20: Lateral Tracking Error (media y desvío)
Los errores en que se incurre al modi car el navegador para considerar el delay en el dato GPS
son consistentes con una ligera degradación de la calidad en los datos de navegación. Sin embargo, las
diferencias son menores, resultando la solución propuesta muy útil y de fácil implementación.
5.3. Tiempos de Ejecución 115
5.3. Tiempos de Ejecución
Debido a la necesidad de implementar el algoritmo descripto en una aplicación de tiempo real,
aparecen restricciones sobre los tiempos de procesamiento de la información. Por un lado, la IMU
impone una frecuencia de trabajo de 100Hz, lo que da una ventana de 10ms para procesar las mediciones
inerciales antes de recibir otro paquete de datos. Sin embargo, el algoritmo de navegación correrá en
una computadora junto con los sistemas de guiado, control, y rutinas de comunicaciones; esta situación
reduce aún más el tiempo disponible para resolver la navegación inercial. Las rutinas de guiado y control
se ejecutan a una tasa de 100ms, lo que libera en gran medida los recursos para correr las rutinas de
navegación.
Cada vez que llega un dato GPS y se calculan las matrices correspondientes a la dinámica, propa-
gación, y actualización del Filtro de Kalman, ocurre un incremento muy importante de la carga com-
putacional. Las operaciones sobre matrices de dimensión 15 × 15 (particularmente el producto de
matrices) requieren de un tiempo de procesamiento sustancialmente mayor al de las operaciones con
vectores de dimensiones reducidas.
Cada 1 segundo las rutinas de navegación (inercial e integrada), guiado, y control coinciden en su
ejecución en un instante tN . Este es el momento de mayor carga computacional, y representa el peor
caso para el hardware desde el punto de vista de los tiempos de cálculo. Idealmente, el algoritmo de
navegación inercial debe seguir ejecutándose en tN + 10ms, momento en que llega un nuevo dato de la
IMU. Esto impone un margen de tiempo demasiado acotado para la nalización de todas las rutinas
descriptas. Gracias a que el sistema de control requiere el siguiente dato de navegación en tN + 100ms,
una opción es proveer el dato en tN antes de ejecutar el Filtro de Kalman (dar el resultado de la
navegación inercial en ese instante). El sistema de control tendrá el dato que necesita, y el Filtro de
Kalman podrá ejecutarse después de esto, con una ventana de 100ms para nalizar los cálculos. En
este caso, algunos paquetes de datos de la IMU se colocarán en un buer para ser procesados cuando
haya recursos disponibles.
Se muestran en la tabla 5.1 los tiempos de ejecución de cada fase del algoritmo de navegación
inercial optimizado, donde se ha utilizado el algoritmo 2.3. Se han indicado los ciclos internos del
algoritmo, considerando los tiempos de ejecución máximos. Por otro lado, se incorporan los tiempos
de ejecución del algoritmo de navegación inercial de baja precisión presentado en la sección 2.5. Debe
mencionarse que en el hardware en consideración, los tiempos medidos son extremadamente regulares,
siendo la variación de los mismos típicamente menor a 10µs, y eventualmente con un incremento menor
a 50µs.
116 Capítulo 5. Incorporación del Sistema de Navegación Integrado en un Simulador
Rutina Tiempo de Ejecución
Corrección de Datos de la IMU 1, 099ms
Ciclo Rápido (cada 10ms) 0, 113ms
Ciclo Medio (cada 10ms) 0, 885ms
Ciclo Lento (cada 100ms) 1, 582ms
Cuadro 5.1: Tiempos de Ejecución de Ciclos del Algoritmo Inercial
Se observa que, cuando se ejecutan todos los ciclos del navegador inercial, y en el peor caso, el
tiempo de ejecución no supera los 4ms. Esto representa menos de la mitad de la ventana de 10ms de
la que se disponía originalmente, dejando los otros 6ms para ejecutar otras operaciones.
En la tabla 5.21 se muestran los tiempos de ejecución para el Filtro de Kalman y la corrección de
variables de navegación, también para el peor caso. Puede observarse que las funciones que trabajan
con multiplicación de matrices son las que insumen mayor tiempo. El tiempo total de ejecución del
Filtro de Kalman y de la corrección de las variables de navegación supera ampliamente el margen inicial
de 6ms que deja el algoritmo de navegación inercial luego de ejecutarse, por lo que deberá ejecutarse
luego de proveer los datos al sistema de control.
Rutina Tiempo de Ejecución
Construcción de Matrices de la Dinámica (Fk , Bk ) 0, 441ms
Propagación (Φk , xk+1/k , Pk+1/k ) 7, 951ms
Observación (yk+1 ) 0, 075ms
Actualización Secuencial (Kk+1 , xk+1/k+1 , Pk+1/k+1 ) 11, 119ms
Corrección de Variables de Navegación (qb ,
e V e , P e , bω , bf ) 0, 185ms
TOTAL 19, 771ms
Figura 5.21: Tiempos de Ejecución del Filtro de Kalman
En la tabla 5.22 se han detallado los tiempos de ejecución totales para cada ciclo del navegador
inercial de precisión, mostrando también los del navegador inercial basado en el método de Euler. Se
puede observar que los ciclos rápido y medio del navegador de precisión se ejecutan en un tiempo
menor que el de Euler, mientras que al calcularse también el ciclo lento, el tiempo se ve incrementado,
tardando 1, 2ms más que el mismo. Basados en esta información, es interesante notar que a pesar de
ofrecer una precisión mucho mayor, el navegador inercial de precisión no demanda mucho más tiempo
5.4. Conclusiones 117
que el de Euler, haciéndolo la opción más apropiada para la aplicación.
Rutina Tiempo de Ejecución
Tiempo total de ejecución cada 10ms 2, 374ms
Tiempo total de ejecución cada 10ms (Euler) 2, 690ms
Tiempo total de ejecución cada 100ms 3, 867ms
Tiempo total de ejecución cada 1s 23, 298ms
Figura 5.22: Tiempos de Ejecución Finales de Ciclos del Algoritmo Integrado
Si se considera que el algoritmo inercial consume 25, 233ms de cálculo cada 100ms, es posible
incorporar el Filtro de Kalman antes de proveer la siguiente salida para el sistema de control. Una
vez que termine de calcularse el Filtro de Kalman, se habrán acumulado en un buer 2 paquetes de
datos de la IMU para ser procesados, por lo que, teniendo en cuenta lso tiempos de cálculo de los
ciclos rápido y medio, el algoritmo estará trabajando en condiciones normales nuevamente a partir de
tN + 30ms.
5.4. Conclusiones
El objetivo de esta Tesis ha sido diseñar un sistema de navegación para un cohete suborbital que
determinara su orientación, velocidad y posición, y cumpliera con los requerimientos de frecuencia
de actualización, precisión, y costo determinados. En el Capítulo 2 se discutió la integración de las
ecuaciones de navegación en una terna de navegación conveniente para obtener los parámetros de
actitud, velocidad y posición buscados. Se desarrollaron dos métodos de integración numérica distintos,
uno basado en el método de Euler, y otro en el trabajo de Savage. Se demostró que bajo condiciones
ideales, sin incertidumbre en las condiciones iniciales o en la IMU, existe una importante diferencia
en la precisión de estos dos algoritmos, que a priori determina una inclinación hacia la elección del
algoritmo de Savage por sobre el de Euler. Sin embargo, en presencia de ruidos instrumentales y errores
en condiciones iniciales, el error debido a la integración numérica deja de ser el factor determinante en
la precisión de ambos algoritmos, situación que los pone en pie de igualdad. Por lo tanto, la ventaja
del navegador inercial de Savage por sobre el de Euler es dependiente de la calidad de la IMU.
Se presentó en el Capítulo 3 el Filtro de Kalman, utilizado comúnmente en algoritmos de navegación
integrada para fusionar datos de una IMU con los de sensores externos. El Filtro de Kalman es un
estimador lineal óptimo en presencia de ruido blanco Gaussiano, que incorpora la dinámica del estado
118 Capítulo 5. Incorporación del Sistema de Navegación Integrado en un Simulador
del sistema y su observación para proveer una estimación de mínima varianza. Se analizó la in uencia de
diversos factores de error sobre la incertidumbre nal por medio del Presupuesto de Errores, observando
que, al extinguirse el efecto de la incertidumbre inicial en las variables de navegación, el ruido de
medición en giróscopos y en la velocidad provista por el GPS son los factores dominantes, seguidos por
el ruido en los acelerómetros. Conociendo esta información, se decidió eliminar los factores de escala
y no ortogonalidad del vector de estados debido a su mínima in uencia en el error nal. Queda en
evidencia que los ruidos en giróscopos y en velocidad GPS actúan como factores limitantes para la
precisión del sistema de navegación, enmascarando el efecto de otras inestabilidades instrumentales.
Una importante reducción en estos factores de ruido permitiría identi car de forma más precisa errores
instrumentales tales como sesgos, ganando precisión en dos frentes. Por lo tanto, se propuso promediar
datos consecutivos de la IMU con el objetivo de promediar el ruido presente en ellos y reducir su
impacto en la navegación.
Una vez desarrollado el algoritmo de navegación inercial y un método de estimación de su error por
medio del Filtro de Kalman, se procedió a integrar los datos provistos por ambos sistemas. Se comprobó
que, cuando se navega en forma integrada, el error en las variables de navegación en presencia de ruido
instrumental e incertidumbre en el estado inicial queda acotado. Del mismo modo, se complementó
el navegador inercial basado en el método de Euler con el Filtro de Kalman, obteniendo resultados
prácticamente idénticos a los del navegador basado en el de Savage. Este hecho demuestra que la
navegación integrada resulta muy robusta ante diferentes tipos de perturbaciones, haciéndola una
técnica muy poderosa para acotar errores de diverso origen.
A partir de la equivalencia en la performance para las dos versiones del navegador integrado, y en
vista de la evidente simplicidad del método de Euler por sobre el de Savage, parece natural utilizar
el primero como navegador inercial. Por medio de la medición de los tiempos de ejecución para las
funciones que componen los algoritmos de navegación inercial y el Filtro de Kalman, se comparó la
e ciencia de las rutinas de navegación, llegando a la conclusión de que no hay una clara ventaja de un
método sobre otro. Más aún, la simplicidad del método de Euler no se ve re ejada en los tiempos de
ejecución de las rutinas de navegación inercial -de hecho, el navegador inercial de Savage se ejecuta en
un tiempo menor que el de Euler, salvo cuando se ejecutan los tres ciclos consecutivamente-. Debido a
que para futuros vehículos se prevé navegar inercialmente utilizando sensores inerciales de alta calidad,
y en vista del resultado recién mencionado, parece interesante utilizar el navegador basado en el trabajo
de Savage en este proyecto, con el n de validarlo en vuelo y evaluar su performance, estableciendo
herencia para futuros desarrollos.
5.5. Futuro Trabajo 119
Aprovechando la dinámica lenta del vehículo, y con el objetivo de aligerar la carga computacional,
se decidió analizar el caso en que se propaga la matriz de covarianza a priori cada 1 segundo, en lugar de
cada 0, 1 segundos. Mediante simulaciones se demostró que la performance del sistema de navegación
no se ve degradada.
Se mostró que el retardo en datos GPS tiene un impacto muy importante en la navegación, dando
origen a correlaciones temporales en las innovaciones. Como solución a este problema, se propuso
calcular la innovación correspondiente al instante tk -en que se realiza la medición GPS- en el instante
tk + δt -en que el dato GPS medido se encuentra disponible-, propagándola hasta el instante tk+1 ,
hecho que, con la aproximación utilizada para el paso de predicción, modi có el algoritmo del Filtro
de Kalman ligeramente. Mediante simulaciones se comprobó que la pérdida de precisión que emerge
de esta modi cación no es signi cativa, manteniendo los desvíos para el error en la estimación en los
niveles obtenidos sin retardo en la información del GPS.
La incorporación del navegador desarrollado en un simulador permitió demostrar que se satisfacen
los requerimientos de guiado y control dados inicialmente. Debido a que el simulador contempla también
la acción de los sistemas de guiado y control, y las variables físicas que afectan al vehículo, este resultado
es de suma importancia en cuanto a la validación del algoritmo de navegación propuesto. Se analizó
también los resultados de la navegación del vehículo cuando existen retardos en la información provista
por el GPS, utilizando el navegador modi cado para tal n. Tal como se observó en simulaciones a
lazo abierto, la precisión del navegador no se ve modi cada signi cativamente, validando nuevamente
el método de actualización implementado. Más aún, el análisis de los errores de guiado del vehículo en
este caso demostró que el navegador modi cado también cumple con los requerimientos del sistema.
5.5. Futuro Trabajo
El proyecto en el que se está trabajando consta de varias etapas que contnúan su desarrollo,
apareciendo en el camino nuevos problemas que requieren soluciones que contemplen el sistema como
un todo. Se analizará la inclusión de giróscopos de una categoría diferente a los MEMS, mucho más
precisos, para determinar con menor incertidumbre el estado inicial del vehículo. Con el mismo n,
existe la posibilidad de incluir un magnetómetro para aumentar la precisión de la estimación de actitud.
Otra de las tareas que deben encararse en el futuro es el suavizado de los datos proporcionados por
el navegador integrado. Luego de procesar las mediciones del GPS, la corrección de las variables de
navegación causa variaciones instantáneas muy importantes, como se puede ver, por ejemplo, en las g-
uras 4.1 a 4.3. Estos cambios bruscos de actitud, velocidad y posición, proporcionados por el navegador,
120 Capítulo 5. Incorporación del Sistema de Navegación Integrado en un Simulador
no son un re ejo del estado físico del vehículo, sino que es un resultado numérico de la incorporación
de mediciones independientes. Cuando los sistemas de guiado y control ven cambios tan veloces en los
parámetros, responden intentando seguir esa dinámica, causando picos en la potencia consumida por
los actuadores. Esta situación puede resolverse ltrando los datos de salida del navegador, situación
que debe ser estudiada con cuidado para mantener la performance.
Apéndice A
Modelo de Gravedad J2
El modelo de gravedad J2 es una aproximación de segundo orden de la expansión del potencial
gravitatorio en armónicas esféricas. La aceleración de la gravedad en un punto dado se expresa como:
3J2 a2
z 2
1+ 1 − 5 x
2r2 r x
3J2 a2
e e GM z 2
+ Ω2 y
γ (R ) = − 3 1+ 1 − 5 y
r
2r2 r
3J2 a2
z 2
0
1+ 3 − 5 z
2r2 r
dondeGM es el producto de la masa de la tierra y la constante de gravitación universal, y r =
p
x2 + y 2 + z 2 es el radio al centro de la tierra. El segundo término contempla la aceleración centrífuga
a la que se ve sometido un cuerpo debido a la rotación de la tierra. Los valores corresponden al sistema
WGS84, mientras que el término J2 corresponde al orden de la aproximación.
Constante Valor
a 6378137, 0m
MG 3, 986004418 × 1014 m3 /s2
Ω 7, 292115 × 10−5 rad/s
J2 1,082629989051944 × 10−3
Cuadro A.1: Constantes
Las componentes del jacobiano del modelo J2 son las siguientes:
3x2 3J2 a 5x2 15J2 a2 z 2 7x2
e GM
J11 (γ ) = − 3 1− 2 + 1− 2 − 1− 2 + Ω2
r r 2r2 r 2r4 r
3y 2 3J2 a 5y 2 15J2 a2 z 2 7y 2
GM
J22 (γ e ) = − 3 1− 2 + 1− 2 − 1− 2 + Ω2
r r 2r2 r 2r4 r
3z 2 9J2 a 5z 2 15J2 a2 z 2 7z 2
GM
J33 (γ e ) = − 3 1− 2 + 1− 2 − 3− 2
r r 2r2 r 2r4 r
2 2 2
GM 3xy 15J2 a xy 105J2 a xyz
J12 (γ e ) = − 3 − 2 − +
r r 2r4 2r6
121
122 Apéndice A. Modelo de Gravedad J2
15J2 a2 xz 7z 2
e GM 3xz
J13 (γ ) = − 3 − 2 − 3− 2
r r 2r4 r
15J2 a xy 105J2 a2 xyz 2
2
GM 3xy
J21 (γ e ) = − 3 − 2 − +
r r 2r4 2r6
2 7z 2
GM 3yz 15J2 a yz
J23 (γ e ) = − 3 − 2 − 3− 2
r r 2r4 r
45J2 a xz 105J2 a2 xz 3
2
GM 3xz
J31 (γ e ) = − 3 − 2 − +
r r 2r4 2r6
45J2 a2 yz 105J2 a2 yz 3
GM 3yz
J32 (γ e ) = − 3 − 2 − +
r r 2r4 2r6
Apéndice B
Presupuesto de Errores con Valores de
Ruido Actualizados
Teniendo en cuenta el promediado de datos analizado en la Sección 3.4.3, se muestran los resul-
tados del presupuesto de errores considerando los nuevos parámetros de ruido para los giróscopos y
acelerómetros.
Giróscopo Acelerómetro Actitud Velocidad Posición
Incertidumbre Inicial - - 1 °0,5 m/s 20 m
Ruido 0,1414 /s 1,7678 mg
° - 0,5 m/s 20 m
Inestabilidad en Sesgo 0,007 /s
° 0,1 mg - - -
Inestabilidad en Factor de Escala 0,0001 % 0,0001 % - - -
Inestabilidad en Factor de No Ortogonalidad 0,00087 % 0,00087% - - -
Cuadro B.1: Valores de parámetros (1σ )
Puede observarse en los grá cos, que aunque cualitativamente el comportamiento de los ruidos es
similar al analizado anteriormente, su magnitud se ve reducida, con una consecuente reducción en su
aporte a la covarianza total en el error en la estimación de cada parámetro. Debe notarse, sin embargo,
que la reducción del ruido en las mediciones de giróscopos y acelerómetros ha provocado una reducción
del impacto del ruido en la velocidad medida por el GPS. Asimismo, la contribución de los errores
instrumentales se ha incrementado levemente, mientras que los errores en condiciones iniciales son
identi cados con mayor velocidad.
123
124 Apéndice B. Presupuesto de Errores con Valores de Ruido Actualizados
B.1. Errores en Actitud
Figura B.1: Errores en ϕx
Figura B.2: Errores en ϕy
B.2. Errores en Velocidad 125
Figura B.3: Errores en ϕz
B.2. Errores en Velocidad
Figura B.4: Errores en Vx
126 Apéndice B. Presupuesto de Errores con Valores de Ruido Actualizados
Figura B.5: Errores en Vy
Figura B.6: Errores en Vz
B.3. Errores en Posición 127
B.3. Errores en Posición
Figura B.7: Errores en Px
Figura B.8: Errores en Py
128 Apéndice B. Presupuesto de Errores con Valores de Ruido Actualizados
Figura B.9: Errores en Pz
Bibliografía
[1] ADIS16364 6DOF Inertial Measurment Unit Datasheet - Analog Devices.
[2] World Geodetic System 1984 - Its De nition and Relationships with Local geodetic Systems. Tech-
nical report, NIMA - National Imagery and Mapping Agency, 2000.
[3] John E. Bortz. A New Mathematical Formulation for Strapdown Inertial Navigation. IEEE
Transactions on Aerospace and Electronic Systems, AES-7 N°1:6166, 1971.
[4] Juan Carrizo. Sistemas de Navegación para Cohetes Suborbitales. Master's thesis, Universidad
de Buenos Aires - Facultad de Ingeniería.
[5] Juan A. Carrizo, Martín España, and Juan I. Giribet. Algoritmo de Navegación Inercial para
Vehículos de Alta Velocidad. In RPIC Estudiantil, 2007.
[6] Gonzalo Castillo. Navegación Integrada INS-GPS: Aplicación a un SAR Aerotransportado.
Master's thesis, Universidad de Buenos Aires - Facultad de Ingeniería, 2012.
[7] John J. Craig. Introduction to Robotics - Mechanics and Control. Pearson Prentice Hall, 2005.
[8] Martín España. Fundamentos De La Navegación Integrada. AADECA, 2010.
[9] Jay Farrell and Matthew Barth. The Global Positioning System and Inertial Navigation.
McGraw-Hill, 1999.
[10] Juan I. Giribet. Algoritmos de Navegación Integrada Aplicados a Vehículos Espaciales. Master's
thesis, Universidad de Buenos Aires - Facultad de Ingeniería, 2003.
[11] Mohinder S. Grewal and Angus P. Andrews. Applications of Kalman Filtering in Aerospace
from 1960 to the Present. IEEE Control Systems Magazine, June:6978, 2010.
[12] Elliott D. Kaplan and Christopher J. Hegarty, editors. Understanding GPS - Principles And
Applications. Artech House, 2006.
129
130 Bibliografía
[13] Anthony Lawrence. Modern Inertial Technology - Navigation, Guidance and Control. Springer,
1998.
[14] Gonzalo Marinsek. Calibración y Caracterización Estocástica de Unidades de Medidas Inerciales
MEMS. Master's thesis, Universidad de Buenos Aires - Facultad de Ingeniería, 2011.
[15] Pratap Misra and Per Enge. Global Positioning System - Signals, Measurement, and Perfor-
mance. Ganga-Jamuna Press, 2001.
[16] A. Srikantha Phani and Ashwin A Seshia. Identi cation of Anisoelasticity and Nonproportional
Damping in MEMS Gyroscopes. NSTI-Nanotech, 2:343346, 2004.
[17] Paul Savage. Strapdown Inertial Navigation Integration Algorithm Design Part I: Attitude Algo-
rithms. Journal of Guidance, Control and Dynamics, 21 N°1:1, 1998.
[18] Paul Savage. Strapdown Inertial Navigation Integration Algorithm Design Part II: Velocity and
Position Algorithms. Journal of Guidance, Control and Dynamics, 21 N°2:1, 1998.
[19] Pablo Servidia, Juan I. Giribet, and Martín España. Kinematic Integral Attitude Tracking
Under Input Saturation. Preprint.
[20] Isaac Skog and Peter Händel. Calibration of a MEMS Inertial Measurement Unit. In XVII
IMEKO World Congress, Rio de Janeiro, Brazil, 2006.
[21] David H. Titterton and John L. Weston. Strapdown Inertial Navigation Technology. The
Institution Of Electrical Engineers & The American Institute Of Aeronautics And Astronautics,
2004.
Agradecimientos
A mis padres, por dar todo sin dudarlo para que pudiera estudiar esta carrera.
A mis hermanos, por su constante interés y colaboración.
A Motta, Pepe, Huevo, Cacho, Foto, Mne, y el Cabezón por las eternas sesiones de estudio
compartidas, miles de grupos de TP exitosos, y los momentos importantes (y no tanto) vividos.
A los enfermizos: Mario, Seba, Marian y Nico, por las clases, ensayos y comidas bizarras.
A Jony, por su ayuda en las primeras materias.
A Pablo Servidia por su enorme ayuda con el simulador.
A Juan Giribet, por darme la oportunidad de trabajar en un proyecto tan interesante y por su
disposición e inmensa paciencia a lo largo de todo el desarrollo de este trabajo.
También podría gustarte
- Fundamentos de Navegación IntegradaAún no hay calificacionesFundamentos de Navegación Integrada238 páginas
- Desarrollo de Un Sistema Integrado de Navegación Inercial PDFAún no hay calificacionesDesarrollo de Un Sistema Integrado de Navegación Inercial PDF74 páginas
- Algoritmos de Navegación para Vehículos AutónomosAún no hay calificacionesAlgoritmos de Navegación para Vehículos Autónomos165 páginas
- Eficiencia Energética en Robots HumanoidesAún no hay calificacionesEficiencia Energética en Robots Humanoides104 páginas
- Olivares - Estudio e Implementación de Algoritmos para La Estimación de La Posición Mediante Sist...Aún no hay calificacionesOlivares - Estudio e Implementación de Algoritmos para La Estimación de La Posición Mediante Sist...36 páginas
- Diseño e Implementación de Un Sistema deAún no hay calificacionesDiseño e Implementación de Un Sistema de11 páginas
- Comparación Numérica de ControladoresAún no hay calificacionesComparación Numérica de Controladores107 páginas
- Algoritmo de Orientación para NanosatélitesAún no hay calificacionesAlgoritmo de Orientación para Nanosatélites71 páginas
- Localización Interior con AndroidAún no hay calificacionesLocalización Interior con Android111 páginas
- Control de Angulo de Cabeceo de Un Avion PDFAún no hay calificacionesControl de Angulo de Cabeceo de Un Avion PDF27 páginas
- Sistemas de Navegación en El Sector AeronáuticoAún no hay calificacionesSistemas de Navegación en El Sector Aeronáutico57 páginas
- Nicolás Barrantes, Carlos SerranoAún no hay calificacionesNicolás Barrantes, Carlos Serrano56 páginas
- TFM OrdunaDiazJ FundamentosyaplicacionesdelposicionamientoAún no hay calificacionesTFM OrdunaDiazJ Fundamentosyaplicacionesdelposicionamiento137 páginas
- Control Visual de UAVs AutónomosAún no hay calificacionesControl Visual de UAVs Autónomos248 páginas
- Procesado de Datos GPS: Código y FaseAún no hay calificacionesProcesado de Datos GPS: Código y Fase339 páginas
- Capítulo 3 Síntesis Gráfi Ca de Eslabonamientos - . - . - . - . - . - . - . - . - . - . 87Aún no hay calificacionesCapítulo 3 Síntesis Gráfi Ca de Eslabonamientos - . - . - . - . - . - . - . - . - . - . 871 página
- Visión Estéreo para Seguimiento RígidoAún no hay calificacionesVisión Estéreo para Seguimiento Rígido74 páginas
- Aplicaciones de Las Matrices y Sistemas de Ecuaciones LinealesAún no hay calificacionesAplicaciones de Las Matrices y Sistemas de Ecuaciones Lineales5 páginas
- Análisis Biomecánico de Salidas en NataciónAún no hay calificacionesAnálisis Biomecánico de Salidas en Natación73 páginas
- Capitulo 1 Handbook Radiosistemas Del AviónAún no hay calificacionesCapitulo 1 Handbook Radiosistemas Del Avión23 páginas
- Tesis Estimaciondelaorientaciondeuncuerporigido 180114051223Aún no hay calificacionesTesis Estimaciondelaorientaciondeuncuerporigido 180114051223118 páginas
- Módulo GPS en FPGA para Sistemas EmbebidosAún no hay calificacionesMódulo GPS en FPGA para Sistemas Embebidos75 páginas
- Avances Robocol 2020: Innovación en RobóticaAún no hay calificacionesAvances Robocol 2020: Innovación en Robótica132 páginas
- Manual Usuario SSSE V1.10 EuropaAún no hay calificacionesManual Usuario SSSE V1.10 Europa168 páginas
- Sistemas de Navegación - Ángel Corbasí OrtínAún no hay calificacionesSistemas de Navegación - Ángel Corbasí Ortín336 páginas
- Arquitectura de Acoplamiento Entre INS/GPS para Navegación Precisa en Trayectorias EstablecidasAún no hay calificacionesArquitectura de Acoplamiento Entre INS/GPS para Navegación Precisa en Trayectorias Establecidas7 páginas
- Sensores en La Técnica de FabricaciónAún no hay calificacionesSensores en La Técnica de Fabricación134 páginas
- Andreu - Validacion de Aproximaciones SBAS y GBASAún no hay calificacionesAndreu - Validacion de Aproximaciones SBAS y GBAS132 páginas
- Ajuste Gaussiano de Redes Metodo Incremento CoordenadasAún no hay calificacionesAjuste Gaussiano de Redes Metodo Incremento Coordenadas201 páginas
- M Villalba Castro T FM 0120 MemoriaAún no hay calificacionesM Villalba Castro T FM 0120 Memoria78 páginas
- Uso Del Efecto Doppler para Detección de Obstáculos en Desplazamiento PeatonalAún no hay calificacionesUso Del Efecto Doppler para Detección de Obstáculos en Desplazamiento Peatonal111 páginas
- Modelado del Movimiento de SubmarinosAún no hay calificacionesModelado del Movimiento de Submarinos40 páginas
- Sistema Didáctico de Posicionamiento Global Aplicado A La Navegación AéreoAún no hay calificacionesSistema Didáctico de Posicionamiento Global Aplicado A La Navegación Aéreo115 páginas
- Curso de Gestión de Seguridad OperacionalAún no hay calificacionesCurso de Gestión de Seguridad Operacional47 páginas
- Preguntas de Peso y Balance AeronáuticoAún no hay calificacionesPreguntas de Peso y Balance Aeronáutico13 páginas
- La familia ante la insuficiencia renalAún no hay calificacionesLa familia ante la insuficiencia renal12 páginas
- Despachador (Cap. 01) - RegulacionesAún no hay calificacionesDespachador (Cap. 01) - Regulaciones6 páginas
- Toque Mire Gire - Billetes NuevOzAún no hay calificacionesToque Mire Gire - Billetes NuevOz4 páginas
- Introducción a la Calidad y Mejora Continua100% (1)Introducción a la Calidad y Mejora Continua10 páginas
- Pca de Lenguaje Nivel Básica SuperiorAún no hay calificacionesPca de Lenguaje Nivel Básica Superior4 páginas
- Guía Práctica Del Trabajo Final - E-Commerce ENEB100% (1)Guía Práctica Del Trabajo Final - E-Commerce ENEB3 páginas
- Manual Configuracion Sap Ps - CompressAún no hay calificacionesManual Configuracion Sap Ps - Compress62 páginas
- Retos del Sector Agropecuario en MéxicoAún no hay calificacionesRetos del Sector Agropecuario en México19 páginas
- La Atencion de Enfermeria en El Paciente Critico Segunda ClaseAún no hay calificacionesLa Atencion de Enfermeria en El Paciente Critico Segunda Clase34 páginas
- Prensa Digital Local - Delgado ArévaloAún no hay calificacionesPrensa Digital Local - Delgado Arévalo9 páginas
- Cuadernillo de Preguntas Diseno de Sistemas Mecanicos - Saber Pro 2018 PDFAún no hay calificacionesCuadernillo de Preguntas Diseno de Sistemas Mecanicos - Saber Pro 2018 PDF24 páginas
- QSP CRAM Año 1 Num 3, Junio de 2023Aún no hay calificacionesQSP CRAM Año 1 Num 3, Junio de 202332 páginas
- Acabado de La Tela de Lana (Cap. 10) . - Planchado y Otros Efectos de La SuperficieAún no hay calificacionesAcabado de La Tela de Lana (Cap. 10) . - Planchado y Otros Efectos de La Superficie8 páginas
- Importancia de Aplicar Estrategias para Comprender El VocabularioAún no hay calificacionesImportancia de Aplicar Estrategias para Comprender El Vocabulario7 páginas
- Actividades Culturales y Económicas en MéxicoAún no hay calificacionesActividades Culturales y Económicas en México3 páginas
- Lista de Precios de Maltas CastleAún no hay calificacionesLista de Precios de Maltas Castle11 páginas
- Autoevaluacion-N02-Revision-De-Intentos INVESTIGACION OPERATIVAAún no hay calificacionesAutoevaluacion-N02-Revision-De-Intentos INVESTIGACION OPERATIVA11 páginas
