PROYECTO FINAL

DE CARRERA
Integracin Kalman de sensores inerciales INS con
GPS en un UAV

Autor: Gonzalo Ferrer Mnguez

Tutor: Antoni Broquetas Ibars
RSLab TSC UPC
Abril, 2009

ndice
1

Introduccin .......................................................................................................... 1
1.1
Objetivo del proyecto .................................................................................... 2
1.2
Resumen........................................................................................................ 2
1.3
El avin ......................................................................................................... 4
Navegacin inercial ............................................................................................... 7
2.1
Conceptos bsicos de la Navegacin Inercial ................................................. 7
2.2
Sistemas de coordenadas ............................................................................... 8
2.2.1
Sistema de coordenadas Inercial I-frame ................................................ 9
2.2.2
Sistema de coordenadas ECI i-frame ...................................................... 9
2.2.3
Sistema de coordenadas ECEF e-frame .................................................. 9
2.2.4
Sistema de coordenadas de Navegacin n-frame .................................. 10
2.2.5
Sistema de coordenadas Body b-frame ................................................. 11
2.3
Relacin entre los sistemas de coordenadas ................................................. 12
2.3.1
DCM ................................................................................................... 12
2.3.2
ngulos de Euler ................................................................................. 13
2.3.3
Cambio de Body a Navegacin con los ngulos de Euler ...................... 14
2.3.3.1 Giro en Yaw ..................................................................................... 15
2.3.3.2 Giro en Pitch .................................................................................... 15
2.3.3.3 Giro en Roll ..................................................................................... 15
2.3.4
Matriz de cambio de coordenadas de e-frame a n-frame ....................... 16
2.4
Rotacin sideral de la Tierra ........................................................................ 17
Sensores .............................................................................................................. 19
3.1
Acelermetro ............................................................................................... 20
3.1.1
Modelo del acelermetro...................................................................... 21
3.2
Magnetmetro ............................................................................................. 23
3.2.1
Campo Magntico Terrestre ................................................................. 24
3.2.2
Caracterizacin del error del magnetmetro ......................................... 26
3.2.3
Modelo del magnetmetro ................................................................... 28
3.3
Giroscopio ................................................................................................... 30
3.3.1
Modelo de giroscopio .......................................................................... 31
3.4
GPS ............................................................................................................. 32
3.4.1
Caracterizacin del error ...................................................................... 33
3.5
Integracin de los sensores en la plataforma del avin ................................. 34
Actitud y Cinemtica ........................................................................................... 39
4.1
Clculo de la actitud .................................................................................... 39
4.1.1
Clculo de la actitud a partir de los giroscopios .................................... 40
4.1.2
Clculo de la actitud a partir de los magnetmetros. ............................. 41
4.2
Dead Reckoning con acelermetros y giroscopios........................................ 43
4.2.1
Cambio de las aceleraciones de Body a Navegacin ............................. 43
4.2.2
Integracin de las aceleraciones ........................................................... 45
4.3
Diagrama de bloques de la actitud y cinemtica ........................................... 45
Filtro de Kalman ................................................................................................. 47
5.1
Ecuaciones de Kalman ................................................................................. 48
5.2
Aplicaciones no lineales del filtro de Kalman .............................................. 51
5.2.1
Linealizacin respecto a la trayectoria nominal .................................... 52
5.2.2
Linealizacin respecto a la trayectoria estimada ................................... 53
III

5.3
Modelo forward ........................................................................................... 54
5.4
Modelo feedback ......................................................................................... 57
6
Medidas reales y resultados ................................................................................. 61
6.1
Procesado de las medidas............................................................................. 62
6.2
Integracin de las medidas de los sensores inerciales ................................... 64
6.3
Filtro de Kalman de 12 estados .................................................................... 66
6.3.1
Modelo forward ................................................................................... 66
6.3.2
Modelo feedback.................................................................................. 69
6.4
Filtro de kalman extendido de 18 estados ..................................................... 71
7
Conclusiones ....................................................................................................... 75
7.1
Futuras lneas de trabajo .............................................................................. 77
Bibliografa..79
Anexo I ....81
Anexo II83

IV

1 Introduccin
Este trabajo forma parte del proyecto del avin que se realiza en el TSC de la
UPC. Antes de comenzar a explicar los objetivos del PFC, es necesario hacer una
breve contextualizacin del departamento y los principales temas que se dedica a
estudiar. Uno de los campos de estudio del grupo del RSlab (Remote Sensing
Laboratory), dentro del grupo del TSC, son los sistemas de radar. Ms
concretamente, el estudio de radares de tipo SAR, es decir, Radares de Apertura
Sinttica. El funcionamiento de este tipo de radares se basa en un complicado
procesado de varios barridos de la antena del radar a lo largo de una trayectoria,
para combinarlos en un solo barrido virtual. El resultado es el mismo que si se
hubiera hecho un barrido con una antena mucho ms grande y ms directiva. El
uso de estas aperturas sintticas est limitado a trayectorias de la antena conocidas
y a blancos inmviles.
Algunas aplicaciones del SAR son los mapas de la superficie del terreno,
indicadores del hundimiento de la tierra, etc.

1.1

CAPTULO 1

Objetivo del proyecto

El objetivo es estimar la actitud y la posicin del UAV (Unmanned Aerial Vehicle)

utilizando sensores inerciales y otros sensores: un acelermetro en 3 ejes,
giroscopio en 3 ejes, magnetmetro en 3 ejes y las medidas de posicin de un GPS.
Todos estos sensores estn instalados rgidamente sobre el fuselaje del avin.
El objetivo principal del proyecto, es utilizar el avin como plataforma para un
radar de apertura sinttica (SAR). La principal ventaja respecto a otras plataformas
donde se embarca un SAR, tpicamente satlites o aviones grandes, es una
reduccin significativa en el coste del proyecto. Sin embargo, uno de los
problemas que se presenta es la inestabilidad de volar con un UAV, de
dimensiones ms reducidas que las plataformas anteriores. As pues, la estimacin
de la actitud y de la posicin del UAV resulta de vital importancia para la
implementacin del SAR, pues este sensor requiere un buen conocimiento de la
trayectoria y actitud angular del avin para reconstruir las imgenes de SAR
correctamente.
Mediante la integracin Kalman de todas las medidas de los sensores, se pretende
obtener una estimacin precisa de la actitud y posicin del mvil. Se cuenta con
dos tipos de dispositivos, los sensores inerciales (acelermetro y giroscopio) que
dan informacin muy precisa a corto plazo, pero despus de un periodo largo de
medida, tienden a ser poco fiables y los dispositivos que dan informacin absoluta
independiente del tiempo, como son el GPS y el magnetmetro. As pues, se
espera que la combinacin de estas dos fuentes de informacin, aunque sean
redundantes, mejoren la precisin que cada una de ellas tiene por separado.
Todas las medidas de los sensores se guardarn en un dataloger y se realizar el
procesado offline.

1.2

Resumen

En el captulo 2 se describen los sistemas de coordenadas que se van a utilizar en

el proyecto. Tambin se indicarn que transformaciones se necesitan para trabajar
con un nico sistema de coordenadas. Es una parte importante del proyecto ya que

Introduccin

el problema presenta muchos sistemas de coordenadas y la informacin no siempre

est referida a los mismos sistemas de coordenadas, pero a la vez es indispensable
referir las medidas al mismo marco de referencia para poder computar toda la
informacin.

En el captulo 3 se describir el funcionamiento de los sensores que se utilizan en

el avin: el magnetmetro, el giroscopio, el acelermetro y muy brevemente el
funcionamiento del GPS. Sus especificaciones y algunas pruebas que se hicieron
para caracterizar su funcionamiento en las condiciones de trabajo del avin.
Adems se har una descripcin de la implementacin de todos estos sensores en
la plataforma del UAV y de la circuitera para almacenar los datos en memoria.

En el captulo 4 se tratar la cinemtica del problema. Tenemos un magnetmetro

que da informacin del campo Magntico terrestre en 3 ejes, un acelermetro que
da informacin de la aceleracin en 3 ejes, un giroscopio que da informacin de la
velocidad de giro y adems las medias de posicin del GPS integrado en el avin.
Se har un desarrollo de las fuerzas que intervienen en el problema y se procesarn
las medidas para tenerlas en el mismo marco de referencia. El objetivo de este
captulo es obtener una salida de posicin, velocidad y actitud del UAV a partir de
las medias de los sensores. Es decir, se establecen las bases matemticas para
futuros captulos y se tratarn los resultados como un bloque independiente dentro
del esquema total de la integracin.

El captulo 5 describe el algoritmo del Filtro de Kalman y se demostrar su

eficacia para obtener informacin optimizada mezclando diversas medidas. Se
simularn varios escenarios donde se integren magnetmetro, acelermetro,
giroscopio y GPS para obtener la actitud y posicin del mvil y se discutirn los
resultados obtenidos en cada uno. Los modelos expuestos hacen referencia directa
a los modelos utilizados en las campaas de medida y no se generaliza demasiado
en las bases matemticas del filtro de Kalman, directamente se hace una

CAPTULO 1

introduccin de las aplicaciones del algoritmo que se utilizan en el proyecto

seguida de la descripcin del modelo empleado para el UAV.

En el captulo 6 se hace referencia a las medidas reales tomadas por el avin y por
los sensores en varias campaas de vuelo. Se procesarn las medidas y se
verificar todo lo expuesto en los captulos anteriores, a travs de figuras y
grficos. Es un captulo puramente explicativo, haciendo uso de los diferentes
montajes explicados antes y se discute brevemente los resultados, haciendo un
pequeo adelanto a las conclusiones del trabajo.

Como ltimo captulo 7, donde se habla de las conclusiones, algunas de ellas ya

adelantadas en los captulos correspondientes y se habla adems de las posibles
futuras lneas de trabajo.

1.3

El avin

Fig. 1.1 Fotografa del avin (UAV)

El avin, fotografiado en la figura 1.1, fue diseado por Albert Aguasca, tanto el
fuselaje como los componentes electrnicos embarcados. Es una plataforma de
vuelo relativamente pequea. Tiene como caractersticas principales:

2 metros de largo y 2.5 de ancho de ala.

Introduccin

17 Kg. de peso neto, con todos los dispositivos embarcados.

Velocidades de hasta 30-40 m/s.

30-45 minutos de autonoma.

Hasta 400 metros de altura de vuelo.

2 Navegacin inercial
El captulo comienza con una serie de definiciones bsicas sobre la navegacin
inercial que sern esenciales para el desarrollo del proyecto. A continuacin, se
describen los principales sistemas de coordenadas que se van a utilizar en el
proyecto. Se definen varios conceptos relevantes en los sistemas de
posicionamiento, como la Matriz de cambio de coordenadas DCM o los ngulos de
Euler. Por ltimo se indican las transformaciones necesarias para trabajar con un
nico sistema de coordenadas, que es el sistema de coordenadas de navegacin.

2.1

Conceptos bsicos de la Navegacin Inercial

Inercia es la propensin de los cuerpos a mantener constante su velocidad de

rotacin y de translacin a no ser que se vean afectados por fuerzas o momentos
respectivamente (primera ley de Newton).
Sistema de referencia Inercial es un sistema de coordenadas en el que las leyes de
Newton del movimiento son vlidas. Los sistemas de referencia inercial ni rotan ni
aceleran.

CAPTULO 2

Los Sensores Inerciales IMU (Inertial Measuring Unit) miden la variacin de

rotacin (girscopos) y la aceleracin (acelermetros). Se hablar de ellos con ms
detalle en captulos siguientes.
La Navegacin Inercial utiliza girscopos y acelermetros para mantener una
estimacin de la posicin, velocidad, actitud y variacin de actitud del vehculo en
el que estn los sensores.
Un Sistema de Navegacin Inercial (INS) consiste en una unidad de medida
inercial que incluye los sensores ligados a una plataforma comn para mantener
las mismas orientaciones relativas y un ordenador para procesar estas medias o
cualquier otro clculo.
Hay diferentes tipos de INS y se clasifican en gimbaled y strapdown. El sistema
gimbaled pretende aislar la plataforma con los sensores inerciales de los
movimientos de rotacin externos. Las plataformas gimbaled estn sujetas a un
marco rgido que rota de tal manera que asla el interior de la plataforma de
rotaciones externas con los ejes del cuerpo. Generalmente es imposible alcanzar un
nivel de aislamiento perfecto y siempre rota algo.
El sistema strapdown donde los ejes de los sensores estn alineados con los ejes
del mvil. Necesita mayor capacidad de clculo ya que es necesario aislar
virtualmente las medidas de los sensores al sistema de referencia inercial. Este
sistema es el que se va a utilizar en este proyecto.

2.2

Sistemas de coordenadas

Un sistema de coordenadas no es ms que un conjunto de vectores y nmeros sin

sentido. Para que estos valores adquieran significado, es necesario relacionarlos
con una referencia conocida.
Existen varios sistemas de coordenadas fundamentales. A no ser que se diga lo
contrario, estos sistemas son ortogonales, dextrgiros y cartesianos. nicamente
difieren en el origen, la orientacin relativa de sus ejes y el movimiento relativo
entre sus planos.

Navegacin Inercial

Dependiendo del movimiento que se quiera estudiar, es ms interesante usar unos

sistemas que otros.

2.2.1 Sistema de coordenadas Inercial I-frame

Es el nico sistema de referencia donde las leyes de newton tienen validez, es
decir, que este sistema idealmente no se mueve ni rota respecto a nada. En la
prctica resulta imposible encontrar un sistema realmente inercial que no se
mueva, ya que las mismas estrellas se mueven as que este sistema se utiliza
como aproximacin terica y para visualizar otros marcos de referencia ms
fcilmente. En la prctica consiste en un conjunto de ejes perpendiculares entre s
(como todos los sistemas de referencia) que no rotan ni aceleran respecto al
espacio inercial.

2.2.2 Sistema de coordenadas ECI i-frame

Este sistema de coordenadas tiene su origen en el centro de masas de la Tierra y se
supone que no rota respecto al espacio inercial, ya que se mueve con el planeta.
Sabemos que eso no es del todo cierto, porque la Tierra gira con respecto al Sol y
adems de su propia rotacin , pero se tomar este sistema como inercial.
Los ejes del sistema ECI estn fijados en las estrellas: El eje Z coincide con el eje
polar y el plano perpendicular al eje Z coincide con el Ecuador. El eje X e Y no
rotan con la tierra, apuntando X directamente al equinoccio Vernal.
El sistema de coordenadas inercial es importante porque las leyes de newton son
casi ciertas y permite visualizar ms fcilmente otros marcos de referencia no
inerciales.

2.2.3 Sistema de coordenadas ECEF e-frame

Tambin se le conoce como sistema de coordenadas geocntrico. Tiene su origen
en el centro de masa de la Tierra y sus ejes rotan con ella. El eje Z se dirige
directamente al norte a lo largo del eje polar. Los ejes X e Y estn en el plano

10

CAPTULO 2

ecuatorial con X dirigido hacia el meridiano de Greenwich (0 latitud, 0 longitud)

y el eje Y 90 hacia el Este.

Fig. 2.1 Esquema de coordenadas ECEF

2.2.4 Sistema de coordenadas de Navegacin n-frame

Tiene su origen en la localizacin del sistema inercial (longitud latitud). Es un
sistema local con sus ejes X Y en el plano tangente al punto de la Tierra donde est
el origen. Tpicamente el eje X apuntar al norte, ele eje Y al Este y el eje Z abajo,
aunque debe ser especificado.
Tambin se le conoce como NED (North, East, Down) ya que sus ejes apuntan a
estas direcciones. Otra posible configuracin, aunque no es la que se ha utilizado
en el proyecto, sera con el eje X apuntando al este, el eje Y apuntando al norte y el
eje Z apuntando hacia arriba, tambin conocido como ENU (East, North, Up).
Es un sistema que presenta dificultades cuando se acerca a latitudes cercanas a los
polos.

Navegacin Inercial

11

Fig. 2.2 Esquema de coordenadas de navegacin NED

2.2.5 Sistema de coordenadas Body b-frame

Este sistema tiene su origen en el centro de masas del vehiculo. Tpicamente usado
en plataformas Strapdown, es decir, cuando los sensores tienen tambin como
centro de masas el vehiculo y sus ejes se mueven con l.

Fig. 2.3 Esquema de coordenadas de Body en un vehculo

12

2.3

CAPTULO 2

Relacin entre los sistemas de coordenadas

Una vez descritos los sistemas de coordenadas con los que se va a trabajar, es
necesario algn tipo de algoritmo para pasar de un sistema a otro. Como siempre,
existen varios mtodos, todos tienen su utilidad pero unos funcionan mejor en
determinadas circunstancias que otros.

2.3.1 DCM
La Matriz de Direcciones Coseno es un sistema de cambio de coordenadas.
Simplemente rota un vector de un marco de referencia a otro, digamos del marco b
al marco a.
c11 c12
C c21 c22
c31 c32
a
b

c13
c23
c33

(2.1)

Cada componente de esta matriz es uno de los cosenos de los ngulos entre los ejes
de los dos sistemas de coordenadas. Acaba siendo una matriz de cambio de
coordenadas.
a

x
X
y C a Y
b

z
Z

(2.2)

Cumple un par de propiedades, igual que todas las matrices de cambio de

coordenadas:
Det( Cba ) = 1

(2.3)

(Cba ) 1 (Cba )T Cab

(2.4)

Se va a trabajar con esta matriz ya que es fundamental para futuros captulos, pero
ya se ver en 2.3.3 la manera de calcular sus elementos.

Navegacin Inercial

13

2.3.2 ngulos de Euler

Otro mtodo bastante popular para especificar la orientacin angular de un sistema
de coordenadas respecto a otro es el uso de los tres ngulos de Euler. Tres ngulos,
que mediante una sucesin ordenada de giros, definen el cambio de un sistema de
coordenadas a otro. Los ngulos de Euler (phi, theta, psi) corresponden con los
ngulos convencionales de roll ( ), pitch (), yaw () que se utilizan en
navegacin para especificar la actitud de un mvil. El ngulo de roll ( ) es el giro
de las alas del avin, el ngulo de pitch () la inclinacin del morro y el ngulo de
yaw () es el giro del morro del avin respecto al norte.

Fig. 2.4 Representacin bidimensional de los giros en roll, pitch y yaw.

Es importante sealar que los ngulos de Euler no estn nicamente definidos y

existen ambigedades. Por eso se deben hacer las rotaciones siempre en el mismo

14

CAPTULO 2

orden ya que los mismos ngulos, si son aplicados con rdenes diferentes, dan
lugar a transformaciones diferentes.

2.3.3 Cambio de Body a Navegacin con los ngulos de Euler

El origen es el centro de masas del mvil que corresponde con los de la plataforma
donde estn los sensores (Strapdown). Esta transformacin es una rotacin de los
ejes, totalmente ordinaria, pero se deben definir bien estos giros. En nuestro caso,
el mvil es un avin. En este sistema de coordenadas Body, el eje xb apunta hacia
el morro, el eje yb apunta al ala derecha y el eje zb apunta hacia abajo.
Estos ejes corresponden a los ejes de roll, pitch y yaw del vehiculo. Los signos de
estos ngulos estarn definidos tal que el ngulo de roll es positivo cuando el
ala derecha se mueve por debajo del plano de horizonte, el ngulo de pitch es
positivo cuando el morro del avin se eleva por encima de la lnea de horizonte y
el ngulo de yaw se define positivo cuando el morro esta rotando desde el norte
hacia el este.

Fig. 2.5 Representacin del avin en los ejes de coordenadas de navegacin (negrita) y
en los ejes de body XYZ. Todos los giros representan ngulos positivos.

Navegacin Inercial

15

En la Fig. 2.5 las lneas rojas representan cual es el sentido de giro para que los
ngulos sean positivos. Estos ngulos van desde los ejes de navegacin hacia los
del cuerpo (body).

2.3.3.1 Giro en Yaw

Es el primero de los giros que se har. Con esta matriz de cambio se puede pasar
del sistema de coordenadas (X, Y, Z) al nuevo sistema (X,Y,Z).
cos
C sin
0

sin
cos
0

0
0
1

(2.5)

2.3.3.2 Giro en Pitch

Giramos el sistema (X,Y,Z) para conseguir (X,Y,Z)
cos
C 0
sin

0 sin
1
0
0 cos

(2.6)

2.3.3.3 Giro en Roll

El ltimo de los giros: de (X,Y,Z) a (x, y, z)
0
1
C 0 cos
0 sin

0
sin
cos

(2.7)

Entonces, definimos la matriz de cambio de coordenadas C nb as:

C nb C C C

(2.8)

16

CAPTULO 2

cos cos
cos sin
sin

C cos sin sin sin cos cos cos sin sin sin cos sin
cos sin cos sin sin sin sin cos cos sin cos cos
b
n

(2.9)

La ecuacin (2.9) corresponde a la matriz de cambio de coordenadas de

navegacin a body. Sin embargo, la transformacin que nos interesa es justo la
inversa, buscamos un cambio de body a coordenadas de navegacin. Aplicando
(2.4) se obtiene que:
Cbn (C nb )T

(2.10)

2.3.4 Matriz de cambio de coordenadas de e-frame a n-frame

Es necesario definir una matriz de cambio de coordenadas del sistema centrado en
la tierra al sistema de coordenadas NED definido por los ejes xn, yn, zn que estn
alineados con las direcciones Norte, Este y Down. Esta transformacin Cen se hace
a travs de dos rotaciones: una del ngulo alrededor del eje ze y la otra del
ngulo alrededor del eje ye.
Rotacin alrededor del eje ze del ngulo :

x e cos
y sin
e
z e 0

sin
cos
0

0 xe
0 y e
1 z e

(2.11)

Una segunda rotacin alrededor del eje ye del ngulo :

xe cos 0 sin xe
y 0
1
0 ye
e
ze sin 0 cos ze

(2.12)

De estas transformaciones obtenemos:

xn cos cos
y sin
n
z n sin cos

cos sin

sin xe
xe

n
cos
0 ye Ce ye
ze
sin sin cos ze

(2.13)

Navegacin Inercial

17

Despus de hacer esta transformacin, se tendr el vector en coordenadas de

navegacin, pero con el origen referido al centro de la tierra. As que aun se debe
desplazar el sistema para que se corresponda con la localizacin de origen ( , )
que caracteriza este sistema de coordenadas.

2.4

Rotacin sideral de la Tierra

Es una magnitud bastante importante en algunas transformaciones o en el clculo

del error de algunos sistemas. Por eso se va a descomponer el vector de rotacin
sideral en coordenadas de navegacin (n-frame).
Dadas unas coordenadas de longitud y latitud ( , ), que son el origen del sistema
de coordenadas local. Los vectores unitarios i, j, k corresponden a los ejes x,y,z de
la tierra en coordenadas ECEF. As, podemos escribir como un vector de la
forma:
ie 0i y j z k

(2.14)

Donde y y z son las componentes en los ejes y, z del n-frame y ie es la

magnitud del vector de rotacin. El tiempo que la Tierra tarda en completar una
vuelta (da sideral) es de 23h, 56 min, 4.09s.

ie

360
15.04106874 deg/h
23 [56 (4.09 / 60)] / 60
ie ie cos j ie sin k

(2.15)
(2.16)

Se ha discutido la importancia de definir claramente los cambios de coordenadas y

los signos en los giros. De todos los sistemas de cambios, se ha demostrado la
robustez de utilizar los ngulos de Euler como principal herramienta de cambio de
coordenadas, por su simplicidad. Si bien es cierto que una vez definida una matriz
de cambio, da igual el mtodo empleado para obtenerla, porque para todos ellos es
la misma matriz.

3 Sensores
En este captulo se van a describir brevemente los diferentes sensores que se
utilizan en la plataforma: acelermetro, magnetmetro, giroscopio y GPS y
adems se va a caracterizar el modelo del error que afecta a sus medidas. Estos
sensores han aportado los datos experimentales que se han utilizado en el proyecto,
pero no se van a analizar en profundidad, porque no es este el objetivo.
Una vez explicados los cuatro sensores embarcados en el UAV y planteado su
modelo de error, se va a describir su implementacin en la plataforma, y la
circuitera que hay para almacenar todas estas medidas.
Adems de la caracterizacin de las medias y del breve resumen de cmo
funcionan estos sensores, tambin se describirn las condiciones en las que estos
sensores van a trabajar, es decir, todos los factores que puedan alterar de alguna
manera estas medias al embarcar los sensores en la plataforma del avin. Ya se
ver que este factor es crtico a la hora de decidir si las medidas de los sensores
son fiables y si existe algn mecanismo para evitar estas alteraciones.

19

20

CAPTULO 3

Algunos de estos dispositivos, tienen salidas analgicas y que deben digitalizarse

para su almacenamiento. La entrada de datos, por tanto tendr una ganancia
adicional a la del propio dispositivo que tambin se debe tener en cuenta:
x

V
x0 G DAC x0
2n

(3.1)

En el diseo, se ha utilizado un ADC de 16 bits con un margen de tensiones de

entrada de 0 a 5 V. La ganancia necesaria es por lo tanto GDAC=5/216. Este trmino,
aparece en todos los sensores que han sido digitalizados y se va a tener en cuenta a
la hora de calcular la ganancia. Es decir, el modelo de los sensores siempre
supondr que se transforma directamente de las lecturas almacenadas, esto es, en
formato binario y se aplican las transformaciones necesarias para que las unidades
de los datos sean consistentes con las unidades de medida utilizadas en el bloque
de procesado. Uno de los factores que aparece siempre en la conversin de
sensores analgicos, ser la ganancia de conversin de digital a analgico GDAC.

3.1

Acelermetro

Existen varios tipos de acelermetros dependiendo de cmo miden los valores de

la aceleracin. Se ha utilizado el sensor inercial LIS3L02AS4 de la casa
STMicroelectronics, que es un acelermetro lineal en 3 ejes. La medida de la
aceleracin se basa en la variacin de una capacitancia en el interior del chip. Se
trata de un MEMS que suspende partculas de silicio, ancladas en un punto fijo,
que se mueven libremente en el eje de medicin. Cuando una aceleracin acta,
toda esta masa de partculas se desplaza respecto a su posicin de origen y crea un
desequilibrio en la capacitancia, que se mide y da informacin de la aceleracin
que est actuando en ese eje.

Sensores

21

Fig. 3.1 Encapsulado del LIS3L02AS4 y esquema de la orientacin de los ejes de medida.

3.1.1 Modelo del acelermetro

Este dispositivo mide aceleracin en unidades de [m/s2]. Se trata de un dispositivo
analgico que mediante un escalado de la tensin a su salida, nos da informacin
de la aceleracin en cada eje. El factor de escala y el bias son parmetros que da
la hoja de especificaciones del fabricante. Por bias se entiende el error sistemtico
que aparece en las medidas y que es diferente del error aleatorio. Este es un
trmino que aparece en prcticamente todas las descripciones de los sensores
realizadas en este captulo.
Sin embargo, tanto el bias como el factor de escala, no son constantes, se van a
modelar como la suma del valor asignado por el fabricante y una variable aleatoria
que representa la variabilidad que estos factores tienen en la salida del sensor.
Adems de los parmetros del sensor, se le debe sumar una componente de ruido
blanco Gaussiano nacc.
a Gacc a0 ( Bacc bacc ) nacc

(3.2)

Siendo Gacc el factor de escala que multiplica la entrada a0, que es la salida directa
de los datos del sensor almacenados y a el valor de la aceleracin perfectamente
calibrada. Ya se coment de dnde viene la ganancia de conversin GADC. Segn el
fabricante, si se utiliza el sensor para que opere en el rango de aceleraciones de
6gs, entonces su ganancia es de 15/VDD [g/V], siendo VDD la alimentacin del
sensor (3.3V). Finalmente, se quiere operar las medidas en unidades del sistema
internacional, de ah que se aada el factor de conversin de gs a m/s2.

22

CAPTULO 3

Gacc GDAC

15
9.8
VDD

(3.3)

El margen de voltajes a la salida del sensor va de 0 a VDDV, pero igualmente se

incluyen aceleraciones negativas, as que se debe corregir esta constante, que el
fabricante especifica como:
Bacc

VDD
9.8
2

(3.4)

La variable aleatoria bacc se ha calculado empricamente. A partir de las medidas

del acelermetro realizadas antes del recorrido y en condiciones estticas se ha
estimado cul debe ser el bias adicional que afecta al sensor en ese momento.
La ganancia tambin se ve afectada por pequeos cambios de un experimento a
otro, pero se ha considerado que no era necesario modelarla y simplemente se ha
dado como correcto el valor del fabricante. Ya se comentar ms adelante cules
son las repercusiones de este modelo y sus posibles ampliaciones.

Fig. 3.2 Medidas del acelermetro en condiciones de esttica (m/s2).

Sensores

23

Como se puede ver en la Fig. 3.2, los valores de la aceleracin se han pasado al
Sistema Internacional (m/s2). A partir de este vector de medidas, que corresponde
a las lecturas de los acelermetros antes de una campaa de adquisicin, con una
ligera inclinacin en el ngulo de pitch. Se ha realizado una calibracin de la
estadstica de la seal y de los bias b en cada uno de los ejes. Es importante sealar
que si se repiten las medidas en esttica, las constantes de calibracin de la
aceleracin cambian. Incluso los bias cambian en un mismo experimento
dependiendo de la dinmica del sistema. Este es un problema bastante grave
porque un bias variable con el tiempo, no slo con el experimento, hace que los
resultados obtenidos nicamente de integrar INS para obtener una posicin sean
poco fiables y en un tiempo relativamente reducido, hagan divergir el sistema. Una
posible solucin, que se ampliar mas adelante, es la estimacin del bias como
vector de estados del filtrado. As se podr seguir su valor y se evita que la
solucin diverja.

Eje X

Eje Y

Eje Z

Bias b [m/s2]

0.01

0.5

-0.6468

n2 [(m/s2)2]

0.0014

0.0021

0.0921 10-4

Tabla. 3.1 Estadstica de la seal del acelermetro en esttica

Los datos de la Tabla. 3.1 son slo vlidos para las condiciones de medida que se
dieron el da de la prueba. Si se repitiese el experimento, sera necesario volver a
calibrar estos parmetros, que aunque no variaran mucho, s lo suficiente.

3.2

Magnetmetro

El magnetmetro que se ha utilizado es el HMC2003 de la casa Honeywell. Es un

sensor que mide la intensidad de campo magntico en 3 ejes. Gracias a estas 3
medidas se obtiene un vector de campo que da informacin del ngulo de azimut

24

CAPTULO 3

del mvil o de su actitud. Vendra a ser la versin ampliada a 3 dimensiones de la

brjula, pero este instrumento es capaz de distinguir giros en los ejes de roll y
pitch.
El campo se mide a travs unas magneto-resistencias que cambian su valor en
funcin del campo que las atraviesa en su direccin.

Fig. 3.3 Magneto-resistencia

3.2.1 Campo Magntico Terrestre

Para entender mejor el funcionamiento de este dispositivo y utilizarlo
correctamente en aplicaciones de navegacin, es necesario estudiar cmo acta el
campo Magntico Terrestre.
La Tierra acta como un dipolo, pero no est alineado con sus ejes de rotacin. Por
eso siempre aparece un trmino de Declinacin, que es la separacin entre el norte
magntico y el norte geogrfico. Por Norte magntico se entiende aquella regin
de la tierra donde las lneas de campo son perpendiculares a la superficie terrestre.
El campo magntico de la tierra se puede describir con 7 parmetros: declinacin,
inclinacin, intensidad horizontal, componentes X (apuntando al norte geogrfico)
e Y (apuntando al Este) en el plano horizontal, intensidad vertical e intensidad
total.
El valor de este campo en medida absoluta oscila entre los 25,000 y los 65,000 nT
(100,000 nT = 1 gauss).

Sensores

25

Gracias a las medidas en los 3 ejes del campo magntico, se puede calcular la
orientacin relativa de un objeto. En el caso de una brjula, siempre se supone que
se mide en el plano horizontal a la Tierra para obtener la direccin del norte. Los
compases magnticos, utilizan un inclinmetro como referencia de posicin para
calcular la orientacin de un objeto. De esta manera resulta sencillo calcular los
ngulos de roll y pitch y posteriormente hacer la compensacin de los ejes de
campo magntico para calcular el heading.
El problema que se nos presenta a nosotros, es que la dinmica del avin hace muy
complicado utilizar un inclinmetro en condiciones de movimiento del mvil. El
inclinmetro es un acelermetro que calcula la direccin del campo gravitatorio,
pero cuando el mvil acelera y gira, este vector no es tan fcil de encontrar y
queda enmascarado por otras aceleraciones.
Asimismo, utilizar la actitud estimada por los giroscopios para poder corregir las
medidas del magnetmetro, no es conveniente pues puede provocar un bucle
recurrente donde los propios resultados son los datos que se necesitan para
calcularlos

Fig. 3.4 Lneas de campo terrestre.

26

CAPTULO 3

3.2.2 Caracterizacin del error del magnetmetro

En las especificaciones del magnetmetro, se dice bien claro que cualquier campo
que afecte a HMC2003 puede cambiar los valores de las magneto-resistencias y
con ellos todas las medidas de campo que se hagan en adelante. Es por eso que el
sensor est embarcado lo ms alejado del resto de aparatos electrnicos del avin,
en su cola.
Sin embargo, se detectaron algunas anomalas en las medidas del magnetmetro
durante las campaas de vuelo. Sin causa aparente, el sensor estaba descalibrado.
El problema principal era que la ganancia en los 3 ejes era completamente
diferente y esto haca poco til la informacin que se lea.
Se decidi hacer un estudio en profundidad del magnetmetro para averiguar si
an se podra contar con este instrumento. El primer paso fue caracterizar las
medias en esttica e ir aadiendo componentes al problema hasta llegar a la
solucin final, es decir, con el sensor embarcado en el avin y ver en que punto del
proceso se desvirtuaban las medidas del magnetmetro y si era posible arreglarlo.

Fig. 3.5 Ganancias de los 3 ejes del magnetmetro antes (azul) y despus de la
calibracin (rojo).

La Fig. 3.5 representa los resultados de la primera prueba que se hizo con el
magnetmetro. Consista en medir el campo magntico terrestre alineando cada
uno de los ejes de medida del magnetmetro con la direccin natural del vector de
campo. Segn el modelo de campo terrestre, en la localizacin donde se hicieron
las medidas, el campo magntico tena una inclinacin de 5646 y una declinacin

Sensores

27

con respecto al norte verdadero de 18 Oeste. As que para medir cada uno de los
ejes, se puso el magnetmetro sobre una plataforma con esta inclinacin y
cuidadosamente preparada para que ningn material magntico estuviera cerca.
Cada eje se midi por sus dos sentidos (dndole la vuelta) y estas medidas son los
puntos que se utilizaron para dibujar la grfica. El valor del cero se ha sacado por
interpolacin, y es un dato importante para luego calcular el bias exacto en cada
eje.
Viendo la Fig. 3.5 es normal entender por qu las medidas no haban sido buenas.
Las grficas azules son las primeras medidas que se hicieron al sacar el sensor del
avin y medir directamente. El eje X haba perdido su polaridad y tena la
ganancia muy reducida, que era la causa de que los resultados con el
magnetmetro fueran totalmente incorrectos.
Para solucionar este problema se ha diseado un circuito de reset de los magnetoresistores (anexo II). El circuito no es ms que un generador de un pulso de
corriente, lo suficientemente elevada (16V) como para reordenar y realinear
magnticamente los componentes de la magneto-resistencia.
Lo que nos hace pensar hasta que punto es conveniente integrar este circuito de
reset al HMC2003 y con qu frecuencia debera hacerse el reset del instrumento.
Una vez calibrado el magnetmetro, se hizo otra prueba, esta vez midiendo desde
el avin. Se gir el avin dando una vuelta completa en intervalos de 90.
La Fig. 3.6 representa la alineacin del avin en cada una de las posiciones en las
que se le hizo girar. En la figura aparecen dos medidas, la orientacin sin
correccin y con correccin utilizando los resultados del experimento anterior
(Fig. 3.5). A priori se piensa que el sensor est perfectamente calibrado despus
del reset y si se aplican los parmetros estndar que propone el fabricante, se
obtiene la solucin en azul. La solucin en rojo se hizo despus de procesar los
datos con la calibracin.

28

CAPTULO 3

Fig. 3.6 Orientacin del avin en una vuelta completa, a intervalos de 90 grados. Los
vectores azules representan los ngulos sin corregir y los rojos, aplicando la correccin
adecuada a cada eje.

Esta solucin es mucho mejor, se aprecian los saltos de aproximadamente 90

grados y la perpendicularidad entre las diferentes posiciones, a diferencia de la
solucin antes de la calibracin (azul) no se vea. An hay errores pero la mejora
proporcionada por la calibracin es evidente.

3.2.3 Modelo del magnetmetro

Se va a utilizar un modelo de errores del magnetmetro muy parecido al que se ha
utilizado con el acelermetro (3.2).
m (Gm g m )m0 ( Bm bm ) nm

(3.5)

Esta vez, el parmetro de Ganancia se ha modelado como la multiplicacin de la

ganancia especificada por el fabricante Gm y la ganancia gm que se ha encontrado
por calibracin a partir del estudio realizado en 3.2.2. En este modo de

Sensores

29

funcionamiento, la ganancia de fabricante es la unidad, y los voltios a la salida del

magnetmetro son los gauss de campo magntico.
Gm G ADC

(3.6)

0
1.0743 0

gm 0
1
0
0
0 1.20537

(3.7)

Cuando se caracteriz la ganancia y se normaliz para obtener (3.7), se podra

haber calculado tambin la ganancia cruzada de cada eje. No obstante y aunque
hubiese sido un dato interesante, no se pens que fuera necesario para caracterizar
el modelo de error del instrumento. Haciendo un breve adelanto de las
conclusiones, una caracterizacin exhaustiva de los sensores es necesaria para
obtener resultados fiables.
Bm 2.5

(3.8)

El bias Bm especificado por el fabricante, es justo la mitad de la tensin de

alimentacin, como en muchos sensores.
Los parmetros aleatorios del bias bm, son los que se han calculado en 3.2.2 y el
clculo de la varianza del ruido se ha hecho estadsticamente a partir de las
medidas en esttica que se hicieron con el magnetmetro antes de una campaa de
adquisicin.
Eje X

Eje Y

Eje Z

Bias bm [gauss]

0.0985

-0.003

0.097

gm Ganancia

1.0743

1.20537

n2 [g2]

1.9 10-5

2.1 10-5

2.2 10-5

Tabla. 3. 2 Estadstica de las variables aleatorias del modelo de magnetmetro

30

3.3

CAPTULO 3

Giroscopio

Se ha embarcado en el avin el ADIS16350, que integra acelermetro en 3 ejes

con giroscopio en 3 ejes, ambas medidas de 14 bits y con salida a travs de un
puerto digital SPI. Las medidas del acelermetro no se han utilizado, ya que el
sensor que haba antes es ms preciso a pesar de la digitalizacin y slo se utiliza
el ADIS como giroscopio. El giroscopio embarcado transforma la fuerza generada
por un movimiento angular en una seal elctrica proporcional a ella. Gracias a
estos sensores se puede conocer la velocidad de giro de un mvil y por tanto,
tambin su orientacin.
El ADIS utiliza iMEMS, igual que el acelermetro y su funcionamiento se basa en
el principio del giroscopio resonador. De manera muy resumida, a travs de una
estructura resonante (microestructura) se mide la fuerza de Coriolis que aparece
cuando se gira, en forma de variaciones en la resonancia. Esta variacin es
cuantificada por una capacitancia y procesada para generar una salida elctrica que
nos sea conocida.

Fig. 3.7 Esquema del giroscopio ADIS y sus ejes de medida

Sensores

31

El giroscopio es el nico sensor que no ha sido necesario digitalizar porque el

ADIS ya daba una salida digitalizada de 14 bits. S que ha sido necesario
programar un microcontrolador PIC para que interrogue va SPI al giroscopio y d
las medidas de giro.
Los ejes dibujados son los ejes sobre los que se miden los giros y estos giros estn
hechos en sentido dextrgiro.

3.3.1 Modelo de giroscopio

Como modelo de error se va a utilizar uno parecido al del acelermetro:
g G g g 0 ( B g bg ) n g

(3.9)

Las unidades en que se mide la variacin de giro son [rad/s]. El fabricante da sus
especificaciones referidas a grados, pero para procesar los datos es ms interesante
expresarlos en radianes.
G g 0.018315

2
360

(3.10)

Se trabaja en modo de 75/s en todos los canales, que es la configuracin por

defecto para el giroscopio ADIS, por lo tanto el fabricante nos facilita este nmero.
Como se ha comentado ms arriba, se quiere trabajar en radianes, de ah que se
aplica el factor de conversin de grados a radianes.
Ya se ha comentado que el ADIS trabaja con una interfaz serie y su salida es
digital. Este es el nico de los sensores al que no ser necesario aplicarle el factor
GADC, pero la codificacin de los datos est en complemento a 2.
El bias Bg es nulo y la componente aleatoria del bias bg se ha calculado igual que
en el apartado anterior, estimando su valor en un vector de datos de medidas
cuando la plataforma estaba en reposo.

32

CAPTULO 3

Eje X

Eje Y

Eje Z

Bias bg [rad]

0.0045

-0.008

-0.0037

n2 [rad2]

2.4473 10-5

1.5596 10-5

3.3791 10-5

Tabla. 3. 3 Estadstica de las variables aleatorias del modelo del giroscopio.

3.4

GPS

Se va a hacer un breve resumen del funcionamiento del GPS y la caracterizacin

de su error. A efectos prcticos, slo se van utilizar los resultados de posicin y
velocidad que da el receptor GPS.
El GPS es un sistema de navegacin de satlites, mantenido por el Departamento
de Defensa de EEUU (DoD). La constelacin actual de GPS consiste en 28
Satlites (II/IIA/IIR), que ocupan 6 planos orbitales inclinados 55 grados con
respecto al ecuador. Los satlites de GPS envan mensajes de navegacin y ofrecen
un sistema de navegacin global, ininterrumpido e independiente del clima. La
idea que hay detrs es garantizar, con todos estos satlites en rbita, que siempre
habr al menos 4 satlites visibles para el usuario y as se podrn calcular los 4
parmetros desconocidos que son x, y, z y el tiempo. Toda esta tecnologa se basa
en la sincronizacin y precisin de los relojes que hay en cada satlite y en el
conocimiento de la posicin de los satlites, sino el sistema no funcionara.
Para solucionar el problema de la posicin de cada satlite, dentro del mensaje que
se emite en la frecuencia asignada, est incluida informacin de sus efemrides.
Las efemrides no es ms que una serie de parmetros que dan informacin de la
elipse orbital que est haciendo el satlite.
Es por eso, que el DoD tiene varias bases distribuidas por el planeta, que se
encargan de la correccin de los parmetros de los relojes de los satlites, de los
parmetros de las efemrides que transmite cada satlite (que se actualizan cada
pocas horas) y del buen funcionamiento del sistema en general.

Sensores

33

3.4.1 Caracterizacin del error

Reloj del satlite: a pesar de que los satlites tienen relojes muy precisos (cesio y
rubidio) y aunque estos parmetros se monitoricen continuamente desde tierra,
introducen un error.
Prediccin de las efemrides: est claro que el mensaje que envan los satlites con
sus efemrides no es ms que una estimacin, que contiene cierto error residual.
Adems, las efemrides se actualizan cada cierta hora y este parmetro dependera
tambin del tiempo, ya que se degradara ms la informacin cuanto ms tiempo
pase.
Retardo ionosfrico: la ionosfera es un medio dispersivo localizado en la regin de
la atmsfera situada entre 70 y 1000 Km. de altura sobre la superficie de la tierra,
al tratarse de un medio dispersivo, el retardo adems de ser funcin del ndice de
refraccin depende de la frecuencia.
Retardo troposfrico: la troposfera es un medio no dispersivo situado en la parte
ms baja de la atmsfera. Al no ser dispersivo, el retardo depende nicamente del
ndice de refraccin que es funcin de la temperatura, la presin, y la humedad
relativa local, pero no depende de la frecuencia. Por tanto la nica forma de
reducir este error es estimar el retardo troposfrico mediante los modelos y
observaciones meteorolgicas locales.
Ruido y resolucin del receptor: teniendo en cuenta que la seal de GPS se recibe
con una potencia de entre -154 y -166 dBW, hay que considerar el ruido interno
del receptor. Otra fuente de error se debe a que la resolucin temporal del
correlador del receptor no es infinita.
Propagacin multicamino y shadowing: el multipath nicamente es importante
cuando en los esquemas de transmisin el retardo con el que llega la seal
interferente es menor que el periodo de chip.
Al final, todas estas fuentes de error se traducen, segn la hoja de especificaciones
del Lassen iQ GPS Receiver de Trimble, en una precisin de las medidas tal que
el error cumple estas especificaciones:
Horizontal: < 5 m (50%) y < 8 m (90%)

34

CAPTULO 3

Altitud: < 10 m (50%) y < 16 m (90%)

Velocidad: 0.06 m/s
Utilizando estas especificaciones y modelando el error del GPS como una variable
aleatoria guassiana, se puede calcular la varianza de estas medidas utilizando una
tabla de la funcin normal.
2
horizontal
23.65m 2
2
vertical
94.6025m 2

Hay que sealar que cuando se habla de plano horizontal y altura, el GPS asume
que est trabajando en un sistema de coordenadas de longitud, latitud y altura, pero
la orientacin de los ejes coincide con el sistema de coordenadas de navegacin,
donde el plano horizontal es tangente a la superficie y el eje de altura apunta hacia
abajo (NED).

3.5

Integracin de los sensores en la plataforma del avin

En los apartados anteriores se ha descrito cada uno de los sensores que hay
embarcados en la plataforma del avin. En este apartado, se va a describir, cmo
estn estos sensores embarcados y que consideraciones se tienen a la hora de
procesar las medidas.
Un punto importante ser el de unificar todos los ejes de coordenadas. Cada sensor
tiene su propia orientacin de los ejes de medidas y no tiene porqu coincidir con
los otros. La orientacin de cada sensor aparece en la Fig. 3.8 y como se explica
tambin en el apartado 6.1, para compensar la desalineacin entre los ejes de los
sensores, simplemente se aplica un cambio de coordenadas o un cambio de ejes.

Sensores

35

Zg
Zm

Yg

Ym

Yg

Xb
Xm

Xg
Xg
Zg

Yb
Zb

Fig. 3.8 Foto de la distribucin de los sensores con sus ejes. Todos los ejes deben cambiar
de orientacin con tal de respetar los ejes de coordenadas del avin.

En color azul, cerca de la cola, est el magnetmetro, para evitar interferencias

magnticas. En verde estn representados los ejes del ADIS y en rojo, los ejes del
acelermetro, ambos sensores embarcados en la cavidad del avin, a la altura de
las alas. El objetivo de los cambios de coordenadas, es que de cara al procesado y
filtrado, todos estos cambios sean transparentes y todas las medidas estn
alineadas en los ejes de coordenadas del sistema de Body del avin (b-frame).

b
mag

1 0 0
0 1 0
0 0 1

(3.11)

36

CAPTULO 3

b
giro

1 0 0
0 1 0
0 0 1

(3.12)

0 1 0
1 0 0
0 0 1

(3.13)

b
acc

Estas ecuaciones multiplican la salida de cada sensor, para que cuando se procesen
las medidas, no se tenga que hacer la transformacin, es decir, una vez aplicados
estos cambios, todas las medidas estn en coordenadas de body y no habr
necesidad de preocuparse.
Ya se ha visto dnde estn situados los sensores pero es igualmente importante
cmo estn interconectados y qu circuitera hay detrs para que su salida,
recordemos analgica en el caso de dos de ellos, se almacene y se pueda procesar
luego.

Acelermetro

SPI

ADC
16 bits

PIC

RS232

DL1

Magnetmetro

SPI

Giroscopio

PIC

RS232

GPS

RS232

DL2

DL3

Fig. 3.9 Esquema de bloques del sistema de almacenamiento de los sensores.

Sensores

37

El acelermetro y el magnetmetro, los dos sensores analgicos que se han

embarcado en el avin, se conectan a un ADC de 16 bits. Este ADC se comunica
con un procesador PIC a travs de una conexin SPI (Serial Peripheral Interface
Bus) y la funcin del PIC (18F4520) es exclusivamente gestionar la entrada
digitalizada de los sensores y envirsela a un dataloger (DL1) a travs del
protocolo RS232. Slo con esta entrada, el dataloger ya gestiona los datos y los
almacena en una memoria Flash.
Para los otros sensores, el esquema es ms sencillo, el giroscopio (ADIS) ya tiene
la salida digitalizada y solo hace falta un PIC para intercomunicarlo con el DL2 y
el GPS ya tiene salida RS232, as que directamente se conecta a otro dataloger. En
total hay 3 datalogers.
La tasa de informacin no es la misma para todos los dispositivos, los sensores
inerciales y el magnetmetro trabajan a 48 muestras por segundo mientras que el
GPS slo 1 por segundo.
Hay que destacar que la calibracin es diferente en cada experimento y vara con
el tiempo. Es decir, estas medidas son slo vlidas para la campaa de medidas
que se hizo el 15 de Enero y cuando estas condiciones cambien, es muy posible
que algunos de estos parmetros hayan cambiado tambin. De ah la importancia
de calibrar siempre los sensores siempre antes de realizar una campaa de medias.

Lo ms relevante de este captulo es la caracterizacin del error que se hace de

cada sensor. As pues, la caracterizacin del magnetmetro fue bastante completa,
aunque eso obligara a realizar algunos pasos hacia atrs, pero el resultado invita a
pensar que mereci el esfuerzo. Como conclusin, cuanto ms minucioso sea el
estudio y el modelo de cada sensor, ms fiable sern sus resultados. Es algo
totalmente obvio, pero nada despreciable cuando se trabaja con sensores de cuya
exactitud dependen los resultados finales.

4 Actitud y Cinemtica
En este captulo se discute cmo se procesan los datos que recogen los sensores, es
decir, lecturas de la aceleracin, de la velocidad de giro y del campo magntico,
todas ellas en el sistema de coordenadas de Body del avin. Se analizan los pasos
que se siguen para convertir estos datos a coordenadas de navegacin, que es el
marco de referencia en el que se calcula la solucin. Una vez est la informacin
referida al sistema de coordenadas adecuado, se detallan las operaciones para
calcular la solucin de posicin y actitud del sistema. El objetivo de este captulo
es obtener una salida de posicin, velocidad y actitud del UAV a partir de las
medias de los sensores. Es decir, una vez acabado el captulo, se tratarn todos los
resultados como un nico bloque, llamado INS, cuya entrada sean los sensores y
cuya salida sean la posicin, velocidad y actitud del mvil en el sistema de
referencia de navegacin.
4.1

Clculo de la actitud

La primera de las variables que se calcula es la actitud del mvil. Este vector es
imprescindible para clculos posteriores de la posicin, ya que la matriz de cambio

39

40

CAPTULO 4

de coordenadas depende directamente de l. Existen varios mtodos para calcular

la actitud. A continuacin se explican dos de ellos.
El primero se basa en las medidas de los giroscopios y es un algoritmo
acumulativo, donde la actitud anterior es necesaria para calcular la actitud
presente.
El segundo mtodo utiliza las medidas del magnetmetro, que son medidas
absolutas para calcular la actitud del UAV. Es un sistema independiente de las
condiciones iniciales, pero presenta el problema de que son necesarias las medias
de roll y pitch y precisamente estos son de los ngulos que se buscan.

4.1.1 Clculo de la actitud a partir de los giroscopios

Para calcular la actitud del mvil slo se necesita conocer los valores de los
giroscopios y el valor inicial de la actitud de la plataforma strapdown.
Haciendo el cambio de coordenadas a partir de los ngulos de Euler para las
velocidades angulares de los giroscopios, se obtiene la ecuacin diferencial, tal y
como se explica en [5]:

1 sin tan

E 0
cos
0 sin sec

cos tan p
sin q
cos sec r

(4.1)

Donde [p q r] son las variaciones en el giro de cada uno de los ejes, medidas por el
giroscopio, es decir, las variaciones de giro en coordenadas de body y [ ]
son las variaciones de los ngulos de Euler en sistema de coordenadas de
Navegacin. Como aproximacin, esta matriz se construye a partir del valor de la
actitud anterior. Se supone que de una muestra a la otra pasa un tiempo
razonablemente pequeo (1/48 s) como para que no cambien tanto los valores de
roll, pitch y yaw.
Este sistema de ecuaciones diferenciales presenta un problema aadido y es que
cerca de 90 aparece una singularidad. Es lo que se conoce como gimbal-lock

Actitud y cinemtica

41

y no debera ser tan raro encontrarse estos valores en el ngulo de pitch, por
ejemplo, en ascensos o descensos muy pronunciados.
Se ha asumido que durante los tramos donde se requiere estimar la actitud del
avin, las condiciones de vuelo nunca sern tan extremas como para llegar al
gimbal-lock. An as, es interesante saber cmo se soluciona el problema de
manera general. Aqu es donde entra en juego el sistema de Quaternions (anexo I),
que no presenta este problema de bloqueo. Sin embargo, Rnnbck [8] concluye
en su trabajo, que un error en uno de los ngulos se propaga en todos los
quaterniones, con lo que el error de transformar y destransformar no hace que sea
tan buen sistema si y slo si los valores del ngulo de pitch nunca llegan a valores
de 90 .
Una vez obtenidos los diferenciales de los ngulos de Euler y contando con los
valores iniciales del vector de actitud E se integra cada resultado para ir
actualizando el valor de la actitud. Se aproxima a una integracin en tiempo
discreto (las medias se recogen cada periodo de tiempo) y se le suma el valor de la
actitud en la muestra anterior. As, el sistema es dependiente de los valores
iniciales de este vector para que los clculos sucesivos sean consistentes.

E (t ) E ( p (t ), q (t ), r (t ))dt E[n 1] E [n]t

(4.2)

Una vez calculada la actitud E[n], se puede calcular la matriz de cambio de

coordenadas aplicando (2.9) o utilizar los valores de los ngulos por s mismos.

4.1.2 Clculo de la actitud a partir de los magnetmetros.

Este mtodo es mucho ms directo pero se basa en que ya se conocen los ngulos
de roll y pitch a partir de las medidas de los sensores inerciales (4.2).
Se trata de proyectar al plano horizontal, las componentes magnticas medidas y
una vez obtenidas, calcular el azimut (yaw) a partir de relaciones trigonomtricas.

42

CAPTULO 4

Fig. 4.1 Proyeccin de las componentes de campo magntico al plano horizontal. Imagen
sacada de [10]

y
arctan h
xh

(4.3)

xh x cos y sin sin z cos sin

(4.4)

yh y cos z sin

(4.5)

Donde

El heading se calcula a partir de la arctangente de los componentes horizontales

del campo. Esta manera de resolver la orientacin es la misma que se utiliza en
algunos compases magnticos electrnicos, donde con un magnetmetro y dos
acelermetros para calcular la inclinacin, se obtiene el ngulo de azimut. El
problema es que nuestros acelermetros, no solo miden la componente
gravitacional en sus ejes, sino que cuando el vehculo est en funcionamiento,
mide otras aceleraciones, con lo que slo son vlidas las medidas de tilt
(inclinacin) si el avin est en reposo.

Actitud y cinemtica

4.2

43

Dead Reckoning con acelermetros y giroscopios

El Dead Reckoning calcula la posicin del vehculo a partir de los acelermetros y

los giroscopios nicamente. Est claro que despus de mucho tiempo, este sistema
acumular un error considerable, ya que no tiene ninguna referencia global y se
basa solamente en la integracin de los acelermetros y los giroscopios, ambos
sensores inerciales. Como se plantea en los objetivos del trabajo, a travs de la
combinacin de los sensores inerciales (INS) ms las medidas absolutas de GPS y
magnetmetro se espera obtener una solucin ptima de precisin.
De cualquier manera, antes de combinar estos datos, se debe calcular primero la
posicin slo a partir de los sensores inerciales, que en un periodo corto de tiempo
tiene ms precisin que cualquier sistema de localizacin global.

4.2.1 Cambio de las aceleraciones de Body a Navegacin

Ya que se va a trabajar con magnitudes que estn referidas a un sistema de
coordenadas, de ahora en adelante, el subndice indicar a qu marco se refieren

dichas medidas. As, las lecturas de los acelermetros ab a x

ay

az

estn

referidas al sistema de Body del avin.

El mdulo del vector de gravedad g se va a suponer constante ya que el mvil est
siempre afectado por valores parecidos del campo gravitacional.
gx
0 g sin

n
g b g y C b 0 g cos sin
g z
g g cos cos

(4.6)

La plataforma inercial est en un sistema inercial que rota. As que se debe

corregir la fuerza resultante que aparecer en las medias de los acelermetros. La
aceleracin del sistema, referida a un sistema de coordenadas idealmente inercial
respecto a la Tierra es:

44

CAPTULO 4

2r
g
t 2 |I

(4.7)

Donde el subndice I indica que se trata de un marco referencia idealmente inercial

2.2.2. Despus de operar esta ecuacin, siguiendo el desarrollo de [5], se obtiene el
resultado donde se sustrae de la aceleracin total, el efecto de Coriolis y la
gravedad a la que est sometida la plataforma:
a Ib ab ( ) vb g b

(4.8)

Se debe vigilar mucho con el significado de la ecuacin (4.8). Realmente, no se ha

hecho ningn cambio de coordenadas, ha sido un cambio puramente conceptual de
ab del sistema de Body a aIb referido a un sistema de Body tericamente inercial.
En la ecuacin, ab es la aceleracin medida por el acelermetro, las omegas son la
cantidad de rotacin total del cuerpo debido a la rotacin de la Tierra y del
propio cuerpo . El giroscopio no distingue entre estas dos rotaciones y mide las
dos juntas. gb es el vector de gravedad y aIb es la aceleracin en coordenadas de
Body una vez se han compensado de la aceleracin de Coriolis y la gravedad.

p q r

(4.9)

La ecuacin (4.8) desarrollada para cada eje:

a Ibx abx vby r vbz q g sin

(4.10)

aIby aby vbx r vbz p g cos sin

(4.11)

a Ibz abz vbx q vby p g cos cos

(4.12)

Con estas ecuaciones se obtiene la aceleracin bajo las condiciones de giro del
sistema de coordenadas de navegacin. Se aplica un cambio de coordenadas al
vector de aceleracin y se obtiene el vector de aceleracin del mvil en
coordenadas de navegacin.
an Cbn a Ib

(4.13)

Actitud y cinemtica

45

4.2.2 Integracin de las aceleraciones

Operar el vector de aceleraciones una vez estn en el sistema de coordenadas de
navegacin resulta sencillo. Se trata de una aceleracin en un sistema de
coordenadas determinado y esta aceleracin describe perfectamente la trayectoria
del mvil. Por lo tanto:

vn [n] a n dt vn [n 1] an [n]t

(4.14)

Se integra la aceleracin utilizando un integrador sencillo de orden 1 y se obtiene

la velocidad

rn [n] vn dt rn [n 1] vn [n]t

(4.15)

Se integra la velocidad para obtener la posicin. Ambas integrales necesitan de las

condiciones iniciales, que son las soluciones de la muestra anterior. De ah que el
error crezca a medida que pasa el tiempo, porque cualquier error en un instante de
tiempo se propaga a todas las muestras siguientes y no hay manera de corregirlo.

4.3

Diagrama de bloques de la actitud y cinemtica

4.1.2

mo

go

[ ]m

4.1.1

[ ]g
CI

ao

4.2.1

an

CI
vn

Fig. 4.2 Diagrama de bloques del procesado de la actitud y la cinemtica

rn

46

CAPTULO 4

Como resumen del captulo es interesante describir un diagrama de bloques donde

se aplican las ecuaciones discutidas anteriormente y adems se indica en qu orden
se han realizado los clculos.
En cada bloque se ha puesto el subapartado donde se ha explicado qu proceso
seguir. Algunos de estos bloques hacen ms de una operacin y no se les ha
asignado ningn nombre. De cara a captulos posteriores, cuando se haga
referencia al procesado de los datos, simplemente se tratar todo el diagrama de
arriba como una caja negra, llamada INS cuyas entradas sern las lecturas de los
sensores y cuya salida ser la actitud, aceleracin, velocidad y posicin en
coordenadas de navegacin.

5 Filtro de Kalman
En este captulo se introducir brevemente el algoritmo de filtrado de Kalman.
Gran parte de la explicacin ser referenciada y slo aparecern las ecuaciones
bsicas. Tambin se har una simulacin simplificada de los modelos que se han
utilizado en el proyecto a modo de verificacin de que el algoritmo funciona
correctamente.
El filtro de Kalman es una ampliacin del algoritmo LMS de Mnimos cuadrados,
pero ms sofisticado. Imaginemos que dos personas caminan a velocidad
constante, en direcciones contrarias por la misma calle. Imaginemos tambin que
estamos adquiriendo la posicin de ambas a la vez, por ejemplo con un radar o con
algn otro instrumento. El mtodo de mnimos cuadrados calculara cul es la
posicin a partir de la minimizacin del error y obtendramos una gran precisin.
Ahora, si se sigue vigilando el movimiento de estas dos personas hasta el momento
en que se crucen, cmo se puede asegurar que la posicin de la persona 1 y de la
persona 2 son correctas? Cmo puede nuestro algoritmo diferenciar entre los 2
mviles una vez que se han cruzado?
Aqu es donde entra en juego el algoritmo de Kalman. Principalmente minimiza el
error, pero lo hace basndose en ms parmetros. Adems predice cul debera ser

47

48

CAPTULO 5

la salida para el estado siguiente y da una estimacin a partir de las medidas en el

mismo instante de tiempo. Todas estas medidas tienen su propia estadstica y
dependiendo de cual sea ms fiable, se otorga ms peso a la prediccin o a la
estimacin. En el fondo se integran estados redundantes para obtener informacin
ms precisa.
As pues y volviendo al ejemplo anterior, en el momento en el que se cruzan,
ambos mviles tendrn la misma posicin, pero sus velocidades sern contrarias.
El algoritmo dice que es muy difcil que hagan un cambio de velocidades tan
brusco como para cambiar de signo, as que mantiene correctamente las
trayectorias y no confunde a los dos mviles.
En este PFC, el filtrado de Kalman se utiliza para obtener la posicin y la actitud
del avin. No ser tan sencillo como el ejemplo, por un lado estn las medidas de
los inerciales, que son muy precisas en un espacio de tiempo reducido y las
medidas del GPS y del magnetmetro, cuyo error no aumenta con el tiempo, sino
que es constante, pero tienen un error ms grande.

5.1

Ecuaciones de Kalman

Todas las variables que el filtro de Kalman utiliza para estimar una solucin
ptima, se agrupan dentro de un mismo vector llamado vector de estado:
x x1

x2

x3 ... x n

(5.1)

El vector de estado incluye no slo los parmetros deseados, sino adems aquellos
que se han aadido al filtro con tal de introducir redundancia al problema. El
vector de medidas, en cambio, es una agrupacin de todas las variables cuya
medida est disponible:
z z1

z2

z 3 ... zl

(5.2)

Se suele dar el caso de que la dimensin (n) del vector de estados x sea ms
grande que la dimensin (l) del vector de medidas z. Ambos vectores, estn
relacionados de manera lineal tal que a partir del vector de estados, se calcula el
vector de variables.

Filtro de Kalman

49

Esta es una de las ecuaciones de Kalman que describen el algoritmo.

z (t ) H (t ) x(t ) v(t )

(5.3)

zk = H k xk + vk

(5.4)

vk ~ N (0, Rk )

(5.5)

Se ha incluido la ecuacin en forma continua y en forma discreta. Todos los

clculos que se han hecho han sido utilizando la forma discreta del filtro de
Kalman, por eso, en adelante slo se describirn las ecuaciones en el dominio
discreto, aunque tambin existe para el dominio continuo.
La matriz Hk relaciona el vector de estados con el vector de medidas, el vector vk
es el ruido de medidas, siendo una variable aleatoria gaussiana, de media cero y su
matriz de varianzas es Rk.
Rk E{vk vk } diag ( z21 , z22 ,..., z2l )

(5.6)

El mtodo de filtrado de Kalman busca el estimador lineal y sin bias, con la

varianza del error mnima y sin conocimiento a priori del estado a estimar. A
travs de la ecuacin de estados se puede hacer la prediccin de cul es el valor del
vector de estados xk a partir de xk-1 .
xk = k 1xk 1 + wk 1

(5.7)

wk ~ N (0, Qk )

(5.8)

La matriz phi da una relacin entre el estado anterior del KF y el estado presente
y es la solucin de la ecuacin diferencial que se presenta en (5.9). Este es el
primer paso para la estimacin ptima: una prediccin slo a partir de las medias
del estado anterior.

x (t ) F (t ) x(t ) w(t )

(5.9)

Las ecuaciones del vector de estados del filtro de Kalman en el dominio continuo.
Es necesario utilizar el dominio continuo para solucionar la ecuacin diferencial y
discretizar luego.

(t ) i i
F
i!
i 0

(t , ) e F (t )

(5.10)

50

CAPTULO 5

La solucin de (5.10) es vlida cuando F es una funcin invariante en el tiempo.

Se va a tomar la linealizacin de primer orden como solucin de la matriz :
I tF

(5.11)

El vector de ruido wk tambin se tiene que modificar cuando se discretiza la matriz

del vector de estados. Siendo:
Q diag ( x21 , x22 ,..., x2n )

(5.12)

la matriz de covarianza del vector de ruido en la forma continua de (5.9).

Entonces, si se discretiza k y siguiendo el desarrollo de [12], la matriz de
covarianza del ruido queda de la forma:
Qk E{wk wk } k Q k t

(5.13)

Una vez presentadas las ecuaciones de estado y de medias, se desarrolla el

algoritmo. Partimos de la base que se busca un estimador x que minimice el error
cuadrtico medio.
Siguiendo el desarrollo que hace Grewal en [7] se llega a la sntesis de las 5
ecuaciones de Kalman que definen el algoritmo. La secuencia en que son
solucionadas estas ecuaciones es la misma en que aparecen escritas, aunque se
trata de un algoritmo cclico y no importa en que paso se empiece.
Estimacin a priori:
xk = k 1 xk 1 ( )

(5.14)

Pk () k 1Pk 1 ( )k 1 Qk 1

(5.15)

Adems de calcular el vector de estado a partir del anterior, se actualiza la matriz

de covarianza del error de la estimacin a priori Pk(-). Este clculo no afecta
directamente al vector de estados, pero es muy importante para ponderar la
ganancia de Kalman.
Estimacin a posteriori:
xk ( ) xk ( ) K k [ zk H k xk ()]

(5.16)

Pk ( ) [ I K k H k ]Pk ()

(5.17)

Filtro de Kalman

51

Ganancia de Kalman:
K k Pk () H k [ H k Pk () H k Rk ]1

(5.18)

Quiz la ecuacin (5.18) sea la ms caracterstica del algoritmo y Kk se conoce

como Ganancia de Kalman. Es gracias a esta ecuacin que se ponderan los valores
de las medidas y los valores de la prediccin de estados y con tal de introducir el
mnimo error a la solucin, se da mayor peso a uno o a otro.
La figura siguiente es el diagrama de bloques del algoritmo. No est incluido el
clculo de las matrices de covarianza Pk(-) y Pk(+) pero tambin se tienen en
cuenta, ya que en cada iteracin se actualizan sus valores y cambian segn cambie
la estadstica del sistema.

zk

~
zk

Kk
-

x k ( )

+
+

k-1

Hk
x k ( )

delay
x k 1 ( )

Fig. 5.1 Diagrama de bloques del Filtro de Kalman.

5.2

Aplicaciones no lineales del filtro de Kalman

Las ecuaciones que relacionan los estados del sistema han pasado a ser no lineales:
xk f ( xk 1 , k 1) wk 1

(5.19)

z k h( xk , k ) vk

(5.20)

52

CAPTULO 5

De la misma forma que antes, la estadstica del ruido w y v es gaussiana y de

media nula. Para resolver este sistema como un filtro de Kalman, es necesario
linealizar estas funciones para poder aplicar el algoritmo descrito en 5.1.
Existen dos mtodos para linealizar el sistema siendo ambos muy parecidos
excepto por el punto alrededor del que se linealiza.

5.2.1 Linealizacin respecto a la trayectoria nominal

Sea una trayectoria la solucin particular de un sistema estocstico. El trmino de
trayectoria nominal se refiere, en este caso, a aquella trayectoria obtenida cuando
las variables aleatorias tienen valores que son sus valores esperados. Por ejemplo,
el vector xknom que se obtiene de la ecuacin:
xknom f ( xknom
1 , k 1)

(5.21)

Tiene ruido cero y media x0nom que como condicin inicial sera la trayectoria
nominal para un sistema discreto en tiempo.
No siempre una solucin ser la trayectoria nominal, sino que se le aade un
trmino de perturbacin que representa los pequeos cambios que se producen
respecto a la trayectoria nominal.

xk xk xknom

(5.22)

z k z k h( xknom , k )

(5.23)

Si la funcin f es diferenciable, entonces las perturbaciones de la trayectoria se

pueden aproximar con las series de Taylor alrededor de la trayectoria nominal.
xk f ( xk 1 , k 1)
f ( xknom
1 , k 1)
xknom

(5.24)
f ( x, k 1)
x

f ( x, k 1)
x

xk 1 ...
x x knom
1

xk 1 ...
x xknom
1

(5.25)

Si suponemos que los trminos de orden superior se pueden despreciar, entonces:

Filtro de Kalman

53

xk [k1]1 xk 1 wk 1
[k1]1

f ( x, k 1)
x

f1
x
1
f 2
x1
f 3

x1

f n
x
1

f1
x2
f 2
x2
f 3
x2

f n
x2

(5.26)
(5.27)

x xknom
1

f1
x3
f 2
x3
f 3
x3

f n
x3

f1
x n

f 2
...
x n
f 3
...

x n

f n
...
x n x x nom
...

(5.28)

k 1

La matriz se puede representar como la derivada en xknom

1 de la funcin f.
De manera anloga al desarrollo de xk , se desarrolla la matriz H, es decir, se
aproxima como la serie de Taylor, siempre y cuando h sea una funcin derivable y
se evala en el punto de la trayectoria nominal. La matriz H tiene l filas y n
columnas.

z k H k[1] x k v k

(5.29)

A partir del cambio de variable (5.22) y con las ecuaciones (5.26) y (5.29) se ha
modificado el sistema para que se aplique el filtrado de Kalman tal y como se ha
explicado al principio del apartado. Un detalle importante del algoritmo es que se
optimiza la perturbacin de la medida y esto supone recomponer la seal a partir
de la trayectoria nominal y de la salida del filtro de Kalman.

5.2.2 Linealizacin respecto a la trayectoria estimada

Este mtodo es muy parecido al anterior. El problema de la linealizacin a partir
de una trayectoria nominal, es que a veces, esta trayectoria es desconocida y por lo
tanto, no se puede aplicar el algoritmo. En vez de asumir que se conoce parte de la
solucin, este mtodo se basa en linealizar alrededor del estado estimado a priori y
a partir de ah, construir las matrices de los estados del filtro.

54

CAPTULO 5

El planteamiento a seguir es idntico al del apartado 5.2.1:

[k1]

f ( x, k )
x x xk ( )

(5.30)

H k[1]

h( x, k )
x

(5.31)

x xk ( )

Una vez definidas las matrices de estado del sistema, las ecuaciones quedan de la
forma:

xk [k1]1 xk 1 wk 1

(5.32)

z k H k[1] x k v k

(5.33)

Donde el trmino x vuelve a denotar la diferencia entre el vector de estados real

x y el estimado a priori x(-).
5.3

Modelo forward

Este es uno de los modelos [8] que se han utilizado en el trabajo para integrar las
medias de los inerciales y las del GPS/magnetmetro. Es la primera aproximacin
al problema y se ha utilizado una integracin descentralizada (Loose Integration).

Acelermetro
Giroscopio

INS

d x

GPS
Mag

dx

KF12

Fig. 5.2 Diagrama de bloques de la solucin descentralizada forward

Filtro de Kalman

55

Una vez presentado el modelo, se ha verificado su funcionamiento con una

simulacin. Para ello, se ha utilizado un filtro de Kalman de 12 estados, donde:

x X

vx

vy

vz

(5.34)

XYZ representan el vector posicin en coordenadas de navegacin (NED)

vx vy vz son las velocidades en el sistema de coordenada de navegacin.

son los ngulos de Euler del mvil desde los ejes del sistema de
navegacin.

son las variaciones de ngulos de Euler.

El vector de medias es:
X INS
INS
Y
Z INS

z v xINS
v INS
yINS
vz
INS

X GPS

Y GPS
Z GPS

v GPS

v GPS
y

v GPS

z
mag

(5.35)

La matriz que relaciona el vector de estados anterior con el presente se ha

construido de acuerdo con el apartado 5.1:
03 x 3
0
3x3
F
03 x 3

03 x 3

I 3x3

03x3

0 3 x3

03x3

0 3 x3

03x3

0 3 x3

03x3

I 3 x3
0
3 x3
k I t F
0 3 x3

0 3 x3

03 x 3
03 x 3

I 3x3

03 x 3

t I 3 x 3

03 x 3

I 3x3

03 x 3

03x3

I 3x3

03x3

03 x 3

(5.36)

03 x3
03 x3

t I 3 x 3

I 3x3

(5.37)

La dimensin de k es nxn siendo 12x12. La matriz Hk de medidas, de acuerdo

con el vector de estados, es de la forma:

56

CAPTULO 5

I 3x3
H k 03 x 3
01x3

0 3 x3

0 3 x1

I 3 x3

0 3 x1

01x 3

0 3 x5
0 3 x5
01x 5

(5.38)

Siendo Hk una matriz de dimensiones 7x12. Ambas matrices presentan una

formulacin muy simple. Esto es debido a que gran parte de las ecuaciones se han
resuelto en el bloque de INS y las entradas del filtro de Kalman se han
transformado a un marco de referencia nico.
Otros modelos, en cambio, incluyen las ecuaciones descritas en el bloque de INS
como propias del algoritmo de Kalman. Es lo que se conoce como integracin
tight, donde las entradas del Kalman son directamente las medidas de los sensores
y todas las ecuaciones estn integradas en un mismo filtro. Es sabido que cuanto
mayor sea la integracin, mejor ser la solucin. Algunos autores [8] evalan los
resultados de utilizar una integracin tight frente a la integracin loose siendo
marginalmente mejor la tight.

Fig. 5.3 Plano NE de la trayectoria del modelo forward.

Filtro de Kalman

57

Se ha simulado una trayectoria circular, para poder comprobar que efectivamente

el algoritmo funciona. Este ha sido un requisito indispensable antes de procesar los
datos reales del UAV porque nos asegura que, como mnimo, el procesado de
Kalman que se realiza, no incluye ningn error a priori visible.
Como se observa en la Fig. 5.1 el filtro de Kalman reconstruye bastante bien la
trayectoria simulada (en color rojo). La posicin del GPS (lnea azul) se ha
simulado para que sea bastante buena, pero es inevitable que en algunos puntos se
aleje demasiado de la trayectoria real (en turquesa, lnea discontinua). Por ltimo,
se ve claramente que la integracin slo con inerciales (lnea verde) no se sostiene
durante un periodo de tiempo prolongado, incluso en un entorno controlado de
simulacin, donde ni hay bias, ni ganancias variables, slo ruido de medida, la
trayectoria que describe acaba divergiendo mucho de la solucin deseada.
Uno de los problemas que presenta este modelo, es que todo el procesado de los
inerciales (el bloque INS) se hace independientemente de la salida del filtro. As
pues, se puede dar un problema de divergencia donde la salida corregida sea tan
grande que el propio filtro deje de funcionar correctamente. El apartado siguiente
es la solucin natural a este detalle.

5.4

Modelo feedback

Los parmetros de este modelo son exactamente los mismos que los del apartado
anterior. Tanto el vector de estados x como el vector de medidas z son los mismos,
adems las matrices y H. La nica diferencia es que existe realimentacin
dentro del bloque de los inerciales.
Con cada nueva correccin del KF, el procesado de los inerciales se actualiza, de
manera que las condiciones iniciales dejan de ser la integracin de la muestra
anterior, y aparecen corregidas en el INS por la salida del filtro de Kalman [8] y
[13].

58

CAPTULO 5

Acelermetro

INS

Giroscopio

d x

+
GPS
Mag

dx

KF12

Fig. 5.4 Diagrama de bloques de la solucin descentralizada feedback.

Igual que en el modelo forward, se ha simulado una trayectoria circular. Esta vez
aparecen menos lneas ya que la solucin de los INS est corregida continuamente
por el filtro de Kalman.

Fig. 5.5 Plano NE de la trayectoria del modelo feedback.

Filtro de Kalman

59

Cabe destacar estos pequeos saltos que se dan con cada solucin de la trayectoria
del INS corregido por Kalman (lnea roja). Ya se explic en el captulo 3 que la
frecuencia de los datos del GPS era de 1 muestra por segundo, mientras que los
datos de los inerciales es de 48 muestras por segundo. As que la variacin brusca
corresponde a una actualizacin del estado de Kalman y el resto del tiempo
simplemente integra la solucin de los inerciales.
Como comparacin de los dos mtodos, se ha calculado el error medio de cada
algoritmo. Estos errores estn referidos a metros y dependen de las condiciones de
simulacin, que se ha intentado que fueran lo ms parecidas a la estadstica de los
sensores detallados en el captulo 3.
Error GPS (XY)

Error Kalman (XY)

Modelo forward

4.9m

3.3m

Modelo feedback

5.0m

3.9m

Fig. 5.1 Desviacin standard de los resultados del modelo forward y feedback.

Los resultados pronostican que el algoritmo mejora algo la resolucin dada por el
GPS, que de por s es bastante buena, siendo ligeramente mejor el algoritmo
fordward. Estos resultados corresponden con el promedio de varias simulaciones
por la estadstica aleatoria de las seales y por los pocos puntos para calcular el
error medio.
El nico problema con estos resultados es que el algoritmo forward slo se puede
implementar para un filtrado limitado en tiempo, porque ya se coment que a la
larga tiende a divergir. Por tanto, es ms recomendable el uso del algoritmo
feedback para periodos de medida prolongados. En el caso estimar la trayectoria
durante una pasada del UAV cuando se hace una medida de SAR, es diferente,
porque este recorrido ser relativamente corto. As que el algoritmo de forward
tampoco es descartable en el contexto del proyecto actual.

6 Medidas reales y resultados

Se ha realizado una campaa de medidas con todos los sensores embarcados en el
avin. Anteriormente se haban realizado ms campaas, pero haban resultado
todas incompletas, ya porque faltara uno de los sensores, no estuviese integrado el
sistema de almacenamiento, etc sin duda la complejidad de hardware que hay
detrs ha hecho que el proceso de habilitar todos los dispositivos en
funcionamiento fuese lento. Paralelamente a la implementacin del hardware de
los sensores, se ha desarrollado la parte terica del proyecto. Hasta la fecha, stas
han sido las medias ms completas que se han podido realizar.
El experimento se ha realizado embarcando la plataforma de sensores sobre un
coche y haciendo un recorrido alrededor del Campus Nord. La dinmica del
movimiento es ms baja que cuando vuela el avin, pero se ha credo conveniente
que antes de hacer otras pruebas ms complicadas, se realizara una donde las
condiciones de navegacin estuvieran mucho mas acotadas.
Bajo estas condiciones, se puede suponer que el ngulo de roll vara muy poco y
siempre est cercano a 0. El eje Z realmente nos da poca informacin del
movimiento, solo cuando subimos o bajamos y sern pendientes muy suaves.

61

62

CAPTULO 6

Fig. 6.1 Trayectoria dibujada sobre Google Earth.

La Fig. 6.1 dibuja el recorrido trazado durante las medidas, segn el GPS. Antes
de empezar el recorrido y al acabar, se aprecia una gran variacin en la posicin.
Esto se debe a errores del GPS en la adquisicin cuando el mvil est parado. Una
vez en movimiento, la posicin que da el GPS es muy buena, debido a que utiliza
tambin las medidas de velocidad para estimar la posicin.

6.1

Procesado de las medidas

Antes de verificar el algoritmo ha sido necesario procesar toda la informacin para

que est referida en las mismas unidades de medida y los mismos ejes de
coordenadas.
Este es el momento de transformar los bits recogidos en el dataloger, a travs de
las ecuaciones detalladas en el captulo 3. Es decir, estas conversiones nos
garantizan que las medias estn en las unidades que deseamos y que se ha

Medidas reales y resultados

63

eliminado, en la medida de lo posible, todos los efectos no deseados de bias y

variaciones en las ganancias.
Como se ha explicado tambin en el apartado 3.5, muchos de los sensores trabajan
de forma independiente, incluso tienen orientaciones diferentes. Esto es un
problema sobretodo cuando se quiere unificar los ejes de coordenadas en los que
se trabaja.
Ya se explic en 3.5, que aplicando unas matrices de cambio en los ejes de los
sensores, estos se alinean con los ejes de coordenadas de body y de ahora en
adelante, los ejes de un sistema de coordenadas nico describen todas las salidas
de los sensores. Sin embargo, es un paso que se debe realizar al principio del
procesado de las medidas.
Otro problema importante que se ha presentado cuando se han procesado todos
estos datos, ha sido establecer una base de tiempos comn para todas las medidas.
En teora, el sistema de adquisicin est sincronizado con la seal de pps del GPS.

Fig. 6.2 Comparacin de la integracin de los giroscopios con el ngulo de heading a

partir de las medidas del magnetmetro.

64

CAPTULO 6

Esta seal da un pulso cada vez que el GPS tiene una lectura de posicin, es decir,
se hace cada segundo y de manera muy exacta.
Adems de la sincronizacin de cada segundo, hay un interruptor que inhibe la
escritura de las lecturas en el dataloger y cuando se conecta, los microprocesadores
PIC del sistema de adquisicin empiezan a guardar los datos.
En la Fig. 6.2 se ha seleccionado una parte del recorrido que tuviera alguna
peculiaridad claramente reconocible de la trayectoria. En este caso, se trata de un
giro y estas graficas se han dibujado aplicando ya la correccin de tiempo
estimada. Es un algoritmo de sincronizacin basado en la bsqueda de
peculiaridades y comparando luego la representacin de estas a travs de sensores
que almacenaban informacin en memorias diferentes.

Base de tiempos

GPS

INS

ADIS

0s

1s

22s

Tabla. 6.1 diferencia de tiempos entre los datalogers para el mismo instante.

Este es un resultado que sorprende mucho, porque era de esperar que no hubiera
una diferencia superior a 1 segundo Un sistema con una base de tiempos comn
habra simplificado el problema, adems de con este mtodo no se tiene la certeza
de que exista un error en la estimacin del sincronismo.

6.2

Integracin de las medidas de los sensores inerciales

En este apartado se sigue el planteamiento del captulo 4. Es decir, se transforman

las medidas de magnetmetro, giroscopios y acelermetros, que tienen sus
medidas referidas al sistema de coordenadas de body, para que pasen a
coordenadas de navegacin.

Medidas reales y resultados

65

El primero de los pasos a seguir es calcular la actitud del mvil slo con las
medias de los giroscopios. Para ello se utiliza (4.1) y (4.2). Esta es la primera de
las medidas que se calculan y es necesaria para hacer clculos posteriores.

Fig. 6.3 Comparacin de la actitud y de las medidas de los giroscopios antes de ser
procesadas en el INS.

Se siguen aplicando las ecuaciones planteadas en el captulo de cinemtica y se

llega al resultado de la Fig. 6.4 que es la posicin en el plano XY de la integracin
de los acelermetros. Se ve claramente que en cuestin de pocos segundos, acaba
divergiendo considerablemente de la trayectoria marcada por el GPS. Sin duda an
hay un problema de calibrado a pesar de la correccin en los sensores hecha en el
captulo 3.

66

CAPTULO 6

Fig. 6.4 Posicin en el plano NE segn el GPS y la integracin del INS.

6.3

Filtro de Kalman de 12 estados

Se han planteado dos modelos de filtrado de Kalman, ambos descritos en el

captulo anterior. Todas las frmulas que se han utilizado, son las mismas que las
descritas y las variables del vector de estados tambin. As pues, este apartado es
slo una recopilacin de los resultados ms destacados, para nada se pretende
volver a explicar el funcionamiento porque este era el objetivo del captulo 5.
En todos los modelos se ha utilizado una solucin descentralizada, es decir, que el
procesado de las actitudes y de las aceleraciones ser siempre el calculado en el
bloque de INS y con esos datos se llevar a cabo el filtrado de Kalman.

6.3.1 Modelo forward

Siguiendo el diagrama de la Fig. 5.2 se han procesado los datos de la campaa de
medidas. Este modelo corrige la salida del bloque de INS pero de manera invisible

Medidas reales y resultados

67

con la integracin de los inerciales. Se puede apreciar como la solucin del INS
diverge pero sin embargo el filtro es capaz de compensar bastante bien este error.
Como se ha dicho anteriormente, este detalle no tiene importancia siempre y
cuando no acabe salindose de rango, cosa que pasara despus de muchos
segundos.

Fig. 6.5 Plano NE del tramo analizado con el modelo forward.

68

CAPTULO 6

Fig. 6.6 Velocidades de los 3 ejes en coordenadas de navegacin en el modelo forward.

Fig. 6.7 ngulo de heading o Yaw del modelo forward

Medidas reales y resultados

69

La solucin de Kalman aparece en todas las grficas como la lnea roja. La

solucin del GPS es bastante buena, como se esperaba, y en cuanto a posicin se
refiere, el filtro de Kalman mejora muy poco. En cambio, en la actitud se observa
una mejora considerable respecto a la integracin exclusiva de los giroscopios.

6.3.2 Modelo feedback

Se ha utilizado el diagrama de bloques propuesto en la Fig. 5.4. Se espera que el
resultado no diverja con el tiempo como le pasa al modelo forward. El procesado
que se ha hecho ha sido el mismo que en el modelo anterior, con las mismas
variables a analizar y las mismas figuras. A priori, el resultado esperado debera
ser parecido al modelo forward, tal y como se vio en el captulo 5.

Fig. 6.8 Plano NE del tramo analizado con el modelo feedback.

70

CAPTULO 6

Fig. 6.9 Velocidades de los 3 ejes en coordenadas de navegacin del modelo feedback.

Fig. 6.10 ngulo de yaw del modelo feedback.

Medidas reales y resultados

71

Esos saltos tan molestos que aparecen en la Fig. 6.8 en la zona de la curva,
estropean mucho el vector de resultados. Si bien es cierto que se est comparando
vectores con densidades de muestras diferentes (1 por segundo del GPS y 48 por
segundo de los inerciales), estos saltos no deberan ser tan abruptos, y ms
teniendo en cuenta que las medidas de los inerciales deberan ser lo
suficientemente fiables en un intervalo de tiempo tan reducido. Otra vez se vuelve
al problema del modelo de error de los inerciales, que a priori parecan bien
calibrados, pero resulta evidente que necesitan un modelo de error ms complejo.

6.4

Filtro de kalman extendido de 18 estados

Como posible solucin al problema planteado en el apartado 6.3.2 con los saltos de
posicin en cada actualizacin del filtro, se propone aumentar el nmero de
estados que el filtro tiene en cuenta y tomar el bias de la aceleracin como variable
adicional al vector de estados:

x X

bx

vx
by

vy
bz

vz
bx

by

bz

(6.1)

Se aade slo el bias de la aceleracin porque se cree que es el responsable de

estos saltos y se espera que disminuya.
La manera en que se ha estimado este bias es a travs de la diferencia entre la
solucin de los inerciales corregida y la solucin del GPS en el punto de mxima
divergencia, es decir, justo antes de hacer el filtrado. Con este resultado se corrige
el bias en el tramo siguiente. Esta magnitud debera ser el cubo del bias de las
aceleraciones y se supone que no cambia tan rpido como para que de un segundo
a otro deje de tener validez.

72

CAPTULO 6

Fig. 6.11 Plano NE del tramo analizado con el modelo de 18 estados

Fig. 6.12 Evolucin de los bias a los largo del tiempo como variable de estados del filtro.

Medidas reales y resultados

73

En vista de los resultados, siguen apareciendo los saltos en el plano NE, incluso
ms pronunciados que antes. Es por eso que no se ha aadido ninguna figura ms,
slo los valores de estos bias durante el filtrado. Puede parecer que el filtro de
Kalman es capaz de encargarse de todas las irregularidades, pero el problema de la
calibracin de los acelermetros es algo que se tiene que solucionar antes, porque
sino resulta muy complicada una solucin a travs del filtrado.
As pues, la solucin de 18 estados queda descartada, siendo mucho ms
recomendable atacar el problema desde el punto de vista de la calibracin de los
sensores que no desde el punto de vista del procesado con el filtrado de Kalman.
Si se comparan los algoritmos de forward y feedback, los resultados son parecidos.
El algoritmo forward presenta mejores resultados en vista de las figuras, y para el
propsito del proyecto, es decir, la estimacin de la trayectoria durante una pasada
del SAR. La pregunta sera si mejorados los modelos de los sensores, entonces los
resultados de ambos algoritmos presentaran grficas parecidas.
Sin embargo, en escenarios donde sea necesaria una integracin de la posicin por
un periodo de tiempo indefinido, entonces el algoritmo de feedback es el ms
recomendable.

7 Conclusiones
La caracterizacin de los sensores se ha visto que es crucial para conseguir una
mejor estimacin de la posicin y de la actitud. Aunque se ha dedicado una parte
importante del proyecto a este tema, los resultados del captulo 6 hacen pensar que
los modelos de error no son lo suficientemente buenos, ni la calibracin ha sido
suficiente precisa si se quiere llegar a un nivel de precisin mejor.
Tambin es muy interesante la integracin de todos los sensores en un nico
dispositivo de almacenamiento, para evitar problemas de sincronismo, ya que la
solucin para sincronizar los datos es un tanto subjetiva y con esta integracin se
ahorran posibles errores de desplazamiento temporal entre diferentes medidas. Sin
duda es una fuente de error adicional y merece la pena el esfuerzo de redisear
algunos de los bloques del sistema.
Aunque el error slo se ha podido cuantificar a travs de las simulaciones, se
observa que la posicin estimada con el filtro de Kalman es ligeramente mejor a la
del GPS. Es un resultado positivo ya que los resultados dados nicamente por los
sensores inerciales hacan pensar que el filtrado de Kalman no corregira tan bien
la solucin. El sistema de GPS, desde que se elimin en febrero de 2000 la funcin
75

76

CAPTULO 7

de S/A que degradaba su posicin significativamente, se ha convertido en un

sistema de localizacin muy bueno, mejorando incluso su precisin cuando el
receptor est en movimiento (a su vez integra un Kalman para estimar la posicin).
Es decir, a pesar de que se mejoran los resultados del GPS con un filtrado de
Kalman no se aprecia una mejora significativa.
La gran ventaja de esta integracin est en la estimacin de la actitud. Ahora se
dispone de un sistema inercial (giroscopio) y de un sistema de medida absoluto
(magnetmetro) para combinar en una estimacin de la mejor actitud. An se
puede mejorar el sistema de la estimacin de la actitud incluyendo en el filtro de
Kalman los tres ngulos de Euler y no slo el ngulo de yaw, aunque para ello
sera necesario estimar de alguna manera la inclinacin de la plataforma durante el
vuelo.
En cuanto al algoritmo a utilizar, tanto el feedback como el forward presentan una
desviacin standard muy similar. La principal ventaja del algoritmo de integracin
feedback es una mayor robustez en integraciones prolongadas. Otro punto a favor
es que presenta una mayor densidad en su vector de resultados, ya que tiene 48
muestras cada segundo, mientras que el algoritmo forward slo tiene 1 muestra
cada segundo. La principal desventaja son los saltos que se producen en cada
actualizacin de Kalman, pero ya se ha comentado que estos saltos deberan
suavizarse una vez los sensores se modelen mejor. Por lo tanto es ms
recomendable el modelo feedback si se busca una resolucin en tiempo de la
solucin deseada mejor que 1 segundo.
Sin embargo, si la resolucin en tiempo no importa y 1 muestra por segundo es
suficiente, se puede utilizar el algoritmo forward, siendo adems el ms sencillo.
La condicin para que funcione correctamente es integrar durante un tiempo de
vuelo limitado, o resetear la solucin de navegacin en un punto conocido, porque
existe la posibilidad de que el algoritmo acabe divergiendo si integra durante
demasiado tiempo. As que por todo esto, el modelo forward presenta muchas
complicaciones y muchos requisitos que lo hacen poco recomendable.

Medidas reales y resultados

7.1

77

Futuras lneas de trabajo

Una mejora, que ya ha aparecido comentada en varios captulos, es la unificacin

de los sensores en un nico diseo. Resulta importante disponer de un vector de
datos que sea fiable, y tener que estimar su retraso con respecto a otros, es una
posible fuente de error. As que se pueden hacer dos cosas para solucionar este
problema: idear un algoritmo de estimacin del retraso que sea mucho ms robusto
y general o cambiar el hardware para que este factor no sea una variable ms a
solucionar y no haya dudas al respecto cuando se almacenen todos los en la misma
memoria o se gestione todo con el mismo microcontrolador. Otro tema respecto a
los sensores es la mejora del modelo de error de estos, tanto en condiciones
estticas como dinmicas. Es decir, sera recomendable idear una batera de
pruebas para obtener ms parmetros del modelo de error, incluyendo coeficientes
cruzados, comportamiento en dinmica, estadstica de los bias y del factor de
escala, vibraciones, etc
Otro punto a destacar es la amortiguacin de las vibraciones. Dentro de la batera
de pruebas una de ellas estara relacionada con el efecto de las vibraciones en los
sensores. Sin embargo, las vibraciones del avin son bastante considerables ya que
vuela con un motor de bastante potencia y dependiendo de los resultados obtenidos
en las pruebas, quiz hagan interesante el diseo de algn mecanismo de
amortiguacin para que los sensores respondan como se espera de ellos. Ms que
filtrar elctricamente, se tratara de un filtrado mecnico para reducir las
vibraciones del motor del avin.
Con respecto al algoritmo de procesado, sera interesante implementar la
integracin tight del algoritmo de Kalman, donde todas las ecuaciones que se han
tratado en los captulos 4 y 5, adems de las ecuaciones de las pseudodistancias del
GPS, pasaran a incorporarse a las matrices de estado y de medidas del sistema. A
priori, la solucin obtenida de esta manera es ms precisa, pero algunos trabajos
apuntan a que no sea una mejora espectacular.
Las ecuaciones que modelan la actitud se pueden mejorar, ya que resultan
sumamente importantes a la hora de corregir la trayectoria. Un error en la actitud
del UAV se traduce en un error en la direccin a donde apunta la antena del SAR.

78

CAPTULO 7

Por tanto, los ngulos de pitch y roll seran incluidos como variables del vector de
estado, bien modelando un inclinmetro virtual para las medias en dinmica o bien
utilizando el modelo de campo magntico terrestre.
Este proyecto puede ser el primer paso para el procesado de la posicin en tiempo
real y as obtener la integracin Kalman de la posicin al instante. El nico
requerimiento sera instalar un ordenador a bordo y ajustar los algoritmos que sean
completamente autosuficientes. Quiz no sea muy interesante si se procesa el SAR
offline pero el siguiente paso lgico, si se introduce un sistema de posicin y
actitud instantneos, es el estudio de un algoritmo de autopilotaje, aprovechando
las funciones de prediccin del filtrado de Kalman.

Bibliografa

79

Bibliografa
[1]

R.Diego Estudio y realizacin de un sistema de navegacin GPS/INS para

mviles de dinmica baja-moderada, Proyecto Fin de Carrera de la
ETSETB (UPC), 1999

[2]

F.Garca Integracin de datos GPS y datos INS (inerciales) para

aplicaciones SAR (radar apertura sinttica), Proyecto Fin de Carrera de la
ETSETB (UPC), 1999, no publicado

[3]

M.Glenat Georeferencing in Google Earth radiometric data from UAV :

Simulation and experimental data processing Proyecto Final de Carrera
de la ETSETB (UPC), 2008, no publicado.

[4]

M.S.Grewal, Weill, Andrews Global Positioning Systems, Inertial

Navigation, and Integration, Wiley 2001.

[5]

Siouris,George M. Aerospace Avionics Systems a modern synthesis,

Academic Press, Inc. 1993.

[6]

Kayton, Avionics Navigation Systems, John Wiley & Sons, Inc. 1997

[7]

M. S. Grewal, Andrews, Kalman Filtering, Theory and practice using

MATLAB, Wiley & Sons, Inc. 2001

[8]

S.Rnnbck, Development of a INS/GPS navigation loop for an UAV

Masters Thesis, Lule University, 2000.

[9]

David H. Titterton, John L. Weston, Strapdown Inertial Navigation

Technology, Institution of Electrical Engineers, American

[10]

Michael J. Caruso, Applications of Magnetic Sensors for Low Cost

Compass Systems Honeywell, SSEC.

[11]

Marc g. Petrovelo Real-Time Integration of a Tactical-Grade IMU and

GPS for High-Accuracy Positioning and Navigation, University of

80

Bibliografa

Calgary , 2003
[12]

Vikas Kumar N. Integration of Inertial Navigation System and Global

Positioning System Using Kalman Filtering, Ph.D. Thesis, Indian Institute
of Technology, Bombay 2004

[13]

A.Farrell, Y. Yeng Magnetometer and Diferencial Carrier Phase GPSAided INS for Advanced Vehicle Control , IEEE trans. Robotics and
automation, vol.19, abril 2003

[14]

http://www.ngdc.noaa.gov

[15]

E. Shin Accuracy Improvement of Low cost INS/GPS for Land

Applications Ph.D. Thesis, University of Calgary, 2001

[16]

D. Li, R. Landry, P. Lavoie Low cost-MEMS Sensors-based Attitude

Determination System by Integration of Magnetometers and GPS: a RealData Test and Performance Evaluation, Universit du Qubec

[17]

T. Bak Spacecraft Attitude Determination: a Magnetometer Approach,

Ph.D. Thesis, Aalbog University, Denmark 1999

Anexo

81

Anexo I: Quaterniones
Sistema de 4 parmetros, equivalente a los ngulos de Euler.
Qab (q0 , q1 , q2 , q3 )

(I.1)

Algunas propiedades de los quaternions:

Q * q0 q1 q2 q3

(I.2)

N (Q ) Q Q q02 q12 q22 q32 1

(I.3)

La idea es que q1 , q2 , q3 describen un vector en el espacio y q0 la cantidad que

rotamos respecto a ese vector.

cos 2

sin cos

Q 2
sin cos
2

sin cos

(I.4)

Con estos cambios, se consigue pasar de un sistema de tres ngulos (Euler) a los
cuatro parmetros de los quaterniones, que definen de igual manera la actitud de
un cuerpo.
La correspondencia entre la matriz direccin coseno y los quaterniones es:

1 2(q22 q32 ) 2(q1q2 q3q0 ) 2(q3q1 q2 q0 )

C 2(q3q0 q1q2 ) 1 2(q32 q12 ) 2(q2q3 q0q1 )

2(q3qq q2 q0 ) 2(q2q3 q0q1 ) 1 2(q12 q22 )

(I.5)

Y entre los componentes del quaternion y los ngulos de Euler:

cos cos sin sin sin

2
2
2
2
2
2

(I.6)

sin cos sin cos cos

2
2
2
2
2
2

(I.7)

q0 cos
q1 sin

82

Anexo

cos cos sin sin cos

2
2
2
2
2
2

(I.8)

q3 sin cos cos sin sin cos

2
2
2
2
2
2

(I.9)

q2 sin

Con estas ecuaciones de relacin, se puede pasar del sistema de ngulos de euler,
incluso de la matriz DCM a quaterniones, resolver la matriz evitando as el
problema del gimbal-lock y volviendo a transformar.
Esta es la matriz que relaciona las velocidades angulares, equivalente a la ecuacin
planteada en (4.1).
e0
0 p q r e0
e

r q e1
1 1 p 0
e2 2 q r
0
p e2

q p 0 e3
e3
r

(I.10)

La gracia de estos cambio es que resolviendo esta ecuacin no hay secantes

involucradas y no existirn singularidades.

Anexo

83

Anexo II
Este es el esquema que se utiliz para construir el circuito de reset del
magnetmetro. Se realiz slo una placa externa al diseo para resetear
puntualmente el magnetmetro.

Fig.II..1 Esquema del circuito de S/R.