Comparacion de algoritmos de estimacion de actitud

J. Rico-Azagra(1), R. Rico(2), P. Maisterra(3), M. Gil-Martınez(4)

Grupo de Ingenierıa de Control, Departamento de Ingenierıa Electrica, Universidad de La Rioja{javier.rico, montse.gil}@unirioja.es

{ramon.rico, paloma.maisterra}@alum.unirioja.es

Resumen

El presente trabajo describe de forma detalladael problema de estimacion de actitud a partir deuna unidad de medida inercial. En el se detallananalıtica y experimentalmente los problemas de es-timacion. Se describen los fundamentos del filtrocomplementario (FC) y su aplicacion en la estima-cion de actitud con acelerometro y giroscopio. Dosmodificaciones del FC que han presentado un altoimpacto en la literatura cientıfica son propuestas.Por un lado, el empleo de un gain scheduling decontroladores para mejorar la estimacion en sis-temas fuertemente acelerados y, por otro, el em-pleo de un FC explıcito. Por ultimo se compruebanlas prestaciones de los diferentes estimadores em-pleando datos reales.

Palabras clave: Estimacion de actitud, Sensoresinerciales, Filtro complementario, Fusion desensores.

1. Introduccion

La estimacion de la actitud es un problema recu-rrente en multitud de aplicaciones. Entre ellas des-tacan los sistemas aeroespaciales [1], la robotica[2], los vehıculos aereos no tripulados [3], la tomade imagenes [4] o la motorizacion de movimien-to en humanos [5], entre otros. Este amplio cam-po de aplicacion ha producido que exista un graninteres en la comunidad cientıfica por desarrollarestrategias de estimacion cada vez mas precisas.Por otro lado, la rapida proliferacion de los sis-temas microelectromecanicos MEMS (Microelec-tromechanical Systems), ha permitido la incorpo-racion al mercado de una serie de instrumentosde medida que presentan reducido tamano y bajocoste. Este tipo de instrumentos son especialmenteutiles en sistemas de estimacion que deben ser mi-niaturizados. Un claro ejemplo de aplicacion es elde los MAVs (Micro Air Vehicles), que requierende una estimacion precisa de su orientacion parapoder ser gobernados. La popularizacion de estetipo de sistemas ha producido un nuevo impulsoen el desarrollo de estrategias de estimacion, cadavez mas eficientes y que requieren de un menor

coste computacional.

Normalmente los sistemas de estimacion de ba-jo coste emplean como unica fuente de datos unaunidad de medida inercial IMU (Inertial Measu-rement Unit). Hay que destacar que este tipo deinstrumentos dan lugar a senales que presentanbaja resolucion, altos niveles de ruido y derivasdinamicas. Para solucionar estos problemas se em-plean estrategias de filtrado que permiten mejorarla estimacion [6]. Estas estrategias se dividen endos grandes grupos: las que emplean Filtros deKalman Extendidos (EKF) [7, 8] y las que em-plean Filtros Complementarios (FC) [9, 10, 11].El primer grupo presenta buenos resultados, pe-ro su principal desventaja radica en que demandauna potencia de calculo que no es admisible paramuchos de los sistemas de bajo coste. Tengase encuenta que muchos de los MAVs emplean sistemasembebidos con procesadores de 8 bits. Las estrate-gias de estimacion que emplean filtros complemen-tarios requieren de un menor coste computacional,permitiendo una implementacion mas robusta ensistemas de bajo coste. Este hecho ha producidoque se popularicen entre la comunidad de desa-rrolladores de software para MAVs (aficionados,investigadores, etc.), logrando un amplio impacto.

El presente trabajo presenta de forma simplificadael problema de estimacion de actitud. Su objetivoes aportar un resumen de los conceptos principa-les, permitiendo a un lector que disponga de va-gos conocimientos sobre estrategias de estimacionalcanzar una perspectiva global del problema ysu solucion. Se presentan los fundamentos emplea-dos en la estimacion directa de actitud, ası comolos problemas asociados a las unidades de medidainercial de bajo coste. Experimentos reales mues-tran de forma didactica las deficiencias de los es-timadores que emplean las medidas de un unicosensor. Los fundamentos del filtro complementa-rio son presentados y desarrollados en profundidadpara el caso particular de la estimacion de actitudcon acelerometro y giroscopio. A continuacion sedescriben dos modificaciones del FC tradicional,que han presentado un alto impacto en la comuni-dad cientıfica. Por ultimo se evalua el desempenode las diferentes estrategias de estimacion presen-

Actas de las XXXVI Jornadas de Automática, 2 - 4 de septiembre de 2015. Bilbao ISBN 978-84-15914-12-9 © 2015 Comité Español de Automática de la IFAC (CEA-IFAC) 911

tadas en el trabajo. Un test de evaluacion realiza-do en un sistema real permite identificar los puntosfuertes/debiles de cada una de ellas.

2. Estimacion directa de la actitud

Las unidades de medida inercial permiten medirlas aceleraciones y velocidades angulares en losejes de un cuerpo que rota y se desplaza. Estasmedidas pueden ser empleadas para determinarsu actitud. En la presente seccion se muestran lasecuaciones necesarias para realizar la estimacionde actitud directamente con las medidas de unacelerometro o de un giroscopio. Previamente esnecesario describir el sistema de referencia utiliza-do (Figura 1). Se aprecia como el eje Zb del B-frame (Inertial System Body Frame) se encuentraalineado con el vector gravedad, apuntando haciael centro de la tierra.

XBYB

ZB

q

r

p

Figura 1: Sistema de referencias B-Frame

2.1. Estimacion directa con giroscopio

El giroscopo registra las velocidades angulares enel sistema de referencia del cuerpo ωb, normalmen-te denominadas ωb = [p, q, r]t. Si para respresentarla actitud se emplean los angulos de Euler, la re-lacion entre las velocidades de rotacion del cuerpoωb y las velocidades de rotacion con respecto delsistema de referencia inercial ω estan determina-das porφθ

ψ

=

1 sinφ tan θ cosφ tan θ0 cosφ cosφ tan θ0 sinφ sec θ cosφ sec θ

pqr

, (1)

donde φ corresponde con la velocidad angular res-pecto del eje X (Roll), θ corresponde con la veloci-dad angular respecto del eje Y (Pitch) y ψ corres-ponde con la velocidad angular respecto del eje Z(Yaw).

A partir de (1), la estimacion de la actitud pue-de alcanzarse integrando las velocidades angularesω. Debemos tener en cuenta que los giroscopiosadolecen de una serie de problemas que limitan laprecision de la estimacion. De entre las deficiencias

destacan la existencia de bias estaticos, derivas obias dinamicos, errores de escala y desalineacionde ejes. Pese a que todos estos problemas afectana la estimacion, la existencia de una deriva varia-ble con el tiempo drift, es la que produce un mayordesajuste entre la variable fısica y la estimacion.

Considerese la medida del sensor como

x(t) = ωb(t) + µ(t) + b, (2)

donde µ corresponde con el ruido de alta frecuen-cia asociado al sensor y b corresponde con unasenal de componente continua que cambia con eltiempo (drift) pero mucho mas lentamente que elresto de variables. Tras aplicar (1) se obtiene la es-timacion de la velocidad de rotacion con respectoal sistema de referencias inercial

ω(t) = ω(t) + µG(t) + bG, (3)

donde ω corresponde con la velocidad estimaday los subındices G indican que las senales inde-seadas µ y b han sufrido una transformacion1. Laestimacion de los angulos de Euler ρ(t) se obtienemediante la integracion de ω(t), dando lugar a

ρ(t) =

∫(ω(t) + µG + bG) dt = ρ(t) + bGt, (4)

si se considera que µG es un ruido blanco ideal.Observese que la deriva, pese a ser pequena pro-ducira un error de estimacion que aumenta conel tiempo, impidiendo el empleo de dicho sensorcomo unico elemento de estimacion.

2.2. Estimacion directa con acelerometro

El acelerometro permite estimar los angulos θ yφ mediante la reconstruccion del vector gravedad.Tengase en cuenta que un acelerometro situado enun cuerpo registra las aceleraciones a las que se en-cuentra sometido el mismo, ademas de la acelera-cion de la gravedad. Considerando que la dinamicadel solido rıgido esta determinada por:

f = m(vb + ωb × vb), (5)

donde vb = [u, v, ω]t es la velocidad lineal del cuer-po, vb su aceleracion lineal, m su masa y f son lasfuerzas que actuan sobre el mismo. Recuerdese queel subındice b indica que las medidas se encuentranreferidas al sistema de referencia del cuerpo.

La aceleracion registrada por el instrumento, quedenominaremos como a, esta determinada por

a = vb + ωb × vb + gb, (6)

1La rotacion de las senales µ y b carece de impor-tancia para el analisis que se lleva a cabo.

Actas de las XXXVI Jornadas de Automática, 2 - 4 de septiembre de 2015. Bilbao ISBN 978-84-15914-12-9 © 2015 Comité Español de Automática de la IFAC (CEA-IFAC) 912

donde gb corresponde con la aceleracion de la gra-vedad en el cuerpo. La relacion puede expresarseen formato matricial segunaxay

az

=

uvw

+

0 w −v−w 0 uv −u 0

pqr

+ g

− sin θcos θ sinφcos θ cosφ

(7)

En condiciones estaticas, es decir, cuando el sis-tema se encuentre en reposo, la unica medida delacelerometro corresponde con la de la aceleracionde la gravedad, dado que ωb = vb = 0. En forma-to matricial las medidas en reposo (expresadas enunidades de g) estan determinada poraxay

az

=

− sin θcos θ sinφcos θ cosφ

(8)

Dividiendo los terminos ay y az se obtiene

ayaz

=sinφ

cosφ= tanφ, (9)

de donde se extrae la condicion para la estimaciondel angulo de roll

φ = arctan

(ayaz

). (10)

Del mismo modo, empleando

−ax√a2y + a2z

=sin θ√

cos2 θ(sin2 φ+ cos2 φ)= tan θ,

(11)se extrae la condicion para la estimacion del angu-lo de pitch

θ = arctan

−ax√a2y + a2z

. (12)

Vease como el angulo de yaw no puede ser esti-mado empleando unicamente un acelerometro, yaque es imposible determinarlo a partir de (8).

En baja frecuencia la estimacion realizada con elacelerometro sera adecuada, dado que este tipode sensores presentan un bias despreciable. Por elcontrario, en alta frecuencia se apreciaran distor-siones notables entre la estimacion y la aceleracionreal, dado que el sensor estara sometido a acele-raciones adicionales a la de la gravedad. Otro delos problemas de este tipo de estimacion es que esmuy vulnerable a las vibraciones producidas porlos actuadores. Este ultimo efecto puede mitigarsefijando la unidad de medida inercial mediante elempleo de plataformas antivibratorias.

2.3. Estimacion directa conmagnetometro

El magnetometro mide el campo magnetico terres-tre en los tres ejes del cuerpo m = [mx,my,mz]t.Estas medidas permiten calcular la orientacion delsistema (Yaw ψ) siempre que se conozca con pre-cision la actitud del mismo (angulos θ y φ). Elprocedimiento de estimacion es similar al emplea-do en el caso del acelerometro, con la salvedadde que en este caso se estima el vector del campomagnetico terrestre m = [mN ,mS ,mD]t (North,East, Down). Tras una serie de operaciones puedeobtenerse que la relacion entre el campo magneti-co en el cuerpo y el campo magnetico terrestreesta dada por

mN cosψmN sinψmD

=

cos θ sin θ sinφ sin θ cosφ0 − cosφ sinφ

− sin θ cos θ sinφ cos θ cosφ

mx

my

mz

. (13)

El angulo ψ puede obtenerse mediante la divisionde las dos primeras filas, obteniendo

ψ = arctan

(xhyh

)(14)

donde xh e yh estan determinadas por

xh = mx cos θ +my sin θ sinφ+mz sin θ cosφ

yh = −my cosφ+mz sinφ .(15)

Vease como una de las condiciones principales pa-ra el empleo del estimador anterior es que la es-timacion de los angulos θ y φ sea adecuada. Encaso contrario, las distorsiones repercutiran en laestimacion de ψ llevada a cabo con la informaciondel magnetometro.

2.4. Ejemplos de estimacion directa

A continuacion se muestran dos ejemplos de es-timacion directa, en los que se ha empleado uni-camente un sensor para determinar la actitud delsistema. Su objetivo es corroborar los resultadosalcanzados en las secciones anteriores.

Para la captura de los datos se ha empleado un sis-tema embebido ArduPilot Mega 2.5 que presentaun procesador ATmega2560 de 8 bits y junto conuna IMU MPU6000. Las caracterısticas de dichaIMU se presentan en la Tabla 1.

Se han llevado a cabo dos experimentos. En el pri-mero se fija la posicion del sistema y se somete al

Actas de las XXXVI Jornadas de Automática, 2 - 4 de septiembre de 2015. Bilbao ISBN 978-84-15914-12-9 © 2015 Comité Español de Automática de la IFAC (CEA-IFAC) 913

Parametro Giroscopio Acelerometro

Rango: X, Y, Z ±500(deg/s) ±8(g)Sensibilidad 65.5(LSB/os) 4.096(LSB/g)

Densidad 0,005(o√Hz/s) 400(µg/

√Hz)

Cuadro 1: Caracterısticas de la IMU MPU 6000

mismo a dos rotaciones, tras las cuales el siste-ma vuelve al estado inicial de reposo. Considereseeste experimento como un caso de funcionamientoideal, ya que el sistema no esta sometido a acelera-ciones lineales, los giros se han realizado de formapoco agresiva, no hay sistemas de actuacion queproduzcan vibraciones, etc. En la Figura 2 se apre-cian las estimaciones de los angulos roll y pitch.En ella podemos comprobar como la estimacionrealizada con el acelerometro es muy precisa en losextremos inicial y final del experimento, cuando elsistema se encuentra en reposo. Por el contrario,cuando se produce el giro se aprecia una compo-nente de ruido de alta frecuencia que distorsionalas medidas. Este fenomeno es especialmente no-table en el intervalo entre 4 y 6 segundos, cuandose produce la rotacion en roll. Por el contrario, laestimacion llevada a cabo con el giroscopio presen-ta un comportamiento dinamico mas preciso, sinapreciarse ni ruidos ni distorsiones. Sin embargo,la estimacion realizada en el tramo final del expe-rimento es notablemente peor a la obtenida con elacelerometro. Esta distorsion es debida a la derivaacumulada durante la rotacion, y crece con el pasodel tiempo.

En conclusion, pese a tratarse de un experimentofavorable se aprecian errores de estimacion impor-tantes que confirman el estudio realizado en lasSecciones 2.1 y 2.2.

El segundo experimento reproduce un movimien-to oscilatorio sobre un plano. Es decir la actituddel sistema se mantiene constante θ = φ = 0o yse desplaza linealmente el sistema. Por lo tanto elacelerometro registra la aceleracion de la gravedadcombinada con las aceleraciones que dan lugar aldesplazamiento del cuerpo. El objeto de esta se-gunda prueba es ilustrar los graves errores de esti-macion que pueden alcanzarse cuando se emplea laestimacion directa con acelerometro. En la Figura3 se observa que el error de estimacion cuando seemplea el acelerometro alcanza valores superioresa 10o. Por el contrario, la estimacion llevada a ca-bo por el giroscopio presenta un valor mucho masproximo a cero.

En conclusion, queda demostrado que es necesa-rio emplear estrategias de estimacion de actitudque permitan fusionar las medidas de varias sen-sores, dado que el empleo de estos de forma indi-vidual impide alcanzar estimaciones con una pre-

0 3 6 9 12 15-5

5

15

25

35

45

55

Rol

l(o )

Estimación de actitud

gyroacel

0 3 6 9 12 15Tiempo (s)

5

-5

-15

-25

-35Pi

tch

(o )gyroacel

Figura 2: Estimacion cuando el sistema no presen-ta aceleracion lineal

0 2 4 6 8 10 12 14 16-15

-10

-5

0

5

10

15

Rol

l(o )

Estimación de actitud

gyroacel

0 2 4 6 8 10 12 14 16Tiempo (s)

-15

-10

-5

0

5

10

15

Pitc

h(o )

gyroacel

Figura 3: Estimacion de actitud cuando el sistemase encuentra acelerado

Actas de las XXXVI Jornadas de Automática, 2 - 4 de septiembre de 2015. Bilbao ISBN 978-84-15914-12-9 © 2015 Comité Español de Automática de la IFAC (CEA-IFAC) 914

cision adecuada.

3. Filtro complementario

El filtro complementario es una herramienta quepermite fusionar las medidas de una variable fısi-ca registradas mediante el empleo de instrumen-tos de medida con comportamiento dinamico com-plementario2. Es decir, permite tomar medidas desensores con errores complementarios para obte-ner una estimacion fiable de dicha variable fısica.

En esencia, el metodo consiste en el empleo de unacoleccion de filtros fi(s), con i = 1, . . . , n encarga-dos de filtrar las senales obtenidas de cada uno delos instrumentos de medida. De este modo la esti-macion de la variable x(t) que denotaremos comox(t) puede obtenerse en el dominio de la frecuenciasegun

x(s) =n∑i

xi(s)fi(s), (16)

donde xi(s) corresponde con la medida de de lavariable x(s) efectuada por el sensor i. Para quela estimacion sea adecuada debera cumplirse

n∑i

fi(s) = 1. (17)

En la mayor parte de los casos el problema se re-duce al empleo de dos sensores. Uno de ellos suelepresentar un buen comportamiento en baja fre-cuencia, mientras que el otro es adecuado en al-ta frecuencia. Vease como la descripcion anteriorencaja a la perfeccion con el problema abordadoen el presente trabajo, donde el giroscopio pre-senta medidas fiables de la velocidad angular enalta frecuencia, mientras que el acelerometro re-porta unicamente buenos resultados en condicio-nes estaticas. En estos casos, basta con seleccionarun filtro pasa bajos y un filtro pasa altos. En casode emplear un filtro de primer orden

f1(s) =1

τs+ 1, (18)

como filtro pasa-bajos, el filtro pasa altos comple-mentario queda seleccionado segun

f2(s) = 1− f1(s) = 1− 1

τs+ 1=

τs

τs+ 1, (19)

tambien conocido como filtro sin distorsion. Reci-be dicho nombre dado que la variable fısica pasadirectamente a la salida, mientras que las senalesindeseadas son atenuadas en las frecuencias en las

2Presentan un comportamiento optimo en diferenteespectro de frecuencias.

que los filtros actuan. Tengase en cuenta que elcomportamiento frecuencial esta determinado por

x(s) = x(s) +1

τs+ 1µ1(s) +

τs

τs+ 1µ2(s), (20)

donde µi corresponde con el error de medida delsensor i.

Una de las formas de implementar un filtro com-plementario es emplear un lazo de control reali-mentado como el de la Figura 4. De esta forma laaportacion de la senal x1 a la salida esta determi-nada por la sensibilidad complementaria T (s) quedebe corresponder con un filtro pasa bajos, mien-tras que la aportacion a la salida de la senal x2esta determinada por la funcion sensibilidad S(s)que corresponde con S(s) = 1− T (s), cumpliendola definicion anterior de filtro complementario.

c(s)x1

x2

x−

Figura 4: Implementacion de filtro complementa-rio con lazo de realimentacion

En el caso mas sencillo (el mostrado en la Figura4), la estimacion queda determinada por

x(s) =c(s)

1 + c(s)x1(s) +

1

1 + c(s)x2(s), (21)

donde c(s) = 1/τs para que la estructura en lazocerrado corresponda con (20).

En el caso que nos ocupa, la estimacion llevada acabo por el giroscopio implica el empleo de unaintegral (ver seccion 2.1), ya que dicho sensor re-porta las velocidades angulares. Por lo tanto, laestructura de la Figura 4 se transforma directa-mente en la estructura de la Figura 5a. Pese a quela estructura de la Figura 5a representa el esque-ma tradicional de un FC, en el caso de la estima-cion de la actitud suele emplearse la estructura dela Figura 5b, donde la integral puede ser consi-derada como la planta de un sistema de controlp(s) = 1/s. Tengase en cuenta que las estructuraspropuestas en la Figura 5 no son equivalentes. Laestructura de la Figura 5a suele denominarse co-mo filtro complementario desacoplado, ya que noemplea el integrador como elemento comun a lasdos senales. Su empleo es residual, ya que como sejustificara a continuacion presenta peores resulta-dos que la estructura de la Figura 5b.

Cuando se emplea la estructura de la Figura 5b,

Actas de las XXXVI Jornadas de Automática, 2 - 4 de septiembre de 2015. Bilbao ISBN 978-84-15914-12-9 © 2015 Comité Español de Automática de la IFAC (CEA-IFAC) 915

c(s)

∫

x1

x2

x2x

−

(a) Filtro complementario desacoplado

c(s)∫x1

x2

x−

(b) Filtro complementario acoplado

Figura 5: Alternativas para la estimacion de acti-tud con filtros complementarios.

la estimacion esta determinada por

x(s) =p(s)c(s)

1 + p(s)c(s)x1(s) +

p(s)

1 + p(s)c(s)x2(s).

(22)Tengase en cuenta que en este caso no es posibleestabilizar el estimador con un controlador inte-gral como el descrito para la estructura de la Fi-gura 4, ya que da lugar a polos de lazo cerradoen el eje jω. El controlador c(s) mas sencillo quepermite la estabilizacion del estimador es un PI

c = kp +kis

=kps+ ki

s, (23)

que da lugar al siguiente comportamiento dinami-co

x(s) =(kps+ ki)x1(s)

s2 + kps+ ki+

sx2(s)

s2 + kps+ ki. (24)

Si los parametros del controlador son selecciona-dos de forma adecuada, el estimador trabajara im-plementado un filtro pasa bajos para x1 y un filtropasa altos para x2. Una de las opciones mas em-pleadas es disenar la ecuacion caracterıstica delestimador de forma que presente polos complejo-conjugados sin pico de resonancia. En dicho casodebe emplearse δ ≤ 1/

√2 en

2δωn = kp, ωn =√ki; (25)

por lo tanto, fijando δ = 1/√

2 los parametros kpy ki se establecen en funcion de la frecuencia decorte segun

kp = ωn

√2, ki = ω2

n; (26)

implicando que la relacion entre los parametrossiempre cumpla

ki = 0,5k2p. (27)

La ventaja principal del filtro complementario aco-plado con respecto del filtro complementario des-acoplado es su capacidad para rechazar perturba-ciones debidas a la deriva o drift. Considerese quela senal captada por el giroscopio x2 presenta unasenal de componente continua b2 (ver Seccion 2.1).Un filtro complementario desacoplado presenta unerror en regimen permanente de b2τ , mientras queel filtro acoplado es capaz de rechazar dicha per-turbacion.

4. Modificaciones del filtrocomplementario

Se describen dos modificaciones del filtro comple-mentario existentes en la literatura cientıfica, quepermiten mejorar algunos aspectos del mismo. Serealizara una breve descripcion de los mismos, pa-ra evaluarlos en la Seccion 5.

4.1. Gain Scheduling de controladores

Una de las deficiencias que presenta el filtro com-plementario es que emplea como senal de referen-cia la estimacion proveniente del acelerometro, da-do que esta senal es la mas precisa en condicionesestaticas. Cuando el sistema se encuentra conti-nuamente acelerado, esta referencia se distorsiona,haciendo que la estimacion sea erronea aunque elgiroscopio este registrando datos adecuados.

Como posible solucion al problema anterior, en[12] se propone el empleo de un Gain-Schedulingde controladores, que modifiquen el ancho de ban-da del filtro complementario en funcion de la ace-leracion registrada.

Conocido que la aceleracion de la gravedad (ex-presada en unidades de g) presenta modulo unita-rio, puede determinarse si el sistema se encuentraacelerado evaluando el modulo del vector acelera-cion medido por la IMU. De este modo puede de-terminarse cuando el estimador es susceptible deacumular errores. En estos casos debe desconec-tarse o limitarse la influencia del acelerometro enel proceso de estimacion. Para ello basta con re-ducir el ancho de banda de la funcion sensibilidadcomplementaria. Del mismo modo, cuando el vec-tor aceleracion se encuentre proximo a la unidadse puede considerar que el sistema se encuentraen condiciones estaticas, por lo que es posible darmayor peso (aumento del ancho de banda) a lasmedidas provenientes del acelerometo.

Actas de las XXXVI Jornadas de Automática, 2 - 4 de septiembre de 2015. Bilbao ISBN 978-84-15914-12-9 © 2015 Comité Español de Automática de la IFAC (CEA-IFAC) 916

Para realizar la seleccion de controladores, en cadaintervalo de muestro se evaluara

α = |√a2x + a2y + a2z − 1|, (28)

que emplearemos como parametro de seleccion delcontrolador. En [12] se establecen tres niveles dede aceleracion

Si α < 0,015g se puede considerar que el sis-tema no se encuentra acelerado. En estos ca-sos la estimacion llevada a cabo con el ace-lerometro sera precisa, ya que dicho instru-mento unicamente registra la aceleraccion dela gravedad. En [12] se propone el empleo deuna frecuencia de corte de 0,1 [rad/s].

Si 0,015g < α < 5g puede considerarse que elsistema se encuentra levemente acelerado. Enestos casos se recomienda reducir el ancho debanda del filtro pasa bajos, situandolo en elentorno de 0,01 [rad/s].

Si α > 5g se considera que el sistema se en-cuentra fuertemente acelerado. En estos ca-sos las aceleraciones adicionales a la gravedaddistorsionan de forma notable la estimacion.Para solucionar dicho problema se elimina lainformacion del acelerometro del proceso deestimacion empleando un filtro ideal con fre-cuencia de corte de 0 [rad/s].

4.2. Filtro complementario explıcito

El empleo de los angulos de Euler como sistemapara representar la orientacion implica una seriede problemas que pueden ser solucionados median-te el empleo de cuaternios. De entre estos proble-mas destaca el Gimbal lock, que se traduce en unbloqueo de los ejes de giro debido a que la ecua-cion (1) no se encuentra definida para θ = 90o.Debido a este problema y algunos otros de menorentidad, como puede ser la acumulacion de erro-res por el empleo de operaciones trigonometricas,es recomendable el empleo de los cuaternios co-mo sistema de representacion. En estimadores quedeben funcionar en sistemas con amplio espectrode rotacion esta recomendacion sera de obligadocumplimiento.

En [9] se propone el empleo de un filtro comple-mentario en el espacio SO(3). Esta modificacionposteriormente ha sido extendida e implementadaempleando cuaternios [10]. La principal ventaja deeste nuevo filtro, denominado Filtro Complemen-tario Explıcito (FCE), es que realiza la estimacionempleando un unico lazo de control como el de laFigura 6.

Como en el caso tradicional, el filtro FCE em-plea la prediccion de la direccion del vector gra-vedad para realizar la estimacion de la actitud.Para ello se emplean las medidas provenientes delacelerometro, que tras ser normalizadas dan lugar

gb =a

|a|, (29)

donde gb es la prediccion del vector gravedad. Co-mo en los casos anteriores, dicha prediccion solosera adecuada en condiciones estaticas.

En este caso no puede computarse el error de esti-macion como cuando se emplea un filtro comple-mentario, ya que la estimacion corresponde con unvector, no con una unica variable. Para solucionareste problema debemos calcular la gravedad esti-mada en el cuerpo gb. Definido q como el cuaternioque estima la orientacion del cuerpo, podemos ex-presar el vector gravedad terrestre g en el cuerpomediante la operacion de rotacion

gb = q ⊗ g ⊗ q∗, (30)

mediante la que se obtiene

gb =

2(q2q3 + q1q4)2(q3q4 + q1q2)

q21 − q22 − q23 + q24

(31)

que no es otra cosa que el vector gravedad que seobtiene a partir de la actitud estimada. Con estainformacion podemos comprobar si el estimadorpresenta errores gb 6= gb. El calculo del error serealiza mediante el producto vectorial

e = gb × gb. (32)

Vease como la operacion anterior da como resul-tado la rotacion relativa entre la aceleracion pre-dicha con el sensor inercial gb y la aceleracion es-timada gb (ver Figura 7). Este producto vectoriales mayor cuanto mas distorsion existe entre losvectores e indica la rotacion que debe aplicarse encada eje para que estos se encuentren alineados.

Como en el filtro complementario se como contro-lador c(s) un PI para garantizar que el error seanulo en regimen permanente. Observese que pe-se a representarse un unico controlador, este es elencargado de gobernar tres pares entrada/salida,uno por cada uno de los ejes del sistema. La ac-cion de control, denominada δ, representa la derivadinamica (drift) del giroscopio con signo negativoen cada uno de los ejes. De este modo la com-binacion de δ y ωb da lugar a ωb, es decir a laestimacion de la velocidad angular en el cuerpo.

Por ultimo la estimacion del cuaternio que repre-senta la orientacion del sistema se lleva a cabo

Actas de las XXXVI Jornadas de Automática, 2 - 4 de septiembre de 2015. Bilbao ISBN 978-84-15914-12-9 © 2015 Comité Español de Automática de la IFAC (CEA-IFAC) 917

a||a|| × c(s) 1

2 q ⊗ p(ωb)∫

2(q2q3 + q1q4)2(q3q4 + q1q2)

q21 − q2

2 − q23 + q2

4

a gb e δ

ωb

ωb˙q q

gb

Figura 6: Estructura del filtro complementario explıcito

g ggg

0

b b

b b

Figura 7: Calculo del error de estimacion comoproducto vectorial gb × gb

empleando el producto

q =1

2q ⊗ p(ωb), (33)

donde p(ωb) = [0, ωb]. Es decir, se emplea la ecua-cion que relaciona la velocidad con la orientacion,equivalente de (1) en cuaternios.

5. Comparacion de alternativas

A continuacion se muestra una comparativa entrelas alternativas estudiadas en la seccion anterior.Por un lado se ha disenado un filtro complemen-tario explıcito cuyo controlador se sintoniza segun

c(s) = 1 +0,5

s, (34)

dando lugar a un ancho de banda de 0,7 [rads/s].Este valor es superior al recomendado en [12], perose considera adecuado ya que las pruebas experi-mentales realizadas someten al cuerpo a acelera-ciones leves.

Por otro lado se ha implementado un Gain-Scheduling para un filtro complementario explıcitoFCE. Notese como no se trata exactamente del fil-tro propuesto en la Seccion 4.1, ya que en dichaseccion se emplea un filtro complementario aco-plado. Se ha considerado oportuna la modificacionpuesto que en una de las pruebas experimentalesse efectuan giros en θ y φ muy amplios, proximos

al gimbal-lock. Este hecho puede producir que elfiltro acoplado falle por el sistema de represen-tacion, impidiendo realizar una comparativa ade-cuada. Por lo tanto, el segundo filtro implemen-tado puede considerarse una combinacion de lasdos modificaciones propuestas en la Seccion 4, enla que se agrupan las ventajas de ambas. El Gain-Scheduling se implementa siguiendo el siguientecriterio:

Si α ≤ 0,02 se emplea el controlador (34).En estos casos se considera que el sistema seencuentra en condiciones estaticas.

Si α > 0,02 se emplea como controladorc(s) = 0. Es decir, se desconecta el acelerome-tro del estimador por considerarse que el sis-tema se encuentra sometido a aceleraciones.

La primera prueba de validacion consiste en so-meter a los dos estimadores a la prueba descritaen la Seccion 2.4 y mostrada en la Figura 2. Losresultados obtenidos con los nuevos estimadores,junto con los anteriores se muestran en la Figura8. Puede comprobarse como los nuevos resultadosmejoran los obtenidos con acelerometro y girosco-pio por separado. Se observa como en ambos casosel comportamiento dinamico es similar al obtenidocon la estimacion del giroscopio. Por el contrario,en la region estatica las estimaciones se igualan alas realizadas con el acelerometro.

Queda demostrado que los dos algoritmos funcio-nan de forma adecuada, pero no se aprecian dis-torsiones entre las actitudes calculadas con cadauno de ellos. Como se indico en la Seccion 2.4 elexperimento presentado representa un caso idealde funcionamiento, hecho que produce que α su-pere el umbral para el switching del controladoren pocos instantes de muestreo.

El segundo experimento de validacion es presen-tado en la Figura 9. En el se aprecian tres fases,la primera y la tercera corresponden con un unmovimiento altamente acelerado, durante el cualse somete al sistema a una serie de rotaciones al-ternativas y cambios bruscos de direccion de des-plazamiento en los tres ejes del dispositivo. Du-

Actas de las XXXVI Jornadas de Automática, 2 - 4 de septiembre de 2015. Bilbao ISBN 978-84-15914-12-9 © 2015 Comité Español de Automática de la IFAC (CEA-IFAC) 918

0 10 20 30 40 50 60 70 80 90

-60

-30

0

30

60

90R

oll(

o )Estimación de actitud

gyroacelFCEGS

gyroacelFCEGS

0 10 20 30 40 50 60 70 80 90Tiempo (s)

-60

-30

0

30

60

Pitc

h(o )

Figura 9: Estimaciones de actitud realizadas para el segundo experimento

0 3 6 9 12 15-5

5

15

25

35

45

55

Rol

l(o )

Estimación de actitud

gyroacelFCEGS

0 3 6 9 12 15Tiempo (s)

5

-5

-15

-25

-35

Pitc

h(o )

gyroacelFCEGS

Figura 8: Estimaciones de actitud realizadas parael primer experimento

rante la segunda fase del experimento el sistemase mantiene en reposo, por lo que su actitud debecorresponder con θ = φ = 0o.

En este caso se observa como pese a presentar bue-nos resultados los dos estimadores, el filtro GS(Gain-Scheduling) presenta un mejor comporta-miento. Por un lado se observa una menor derivacuando el sistema se encuentra acelerado y porotro se logra una convergencia al valor de regimenestacionario en un menor tiempo. Estos dos bene-ficios son los esperados, ya que las aceleracionesimplicadas en el experimento producen un erroren la prediccion del vector gravedad. Este error deprediccion produce que la senal de error empleadapor el controlador no sea la correcta. El empleo deun integrador en c(s) produce que estos errores seacumulen, efecto similar al Wind Up integral. Porello el FCE demanda un tiempo mayor en conver-ger al valor adecuado cuando se entra en la regionestatica.

6. Conclusiones

Se han presentado las ecuaciones que permiten es-timar la actitud de un sistema empleando una uni-dad de medida inercial. Un estudio analıtico y ex-perimental demuestra que el empleo de un unicosensor, vease acelerometro o giroscopio, para lle-var a cabo la estimacion limita la precision de la

Actas de las XXXVI Jornadas de Automática, 2 - 4 de septiembre de 2015. Bilbao ISBN 978-84-15914-12-9 © 2015 Comité Español de Automática de la IFAC (CEA-IFAC) 919

misma. El concepto de filtro complementario esdescrito de forma generica y extendido al caso par-ticular de trabajo, mostrando dos alternativas deimplementacion. Se han descrito dos modificacio-nes del filtro complementario clasico que permitenmejorar los resultados del mismo. Por ultimo, seha llevado a cabo una comparativa entre algorit-mos que permite determinar que el empleo de unGain-Scheduling de controladores junto con un fil-tro complementario explıcito mejora los resultadosde estimacion.

Agradecimientos

Los autores agradecen la ayuda prestada por elGobierno de La Rioja a traves del proyecto ADER2012-I-IDD-00093.

Referencias

[1] D. Gebre-Egziabher, R. C. Hayward, andJ. D. Powell. Design of multi-sensor atti-tude determination systems. IEEE Transac-tions on Aerospace and Electronic Systems,40(2):627–649, 2004.

[2] Billur Barshan and Hugh F. Durrant-Whyte.Inertial navigation systems for mobile robots.IEEE Transactions on Robotics and Automa-tion, 11(3):328–342, 1995.

[3] H. G. De Marina, F. J. Pereda, J. M. Giron-Sierra, and F. Espinosa. Uav attitude estima-tion using unscented kalman filter and triad.IEEE Transactions on Industrial Electronics,59(11):4465–4474, 2012.

[4] Z. Hurak and M. Rezac. Image-basedpointing and tracking for inertially stabi-lized airborne camera platform. ControlSystems Technology, IEEE Transactions on,20(5):1146–1159, Sept 2012.

[5] L. Fang, P. J. Antsaklis, L. A. Montestru-que, M. B. McMickell, M. Lemmon, Y. Sun,H. Fang, I. Koutroulis, M. Haenggi, M. Xie,

and X. Xie. Design of a wireless assistedpedestrian dead reckoning system - the nav-mote experience. IEEE Transactions on Ins-trumentation and Measurement, 54(6):2342–2358, 2005.

[6] J. L. Crassidis, F. Landis Markley, andY. Cheng. Survey of nonlinear attitude esti-mation methods. Journal of Guidance, Con-trol, and Dynamics, 30(1):12–28, 2007.

[7] D. Choukroun, I. Y. Bar-Itzhack, and Y. Osh-man. Novel quaternion kalman filter. IEEETransactions on Aerospace and ElectronicSystems, 42(1):174–190, 2006.

[8] W. Li and J. Wang. Effective adaptive kal-man filter for mems-imu/magnetometers in-tegrated attitude and heading reference sys-tems. Journal of Navigation, 66(1):99–113,2013.

[9] R. Mahony, T. Hamel, and J. . Pflimlin. Non-linear complementary filters on the specialorthogonal group. IEEE Transactions on Au-tomatic Control, 53(5):1203–1218, 2008.

[10] M. Euston, P. Coote, R. Mahony, J. Kim, andT. Hamel. A complementary filter for atti-tude estimation of a fixed-wing uav. In 2008IEEE/RSJ International Conference on Inte-lligent Robots and Systems, IROS, pages 340–345, 2008.

[11] S. O. H. Madgwick, A. J. L. Harrison, andR. Vaidyanathan. Estimation of imu andmarg orientation using a gradient descent al-gorithm. In IEEE International Conferenceon Rehabilitation Robotics, 2011.

[12] T. S. Yoo, S. K. Hong, H. M. Yoon, andS. Park. Gain-scheduled complementary filterdesign for a mems based attitude and headingreference system. Sensors, 11(4):3816–3830,2011.

Actas de las XXXVI Jornadas de Automática, 2 - 4 de septiembre de 2015. Bilbao ISBN 978-84-15914-12-9 © 2015 Comité Español de Automática de la IFAC (CEA-IFAC) 920