Fusi on de SLAM Monocular´ con Sensores Inerciales para la ... · Aplicando Filtro Kalman por el Ingeniero Electr onico´ Carlos Alberto Vianchada Est evez´ Tesis sometida como

Fusion de SLAM Monocularcon Sensores Inerciales para laEstimacion de la Orientacion

Aplicando Filtro Kalman

por el Ingeniero Electronico

Carlos Alberto Vianchada Estevez

Tesis sometida como requisito parcial paraobtener el grado de:

MAESTRO EN CIENCIAS EN LAESPECIALIDAD DE ELECTRONICA

en el

Instituto Nacional de Astrofısica,Optica yElectronicaAgosto 2012

Tonantzintla, Puebla

Supervisada por:

Dr. Ponciano Jorge Escamilla AmbrosioDr. Juan Manuel Ramırez Cortes

c©INAOE 2012El autor otorga al INAOE el permiso de

reproducir y distribuir copias en su totalidad o enpartes de esta tesis

i

RESUMEN

TITULO:

FUSION DE SLAM MONOCULAR CON SENSORES INERCIALES PARA LA ESTIMACION DE LA

ORIENTACION APLICANDO FILTRO KALMAN

AUTOR: CARLOS ALBERTO VIANCHADA ESTEVEZ

PALABRAS CLAVE: Sistema de Referencia de Orientacion y Rumbo, Fusion de Medidas, Marcos de

Referencia, Matrices de Rotacion, Cuaternion, Angulos de Euler, Filtro Kalman.

CONTENIDO:

La gran variedad de beneficios que ofrece el sistema SLAM basado en un sensor de vision monocular

se ve en gran parte limitada por la inexistencia de sensores adicionales que contribuyan a la estimacion

de la orientacion y trayectoria seguida por la camara. En este trabajo se adiciona un sensor inercial al

sistema SLAM con el objetivo de que la combinacion de los sensores se consolide como una fuente de

informacion cuya calidad es mejor que la provista por cualquier sensor de manera individual, del mismo

modo, se incremente la confiabilidad y disponibilidad de un sistema de medicion ante la falla de alguno de

ellos.

De manera mas especıfica, este trabajo consistio en la fusion de medidas de orientacion proporcionadas

por dos sistemas independientes uno del otro. En primera instancia se presenta una breve discusion sobre

la tecnica de mapeo y localizacion simultanea que utiliza una camara monocular como sensor del entorno.

Posteriormente se mencionan las principales caracterısticas del modulo inercial de bajo costo utilizado

para la elaboracion de un sistema de referencia de orientacion y rumbo (AHRS por sus siglas en ingles)

usando cuaterniones como herramienta de descripcion de la orientacion. La fusion de medidas fue llevada

a cabo implementando metodologıas que usan como estimador secuencial el filtro Kalman con el objetivo

de identificar la tecnica que ofreciera un menor error en la estimacion.

Los resultados evidencian una reduccion del error en la estimacion de la orientacion aunque esta varıe

dependiendo de la tecnica aplicada. Se observa que los modelos de fusion que presentan menor error son

aquellos que retroalimentan al sistema la combinacion de las medidas.

ii

ABSTRACT

TITLE:

FUSION OF MONOCULAR SLAM WITH INERTIAL SENSORS TO THE ATTITUDE ESTIMATION

APPLYING KALMAN FILTER

AUTHOR: CARLOS ALBERTO VIANCHADA ESTEVEZ

KEY WORDS: Attitude and Heading Reference System, Measurements Fusion Models, Reference Frames,

Rotation Matrix, Quaternion, Euler Angles, Kalman Filter.

CONTENTS:

The variety of benefits that SLAM based on monocular vision offers, is limited by the absence of additional

sensors that contribute to the estimation of orientation and the trajectory followed by the camera. This

dissertation adds an inertial sensor to SLAM system in order to consolidate a source information which its

quality is better than that provided by any individual sensor. In the same way, the combination increases

the reliability and availability of a measurement system before the failure of some sensor.

Specifically this work consisted in the orientation measures fusion provided by two independent systems.

In the first instance, is presented a brief discussion about the technique of simultaneous localization and

mapping which uses a monocular camera as environment sensor. Subsequently the main features of the

low cost inertial module used to the elaboration of the attitude and heading reference system (AHRS)

are mentioned, also, the use of quaternion as orientation descriptor. The measures fusion was implemented

using methodologies based in Kalman filter with the objective to identify a technique which offers minor

error in the estimation.

The results showed a error reduction in the orientation estimation although it changes depending of the

technique applied. The fusion models that have less error in the estimation are those which feedback the

measures combination.

Agradecimientos

Primero quiero agradecer a Dios por llenar mi vida de bendiciones.

A mi familia por su apoyo incondicional a todo lo que me propongo, especialmente a mi mama, mi

papa y mi hermano.

A los doctores Jorge Escamilla y Juan Manuel Ramırez por supervisar mi trabajo.

Al Instituto Nacional de Astrofısica Optica y Electronica (INAOE) por ser la institucion que me

abrio sus puertas para poder realizar mis estudios.

Al Consejo Nacional de Ciencia y Tecnologıa (CONACY T ) por birndarme el apoyo financiero para

poder culminar mis estudios de maestrıa.

Carlos Alberto Vianchada Estevez

iv

Indice general

Resumen I

Abstract II

Agradecimientos III

1. Introduccion 1

1.1. Antecedentes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.2. SLAM basado en vision . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.2.1. Formulacion general del problema SLAM visual . . . . . . . . . . . . . . . . . . . 5

1.2.2. Representacion de los estados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

1.2.3. Modelo del proceso . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.2.3.1. Modelo de posicion constante . . . . . . . . . . . . . . . . . . . . . . . . 8

1.2.3.2. Modelo de velocidad constante . . . . . . . . . . . . . . . . . . . . . . . 9

1.2.4. Modelo de la medida . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

1.2.5. Modelo de observacion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

1.2.5.1. Inicializacion de caracterısticas . . . . . . . . . . . . . . . . . . . . . . . 13

2. Modulo inercial 15

2.1. Especificaciones del modulo inercial . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

2.2. Microcontrolador . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

2.3. Sensores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

2.3.1. Acelerometro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

2.3.2. Giroscopo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

2.3.3. Magnetometro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

2.4. Caracterizacion de los sensores inerciales . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

2.5. Marco de referencia del modulo inercial . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3. Sistema de referencia de actitud y rumbo (AHRS) 23

3.1. Marcos de referencia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

3.1.1. Marco inercial i . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

v

vi INDICE GENERAL

3.1.2. Marco de la tierra e . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.1.3. Marco de navegacion n . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.1.4. Marco del cuerpo b . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.2. Matrices de Rotacion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

3.3. Representacion de la orientacion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

3.3.1. Angulos de Euler . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

3.3.2. Cuaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3.4. Computacion de la actitud . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

3.4.1. Actitud en terminos de cuaterniones . . . . . . . . . . . . . . . . . . . . . . . . . 33

3.4.2. Deteccion de la inclinacion empleando acelerometros . . . . . . . . . . . . . . . . 34

3.4.3. Deteccion del rumbo empleando magnetometros . . . . . . . . . . . . . . . . . . 35

3.4.4. Algoritmo para la computacion de la actitud . . . . . . . . . . . . . . . . . . . . 37

3.5. Implementacion del algoritmo para la computacion de la actitud . . . . . . . . . . . . . 42

4. Algoritmos de fusion 45

4.1. Modelos de fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

4.1.1. Modelos de fusion de medidas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

4.1.1.1. Aumento del vector de observacion . . . . . . . . . . . . . . . . . . . . . 47

4.1.1.2. Estimacion de mınima media cuadratica . . . . . . . . . . . . . . . . . . 47

4.1.2. Modelos de fusion track to track . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

4.1.2.1. Algoritmo de fusion track to track . . . . . . . . . . . . . . . . . . . . . 48

4.1.2.2. Algoritmo de fusion modificado track to track . . . . . . . . . . . . . . 50

5. Resultados experimentales 53

5.1. Modelo de fusion de las medidas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

5.2. Fusion track to track . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

5.3. Fusion modificada track to track . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

6. Conclusiones 61

6.1. Trabajo futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

Bibliografıa 67

Referencia Grafica 69

A. Problema general de estimacion secuencial 71

A.1. Filtro Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

A.2. Filtro Kalman Extendido . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

B. Modelo de formacion de la imagen 77

B.1. Perspectiva de proyeccion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

B.2. Posicion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79

Indice de figuras

1.1. Sistema SLAM basado en vision. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.2. Esquema del funcionamiento del sistema SLAM basado en vision. . . . . . . . . . . . . 5

1.3. Problema general del SLAM basado en vision. . . . . . . . . . . . . . . . . . . . . . . . 6

1.4. Modelo de posicion constante. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

1.5. Modelo de velocidad constante. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

1.6. Parametrizacion de la caracterıstica y ecuacion de medida. . . . . . . . . . . . . . . . . . 12

2.1. Modulo inercial iNEMO STEVAL-MKI062V2. . . . . . . . . . . . . . . . . . . . . . . . . 15

2.2. Medidas del giroscopo en reposo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

2.3. Medidas del acelerometro en reposo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

2.4. Medidas del magnetometro en reposo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

2.5. Sistema de referencia de iNEMO. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

2.6. Sentido de giro en el que las medidas del giroscopo son positivas. . . . . . . . . . . . . . 21

3.1. Sistema de referencia de actitud y rumbo (AHRS). . . . . . . . . . . . . . . . . . . . . . 23

3.2. Marco de referencia inercial i, de la tierra e y de navegacion n . . . . . . . . . . . . . . 24

3.3. Marco de navegacion n . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

3.4. Marco del cuerpo b. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

3.5. Rotacion sobre el eje Z. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.6. Rotacion sobre el eje X. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

3.7. Rotacion sobre el eje Y . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

3.8. Angulos de Euler: alabeo θ, cabeceo φ y guinada ψ. . . . . . . . . . . . . . . . . . . . . . 28

3.9. Vector antes y despues de una rotacion. . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3.10. Inconvenientes para el calculo de la orientacion a partir de acelerometros y giroscopos. . 32

3.11. Deteccion del alabeo θ y cabeceo φ con el acelerometro. . . . . . . . . . . . . . . . . . . 35

3.12. Campo magnetico en coordenadas X Y Z. . . . . . . . . . . . . . . . . . . . . . . . . . . 36

3.13. Filtro Kalman extendido para la estimacion de la orientacion. . . . . . . . . . . . . . . . 37

3.14. Estimacion del bias presente en las medidas del giroscopo. . . . . . . . . . . . . . . . . . 42

3.15. Consistencia del filtro Kalman extendido para la orientacion. . . . . . . . . . . . . . . . 43

3.16. Comparacion del iNEMO con el AHRS. . . . . . . . . . . . . . . . . . . . . . . . . . . 44

vii

viii INDICE DE FIGURAS

4.1. El proceso de fusion de las medidas. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

4.2. Fusion track to track. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

4.3. Fusion modificada track to track. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

5.1. Esquema general de implementacion de la fusion de medidas. . . . . . . . . . . . . . . . 54

5.2. Resultados del modelo de fusion de medidas. . . . . . . . . . . . . . . . . . . . . . . . . 55

5.3. Resultados de la fusion track to track. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

5.4. Covarianza del error para la fusion track to track ignorando P 12k|k. . . . . . . . . . . . . . 58

5.5. Resultados de la fusion modificada track to track. . . . . . . . . . . . . . . . . . . . . . . 59

B.1. Camara monocular. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

B.2. Geometrıa del modelo de la camara de agujero de alfiler. . . . . . . . . . . . . . . . . . . 78

B.3. Transformaciones entre el marco de referencia n y del cuerpo b. . . . . . . . . . . . . . . 79

Indice de tablas

2.1. Caracterısticas del acelerometro. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

2.2. Caracterısticas de los giroscopos. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

2.3. Caracterısticas del magnetometro. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

2.4. Momentos estadısticos de la medida del giroscopo . . . . . . . . . . . . . . . . . . . . . . 18

2.5. Momentos estadısticos de la medida del acelerometro . . . . . . . . . . . . . . . . . . . . 20

2.6. Momentos estadısticos de la medida del magnetometro . . . . . . . . . . . . . . . . . . . 20

3.1. Momentos estadısticos de los angulos medidos . . . . . . . . . . . . . . . . . . . . . . . . 41

3.2. Error medio cuadratico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

5.1. Momentos estadısticos de los angulos medidos (SLAM) . . . . . . . . . . . . . . . . . . 54

5.2. Error medio en el aumento del vector de observacion . . . . . . . . . . . . . . . . . . . . 56

5.3. Error medio en la estimacion de mınima media cuadratica . . . . . . . . . . . . . . . . . 56

5.4. Error medio en la fusion track to track . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

5.5. Error medio en la fusion track to track ignorando P 12k|k . . . . . . . . . . . . . . . . . . . 58

5.6. Error medio en la fusion modificada track to track . . . . . . . . . . . . . . . . . . . . . 58

5.7. Error medio de los modelos de fusion de medidas . . . . . . . . . . . . . . . . . . . . . . 59

5.8. Error medio de los modelos de fusion track to track . . . . . . . . . . . . . . . . . . . . . 59

ix

Capıtulo 1

INTRODUCCION

La posible creacion de mapas y localizacion en ambientes totalmente desconocidos ha sido uno de los

mas notable sucesos en la comunidad robotica en esta ultima decada. Por tal motivo, el sistema de loca-

lizacion y mapeo simultaneos SLAM1, ha sido considerado como el “santo grial” de la robotica, debido

a que esta tecnica proporcionarıa los medios necesarios para la construccion de un robot verdadera-

mente autonomo. Actualmente, en el area de la robotica tradicional el SLAM puede ser considerado

como un problema resuelto y esto ha permitido que pueda ser implementado en diferentes ambientes

interiores como exteriores, al igual que en vehıculos terrestres, aereos y acuaticos [1][2][3]. Sin embargo,

siguen existiendo problemas importantes en la realizacion de soluciones mas generales y en propor-

cionar mapas ricos en informacion.

Los algoritmos SLAM han sido elaborados usando una gran variedad de sensores como laser, sonar,

radar y encoders, entre otros; pero el uso de camaras como sensor del SLAM no habıa sido el centro

de atencion por la comunidad de vision por computadora. Esta idea cambio con el advenimiento de

camaras modernas, que actualmente son compactas, precisas, no invasivas, bien entendidas y relativa-

mente baratas. Adicionalmente, la tasa de transferencia de datos proveniente de una camara es mucho

mayor en comparacion con la de otros sensores, sin olvidar que la vision para los seres humanos como

para animales es una de las principales herramientas usadas para la navegacion. Este suceso ha per-

mitido que la localizacion y mapeo simultaneos usando una camara monocular haya recibido mucha

atencion en los ultimos anos [4][5][6].

Los sistemas SLAM basados en vision proporcionan conocimiento espacial para robots, permitiendo

la estimacion de movimentos en ambientes totalmente desconocidos. A pesar de ello, el uso de una

camara como unico sensor del entorno presenta algunos inconvenientes. Principalmente sucede cuando

el tamano del ambiente empieza a crecer, implicando un incremento en los requerimientos computa-

cionales de los algoritmos y dificultando la operacion en tiempo real. Como resultado, la consistencia y

precision en la estimacion obtenida decrece. Por otra parte, la escala de la escena no es observable. Es

decir, que el mapa generado presenta variaciones en las medidas de distancia estimadas desde la camara

hasta las caracterısticas visuales detectadas entre experimentos. En consecuencia, la gran variedad de

beneficios de localizacion y mapeo que ofrece el sistema SLAM basado en vision se ve en gran parte

limitada.

1Simultaneous Localization and Mapping

1

2 CAPITULO 1. INTRODUCCION

La fusion de sensores ha surgido como solucion a diferentes problematicas principalmente porque la

combinacion de sensores consolida una fuente de informacion cuya calidad es mejor que la provista por

cualquier sensor de manera individual. La informacion redundante proporcionada por varios sensores

reduce la incertidumbre e incrementa la exactitud, del mismo modo, se incrementa la confiabilidad y

disponibilidad de un sistema de medicion ante la falla de algun sensor. El objetivo de este trabajo es

mejorar la estimacion realizada por el sistema SLAM de la orientacion de la camara. Por consiguiente,

la idea consiste en utilizar un sensor adicional con el cual se pueda generar otra estimacion de dichos

estados y realizar una fusion de sensores con el sistema SLAM basado en vision utilizando tecnicas de

fusion de medidas basadas en el filtro Kalman. Para ello se ha optado por usar una unidad de medida

inercial IMU2 de bajo costo que dispone de sensores inerciales tales como acelerometros, giroscopos

y magnetometros que suministran informacion de la aceleracion lineal y velocidad angular detectada

en el vehıculo (en este caso la camara). Todo bajo la hipotesis de que las medidas inerciales ayudan a

mejorar la orientacion estimada de la camara y por ende, se puede obtener una mejora en el factor de

escala del mapa [7].

A diferencia del trabajo realizado por Pinies [8] que consiste en incluir las medidas inerciales en el

modelo del proceso del SLAM ; lo que se propone en este trabajo esta orientado a elaborar un sistema

de referencia de actitud y rumbo (AHRS3) que estime la orientacion de la camara, para que poste-

riormente puedan ser fusionadas con las medidas del sistema SLAM basado en vision. Por lo tanto,

la fusion es implementada usando dos sistemas independientes uno del otro. Los objetivos del trabajo

son los siguientes:

Elaborar un sistema de referencia de actitud y rumbo (AHRS) para describir la orientacion del

vehıculo usando cuaterniones.

Estudiar las tecnicas de fusion de medidas que usan como estimador secuencial el filtro Kalman.

Implementar la fusion de las medidas del AHRS con el sistema SLAM basado en vision.

1.1. Antecedentes

La combinacion de vision con sensores inerciales logra que los diferentes tipos de informacion sean

apropiados para las distintas circunstancias, lo que hace posible observar mas detalladamente el en-

torno y realizar una estimacion robusta de su propio movimiento mitigando los errores presentes en

cada sensor, en una escena 3D sin infraestructura externa. Las medidas inerciales generalmente se usan

para la elaboracion de sistemas de navegacion, pero su gran inconveniente es el offset que aparece en

las lectura de los sensores dentro del IMU . Con la inclusion de la vison este efecto puede ser reduci-

do buscando una mejora en la navegacion inercial [9], adicionalmente, ha dado pie para crear nuevas

alternativas en sistemas de navegacion como por ejemplo para peatones [10][11] y para vehıculos [12].

2Inertial Measurement Unit3Attitude Heading Reference System

1.2. SLAM BASADO EN VISION 3

Aunque se podrıa pensar que la principal aplicacion de estos sistemas serıan en robotica y sistemas

de navegacion, tambien existen aplicaciones para realidad aumentada [13] y hasta aplicaciones en la

medicina en intervenciones cistoscopicas [14].

Los estudios con sensores inerciales y vision han sido enfocados en la mejora de los sistemas de nave-

gacion, principalmente buscando una mejor estimacion de la posicion del vehıculo [15][16]. La diferencia

en dichos trabajos radica en las diferentes tecnicas o metodologıas usadas para extraer informacion a

traves del procesamiento de la imagen. De la misma forma, no solo se ha mejorado la estimacion de la

posicion del vehıculo sino que adicionalmente los procedimientos han sido extendidos para establecer

la orientacion y velocidad del mismo [17][18].

El procedimiento general que realiza la fusion de las medidas inerciales con una camara monocular

es llevado a cabo aplicando un estimador secuencial como el filtro Kalman y sus variaciones. Un sensor

es el encargado de realizar una estimacion de los estados y el otro permite realizar las correcciones a

dichas estimaciones [19][20][21]. De esta manera es posible reducir el tiempo de computacion y mejorar

el desempeno en el procesamiento de la imagen, el inconveniente es que la medidas eventualmente se

pueden alejar cuando exista perdida de informacion proveniente de la camara (oclusion, movimientos

rapidos).

Propiamente el trabajo que fusiona un sistema SLAM con camara monucular con sensores inerciales

fue elaborado por Pinies [7][8]. Aprovechando que el sistema SLAM basado en vision utiliza un filtro

Kalman extendido para la estimacion de sus estados, las medidas inerciales son incluidas en el modelo

del proceso del filtro el cual realiza la estimacion. Seguidamente, las observaciones de los estados son

extraıdas mediante el procesamiento de la imagen mientras el modelo de la medida se mantiene igual.

Sus resultados reflejan una mejora en la estimacion de la trayectoria seguida por la camara y una

reduccion en la ambuiguedad de la escala del mapa obtenida.

1.2. SLAM basado en vision

El SLAM , (Simultaneous Localization and Mapping) es una tecnica que permite construir mapas

consistentes de entornos desconocidos, mientras simultaneamente determina su ubicacion dentro del

mismo. El resultado final de la aplicacion de este tecnica, es la obtencion de mapas probabilısticos

basado en caracterısticas.

De manera general, el sistema SLAM basado en vision es capaz de detectar caracterısticas (esquinas,

figuras con formas conocidas como rectangulos, circulos, cambio de contraste y demas) en una secuen-

cia de imagenes proporcionada por la camara. Un procesamiento de la imagen hace posible estimar su

ubicacion, la trayectoria seguida por la camara y adicionalmente proporciona una medida de distancia

entre la camara y la caracterıstica detectada, permitiendo la recreacion del entorno tal como se ilustra

en la figura 1.1(b).


(a) Deteccion de caracterısticas en la imagen. (b) Mapa probabilıstico.

Figura 1.1: Sistema SLAM basado en vision: (a) Las elipses representan la region donde es buscada la

caracterıstica y la incertidumbre en su ubicacion. Las elipses verdes hacen referencia a las marcas que

han sido satisfactoriamente detectados, mientras que los rojos hacen referencia a los puntos perdidos.

(b) Incremento en la incertidumbre de la posicion de las marcas visuales detectadas a medida que se

explora nuevas areas (elipses rojas). Trayectoria de la camara (puntos verdes).

El problema del SLAM siempre ha recibido un enfoque probabilıstico desde que fue introducido,

por tal motivo, varios grupos de investigadores han estado buscando y aplicando varios metodos de

estimacion teorica para solucionar el problema de la localizacion y mapeo simultaneo. Principalmente,

los estudios realizados por Smith y Cheesman [22] y Durrant-Whyte [23] fueron claves en este proceso.

Debido a que se logro demostrar la existencia de un alto grado de correlacion entre la estimacion de

localizacion de diferentes puntos de referencia en un mapa y que estas crecerıan con las observaciones

sucesivas. Pocos anos despues, Smith en [24] presento la base de lo que es hoy en dıa el SLAM . Men-

ciono que para lograr una completa y consistente solucion para el problema de localizacion y mapeo

simultaneo, se requerirıa una union de estados compuestos por la actitud del vehıculo y la posicion

de cada punto de referencia, para ser actualizado en cada observacion. A su vez, se necesitarıa que el

estimador empleara un vector de estado tan grande como el numero de puntos de referencia que se

mantienen en el mapa.

Actualmente, la localizacion y mapeo simultaneos basado en vision es un enfoque alternativo para

la solucion del problema SLAM en lınea. Esta tecnica aplica un filtrado probabilıstico para actualizar

de forma secuencial las estimaciones de las posiciones de las caracterısticas en el mapa, y la ubicacion

actual de la camara. El SLAM basado en vision tiene diferentes fortalezas y debilidades en odometrıa

visual, siendo capaz de construir mapas consistentes y libres de derivas, pero con un numero limitado

de caracteristicas [25]. Asi mismo, el SLAM usando como nucleo el filtro Kalman extendido fue la


primera tecnica aplicada con exito en tiempo real para el seguimiento usando una camara monocular,

la cual fue desarrollada por Davison [4][5].

1.2.1. Formulacion general del problema SLAM visual

El sistema SLAM basado en vision puede ser visto como un problema general de estimacion secuen-

cial donde el objetivo es estimar la distribucion de probabilidad de un espacio de estados [26][27]. El

espacio de estados es n-dimensional y para este caso en particular esta constituido por la ubicacion 3D

de la camara y un conjunto de caracterısticas visuales. La camara tiene un vector de estados xv(k),

que describe su posicion relativa a algun marco de referencia conocido en el instante de tiempo k. A la

vez que esta se mueve a traves del ambiente, la camara toma observaciones zi(k), de la caracterıstica

i. Cada caracterıstica es representada por un vector de estados mi, el cual corresponde a la posicion

invariante en el tiempo de la i-esima marca relativa en un marco de referencia conocido.

KX

K+1X

K+2X

K+3X

1m

2m

3m

4m

5m

KZ

K+1Z

K+2Z

Robot Marca

Estimada

Real

Figura 1.2: Esquema del funcionamiento del sistema SLAM basado en vision.

La posicion de la camara y de las caracterısticas visuales en el instante de tiempo k depende de

la posicion inicial, xv(0), de la camara y la subsiguiente secuencia de medidas, z(0 : k), de todas las

caracterısticas visuales. Esto puede ser formulado como un problema general de estimacion secuencial

y representado por una funcion de densidad de probabilidad conjunta.

p(xv(k),m|z(0 : k), xv(0)) (1.1)

Donde m es el conjunto completo de posiciones de las caracterısticas. Para llevar a cabo este procedi-

miento eficentemente en tiempo real, el objetivo es encontrar una solucion recursiva de manera que el


CámaraXv(k)

Z1(k)

Z2(k)m1(k)

m2(k)

puntos 3-D(marcas)

Perspectiva deproyección

(a) Componentes de estado.

Las marcas estimadaspermanecen sin cambios

Movimiento dela cámara

La incertidumbre en la posición estimada de la

ćamara crece

(b) Prediccion de la posicion de la camara.

Marginalización de la anteriorposición de la cámara

Observación de las marcas

(c) Marginalizacion y observacion.

Condicionamiento sobrelas nuevas obsevaciones

Las incertidumbresestimadas se reducen

(d) Condicionamiento.

Figura 1.3: Problema general del SLAM basado en vision.

conjunto de observaciones z(k), del actual instante de tiempo pueda ser combinado con la distribucion

de estado en el instante de tiempo previo para encontrar la distribucion de estados actual.

p(xv(k − 1),m|z(0 : k − 1), xv(0)) (1.2)

La evolucion de la distribucion de estados es controlada por dos procesos. Tomando la posicion de

la camara del intervalo de tiempo anterior y asumiendo que las caracterısticas visuales en el mapa son

estaticas, una funcion de proceso predice la nueva distribucion del estado en el intervalo de tiempo k:

p(xv(k),m|x(k − 1)) (1.3)

Luego la distribucion de estados basada en todas las observaciones previas puede ser recursivamente

actualizada por la marginalizacion de la dependencia en el estado anterior:


p(xv(k),m|z(0 : k − 1), xv(0)) =

∫p(xv(k),m|xv(k − 1))

p(xv(k − 1),m|z(0 : k − 1), xv(0))dxv(k − 1)

(1.4)

Una funcion de medida usa la prediccion de la distribucion del estado actual para definir la distribu-

cion del conjunto de observaciones generados en el intervalo de tiempo actual:

p(z(k)|xv(k),m) (1.5)

Esta permite que la distribucion del estado pueda ser recursivamenten actualizada usando el teorema

de Bayes teniendo en cuenta el efecto de las nuevas observaciones:

p(xv(k),m|z(0 : k), xv(0)) =p(z(k)|xv(k),m)p(xv(k),m|z(0 : k − 1), xv(0))

p(z(k)|z(0 : k − 1))(1.6)

La secuencia recursiva de marginalizacion y condicionamiento de la distribucion de el estado ilustrada

en la figuras 1.3(a), 1.3(b), 1.3(c) y 1.3(d), es el proceso fundamental seguido por los filtros de estimacion

secuencial y es independiente del tipo de distribucion. Sin embargo, si el vector de estados, x, que

contiene a xv y m tiene una distribucion gaussiana, sus estadısticas pueden ser representadas por su

media, x y covarianza, P , y el filtro Kalman y sus variaciones pueden ser usadas como estimador

secuencial del problema. Una revision del filtro Kalman es presentada en el apendice A.

1.2.2. Representacion de los estados

Generalmente en implementaciones SLAM monoculares, el vector de estados del sistema x, esta com-

puesto por un conjunto de componentes individuales que representan la posicion y caracterısticas

dinamicas de la camara xv y la posicion de las marcas visuales mas relevantes del entorno mi. Cada

componente tiene asociada una serie de procesos, medidas y modelos de asociacion de datos los cuales

gobiernan el comportamiento del componente durante el ciclo de actualizacion del filtro. El vector de

estados se representa de la siguiente manera:

x =

xv

m1

m2

...

mi

(1.7)

Donde xv representa la posicion de la camara (rotacion y traslacion) y m1−i corresponde a la posicion

3D de todas las marca detectadas, en el instante de tiempo (cuadro de imagen) k.


mi =

Xi

Yi

Zi

(1.8)

La matriz de covarianza P de los estados estimados x = (xv,mi)T con media x = (xv, mi)

T tendra la

siguiente estructura:

P =

Pxvxv Pm1xv Pm2xv · · · Pmixv

Pxvm1Pm1m1

Pm2m1· · · Pmim1

Pxvm2Pm1m2

Pm2m2· · · Pmim2

......

.... . .

...

Pxvmi Pm1mi Pm2mi · · · Pmimi

(1.9)

1.2.3. Modelo del proceso

El modelo del proceso esta regido por la suposicion de como la camara se esta moviendo a traves del

entorno. La evolucion dinamica de los estados en el tiempo esta dada por una funcion de transicion de

estados no lineal.

x(k + 1) = f(x(k), w(k)) = f

(xv(k), w(k)

mi(k)

)=

(fv(xv(k), w(k))

fm(mi(k))

)(1.10)

La funcion del proceso puede ser vista como un vector de dos componentes f = (fv, fm)T . Donde

fv actua sobre los estados correspondientes a la posicion y caracterısticas dinamicas de la camara xv.

La funcion fm es una funcion identidad que actua sobre la posicion de las marcas visuales detectadas

en la imagen. Por tanto, la parte del mapa no cambia entre cuadros porque las caracterısticas visuales

son asumidas estaticas. El vector w(k) representa un ruido gaussiano no correlacionado de media cero

con covarianza Q. A continuacion se presentan dos modelos utlizados para modelar el movimiento de

la camara.

1.2.3.1. Modelo de posicion constante

El modelo de posicion constante, supone que la camara permanece estacionaria entre las actuali-

zaciones del filtro y es afectado por ruido blanco gaussiano. Los estados estimados en este modelo

corresponden a la rotacion y traslacion de la camara. La funcion del proceso asociada al modelo de

posicion constante es escrito de la siguiente manera:

xv =

[rnk+1

qnk+1

]=

[rnk + (vnk + V n)∆t

qnk q((ωbk + Ωb)∆t)

](1.11)


Donde denota la multiplicacion de cuaterniones, rn es un vector con la posicion en 3D de la camara

en el marco de referencia de navegacion n (traslacion) y qn es un cuaternion el cual describe la orien-

tacion de la camara (rotacion). El vector de velocidad lineal vn requerido para la estimacion de la

posicion es afectado por un ruido en la velocidad lineal V n. Del mismo modo, un vector de ruido en

la velocidad angular Ωb afecta la estimacion de la orientacion. Los marcos de referencia de navegacion

se ilustran en la seccion 3.1.

Debido a que el modelo de posicion constante no predice el movimiento de la camara, la covarianza

en el ruido del proceso Qv va a ser mayor para permitir posibles cambios en la posicion de la camara.

Por consiguiente, las predicciones seran menos precisas y la busqueda de la marcas en la imagenes

abracara regiones mas grandes, aumentando el tiempo de computacion. Sin embargo, este modelo es

beneficioso cuando el seguimiento es perdido [27].

Incremento en la covarianzapara cubrir el posible

movimiento de la cámara

La posición de la cámaraes predecida para

permanecer constante

Xv(k+1)

Xv(k)

-

Figura 1.4: Modelo de posicion constante.

La figura 1.4 ilustra el comportamiento del modelo de posicion constante. Donde Xv(k) corresponde

al estado de la camara anterior y X−v (k + 1) es la prediccion realizada por el modelo del proceso

teniendo en cuenta el estado de la camara anterior.

1.2.3.2. Modelo de velocidad constante

El modelo de velocidad constante es el modelo que extensamente ha sido usado en sistemas SLAM

visuales [27]. Este supone que el movimiento de la camara se realiza a velocidad angular y lineal cons-

tante. Un ruido blanco gaussiano afecta la velocidad lineal y angular de la camara para hacer frente a

los pequenos cambios en el modelo de velocidad constante. La evolucion de los estados de la camara

usando el modelo de velocidad constante es la siguiente:

xv =

rnk+1

qnk+1

vnk+1

ωbk+1

=

rnk + (vnk + V n)∆t

qnk q((ωbk + Ωb)∆t)

vnk + V n

ωbk + Ωb

(1.12)


Donde denota la multiplicacion de cuaterniones. Los estados del modelos estan compuestos por un

vector de posicion rn, un cuaternion de orientacion qn, vector de velocidad lineal vn y un vector de

velocidad angular ωb, donde n el es marco de referencia de navegacion y b el marco de referencia del

cuerpo. Los marcos de referencia se ilustran en la seccion 3.1.

El modelo de velocidad constante no significa que la camara se mueva con una velocidad constante

en todo el tiempo. El modelo implica que la velocidad de la camara es un promedio esperado para

mantenerse constante entre intervalos de tiempo, mientras que las aceleraciones no observadas son

modeladas como un ruido Gaussiano [28]. Por su parte el ruido es expresado como:

w(k) =

[V n

Ωb

]=

[an∆t

αb∆t

](1.13)

El ruido puede ser interpretado como las aceleraciones lineales an y angulares αb, las cuales producen

variaciones en la velocidad lineal y angular.

La posición de la cámara espredecida de las velocidades estimadas

La covarianza crece en pequeñaproporción para cubrir las aceleraciones

en el modelo de movimiento

Xv(k+1)-v

w

Xv(k)

Figura 1.5: Modelo de velocidad constante.

Los beneficios de usar un modelo de velocidad constante es que proporciona predicciones precisas

cuando la camara se esta moviendo sobre una trayectoria suave. Esto implica que la ubicacion de carac-

terısticas en la imagen requiera de menos busqueda para encontrar observaciones. Desafortunadamente,

el inconveniente de este modelo es que la camara continua siguiendo su trayectoria estimada en perıodos

en que el seguimiento se ha perdido, por ejemplo, cuando la camara esta ocluida. Esto significa que es

facil que el sistema sea inconsistente durante los perıodos de seguimiento inestable [27].


1.2.4. Modelo de la medida

La deteccion de nuevas caracterısticas en la imagen es quizas uno de los mas grandes problemas

en sistemas SLAM basados en vision, debido a que dependiendo del metodo utilzado este proceso

puede demandar mayores requerimientos computacionales y por ende el aumento en el tiempo de

procesamiento. En algunos trabajos la inicializacion de nuevas caracterısticas en el mapa es retrasada

(delayed) hasta que haya suficiente paralaje (cuando un objeto parece que cambia su posicion porque

la persona u objeto que lo observa ha cambiado su posicion) para estimar la profundidad de las car-

acterısticas detectadas [29]. Pero tambien existen tecnicas que no lo son, como por ejemplo el inverso

de la profundidad de las caracterısticas (metodo undelayed) [30]. Esta tecnica es capaz de lidiar de

manera uniforme, con caracterısticas cercanas y lejanas desde el primer momento en que se detectan.

Sin embargo, tanto la inicializacion con retraso o sin retraso tienen ciertos beneficios y otras desven-

tajas algunas de ellas se presentan en [31].

Las observaciones de las caracterısticas en el mapa son modeladas por un modelo de medida que

representa la proyeccion de las caracterısticas al plano de imagen de la camara:

zi(k) = hi(xv(k),mi, v) (1.14)

Donde zi es la observacion de la caracterıstica mi, hi es una funcion no lineal de medida, y v es un

ruido blanco gausiano variable representando una medida de ruido desconocida con covarianza R.

De manera general, el modelo de la camara de agujero de alfiler4 es usado para realizar una medicion

sobre la ubicacion de la caracterıstica en la imagen (ver apendice B). Este modelo es el que permite

establecer la funcion de medida hi, en donde la posicion de las caracterısticas detectadas en la imagen

es proyectada sobre el plano de la imagen usando la ecuacion B.8:

hi = Π(Cbn(mni − rn)) (1.15)

Donde mni es la posicion de las caracterısticas detectadas y la posicion de la camara rn dadas en el

marco de referencia de navegacion n. Cbn es la matriz de rotacion que transforma las medidas al marco

de referencia de la camara b. Los marcos de referencia y matrices de rotacion son presentadas en las

secciones 3.1 y 3.2 respectivamente. La funcion de medida es presentada de manera mas completa en

[27], en donde se tienen en cuenta los efectos de la distorsion en la imagen y los parametros intrınsecos

de la camara los cuales son obtenidos por calibracion.

4Pinhole camera model


1.2.5. Modelo de observacion

El inverso de la profundidad es un modelo de observacion que permite una inicializacion sin retraso

para nuevas caracterısticas. Esta formulacion es mucho mas facil para manejar y mantener el tamano

total del estado mas pequeno [25] [30]. La clave es que el inverso de la profundidad de la caracterıstica

relativa a la posicion de la camara de donde esta fue vista por primera vez, tiene casi una distribucion

gaussiana. Por lo tanto, la posicion de cada marca i puede ser determinada con ayuda de los siguientes

parametros organizados en un vector de 6 dimensiones:

yi = (xi, yi, zi, θi, φi, ρi)T

(1.16)

Donde xi, yi y zi corresponde a la posicion de la camara descrito en coordenadas de navegacion

n, donde la caracterıstica fue observada por primera vez. Los angulos θi, φi definen el azimut y la

elevacion del vector unitario de direccion u(θi, φi). Finalmente, ρi = 1/di es el inverso de la distancia

di entre la posicion de la camara y la caracterıstica (ver figura 1.6).

Xn

Yn

Zn

Xb Z

b

Yb

rn

(r , q )n n

xiyi

zi

rn-

xiyi

zi

a

hb

pi=

1 di

m( ),

ángulo deparalaje

xiyi

zi

+pi

1m( ),

Figura 1.6: Parametrizacion de la caracterıstica y ecuacion de medida.

La posicion en donde se encuentra ubicada la caracterıstica detectada puede ser obtenida utilizando

la siguiente ecuacion:

mi =

Xi

Yi

Zi

=

xi

yi

zi

+1

ρiu(θi, φi) (1.17)


u(θi, φi) = (senθicosφi − senφi cosθicosφi)T

(1.18)

La observacion de un punto mi desde la ubicacion de la camara, es expresada como un vector

hb = (hx, hy, hz) dado en el marco de referencia de la camara b:

hb = Cbc =

xi

yi

zi

+1

ρiu(θi, φi)− rn

(1.19)

La camara no observa directamente la distancia hb, pero si su proyeccion usando el modelo de la

camara de agujero de alfiler (ver apendice B).

h =

(u

v

)=

[u0 − fhx

sxhz

v0 − fhy

syhz

](1.20)

Donde, u0, v0 son el centro del plano de imagen de la camara en pixeles, f es la longitud focal y sx,

sy el tamano del pixel.

1.2.5.1. Inicializacion de caracterısticas

La ubicacion inicial para la caracterıstica observada es definida con ayuda del estado de la camara

(posicion rn y orientacion qn), la proyeccion del punto al plano de la imagen h y un valor de profundidad

inicial ρ0 tomado heurısticamente:

yi = (rn, qn, h, ρ0) = (xi, yi, zi, θi, φi, ρi)T

(1.21)

La posicion inicial de la camara de donde fue dectectada por primera vez la marca es tomada de rn.

La distancia entre la camara y la marca detectada es establecida a continuacion:

hn = Cnb hb

u

v

1

(1.22)

Los parametros u y v son las coordenadas en el plano de la imagen normalizadas. A pesar que hn

no es un vector direccional no unitario, los angulos son calulados de la siguiente manera:

(θi

φi

)=

arctan(−hny ,

√hn2

x + hn2

z

)arctan (hnx , h

nz )

(1.23)


La covarianza para xi, yi, zi θi y φi es obtenida de la covarianza en el error de la medidad en la imagen

R y la covarianza de los estados estimados P . El valor inicial de ρ0 es determinado heurısticamente para

cubrir una region del 95 % de aceptacion en el espacio de trabajo del infinito a una distancia predefinida.

Para mayor informacion sobre como es realizada la inicializacion de la caracterıstica detectada mi el

lector puede revisar las referencias [25] y [30].

Capıtulo 2

MODULO INERCIAL

El iNEMO es un modulo inercial que combina acelerometros, giroscopos y magnetometros con sen-

sores de presion y temperatura que proporcionan un sensado de moviemto lineal, angular y magnetico

en los 3 ejes [32]. Es el sensor inercial seleccionado para la realizar la fusion con el sistema SLAM con

camara monocular debido a su bajo costo y disponibilidad.

Figura 2.1: Modulo inercial iNEMO STEVAL-MKI062V2.

2.1. Especificaciones del modulo inercial

Las principales caracterısticas del modulo inercial iNEMO son las siguientes:

15

16 CAPITULO 2. MODULO INERCIAL

Dos fuentes de alimentacion: por conector USB y por conector externo.

Conector extendido para conectividad wireless.

Ranura para tarjeta MicroSD.

Conexion USB 2.0 a toda velocidad.

De manera adicional, es posible configurar el dispositivo para obtener una frecuencia de adquision

de datos de 1, 10, 25, 30 y 50 Hz.

2.2. Microcontrolador

Para procesar las medidas de los diferentes sensores, el iNEMO dispone de un microcontrolador

STM32F103RET de STMicroelectronics. Es un dispositivo de baja potencia y alto desempeno de 32

bits con nucleo ARMrCortexTM −M3. Opera a 72 MHz y dispone de una memoria flash de 512

Kbytes y SRAM de 64Kbytes, con un voltaje de operacion de 2 - 3.6 Volts.

2.3. Sensores

Los sensores requeridos para la realizacion de la aplicacion son el acelerometro, giroscopo y mag-

netometro todos con capacidad de sensado en sus 3 ejes. El iNEMO dispone un giroscopo de 2 ejes

para los movimiento de alabeo y cabeceo (roll and pitch) y un giroscopo adicional que solo mide 1

eje correspondiente al movimiento de guinada (yaw). Por su parte el acelerometro y el magnetometro

estan ubicados en el mismo dispositivo, ambos con capacidad de medida en sus 3 ejes.

2.3.1. Acelerometro

Este dispositivo tiene la capacidad de medir la aceleracion lineal. Perciben la variacion de la compo-

nente traslacional de aceleracion y la aceleracion gravitacional. El acelerometro esta disponible en el

modulo geomagnetico LSM303DLH. Las caracterısticas del acelerometro se presentan en la siguiente

tabla:

Dispositivo Fabricante Rango [g] Ejes Salida Comunicacion

LSM303DLH ST Microelectronics 2 / 4 / 8 XYZ Digital I2C

TABLA 2.1: Caracterısticas del acelerometro.

Una ventaja de los acelerometros digitales con respecto a los analogicos es la inmunidad al ruido,

aunque no dejan de ser sensores muy ruidosos. Los acelerometros generalmente requieren de una com-

pensacion para que la medida pueda ser correcta. Esto se debe a que la senal de salida presenta un

ligero offset cuando se encuentra en reposo.

2.3. SENSORES 17

El acelerometro de 3 ejes mide la aceleracion gravitacional y lineal de un vehıculo. Si la aceleracion

lineal es pequena comparada con la aceleracion gravitacional, entonces la lectura del acelerometro indica

las componentes del vector gravedad. Las senales medidas con el acelerometro pueden ser descritas

como la suma de la aceleracion lineal a, la gravedad g, un offset fbias y el ruido blanco presente en

la medida vf . Todos vectores de 3 dimensiones.

fm = a+ g + fbias + vf (2.1)

Esta aceleracion se cuantifica en las unidades del sistema internacional en metros sobre segundo al

cuadrado [m/s2], aunque algunos fabricantes prefieren dar sus especificaciones en terminos de la fuerza

G, ejemplo: miligravedades [mG] donde 1G = 9.8 m/s2.

2.3.2. Giroscopo

Como ya se menciono, el sensado de la velocidad angular es realizado a traves de dos giroscopos. El

primero el LPR430AL que proporciona las medidas de los ejes de alabeo y cabeceo y el LY 330ALH

que sensa la guinada. Sus caracterısticas se presentan a continuacion:

Dispositivo Fabricante Rango ±[dps] Ejes Salida Sensibilidad [mV/o/s]

LPR430AL ST Microelectronics 300/1200 XY Analogico 3.33/0.83

LY330ALH ST Microelectronics 300 Z Analogico 3.33

TABLA 2.2: Caracterısticas de los giroscopos.

El giroscopo mide las tres componentes de velocidad angular. Desde que los giroscopos sufren de

derivas (drift), el modelo de medida del giroscopo esta descrito por la suma de la velocidad angular

w, el offset ωbias que es lentamente variante en el tiempo y el ruido blanco gaussiano presente en la

medicion vw.

wm = w + ωbias + vw (2.2)

Las unidades de la velocidad angular medida son de grados sobre segundo [o/s]. Debido a la deriva

presente en la medicion del giroscopo, no es aconsejable realizar una simple integracion para obtener

la orientacion pues esta presenta errores a medida que transcurre el tiempo.

2.3.3. Magnetometro

El magnetometro por su parte tiene la capacidad de medir el campo magnetico de la tierra. Los

magnetometros de 3 ejes miden la componente del campo magnetico en una direccion particular. las

caracterısticas son las siguientes:


Dispositivo Fabricante Rango [Gauss] Ejes Salida Comunicacion

LSM303DLH ST Microelectronics 1.3-8.1 XYZ Digital I2C

TABLA 2.3: Caracterısticas del magnetometro.

La distorsion en la medida del magnetometro principalmente se debe a la cercanıa de materiales fe-

rrosos al dispositivo y campos magneticos externos (iman). Estos metales como el hierro, niquel, acero

y cobalto se encuentran en los vehıculos, aeronaves y plataformas sobre las cuales los magnetometros

son montados, por consiguiente, distorsionan o curvan el campo magnetico de la tierra provocando

errores en las medidas. Los inconvenientes causados por estos materiales pueden ser removidos si el

dispositivo es instalado de manera segura [33].

Las senales medidas por el magnetometro estan descritas por la suma del campo magnetico terrestre

en cada uno de sus ejes h, una perturbacion d y el ruido blanco presente en la medicion vm.

Hm = h+ d+ vm (2.3)

Donde las unidades de la medida del campo magnetico estan dadas en [Gauss] y miligauss [mGauss]

y estan dadas en el marco de referencia del cuerpo b.

2.4. Caracterizacion de los sensores inerciales

Para estimar el ruido que afectan las mediciones del giroscopo, el acelerometro y magnetometro, se

procedio a realizar medidas cuando el modulo inercial iNEMO se encuentra en reposo durante un

periodo de tiempo establecido. Este experimento permite determinar las caracterısticas presentes en la

medida de los sensores y comparar los resultados con los modelos de medida propuestos anteriormente.

A las medidas realizadas en cada uno de sus ejes se calculo su valor medio y su desviacion estandar.

Los resultados se presentan en las tablas 2.4, 2.5 y 2.6.

TABLA 2.4: Momentos estadısticos de la medida del giroscopo

Parametro eje x eje y eje z Unidades

m 0.0930 -3.9590 4.8400 o/seg

σ 0.3006 0.6319 0.4386 o/seg

Cabe aclarar que para tomar las medidas del acelerometro en el eje z cuando el sistema se encuentra

en reposo, fue necesario ubicar el iNEMO en posicion vertical para que la medida en el eje z no fuera

afectada por la gravedad. El bias tanto en las medidas de aceleracion y velocidad angular como se

observan en las tablas anteriores pueden tomar valores positivos y negativos. Esta toma de medidas

permite establecer de cierta manera los errores cometidos en la medicion que pueden afectar el resultado

final, por ejemplo, el calculo de la orientacion del vehıculo.

2.4. CARACTERIZACION DE LOS SENSORES INERCIALES 19

0 2 4 6 8 10 12 14 16 18 20−6

−4

−2

0

2

4

6

Medidas del Giróscopo en Reposo

Tiempo [seg]

Velo

cid

ad

An

gu

lar

[º/s

eg

]

Wx

Wy

Wz

Figura 2.2: Medidas del giroscopo en reposo.

0 2 4 6 8 10 12 14 16 18 20−40

−30

−20

−10

0

10

20

30

40

Medidas del Acelerómetro en Reposo

Tiempo [seg]

Fu

erz

a G

[m

G]

Ax

Ay

Az

Figura 2.3: Medidas del acelerometro en reposo.


TABLA 2.5: Momentos estadısticos de la medida del acelerometroParametro eje x eje y eje z Unidades

m 15.3690 -22.8380 16.9750 mG

σ 2.6448 2.2104 3.6952 mG

0 2 4 6 8 10 12 14 16 18 20−700

−600

−500

−400

−300

−200

−100

0

Medidas del Magnetómetro en Reposo

Tiempo [seg]

Cam

po

Mag

néti

co

[m

Gau

ss]

Hx

Hy

Hz

Figura 2.4: Medidas del magnetometro en reposo.

TABLA 2.6: Momentos estadısticos de la medida del magnetometro

Parametro eje x eje y eje z Unidades

m -37.1920 -100.5750 -625.538 mGauss

σ 3.3718 3.5446 4.0485 mGauss

2.5. Marco de referencia del modulo inercial

Cada sensor inercial presenta unos ejes coordenados sobre los cuales son llevadas a cabo las medi-

das. El modulo inercial iNEMO se ha encargado de ubicar cada sensor de manera que sus marcos

de referencia coincidan y por tanto es posible establecer un unico marco de referencia para todo el

dispositivo, tal como se muestra en la figura 2.5.

2.5. MARCO DE REFERENCIA DEL MODULO INERCIAL 21

Figura 2.5: Sistema de referencia de iNEMO.

Esta configuracion evita que las medidas de los sensores tengan que ser transformadas de un sistema

de referencia a otro, obteniendo una mayor comodidad en la lectura de datos e incurriendo en menor

error en la medicion.

Z

XY

X Y

Z

Figura 2.6: Sentido de giro en el que las medidas del giroscopo son positivas.


Capıtulo 3

SISTEMA DE REFERENCIA DE

ACTITUD Y RUMBO

Un sistema de referencia de actitud y rumbo (AHRS) esta dedicado a la estimacion de la orientacion

y rumbo de un vehıculo (plataforma movil) y generalmente puede ser elaborado usando sensores iner-

ciales tales como acelerometros, giroscopos y magnetometros tridimensionales. El diagrama de bloques

de un AHRS solo requiere de un IMU y de un procesamiento conocido como computacion de la

actitud, que utiliza las medidas realizadas para solucionar las ecuaciones que describen la orientacion.

Sensores inerciales

Electrónica delos instrumentos

Computaciónde la actitud

Unidad de medida inercial (IMU)

Sistema de referencia de actitud y rumbo (AHRS)

Figura 3.1: Sistema de referencia de actitud y rumbo (AHRS).

En las siguientes seciones, la computacion de la actitud es descrita con mas detalle al igual que

conceptos adicionales necesarios para su comprension.

3.1. Marcos de referencia

En esta seccion se presentan los diferentes marcos de referencia usados para describir la orientacion

de un vehıculo. Estas definiciones son importantes por varias razones, entre ellas: las mediciones prove-

nientes de los sensores inerciales y la camara se entienden dentro de diferentes marcos de referencia,

mientras que algunas especificaciones como las trayectorias, mediciones de posicion, velocidad, GPS

se determinan sobre un marco de referencia inercial o de navegacion.

23

24 CAPITULO 3. SISTEMA DE REFERENCIA DE ACTITUD Y RUMBO (AHRS)

3.1.1. Marco inercial i

Tiene su origen en el centro de la tierra, su eje zi coincide con el eje polar de la tierra, el eje xi

apunta hacia el equinoccio vernal (Es el nodo ascendente entre el plano ecuatorial y la eclıptica [36]),

y el eje yi completa el marco ortogonal.

xe

ze

Ye

xn

nY

zn

x i

Yi

zi

Figura 3.2: Marco de referencia inercial i, de la tierra e y de navegacion n

3.1.2. Marco de la tierra e

El marco de referencia de la tierra tiene su origen en el centro de masa de la tierra. El eje xe se

encuentra a lo largo de la interseccion del plano del meridiano de Greenwich con el plano ecuatorial

de la tierra, el eje ze es paralelo al eje de rotacion de la tierra y perpendicular al plano ecuatorial, por

ultimo, el eje ye completa el marco ortogonal. El marco de referencia de la tierra e rota con respecto

al marco inercial i, a una velocidad Ω sobre el eje zi.

3.1.3. Marco de navegacion n

El marco de referencia de navegacion presenta un sistema de coordenadas fijo sobre un punto local

de la tierra. Es representado por un vector ortogonal (xn, yn, zn) donde xn apunta al Norte, yn al Este

y zn se dirige al centro de la tierra. Tambien llamado marco de referencia inercial o NED1. (ver figuras

3.2 y 3.3).

3.1.4. Marco del cuerpo b

El marco de referencia del cuerpo esta asociado al vehıculo, en este caso a la camara o al modulo

inercial. Es representado por el vector (xb, yb, zb) tal como se ilustra en la figura 3.4.

1North East Down

3.2. MATRICES DE ROTACION 25

xn

nY

zn

Norte

Este

Abajo

Figura 3.3: Marco de navegacion n

Y

Z

Xb

b

b

Figura 3.4: Marco del cuerpo b.

3.2. Matrices de Rotacion

En el espacio las matrices de rotacion representan la orientacion de un sistema de coordenadas que

gira alrededor de uno de los ejes de un sistema de referencia fijo. En la Figura 3.5, el sistema de coor-

denadas UVW gira entorno al eje Z del sistema de referencia XY Z un angulo ψ. Cada giro sobre cada

uno de los ejes del sistema de coordenadas de referencia tendra asociada una correspondiente matriz

de rotacion.

El concepto de matrices de rotacion permite hacer la transformacion de las coordenadas de un

vector de un sistema de referencia a las del otro [37]. La notacion Cxyzuvw corresponde a la matriz de

rotacion del sistema de coordenadas UVW a las coordenadas XY Z, rotado un angulo ψ sobre el eje Z.

Cxyzuvw(ψ) =

cosψ −senψ 0

senψ cosψ 0

0 0 1

(3.1)


Z

W

X UY

V

Figura 3.5: Rotacion sobre el eje Z.

Z

W

X

U

Y

V

Figura 3.6: Rotacion sobre el eje X.

Procediendo de manera similar, cuando el sistema de coordenadas UVW gira sobre el eje X un

angulo θ, se obtiene:

Cxyzuvw(θ) =

1 0 0

0 cosθ −senθ0 senθ cosθ

(3.2)

Por ultimo cuando el sistema de coordenadas UVW gira sobre el eje Y un angulo φ se obtiene:

Cxyzuvw(φ) =

cosφ 0 senφ

0 1 0

−senφ 0 cosφ

(3.3)

La transformaciones del marco de referencia del cuerpo b al de navegacion n, esta basado en las

3 anteriores rotaciones pero de manera consecutiva. Por tanto, la matriz de rotacion del marco de

referencia del cuerpo b al de navegacion n esta dada por:

3.3. REPRESENTACION DE LA ORIENTACION 27

Z

W

X

U

Y

V

Figura 3.7: Rotacion sobre el eje Y .

Cnb (θ, φ, ψ) =

cosψ −senψ 0

senψ cosψ 0

0 0 1

cosφ 0 senφ

0 1 0

−senφ 0 cosφ

1 0 0


(3.4)

3.3. Representacion de la orientacion

Un sistema robotico queda totalmente definido en el espacio tridimensional a traves de dos especifi-

caciones: La posicion y la orientacion con respecto a un sistema de referencia. Estas descripciones se

pueden hacer siguiendo diferentes metodologıas. A continuacion se presentan dos conceptos utilizados:

Los angulos de Euler y Cuaterniones.

3.3.1. Angulos de Euler

Mediante tres angulos ψ, θ, φ denominados angulos de Euler es posible describir la orientacion

de cualquier objeto con respecto a un sistema de referencia. Es la representacion mas utilizada en

aeronautica, e indica los giros realizados sobre cada uno de los ejes del sistema fijo.

Como se observa en la Figura 3.8, cuando el sistema de coordenadas de la aeronave gira un angulo ψ

entorno al eje Z del sistema de referencia, el giro corresponde al movimiento aeronautico denominado

como guinada (yaw). Cuando gira un angulo θ entorno al eje X, el movimiento correspondiente es

llamado alabeo (roll). Por ultimo, cuando gira un angulo φ entorno a eje Y , el movimiento correspon-

diente es llamado cabeceo (pitch).


Yb

Zb

Xb

Figura 3.8: Angulos de Euler: alabeo θ, cabeceo φ y guinada ψ.

Una representacion con angulos de Euler aunque es sencilla es indeseable, debido a que presenta

singularidades cuando dos de sus ejes de rotacion estan alineados. Sin embargo, existen dos representa-

ciones adecuadas que no presentan este tipo de inconvenientes: cuaternion y mapas exponenciales.

3.3.2. Cuaternion

El cuaternion es una representacion de cuatro parametros basada en que la transformacion de un

sistema coordenado a otro puede ser efectuada por una sencilla rotacion sobre un vector v definido en

el marco de referencia utilizado, tal como se ilustra en la figura 3.9. El cuaternion proporciona una

representacion de angulos que evita singularidades, adicionalmente, es una herramienta matematica

de gran versatilidad computacional. Un cuaternion se representa de la siguiente manera:

Z

X

Y

v (0,0,1) -X

r (-1,1,0)

br (1,1,0)

n

Figura 3.9: Vector antes y despues de una rotacion.


q = qs + qx· i+ qy· j + qz· k (3.5)

Consiste de una parte real escalar qs y un vector de 3 partes (qx, qy, qz). De manera vectorial:

q = [qs, qx, qy, qz]T

(3.6)

Para utilizar un cuaternion como forma de representacion de la orientacion, se asocia un angulo θ a

un vector v = (v1, v2, v3), por lo tanto, el cuaternion q se encuentra definido como:

q = Rot(θ, v) =

(cos

θ

2, v· senθ

2

)(3.7)

q = cosθ

2+ v1· sen

θ

2· i+ v2· sen

θ

2· j + v3· sen

θ

2· k

Vectorialmente:

q =

qs

qx

qy

qz

=

cos( θ2 )

v1· sen( θ2 )

v2· sen( θ2 )

v3· sen( θ2 )

(3.8)

El conjungado, norma e inverso de un cuaternion se definen como:

q = [qs,−qx,−qy,−qz]T

|q| =√q2s + q2x + q2y + q2z

q−1 =q

|q|

Para la representacion de la orientacion usando cuaterniones solo serıa necesario realizar la multipli-

cacion de los cuaternios correspondientes a cada angulo de navegacion y su vector de referencia.

q = qyaw qpitch qroll (3.9)

Donde es el operador que simboliza la multiplicacion de cuaterniones.

qyaw = Rot(ψ, [0, 0, 1])


qpitch = Rot(φ, [0, 1, 0])

qroll = Rot(θ, [1, 0, 0])

El producto de 2 cuaterniones, q = qs + qx· i+ qy· j + qz· k y p = ps + px· i+ py· j + pz· k es obtenido

usando las reglas generales del producto de dos numero complejos.

i· i = −1 i· j = k j· i = −k

Entonces:

q p = qsps − qxpx − qypy − qzpz + (qspx + qxps + qypz − qzpy)· i

(qspy − qxpz + qyps + qzpx)· j + (qspz + qxpy − qypx + qzps)· k (3.10)

La multiplicacion entre cuaterniones puede ser vista como una multiplicacion matricial entre una

matriz compuesta por los elementos del primer cuaternion por el segundo cuaternion p:

r = q p = Q(q)· p (3.11)

rs

rx

ry

rz

=

qs −qx −qy −qzqx qs −qz qy

qy qz qs −qxqz −qy qx qs

ps

px

py

pz

Usando cuaterniones es posible que a partir de un vector definido en el marco de referencia del cuerpo

rb este puede ser expresado en el marco de navegacion rn. Este procedimiento se lleva a cabo a traves

de la siguiente ecuacion:

rn′

= q rb′ q (3.12)

Donde rb′

en la ecuacion 3.12 corresponde al cuaternion con parte escalar igual a cero y la parte

vectorial igual al vector rb. El resultado rn′

tambien correspondera a un cuaternion donde su parte

escalar es igual a cero y la parte vectorial contienen las coordenadas del vector despues de su transfor-

macion. A manera de ejemplo, estableciendo que el angulo de rotacion en la figura 3.9 es igual a 90o,

se tiene que q = Rot(90o, [0, 0, 1]), por ende:


[0,−1, 1, 0]T

=

[√2

2, 0, 0,

√2

2

]T [0, 1, 1, 0]

T [√

2

2, 0, 0,

−√

2

2

]T

Estableciendo que q describa la orientacion del vehıculo en cada uno de sus ejes (alabeo, cabeceo,

guinada) la ecuacion 3.12 puede ser escrita de la siguiente manera:

rn′

= C ′rb′

Donde:

C ′ =

[0 0

0 C

]rb

′=

[0

rb

]

C =

(q2s + q2x − q2y − q2z) 2(qxqy − qsqz) 2(qsqy + qxqz)

2(qsqz + qxqy) (q2s − q2x + q2y − q2z) 2(qyqz − qsqx)

2(qxqz − qsqy) 2(qyqz + qsqx) (q2s − q2x − q2y + q2z)

(3.13)

Vectorialmente es equivalente escribir:

rn = Crb (3.14)

Donde C en la ecuacion 3.14 es equivalente a la matriz de rotacion Cnb . Los angulos de euler obtenidos

a partir del cuaternion se calculan de la siguiente manera utilizando los terminos de la matriz C.

θ = arctan2

(2(qyqz + qsqx)

(q2s − q2x − q2y + q2z)

)(3.15)

φ = arcsen(−2(qxqz − qsqy)) (3.16)

ψ = arctan2

(2(qsqz + qxqy)

(q2s + q2x − q2y − q2z)

)(3.17)

Para situaciones en las que φ se aproxima a π/2 radianes, la ecuaciones para θ y ψ se hacen in-

determinadas por que tanto el numerador como el denominador de esas expresiones se hace cero

simultaneamente. Esta dificultad puede ser superada usando otros elementos de la matriz de cosenos

directores C [35]. Para φ cercano a +π/2:

ψ − θ = arctan2

(c23 − c12c13 + c22

)(3.18)

Para φ cercano a −π/2:


ψ + θ = arctan2

(c23 + c12c13 − c22

)(3.19)

En consecuencia ψ o θ debe ser elegido arbitrariamente para satisfacer la ecuacion. Para evitar saltos

en las medidas entre calculos sucesivos se aconseja congelar en su valor actual a uno de los 2 angulos,

por ejemplo, ψ y θ se determina aplicando la ecuacion necesaria. En la siguiente iteracion se congela

θ y se calcula ψ.

3.4. Computacion de la actitud

El enfoque convencional para la determinacion de la actitud es computar la matriz de cosenos di-

rectores Cnb , relacionando el marco de referencia del cuerpo b del vehıculo al sistema de referencia

coordenado, o su cuaternion equivalente, usando un esquema de integracion numerica [25].

0 5 10 15 20 25 30 35 40 45−50

−40

−30

−20

−10

0

10

20

Integración de la medida del giróscopo

Tiempo [Seg]

Án

gu

lo m

ed

ido

[º]

(a) Integracion de la senal del giroscopo.

0 5 10 15 20 25 30 35 40 45−40

−30

−20

−10

0

10

20

Ángulo de inclinación a partir del acelerómetro

Tiempo [Seg]

Án

gu

lo m

ed

ido

[º]

(b) Orientacion a partir del acelerometro.

Figura 3.10: Inconvenientes para el calculo de la orientacion a partir de acelerometros y giroscopos.

Las medidas inerciales juegan un papel importante en la estimacion de la actitud porque proporcionan

informacion suficiente para el calculo de la orientacion del vehıculo. La velocidad angular suministrada

por el giroscopo hace pensar que es posible obtener la representacion de la orientacion integrando

las senales, pero este procedimiento presenta errores a medida que transcurre el tiempo por la deriva

(drift) presente en las medidas. Por tanto, se acumulan por accion de la integracion. Por otra parte,

a partir de las medidas de aceleracion y campo magnetico de la tierra tambien es posible obtener esta

representacion, el principal inconveniente es que es una medida muy ruidosa y de manera adicional

puede ser afectada por fuerzas externas como vibracion. Estas mediciones son acertadas cuando el

vehıculo no presenta desplazamientos, debido a que las aceleraciones lineales son detectadas por el

3.4. COMPUTACION DE LA ACTITUD 33

acelerometro influyendo en la medida (ver seccion 3.4.2). La principal causa de errores en la medicion

realizada por los magnetometros es la presencia de fuentes externas de campo magnetico y materiales

ferromagneticos.

Un analisis de la literatura refleja que los autores resolvieron el problema de la determinacion de la

orientacion usando sensores inerciales, aplicando filtrado Kalman. Por consiguiente, el filtro Kalman

ha sido ampliamente utilizado como algoritmo de fusion de sensores permitiendo corregir las diferentes

imperfecciones presentadas en las mediciones. Actualmente, nuevos enfoques han sido desarrollados

y han probado ser superiores al filtro Kalman extendido. Muchas de estos enfoques mantienen la

estructura basica del EKF , pero emplean varias modifiaciones en orden de proporcionar una mejor

convergencia o mejorar otras caracterısticas de desempeno [38]. Una breve revison del filtro Kalman

se presenta en el apendice A.

3.4.1. Actitud en terminos de cuaterniones

Si q es la actitud o el cuaternion de orientacion, entonces la tasa de cambio del cuaternion de un

cuerpo con velocidad angular esta dada por:

q =1

2Ωk· qk (3.20)

Donde Ωk representa una matrix que contiene las velocidades angulares de cada uno de los ejes en

cada instante de tiempo k:

Ωk =

0 −wx −wy −wzwx 0 wz −wywy −wz 0 wx

wz wy −wx 0

Donde wx, wy, wz son las velocidades angulares en el instante de tiempo k. La funcion que relaciona

el estado actual del cuaternion con el anterior y que es solucion a la ecuacion 3.20 es:

qk+1 =[exp(1

2

∫ tk+1

tk

Ωk· dt)]· qk (3.21)

Por lo tanto:

∫ tk+1

tk

Ωk· dt = Ωk·∆t =

0 −σx −σy −σzσx 0 σz −σyσy −σz 0 σx

σz σy −σx 0

(3.22)


Donde ∆t = tk+1 − tk corresponde al tiempo de muestreo. Reescribiendo la ecuacion 3.21:

qk+1 = exp(Ωk∆t

2

)· qk (3.23)

Expandiendo la funcion exponencial de la ecuacion 3.23 por series de taylor alrededor de cero, el

termino exponencial puede ser escrito como [35]:

qk+1 = qk rk (3.24)

Donde rk:

rk =

ac

σx· asσy· asσz· as

(3.25)

ac = cos(σ

2) (3.26)

as =sen(σ2 )

σ(3.27)

σ =√σ2x + σ2

y + σ2z (3.28)

La ecuacion 3.24 tambien puede ser escrita de la siguiente manera [39]:

qk+1 = M(σ)· qk (3.29)

Donde M(σ):

M(σ) = cos(σ

2

)· I4X4 +

sen(σ2 )

σ·Ωk∆t (3.30)

3.4.2. Deteccion de la inclinacion empleando acelerometros

El acelerometro sirve como inclinometro para medir los angulos de alabeo y cabeceo bajo la suposicion

de que el objeto donde el AHRS esta ubicado no se esta moviendo o moviendo a velocidad constante.

Por lo tanto la gravedad es la unica fuente de aceleracion que actua sobre los sensores [40] [41]. Es


decir, que este metodo implica que las aceleraciones observadas en el marco de referencia del cuerpo

b son exclusivamente debidas a la aceleracion gravitacional. De acuerdo a la figura 3.11 las ecuaciones

que determinan los angulos de alabeo θ y cabeceo φ son:

Ay

Ax

zA

X

YZ

X

Y

Z

bb

n

b

n

n

Figura 3.11: Deteccion del alabeo θ y cabeceo φ con el acelerometro.

θ = tan−1(AyAz

)(3.31)

φ = tan−1

Ax√A2y +A2

z

(3.32)

El sensor no debe ser afectado por aceleraciones adicionales. En caso de movimiento del sensor, es

aconsejable que esta medida tenga un menor peso, que no sea asumida como verdadera o sustraer de

las medidas las componentes de aceleracion diferentes a la gravedad. En [42] se propone una manera

de determinar el angulo de guinada ψ o rumbo, pero esta no es muy acertada debido a que carece de

referencia (ejemplo: norte magnetico de la tierra).

3.4.3. Deteccion del rumbo empleando magnetometros

Para determinar el rumbo del vehıculo a partir del magnetometro, la componente vertical del campo

magnetico es ignorada y solo se usan las componentes X y Y , tal como se muestra en la figura 3.12,

por consiguiente el rumbo puede ser calculado a traves de la ecuacion 3.33.

ψ = tan−1(Hy

Hx

)(3.33)


x

Y

z

Norte

Este

Abajo

Hx

Hy

Hz

Figura 3.12: Campo magnetico en coordenadas X Y Z.

Generalmente las brujulas estan montadas sobre una aeronave o vehıculo, el cual no siempre se

encuentra en posicion horizontal a la superficie de la tierra. Esta situacion hace que la medida sea

afectada por los angulos de inclinacion, introduciendo un error [33]. Para la solucion del inconveniente

resulta necesario conocer los angulos de alabeo θ y cabeceo φ, con el fin de transformar las medidas del

magnetometro realizadas en el marco de referencia del cuerpo b al marco de referencia de navegacion n,

procedimiento que permite garantizar que las medidas obtenidas se encuentren en un plano horizontal

a la superficie de la tierra.

Hn = Cnb (φ, θ)·Hb =

cosφ 0 −senφ

0 1 0

senφ 0 cosφ

1 0 0


Hbx

Hby

Hbz

(3.34)

La matriz de rotacion correspondiente al angulo de cabeceo φ no es la misma a la presentada en la

ecuacion 3.4, debido a que los angulos de euler medidos son positivos de acuerdo al sentido de giro en

el que las medidas del giroscopo son positivas (figura 2.6). Por consiguiente, fue necesario modificar la

matriz de rotacion del angulo de cabeceo φ de manera que se ajuste al sistema de referencia utilizado.

Hnx

Hny

Hnz

=

Hbx· cosφ− senφ· [Hb

y· senθ +Hbz · cosθ]

Hby· cosθ −Hb

z · senθHbx· senφ+ cosφ· [Hb


Finalmente, es posible concluir que a pesar de que se requieran las componentes X y Y del campo

magnetico para calcular el rumbo, es necesario disponer de un magnetometro de 3 ejes.

ψ = tan−1

(Hby· cosθ −Hb

z · senθHbx· cosφ− senφ· [Hb


)(3.35)


3.4.4. Algoritmo para la computacion de la actitud

El algoritmo implementado para la determinacion de la actitud utiliza como estimador secuencial el

filtro Kalman extendido EKF 2, principalmente por que las relaciones entre las variables a estimar no

son lineales. Para el modelo del proceso se ha establecido que a partir de las medidas del giroscopo se

obtenga una representacion de la orientacion. Asi mismo, las medidas realizadas por el acelerometro y

magnetometro rigen el modelo de la medida. La figura 3.13 ilustra el procedimiento realizado para el

calculo de la orientacion aplicando directamente el EKF [43].

Giróscopo

Acelerómetro yMagnetómetro

Compensación del error

Compensación del error

FiltroKalman

Extendido

Inclinómetroy compás

, ,

w w -

q

f , Hb b

f , Hb b

bias

biasb b

Figura 3.13: Filtro Kalman extendido para la estimacion de la orientacion.

La clave del procedimiento radica en la estimacion del bias presente en las medidas del giroscopo, a

partir de la diferencia existente entre el estado estimado por el modelo del proceso y el de la medida.

En consecuencia el error y el ruido presente en el estado estimado es reducido de manera considerable.

El bloque de compensacion del error en el giroscopo es realizado internamente en el modelo del proceso

en el EKF . La compensacion del error en las medidas del acelerometro y magnetometro consiste en

eliminar el offset en la aceleracion y reducir el efecto producido por campos magneticos externos y

materiales ferromagneticos [33]. El compas hace uso de los angulos de alabeo θ y cabeceo φ obtenidos

en el inclinometro para determinar el correspondiente angulo de guinada ψ del vehıculo. El vector de

estados referente a la descripcion de la orientacion es el siguiente:

xk =

biasx

biasy

biasz

qs

qx

qy

qz

=

x1

x2

x3

x4

x5

x6

x7

(3.36)

2Extended Kalman Filter


De acuerdo a lo mencionado anteriormente, la ecucacion 3.36 presenta 3 estados adicionales corres-

pondientes al bias en la medicion del giroscopo. La orientacion es descrita empleando cuaterniones.

El modelo del proceso es representado por la ecuacion 3.23 cuya solucion describe la orientacion del

vehıculo. A diferencia del procedimiento mostrado en la seccion 3.4.1 el termino exponencial de la

ecuacion 3.23 fue aproximado a solo los dos primeros terminos de la serie de taylor alrededor de cero

obteniendo [44] [45]:

qk+1 =(I4x4 +

1

2Ωk∆t

)· qk (3.37)

Esta aproximacion fue realizada por comodidad para facilitar el calculo de la matriz jacobiana A[i,j].

Las senales de control presentadas en el modelo del proceso hacen referencia a las velocidades angulares

medidas, principalmente porque son necesarias para la prediccion del cuaternion. Teniendo en cuenta

la compensacion del error en las medidas de velocidad angular, la funcion no lineal que relaciona el

estado actual con el anterior esta dada por:

x1

x2

x3

x4

x5

x6

x7

k

=

1 0 0 0 0 0 0

0 1 0 0 0 0 0

0 0 1 0 0 0 0

0 0 0 1 − 12k(wx − x1)∆t − 1

2k(wy − x2)∆t − 1

2k(wz − x3)∆t

0 0 0 12k(wx − x1)∆t 1 1

2k(wz − x3)∆t − 1

2k(wy − x2)∆t

0 0 0 12k(wy − x2)∆t − 1

2k(wz − x3)∆t 1 1

2k(wx − x1)∆t

0 0 0 12k(wz − x3)∆t 1

2k(wy − x2)∆t − 1

2k(wx − x1)∆t 1

x1

x2

x3

x4

x5

x6

x7

k−1

Observese que se substrae el bias estimado de la respectiva medida de velocidad angular para obtener

el valor real de la medida del giroscopo. Las unidades para los estados x1, x2 y x3 es [o/seg] para los

demas estados es [rad]. k = π/180 es el factor de conversion de grados a radianes para los estados

correspondientes al cuaternion. Despues de la multiplicacion matricial se obtiene:

x1

x2

x3

x4

x5

x6

x7

k

= f(xk−1, uk−1, 0) =

x1

x2

x3

x4 − 12k(wx − x1)∆t·x5 − 1

2k(wy − x2)∆t·x6 − 1

2k(wz − x3)∆t·x7

12k(wx − x1)∆t·x4 + x5 + 1

2k(wz − x3)∆t·x6 − 1

2k(wy − x2)∆t·x7

12k(wy − x2)∆t·x4 − 1

2k(wz − x3)∆t·x5 + x6 + 1

2k(wx − x1)∆t·x7

12k(wz − x3)∆t·x4 + 1

2k(wy − x2)∆t·x5 − 1

2k(wx − x1)∆t·x6 + x7

k−1

La matriz jacobiana A[i,j] de las derivadas parciales de f con respecto a cada estado x es:

A[i,j] =∂f[i]

∂x[j](xk−1, uk−1, 0) =


1 0 0 0 0 0 0

0 1 0 0 0 0 0

0 0 1 0 0 0 012k∆t·x5

12k∆t·x6

12k∆t·x7 1 − 1

2k(wx − x1)∆t − 1

2k(wy − x2)∆t − 1

2k(wz − x3)∆t

− 12k∆t·x4

12k∆t·x7 − 1

2k∆t·x6

12k(wx − x1)∆t 1 1

2k(wz − x3)∆t − 1

2k(wy − x2)∆t

− 12k∆t·x7 − 1

2k∆t·x4

12k∆t·x5

12k(wy − x2)∆t − 1

2k(wz − x3)∆t 1 1

2k(wx − x1)∆t

12k∆t·x6 − 1

2k∆t·x5 − 1

2k∆t·x4

12k(wz − x3)∆t 1

2k(wy − x2)∆t − 1

2k(wx − x1)∆t 1

Los terminos de la matriz A[i,j] para los estados correspondientes al bias del giroscopo no son cero,

indicando que se va a tener un comportamiento dinamico, es decir, variante con el tiempo.

La matriz Q que simboliza el ruido en el proceso depende del ruido presente en las mediciones

realizadas por el giroscopo. Esta afirmacion esta basada en que el proceso esta regido por las mediciones

de velocidad angular. La tabla 2.4 presenta los momentos estadısticos del giroscopo. Por tanto, es

posible concluir que la media calculada en cada eje corresponde al bias, debido a que el sensor no debe

indicar ninguna velocidad angular cuando este se encuentra en reposo. En consecuencia, la desviacion

estandar en cada eje representa el ruido presente en los estados x1, x2 y x3. Asi mismo, se asume que

la desviacion estandar tambien representa el ruido obtenido en cada medida realizada en el giroscopo

para cada uno de sus ejes. Con base en la tabla 2.4 se obtiene los siguiente:

σ2wx

= 0,0904 [o/seg]2

σ2wy

= 0,3993 [o/seg]2

σ2wz

= 0,1924 [o/seg]2

Debido a que el calculo de los demas estados hace uso de todas las medidas de velocidad angular se

establece que:

1

σ2gyro

=1

σ2wx

+1

σ2wy

+1

σ2wz

= 18,7638 [seg/o]2 (3.38)

Entonces Q esta definida como:

Q =

σ2wx

0 0 0 0 0 0

0 σ2wy

0 0 0 0 0

0 0 σ2wz

0 0 0 0

0 0 0 σ2gyro 0 0 0

0 0 0 0 σ2gyro 0 0

0 0 0 0 0 σ2gyro 0

0 0 0 0 0 0 σ2gyro

(3.39)


Como en el proceso para el calculo del cuaternion las unidades usadas son los radianes, entonces es

necesario realizar una conversion a [rad/seg]2 para el termino σ2gyro. La matriz Q es:

Q =

0,0904 0 0 0 0 0 0

0 0,3993 0 0 0 0 0

0 0 0,1924 0 0 0 0

0 0 0 1,6236e−5 0 0 0

0 0 0 0 1,6236e−5 0 0

0 0 0 0 0 1,6236e−5 0

0 0 0 0 0 0 1,6236e−5

La matriz W corresponde a la matriz identidad W[i,j] = I7X7.

El calculo de la orientacion con acelerometros y magnetometro no presenta deriva (drift) en sus

medidas, por consiguiente la innovacion en el filtro Kalman permite estimar el bias en la medida del

giroscopo gracias a que provienen de diferentes fuentes de informacion. El modelo de la medida es el

siguiente:

h(xk, 0) =

θ

φ

ψ

=

arctan2(2x6x7 + 2x4x5, (x

24 − x25 − x26 + x27))

arcsen(2x4x6 − 2x5x7)

arctan2(2x5x6 + 2x4x7, (x24 + x25 − x26 − x27))

(3.40)

La funcion h(xk) es la encargada de calcular la orientacion de la camara a partir de los estados

estimados, para su posterior comparacion con los datos medidos. Los angulos φ, θ, ψ (alabeo, cabeceo

y guinada) correspondientes a las medidas realizadas, son calculados con las ecuaciones 3.31, 3.32 y

3.35 con la ayuda del acelerometro y el magnetometro.

θ = tan−1(AyAz

)

φ = tan−1

Ax√A2y +A2

z

ψ = tan−1

(Hby· cosθ −Hb

z · senθHbx· cosφ− senφ· [Hb


)

La matriz jacobiana H[i,j] de las derivadas parciales de h(xk, 0) con respecto a cada estado x es:

H[i,j] =∂h[i]

∂x[j](xk, 0)


H[i,j] =

0 0 0 2x5·c33−2x4·c32

c233+c232

2x4·c33+2x5·c32c233+c

232

2x7·c33+2x6·c32c233+c

232

2x6·c33−2x7·c32c233+c

232

0 0 0 2x6√1−c231

−2x7√1−c231

2x4√1−c231

−2x5√1−c231

0 0 0 2x7·c11−2x4·c21c211+c

221

2x5·c11−2x6·c21c211+c

221

2x5·c11+2x6·c21c211+c

221

2x4·c11+2x7·c21c211+c

221

(3.41)

Donde las variables cij son terminos de la matriz de cosenos directores de la ecuacion 3.13.

c33 = x24 − x25 − x26 + x27

c32 = 2x6x7 + 2x4x5

c31 = 2x4x6 − 2x5x7

c11 = x24 + x25 − x26 − x27c21 = 2x5x6 + 2x4x7

Los datos obtenidos en la tabla 2.5 no pueden ser usados para determinar el ruido en la medicion,

pero si proporciona la medida del offset del aceleremetro que necesariamente debe ser sustraido a la

medida de aceleracion (bloque de compensacion del error). Con el objetivo de obtener el verdadero

valor de medida e incurrir en menos error cuando se calcule el correspondiente angulo usando el in-

clinometro y el compas.

Para calcular el ruido en la medicion se realizo basicamente el mismo procedimiento para el calculo

de los momentos estadısticos de los sensores inerciales, pero en este caso se midieron los angulos cuando

el sensor se encontraba en reposo utilizando las ecuaciones de medicion 3.31, 3.32 y 3.35. Los resultados

se ilustran en la tabla 3.1.

TABLA 3.1: Momentos estadısticos de los angulos medidos

Parametro θ φ ψ Unidades

m 0.8195 -1.4735 -120.1920 o

σ 0.0640 0.0770 1.3385 o

Despues del cambio de unidades de [o] a [rad], la matriz R puede ser definida como:

R =

σ2θ 0 0

0 σ2φ 0

0 0 σ2ψ

=

1,2461e−6 0 0

0 1,8057e−6 0

0 0 5,4578e−4

(3.42)

La matriz V corresponde a la matriz identidad V[i,j] = I3X3.


3.5. Implementacion del algoritmo para la computacion de la

actitud

El algoritmo presentado en la seccion 3.4.4 fue implementado en MATLAB usando las medidas

inerciales proporcionadas por el iNEMO. En primer lugar para comprobar que las estimaciones del

bias que afecta en el giroscopo son acertadas se realizo una prueba en reposo. El resultado esperado es

que la estimacion de dichos estados sean similares a los obtenidos experimentalmente. Los resultados

son presentados en la figura 3.14.

0 2 4 6 8 10 12 14 16 18 20−6

−5

−4

−3

−2

−1

0

1

Bias Estimado

Tiempo [seg]

Velo

cid

ad

An

gu

lar

[º/s

eg

]

biasWx

biasWy

biasWz

Figura 3.14: Estimacion del bias presente en las medidas del giroscopo.

La comparacion de los resultados obtenidos en la grafica 3.14 y tabla 2.4 refleja la convergencia del

filtro y se comprueba su correcto funcionamiento bajo las mismas condiciones. La estimacion obtenida

para el bias en el eje z biasWz es negativa por que el sentido de giro establecido para este eje como

positivo es contrario al presentado en la figura 2.6.

Para demostrar la consistencia del filtro Kalman Bar-Shalom [46] propuso los siguientes criterios:

El estado estimado xk/k no presenta bias, ejemplo: el error del estado estimado es de media cero.

La covarianza del estado estimado, Pk/k, concuerda con la covarianza del error del estado.

La innovacion zk −H · xk/k−1, es un vector de ruido blanco aleatorio, ejemplo: de media cero y

no correlacionado.

3.5. IMPLEMENTACION DEL ALGORITMO PARA LA COMPUTACION DE LAACTITUD 43

0 2 4 6 8 10 12 14 16 18 20−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

Innovación para el ángulo de Alabeo

Tiempo [seg]

Err

or

[º]

(a) Innovacion para el angulo de alabeo θ.

0 2 4 6 8 10 12 14 16 18 20−1.5

−1

−0.5

0

0.5

1

Innovación para el ángulo de Cabeceo

Tiempo [seg]

Err

or

[º]

(b) Innovacion para el angulo del cabeceo φ.

0 2 4 6 8 10 12 14 16 18 20−120

−100

−80

−60

−40

−20

0

20

Innovación para el ángulo de Guiñada

Tiempo [seg]

Err

or

[º]

(c) Innovacion para el angulo de guinada ψ.

0 2 4 6 8 10 12 14 16 18 200

0.2

0.4

0.6

0.8

1

1.2x 10

−3 Traza de la Matriz de Covarianza del Error

Tiempo [seg]

Co

vari

an

za d

el E

rro

r [r

ad

2]

(d) Covarianza del error.

Figura 3.15: Consistencia del filtro Kalman extendido para la orientacion.

La innovacion para cada uno de los angulos de Euler se asemeja a un ruido blanco de media cero. Por

otra parte, la covarianza del error Pk/k presenta un comportamiento decreciente comprobando la con-

vergencia del filtro. La covarianza del error Pk/k corresponde a la suma de los errores de cada termino

del cuaternion. Para un experimento en el cual se hicieron los tres movimientos de manera consecutiva

en cada uno de sus ejes, las mediciones realizadas con el AHRS elaborado fueron comparadas con el

sistema de descripcion de la orientacion que dispone el modulo inercial iNEMO.

Los resultados reflejan un mismo comportamiento en los dos sistemas pero se observa un offset

entre las medidas atribuido a las condiciones establecidas como referencia. Para el caso de la figura

3.16(c) es mas facil detectar que los dos sistemas tienen su propia referencia y no es la misma. El error

medio cuadratico calculado asumiendo como verdadera la medida proporcionada por el iNEMO y


0 5 10 15 20 25 30 35 40 45−40

−30

−20

−10

0

10

20

Ángulo de Alabeo

Tiempo [seg]

Án

gu

lo m

ed

ido

[º]

iNEMO

AHRS

(a) Resultados para el angulo de alabeo θ.

0 5 10 15 20 25 30 35 40 45−30

−20

−10

0

10

20

30

40

Ángulo de Cabeceo

Tiempo [seg]

Án

gu

lo m

ed

ido

[º]

iNEMO

AHRS

(b) Resultados para el angulo del cabeceo φ.

0 5 10 15 20 25 30 35 40 45−140

−120

−100

−80

−60

−40

−20

0

20

Ángulo de Guiñada

Tiempo [seg]

Án

gu

lo m

ed

ido

[º]

iNEMO

AHRS

(c) Resultados para el angulo de guinada ψ.

Figura 3.16: Comparacion del iNEMO con el AHRS.

TABLA 3.2: Error medio cuadraticoAngulo Error Unidades

θ 1.1670 o

φ 1.0858 o

ψ 3.7582 o

estableciendo la misma referencia para las dos medidas se muestra en la tabla 3.2.

Capıtulo 4

ALGORITMOS DE FUSION

La teorıa de la probabilidad fue utilizada durante mucho tiempo para hacer frente a casi todo tipo de

informacion imperfecta, porque era la unica teorıa existente. Como resultado, las tecnicas probabilısti-

cas tales como la red basada en modelos, filtrado Kalman y la simulacion Monte Carlo secuencial han

sido las tecnicas de fusion de datos mas comunes. Las tecnicas alternativas como el calculo de inter-

valos, la logica difusa y razonamiento probatorio se han propuesto para hacer frente a las limitaciones

observadas en los metodos probabilısticos tales como la complejidad, la incoherencia, la precision de los

modelos y la incertidumbre. Recientemente, algunos investigadores tambien han tratado de resolver la

fusion de datos dentro de un marco de optimizacion. Por ultimo, hay metodos hıbridos que combinan

la fusion de varios enfoques para desarrollar un algoritmo de meta-fusion [48].

El proposito de la fusion de datos es producir un modelo o representacion de un sistema a partir

de un conjunto independiente de fuentes de datos. La combinacion de la informacion de los sensores

y posterior estimacion del estado del entorno debe hacerse de forma coherente consistente, de manera

tal que la incertidumbre sea reducida. Por consiguiente, los algoritmos deben ser capaces de tratar con

multiples observaciones, multiples sensores y multiples objetivos.

4.1. Modelos de fusion

El filtro Kalman es el algoritmo recursivo mas conocido y mas ampliamente aplicado en fusion de

sensores, capaz de calcular de manera optima el estado desconocido de un sistema dinamico lineal de

observaciones ruidosas. El algoritmo utiliza un modelo predefinido del sistema para predecir el estado

en el proximo paso del tiempo. A esto se anade un componente para actualizar los errores en el modelo

con las observaciones reales del sistema. La prediccion y la actualizacion se combinan con la ganancia

de Kalman que se calcula para minimizar el error cuadratico medio de la estimacion del estado.

En el modelo de fusion que aplica filtrado Kalman la dinamica del objetivo esta gobernada por la

ecuacion 4.1. Por simplicidad se asume que la frecuencias de muestreo de los sensores son identicas.

xk+1 = Fkxk +Gkwk (4.1)

45

46 CAPITULO 4. ALGORITMOS DE FUSION

Donde xk es el vector de estado en el instante de tiempo k, y wk−1 es una variable aleatoria que

representa el ruido del proceso, blanco y con una distribucion de probabilidad normal.

E[wk] = 0; E[wkwTk ] = Qk

La diferencia radica en la ecuacion de medida, ya que al incluir multiples sensores esta puede ser

descrita de la siguiente manera:

zmk = Hmk xk + vmk , m = 1, 2, · · · , n (4.2)

Donde zmk es la medida del sensor m en el instante de tiempo k, y el ruido en las medidas vmk son de

media cero, blancos, con covarianza Rmk y mutuamente independientes.

E[vmk ] = 0; E[vmk vmkT ] = Rmk

E[v1kv2kT

] = E[v2kv1kT

] = 0

Establecido este modelo, la fusion ahora puede tener lugar al nivel del vector de estado o a nivel del

vector de medicion. La fusion del vector de estados y la fusion de medidas son los tipos de metodos

convencionales para la fusion de datos basados en el filtro Kalman [49], y la fusion de medidas presenta

un menor error de estimacion, pero un mayor costo computacional. Este procedimiento puede ser

extendido para las diferentes variaciones del filtro Kalman (ejemplo: filtro Kalman extendido).

4.1.1. Modelos de fusion de medidas

Predicción Correcciónxk|k-1^ xk|k

z-1

xk-1|k-1

Filtro Kalman

Fusión

zk

zkzk2 1

^ ^

Figura 4.1: El proceso de fusion de las medidas.

4.1. MODELOS DE FUSION 47

Hay esencialmente dos metodos para la fusion de las medidas. La primera simplemente es la mezcla de

las mediciones en un vector de observacion aumentado y el segundo combina las mediciones utilizando

estimaciones de mınima media cudratica [50]. El esquema general del proceso de fusion de las medidas

se ilustra en la figura 4.1.

4.1.1.1. Aumento del vector de observacion

En esta tecnica de fusion de medidas, los vectores de medida z1k y z2k de dos o mas sensores son

fusionados dentro de un nuevo vector de medida aumentado:

zk = [(z1k)T (z2k)T ]T (4.3)

En consecuencia Hk = [(H1k)T (H2

k)T ]T y vk = [(v1k)T (v2k)T ]T tambien son modificadas, permitiendo

establecer la nueva ecuacion de medida:

zk = Hkxk + vk (4.4)

Basados en la suposicion de que los dos sensores son estadısticamente independientes la matriz de

covarianza Rk para el ruido en la medida fusionado vk es definido como:

Rk =

(R1k 0

0 R2k

)

Por ultimo la estimacion del vector de estado xk/k puede ser determinado aplicando el filtro Kalman

convencional.

4.1.1.2. Estimacion de mınima media cuadratica

Este enfoque para la fusion de las medidas consiste en ponderar las medidas individuales de cada

sensor y luego seguir la pista de esas medidas fusionadas por un filtro Kalman para obtener una

estimacion del vector de estados [51]. Dado que el ruido en la medida es independiente para los dos

sensores, la ecuacion para la fusion de los vectores de medida z1k y z2k, en forma recursiva, para una

estimacion de mınima media cuadratica esta dada por:

zk = z1k +R1k(R1

k +R2k)−1(z2k − z1k) (4.5)

Donde Rmk es la matriz de covarianza del vector de medida del sensorm. Luego la matriz de covarianza

de las medidas fusionadas zk puede ser obtenida de la siguiente manera:

Rk = [(R1k)−1 + (R2

k)−1]−1 (4.6)


Estas medidas fusionadas pueden ser seguidas para obtener el vector de estados estimado usando el

filtro Kalman convencional.

Estos algoritmos son matematicamente equivalentes a el filtro Kalman con un sensor fusionado. Un

hecho interesante es que los dos metodos de fusion son funcionalmente equivalentes uno con el otro

cuando las matrices de medida son iguales H1k = H2

k y es independiente del caracterıstico ruido de

estado Qk [50][52].

4.1.2. Modelos de fusion track to track

En muchas situaciones practicas el entorno es observado por una variedad de sensores donde cada

sensor procesa sus propias mediciones y las mantiene por separado, una pregunta importante es como

decidir si dos mediciones procedentes de diferentes sensores representan el mismo objetivo. Este es

el problema de track to track association o probelma de correlacion. El problema de track to track

association y la subsecuente fusion fue discutida bajo la suposicion que los errores de estimacion

en dos archivos con observaciones correspondientes al mismo objetivo y provenientes de diferentes

sensores, son independientes. Pero en [53] se demostro que dichos errores estan correlacionados porque

el ruido del proceso del objetivo esta presente en los dos errores de estimacion cuando las estimaciones

tienen el mismo origen. El proceso de decision involucrado en la combinacion (fusion) de las mediciones

para obtener un unico objetivo que tenga menor incertidumbre que los obtenidos individualmente, es

conocido como track to track fusion problem (que tambien proporciona una compresion de datos).

4.1.2.1. Algoritmo de fusion track to track

Uno de los metodos de fusion frecuentemente usado es el llamado algoritmo de fusion track to track

que fue propuesto por Bar-Shalom et al [54] [55] y su diagrama de bloques es ilustrado en la figura

4.2. En este metodo se fusionan las estimaciones del estado x1k|k y x2k|k correspondientes al sensor 1 y

2, respectivamente, en una nueva estimacion del vector de estado. La nueva estimacion del vector de

estado xk|k se genera a partir de la siguiente ecuacion de estimacion lineal estatica:

xk|k = x1k|k + [P 1k|k − P 12

k|k] · [P 1k|k + P 2

k|k − P 12k|k − P 21

k|k]−1 · (x2k|k − x1k|k) (4.7)

Pmk|k es la matriz de covarianza para la estimacion xmk|k basada en la medida del sensor m (m = 1, 2),

y P 12k|k = (P 21

k|k)T es la matriz de covarianza cruzada entre x1k|k y x2k|k. La matriz de covarianza para

las estimaciones fusionadas se calcula utilizando:

Pk|k = P 1k|k − [P 1

k|k − P 12k|k] · [P 1

k|k + P 2k|k − P 12

k|k − P 21k|k]−1 · [P 1

k|k − P 21k|k] (4.8)



z-1

xk-1|k-1

Filtro Kalman

FUSIÓN^ ^


z-1

xk-1|k-1^ ^1 1 1

2 2 2

xk|k^

Figura 4.2: Fusion track to track.

La matriz de covarianza cruzada P 12k|k puede ser calculada de manera recursiva a partir de las matrices

de observacion y las ganancias del filtro Kalman aplicando la siguiente ecuacion:

P 12k|k = (I −K1

kH1k)Fk−1P

12k−1|k−1F

Tk−1(I −K2

kH2k)T + (I −K1

kH1k)Gk−1Q

12k−1G

Tk−1(I −K2

kH2k)T (4.9)

Sin embargo, hay ocasiones en donde las matrices H, K, F y Q no son conocidas (sensores indus-

triales). Para este caso una manera alternativa de calcular P 12k|k es propuesta en [56], donde la matriz de

covarianza cruzada es aproximada por el producto de Hadamard entre las dos matrices de covarianzas.

P 12k|k = ρ ·

√P 1k|k · P 2

k|k (4.10)

Donde ρ representa un coeficiente de correlacion efectivo. Este puede ser determinado numerica-

mente por una simulacion Monte Carlo para una configuracion especıfica; en [56] el valor de ρ = 0,4

fue encontrado como una buena aproximacion para situaciones tıpicas de seguimiento.

En la ausencia de dependencia P 12k|k = 0, o cuando la covarianza cruzada puede ser ignorada, el

algoritmo de fusion esta dado por las siguientes ecuaciones y se conoce como la combinacion convexa

basica:

xk|k = x1k|k + P 1k|k · [P 1

k|k + P 2k|k]−1 · (x2k|k − x1k|k) (4.11)

Pk|k = P 1k|k − P 1

k|k · [P 1k|k + P 2

k|k]−1 · P 1k|k (4.12)


4.1.2.2. Algoritmo de fusion modificado track to track

El modified track to track fusion (MTF ) es otro algoritmo de fusion que esta basado en la retroali-

mentacion de la estimacion del estado fusionado xk|k a la etapa de prediccion filtro Kalman en vez de

retroalimentar las estimaciones individuales x1k|k y x2k|k. La idea es que el procedimiento de prediccion

del filtro Kalman sea realizado por un unico bloque y mejorado con la inclusion de la estimacion del

estado fusionado (ver figura 4.3).

Predicción

Correcciónxk|k

z-1

FUSIÓN^

Corrección

xk|k-1^

xk|k

xk-1|k-1^

^1

2

xk|k^

Filtro Kalman

Figura 4.3: Fusion modificada track to track.

La nueva estimacion fusionada xk|k y su matriz de covarianza Pk|k estan dadas por las ecuaciones

4.7 y 4.8, respectivamente. El algoritmo de fusion modificado track to track basado en el filtro Kalman

tiene la siguiente estructura:

xk|k−1 = Fkxk−1|k−1

Pk|k−1 = FkPk−1|k−1FTk +GkQkG

Tk

Kmk = Pk|k−1(Hm

k )T [Hmk Pk|k−1(Hm

k )T +Rmk ]−1 m = 1, 2

xmk|k = xk|k−1 +Kmk [zmk −Hm

k xk|k−1] m = 1, 2

Pmk|k = [I −Kmk H

mk ]Pk|k−1 m = 1, 2

P 12k|k = (P 21

k|k)T = [I −K1kH

1k ]Pk|k−1[I −K2

kH2k ]T

Al final se obtiene la estimacion del estado fusionado necesario para el siguiente ciclo.


xk|k = x1k|k + [P 1k|k − P 12

k|k] · [P 1k|k + P 2

k|k − P 12k|k − P 21

k|k]−1 · (x2k|k − x1k|k)

Pk|k = P 1k|k − [P 1

k|k − P 12k|k] · [P 1

k|k + P 2k|k − P 12

k|k − P 21k|k]−1 · [P 1

k|k − P 21k|k]

El algoritmo anterior es similar en estructura al algoritmo de fusion track to track, pero con diferentes

ganancias de Kalman Kmk . Sin embargo, el costo computacional del algoritmo es identico al algoritmo

de fusion track to track.


Capıtulo 5

RESULTADOS EXPERIMENTALES

Para la aplicacion de los modelos de fusion de medidas se cuenta con dos sistemas independientes

uno del otro. El primero es el sistema SLAM basado en vision y el otro es el sistema de referencia

de actitud y rumbo (AHRS), los cuales entregan diferentes mediciones de los mismos estados. Una

ventaja es que los dos sistemas usan el filtro Kalman como estimador secuencial, facilitando la imple-

mentacion de las tecnicas de fusion mencionadas anteriormente.

La implementacion de los modelos de fusion fue realizada en MATLAB de manera off − line por

las siguientes razones:

Desincronizacion. Es decir, los dos sistemas no presentan la misma frecuencia de muestreo, adi-

cionalmente, la frecuencia de muestreo del sistema SLAM es variante en el tiempo (depende del

tiempo en que tarde en procesar la imagen).

No se tiene acceso al sistema SLAM . Se requiere un conocimiento apropiado del lenguaje de

programacion en el que el sistema esta elaborado para realizar cualquier tipo de modificacion.

Aunque los incovenientes anteriores son solucionables la idea de este trabajo es presentar resulta-

dos preliminares de la aplicacion de esta metodologıa. La fusion solo fue realizada para determinar la

orientacion del vehıculo la cual utilizo como base el modelo de prediccion descrito para la elaboracion

del AHRS. La prediccion y medicion de los estados a estimar es mucho mas sencilla para el AHRS

en comparacion con el sistema SLAM basado en vision. Se maneja menos informacion (en el SLAM

se tiene los estados de la camara y de las caracterısticas visuales). Por tanto, el costo computacional

es menor (menos operaciones).

El experimento consistio en acoplar el modulo inercial iNEMO a la camara monocular que sirve

como sensor del entorno del sistema SLAM de manera tal que ambos elementos originara un unico

dispositivo. Los dos sistemas fueron encendidos al mismo tiempo y se procedio a realizar los movimien-

to de cabeceo φ, alabeo θ y guinada ψ que describen la orientacion de la plataforma movil. A medida

que transcurrıa la prueba los datos propocionados por ambos sistemas (SLAM e iNEMO) fueron

almacenadas en 2 laptops distintas. La informacion adquirida del SLAM fue la estimacion de la ori-

entacion, su covarianza del error y un vector de tiempo. Por su parte, el modulo inercial suministro las

todas las medidas inerciales y su vector de tiempo.

53

54 CAPITULO 5. RESULTADOS EXPERIMENTALES

En MATLAB, a partir de las mediciones inerciales se estimaba la orientacion y su covarianza del

error muestra a muestra utilizando el modulo AHRS descrito en secciones anteriores. Aprovechando

que la frecuencia de los sensores inerciales es periodica, una manera de evadir el problema de desin-

cronizacion fue utilizar un retenedor para el SLAM , es decir, mantener la muestra el tiempo necesario

hasta que haya una actualizacion. De esa manera se garantiza que existen las dos medidas de los dos

sistemas para hacer las respectivas correcciones. Luego de obtener las dos mediciones se aplicaba los

diferentes modelos de fusion de medidas.

SLAM

iNEMO

Modelo deFusión de Medidas

AHRS

, ,

zk

zk2

1

Figura 5.1: Esquema general de implementacion de la fusion de medidas.

5.1. Modelo de fusion de las medidas

Para la estimacion de la orientacion, el modelo de prediccion es el mismo al presentado en la seccion

3.4.4, conservando la estructura de la funcion no lineal que relaciona el estado actual con el anterior

f(xk−1, uk−1, 0) y las matrices A[i,j] y Q. Al ser el mismo modelo de prediccion, se tiene que Hk =

[(HAHRSk )T (HAHRS

k )T ]T , el vector de medida y la matriz de covarianza Rk para el ruido en la medida

fusionados es establecido como:

zk = [(zAHRSk )T (zSLAMk )T ]T (5.1)

Rk =

(RAHRSk 0

0 RSLAMk

)(5.2)

Para el ruido en la medida del sistema SLAM se realizaron unas medidas del angulo cuando se

encontraba en reposo, mientras tanto el ruido en la medida para el AHRS fue el mismo. Los resultados

son ilustrados en la figura 5.2.

TABLA 5.1: Momentos estadısticos de los angulos medidos (SLAM)

Parametro θ φ ψ Unidades

m 0.2625 -0.3539 -0.0856 o

σ 0.1788 0.1247 0.0427 o

5.1. MODELO DE FUSION DE LAS MEDIDAS 55

0 5 10 15 20 25 30 35 40 45−40

−30

−20

−10

0

10

20

Ángulo de Alabeo

Tiempo [seg]

Gra

do

s [

º]

SLAM

AHRS

iNEMO

Fusión

(a) Angulo de alabeo θ.

0 5 10 15 20 25 30 35 40 45−30

−20

−10

0

10

20

30

40

Ángulo de Cabeceo

Tiempo [seg]

Gra

do

s [

º]

SLAM

AHRS

iNEMO

Fusión

(b) Angulo del cabeceo φ.

0 5 10 15 20 25 30 35 40 45−140

−120

−100

−80

−60

−40

−20

0

20

Ángulo de Guiñada

Tiempo [seg]

Gra

do

s [

º]

SLAM

AHRS

iNEMO

Fusión

(c) Angulo de guinada ψ.

0 5 10 15 20 25 30 35 40 450

0.2

0.4

0.6

0.8

1

1.2

1.4x 10


Tiempo [seg]

Err

or

[rad

2]

AHRS

SLAM

Fusión


Figura 5.2: Resultados del modelo de fusion de medidas.

Aunque el sistema AHRS tiene menor error en comparacion con el sistema SLAM , la fusion refleja

que este parametro no tiene ningun efecto en el resultado. Como se observa en la grafica 5.2, en

ocasiones la medida seguida es la del sistema SLAM y en otras sigue el comportamiento del AHRS.

La tendencia es a seguir la curva que ofrezca un menor error de correccion. La media del error obtenido

de la grafica 5.2(d) se presenta en la tabla 5.2.


TABLA 5.2: Error medio en el aumento del vector de observacionAngulo Error Unidades

SLAM 3,2344e−4 rad2

AHRS 2,1029e−5 rad2

Fusion 1,3095e−5 rad2

Como era de esperarse el error cuando los datos son fusionados es reducido. El factor de reduccion

del error es aproximadamente 25 veces el error del sistema SLAM . Al aplicar la tecnica de fusion de

medidas de mınima media cuadratica los resultados son similares a los mostrados en la figura 5.2 y

la media del error cambia muy poco con respecto a la presentada en la tabla 5.2, comprobando la

similitud matematica existente entre los dos modelos cuando H1k = H2

k .

TABLA 5.3: Error medio en la estimacion de mınima media cuadraticaAngulo Error Unidades




5.2. Fusion track to track

Aunque se dispone de las estimaciones de los estados xmk|k y su correspondiente matriz de covarianzas

en los dos sistemas Pmk|k, es muy difıcil obtener la informacion correspondiente a la matriz de obser-

vacion H y la matriz de la ganancia del filtro Kalman K para el sistema SLAM basado en vision.

Por lo tanto se recurrio la ecuacion 4.10 para establecer la matriz de covarianzas cruzadas P 12k|k. Se

aplicaron las ecuaciones 4.7 y 4.8 para la fusion de los estados.

En los resultados, se observa una tendencia a seguir, que es el compartamiento de la curva que

presenta menor error en la estimacion, pero esta tiene en cuenta las dos mediciones realizadas por los

sistemas.

TABLA 5.4: Error medio en la fusion track to trackAngulo Error Unidades




5.2. FUSION TRACK TO TRACK 57

0 5 10 15 20 25 30 35 40 45−40

−30

−20

−10

0

10

20

Ángulo de Alabeo

Tiempo [seg]

Gra

do

s [

º]

SLAM

AHRS

iNEMO

Fusión


0 5 10 15 20 25 30 35 40 45−30

−20

−10

0

10

20

30

40

Ángulo de Cabeceo

Tiempo [seg]

Gra

do

s [

º]

SLAM

AHRS

iNEMO

Fusión


0 5 10 15 20 25 30 35 40 45−140

−120

−100

−80

−60

−40

−20

0

20

Ángulo de Guiñada

Tiempo [seg]

Gra

do

s [

º]

SLAM

AHRS

iNEMO

Fusión


0 5 10 15 20 25 30 35 40 450

0.2

0.4

0.6

0.8

1

1.2

1.4x 10

−3 Traza de la matriz de covarianza

Tiempo [seg]

Err

or

[rad

2]

SLAM

AHRS

Fusión


Figura 5.3: Resultados de la fusion track to track.

La matriz de covarianza del AHRS es diagonal por lo que el resultado de la matriz de covarianzas

cruzada tambien lo sera. Esta consideracion implica que la matriz P 12k|k puede ser ignorada y no ten-

dra un efecto relevante a la hora de aplicar las ecuaciones 4.7 y 4.8. Los resultados obtenidos ignorando

la matriz de covarianzas cruzadas son muy similares a los presentados en la figura 5.3, pero el error

sı es reducido.


0 5 10 15 20 25 30 35 40 450

0.2

0.4

0.6

0.8

1

1.2

1.4x 10

−3 Traza de la matriz de covarianza

Tiempo [seg]

Err

or

[ra

d2]

SLAM

AHRS

Fusión

Figura 5.4: Covarianza del error para la fusion track to track ignorando P 12k|k.

TABLA 5.5: Error medio en la fusion track to track ignorando P 12k|k

Angulo Error Unidades




5.3. Fusion modificada track to track

El procedimiento es muy parecido al realizado en el modelo de fusion de medidas, solo que para este

caso se hacen dos correcciones de manera independiente con las mediciones de los 2 sistemas y luego

son fusionadas para ser realimentados. Los resultados son los siguientes:

TABLA 5.6: Error medio en la fusion modificada track to trackAngulo Error Unidades




Este metodo arroja mejores resultados en comparacion con los demas esquemas de fusion gracias a la

realimentacion de los estados estimados y que la etapa de prediccion es la misma para los dos sistemas.

Esta configuracion permite que el error pueda ser calculado a partir de su valor anterior en cada

instante de tiempo k. En resumen los errorres obtenidos en cada metodo para el mismo experimento

son los siguientes:

5.3. FUSION MODIFICADA TRACK TO TRACK 59

0 5 10 15 20 25 30 35 40 45−40

−30

−20

−10

0

10

20

Ángulo de Alabeo

Tiempo [seg]

Gra

do

s [

º]

SLAM

AHRS

iNEMO

Fusión


0 5 10 15 20 25 30 35 40 45−30

−20

−10

0

10

20

30

40

Ángulo de Cabeceo

Tiempo [seg]

Gra

do

s [

º]

SLAM

AHRS

iNEMO

Fusión


0 5 10 15 20 25 30 35 40 45−140

−120

−100

−80

−60

−40

−20

0

20

Ángulo de Guiñada

Tiempo [seg]

Gra

do

s [

º]

SLAM

AHRS

iNEMO

Fusión


0 5 10 15 20 25 30 35 40 450

0.2

0.4

0.6

0.8

1

1.2

1.4x 10


Tiempo [seg]

Err

or

[rad

2]

AHRS

SLAM

Fusión


Figura 5.5: Resultados de la fusion modificada track to track.

TABLA 5.7: Error medio de los modelos de fusion de medidasModelo Error Unidades

Aumento del vector de observacion 1,3095e−5 rad2

Estimacion de mınima media cuadratica 1,3213e−5 rad2

TABLA 5.8: Error medio de los modelos de fusion track to trackModelo Error Unidades

Fusion track to track 1,9540e−5 rad2

Combinacion convexa basica 1,7874e−5 rad2

Fusion modificada track to track 1,2697e−5 rad2


Capıtulo 6

CONCLUSIONES

Aunque el SLAM es un dispositivo que ofrece muchos beneficios en localizacion y mapeo, este todavıa

conserva algunos inconvenientes los cuales no permiten sacar todo el provecho a esta herramienta. Por

consiguiente, el objetivo de este trabajo ha sido proporcionar una mejora al sistema SLAM con lo que

tiene que ver con el factor de escala del mapa. Esta problematica ha sido atacada adicionando sensores

inerciales con la intencion de que dichas mediciones contribuyan a mejorar la trayectoria seguida por la

camara y soporten la informacion obtenida a traves del procesamiento de la imagen. Una revision de la

literatura refleja que los trabajos han sido enfocados en utilizar los sensores inerciales para predecir los

estados involucrados con la dinamica de la camara, mientras la observacion de los mismos es obtenida

a traves de la informacion suministrada por la imagen obteniendo buenos resultados.

Demostrada la efectividad del uso de sensores inerciales para la mejora de la trayectoria y por ende

reduccion del escalamiento en el mapa, se ha recurrido a la fusion de sensores como una alternativa

adicional con la que se puede hacer la combinacion de los sensores. Por tanto, dos sistemas que actuan

como sensores del entorno uno independiente del otro fueron fusionados. En una primera instancia se

elaboro un sistema de referencia actitud y rumbo (AHRS) para que ofreciera una estimacion de la orien-

tacion de la camara a manera de que coincidieran con los proporcionados por el sistema SLAM . Por

ultimo, se implementaron diferentes modelos de fusion de sensores que usan como estimador secuencial

el filtro Kalman. Las conclusiones obtenidas del trabajo descrito anteriormente son las siguientes:

La escala del mapa, la posicion y orientacion obtenida a partir del sistema SLAM puede cambiar

entre observaciones. Es decir, que se pueden tener resultados diferentes para el mismo experi-

mento y depende de las caracterısticas visuales detectadas en la imagen y la perdida de las

mismas.

Se elaboro un sistema de referencia de actitud y rumbo (AHRS) a partir de sensores inerciales

y el uso del cuaterinion como herramienta para la descripcion de la orientacion sin presentar

singularidades y comparable con dispositivos comerciales.

No existen criterios para establecer con las medidas inerciales si un vehıculo se encuentra en

reposo o por el contrario en movimiento. Se requiere un sensor adicional que permita identificar

este fenomeno y poder proporcionar una estimacion de la velocidad y posicion seguida por la

camara.

61

62 CAPITULO 6. CONCLUSIONES

Es aconsejable en lo posible que el modelo del proceso y de la medida en el filtro Kalman

para la variable que se desea estimar manejen la misma referencia. Con el objetivo de obtener

resultados mas acertados y la posibilidad de eliminar los distintos inconvenientes que pueda

presentar cualquier sensor (ejemplo: offset).

La fusion de sensores no garantiza que la medida verdadera sea la estimada. Principalmente

sucede cuando los sensores a fusionar no tienen la misma referencia en sus mediciones. La esti-

macion puede tener una tendencia a seguir una de las dos medidas o por el contrario un promedio

de las dos en el peor caso.

En la fusion de sensores no siempre la tendencia es a seguir la curva que presenta menor error.

Esta siempre es estimada teniendo en cuenta las dos mediciones realizadas por los sensores aunque

se observe lo contrario.

En terminos del error obtenido, la fusion de sensores refleja un mejor resultado que el propor-

cionado por los demas sensores de manera individual sin importar el modelo de fusion utilizado.

Pero los modelos que presentan menor error son aquellos que combinan las medidas de los 2

sensores en la etapa de correccion y son retroalimentadas al sistema.

6.1. Trabajo futuro

Como trabajo futuro se propone elaborar un sistema de navegacion inercial (INS) a partir de sen-

sores inerciales diferente al modelo de integracion de la senal de aceleracion proporcionada por los

acelerometros con el fin de evitar los problemas de acumulacion del error debido a la integracion bias

presente en las mediciones de los giroscopos y acelerometros. O en caso contrario, utilizar un sensor

adicional que proporciones otras observaciones de posicion y velocidad que permita establecer los erro-

res.

Implementar en tiempo real un algoritmo que fusione el sistema de navegacion inercial (INS) con el

sistema (SLAM) basado en vision, en vez de usar un sensor como predictor y el otro como corrector.

Para ello se requiere conocer como esta elaborado el SLAM para poder realizar cualquier tipo de

modificacion. Adicionalmente, retroalimentar los resultados obtenidos en la fusion de los dos sistemas

al SLAM para evaluar las mejoras obtenidas en el factor de escala del mapa y por ende la estimacion

de la trayectoria seguida por la camara.

Explorar si con las medidas inerciales se pueden tener predicciones mas precisas de la ubicacion de

las caracterısticas en la imagen.

Implementar fusion de sensores aplicando otras metodologıas como redes neuronales y elaborar sen-

sores inteligentes apoyados con los resultados obtenidos a partir de la aplicacion del filtro Kalman.

Bibliografıa

[1] J.-H. Kim and S. Sukkarieh, “Airborne simultaneous localisation and map building,” in Robotics

and Automation, 2003. Proceedings. ICRA ’03. IEEE International Conference on, vol. 1, sept.

2003, pp. 406 – 411 vol.1.

[2] J. Kim and S. Sukkarieh, “Real-time implementation of airborne inertial-slam,” Robotics

and Autonomous Systems, vol. 55, no. 1, pp. 62 – 71, 2007. [Online]. Available:

http://www.sciencedirect.com/science/article/pii/S0921889006001485

[3] I. Mahon and S. Williams, “Slam using natural features in an underwater environment,” in Con-

trol, Automation, Robotics and Vision Conference, 2004. ICARCV 2004 8th, vol. 3, dec. 2004,

pp. 2076 – 2081 Vol. 3.

[4] A. Davison, I. Reid, N. Molton, and O. Stasse, “Monoslam: Real-time single camera slam,” Pattern

Analysis and Machine Intelligence, IEEE Transactions on, vol. 29, no. 6, pp. 1052 –1067, june

2007.

[5] A. Davison, “Real-time simultaneous localisation and mapping with a single camera,” in Computer

Vision, 2003. Proceedings. Ninth IEEE International Conference on, oct. 2003, pp. 1403 –1410

vol.2.

[6] D. Schleicher, L. Bergasa, R. Barea, E. Lopez, and M. Ocana, “Real-time simultaneous localization

and mapping using a wide-angle stereo camera and adaptive patches,” in Intelligent Robots and

Systems, 2006 IEEE/RSJ International Conference on, oct. 2006, pp. 2090 –2095.

[7] P. Pinies, T. Lupton, S. Sukkarieh, and J. Tardos, “Inertial aiding of inverse depth slam using a

monocular camera,” in Robotics and Automation, 2007 IEEE International Conference on, april

2007, pp. 2797 –2802.

[8] P. Pinies, “Slam in large environments with wearable sensors,” Ph.D. dissertation, Universidad

de Zaragoza, mar 2009.

[9] S. Roumeliotis, A. Johnson, and J. Montgomery, “Augmenting inertial navigation with image-

based motion estimation,” in Robotics and Automation, 2002. Proceedings. ICRA ’02. IEEE In-

ternational Conference on, vol. 4, 2002, pp. 4326 – 4333 vol.4.

63


64 BIBLIOGRAFIA

[10] E. Foxlin and L. Naimark, “Vis-tracker: A wearable vision-inertial self-tracker,” Virtual Reality

Conference, IEEE, vol. 0, p. 199, 2003.

[11] S.-K. Park, Y. S. Suh, and T. N. Do, “The pedestrian navigation system using vision and inertial

sensors,” in ICCAS-SICE, 2009, aug. 2009, pp. 3970 –3974.

[12] D. Randeniya, S. Sarkar, and M. Gunaratne, “Vision-imu integration using a slow-frame-rate

monocular vision system in an actual roadway setting,” Intelligent Transportation Systems, IEEE

Transactions on, vol. 11, no. 2, pp. 256 –266, june 2010.

[13] T. Oskiper, S. Samarasekera, and R. Kumar, “Tightly-coupled robust vision aided inertial naviga-

tion algorithm for augmented reality using monocular camera and imu,” in Mixed and Augmented

Reality (ISMAR), 2011 10th IEEE International Symposium on, oct. 2011, pp. 255 –256.

[14] A. Behrens, J. Grimm, S. Gross, and T. Aach, “Inertial navigation system for bladder endoscopy,”

in Engineering in Medicine and Biology Society,EMBC, 2011 Annual International Conference of

the IEEE, 30 2011-sept. 3 2011, pp. 5376 –5379.

[15] H. Rehbinder and B. Ghosh, “Pose estimation using line-based dynamic vision and inertial sen-

sors,” Automatic Control, IEEE Transactions on, vol. 48, no. 2, pp. 186 – 199, feb. 2003.

[16] Y. M. Choi, Y. S. Suh, and S. Kyeong, “Pose estimation from landmark-based vision and inertial

sensors,” in SICE-ICASE, 2006. International Joint Conference, oct. 2006, pp. 1668 –1671.

[17] A. Martinelli, “Closed-form solution for attitude and speed determination by fusing monocular

vision and inertial sensor measurements,” in Robotics and Automation (ICRA), 2011 IEEE In-

ternational Conference on, may 2011, pp. 4538 –4545.

[18] ——, “Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale, and

bias determination,” Robotics, IEEE Transactions on, vol. 28, no. 1, pp. 44 –60, feb. 2012.

[19] J. Hol, T. Schon, H. Luinge, P. Slycke, and F. Gustafsson, “Robust real-time tracking

by fusing measurements from inertial and vision sensors,” Journal of Real-Time Image

Processing, vol. 2, pp. 149–160, 2007, 10.1007/s11554-007-0040-2. [Online]. Available:

http://dx.doi.org/10.1007/s11554-007-0040-2

[20] G. Klein and T. Drummond, “Tightly integrated sensor fusion for robust visual tracking,” Image

and Vision Computing, vol. 22, no. 10, pp. 769 – 776, 2004, british Machine Vision Computing

2002. [Online]. Available: http://www.sciencedirect.com/science/article/pii/S0262885604000447

[21] G. Bleser and D. Stricker, “Advanced tracking through efficient image processing and

visual–inertial sensor fusion,” Computers and Graphics, vol. 33, no. 1, pp. 59 – 72, 2009. [Online].

Available: http://www.sciencedirect.com/science/article/pii/S0097849308001465

[22] R. Smith and P. Cheeseman, “On the representation and estimation of spatial uncertainty,”

Robotics Research, The international Journal of, vol. 5, no. 4, pp. 56 –68, dec 1986.

http://dx.doi.org/10.1007/s11554-007-0040-2



BIBLIOGRAFIA 65

[23] H. Durrant-Whyte, “Uncertain geometry in robotics,” Robotics and Automation, IEEE Journal

of, vol. 4, no. 1, pp. 23 –31, feb 1988.

[24] R. Smith, M. Self, and P. Cheeseman, “Estimating uncertain spatial relationships in robotics,” in

Robotics and Automation. Proceedings. 1987 IEEE International Conference on, vol. 4, mar 1987,

p. 850.

[25] J. Civera, A. Davison, and J. Montiel, “Inverse depth parametrization for monocular slam,”

Robotics, IEEE Transactions on, vol. 24, no. 5, pp. 932 –945, oct. 2008.

[26] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and mapping: part i,” Robotics

Automation Magazine, IEEE, vol. 13, no. 2, pp. 99 –110, june 2006.

[27] A. P. Gee, “Incorporating higher level structure in visual slam,” Ph.D. dissertation, University of

Bristol, may 2010.

[28] D. Chekhlov, “Towards robust data association in real-time visual slam,” Ph.D. dissertation,

University of Bristol, sep 2009.

[29] R. Munguia and A. Grau, “Delayed features initialization for inverse depth monocular slam,” in

European Conference on Mobile Robots ECMR 2007, sep 2007, pp. 1–6.

[30] J. Civera, A. J. Davison, and J. M. M. Montiel, “Unified inverse depth parametrization for monoc-

ular slam,” in In Proceedings of Robotics: Science and Systems, 2006.

[31] J. Sola, A. Monin, M. Devy, and T. Lemaire, “Undelayed initialization in bearing only slam,” in

Intelligent Robots and Systems, 2005. (IROS 2005). 2005 IEEE/RSJ International Conference

on, aug. 2005, pp. 2499 – 2504.

[32] STMicroelectronics, iNEMO user manual UM0937, may. 2010.

[33] M. Caruso, “Applications of magnetoresistive sensors in navigation systems,” SAE Technical Paper

970602, 1997.

[34] M. S. Grewal, L. R. Weill, and A. P. Andrews, Global Positioning System, Inertial

Navigation and Integration. John Wiley and Sons, Inc., 2002. [Online]. Available:

http://dx.doi.org/10.1002/0471200719.fmatter indsub

[35] D. Titterton and J. Weston, Strapdown Inertial Navigation Technology, 2nd ed. The American

Institute of Aeronautics and Astronautics, 2004.

[36] S. Eun-Hwan, “Accuracy improvement of low cost ins/gps for land applications,” Master’s thesis,

University of Calgary, dec 2001.

[37] A. Barrientos, Fundamentos de robotica, 2nd ed. McGraw-Hill, 2007.

[38] J. L. Crassidis, F. L. Markley, and Y. Cheng, “Nonlinear attitude filtering methods,” in Navigation,

and Control Conference and Exhibit; San Francisco, CA, USA, aug 2005, pp. 1–32.

http://dx.doi.org/10.1002/0471200719.fmatter_indsub

66 BIBLIOGRAFIA

[39] R. Vepa, Biomimetic Robotics Mechanisms and Control. Cambridge University Press, 2009.

[40] Y.-C. Lai, S.-S. Jan, and F.-B. Hsiao, “Development of a low-cost attitude and heading reference

system using a three-axis rotating platform,” Sensors, vol. 10, no. 4, pp. 2472–2491, 2010.

[Online]. Available: http://www.mdpi.com/1424-8220/10/4/2472

[41] G. Schall, D. Wagner, G. Reitmayr, E. Taichmann, M. Wieser, D. Schmalstieg, and B. Hofmann-

Wellenhof, “Global pose estimation using multi-sensor fusion for outdoor augmented reality,” in

Mixed and Augmented Reality, 2009. ISMAR 2009. 8th IEEE International Symposium on, oct.

2009, pp. 153 –162.

[42] K. Tuck, Tilt Sensing Using Linear Accelerometers, Freescale Semiconductor, Application note,

jun. 2007.

[43] E. Foxlin, “Inertial head-tracker sensor fusion by a complementary separate-bias kalman filter,”

in Virtual Reality Annual International Symposium, 1996., Proceedings of the IEEE 1996, mar-3

apr 1996, pp. 185 –194, 267.

[44] M. Marmion, “Airborne attitude estimation using a kalman filter,” Master’s thesis, University

Centre of Svalbard, Norwegian University of Science and Technology, Superior National School of

Electrical Engineering, feb 2006.

[45] S. Fux, “Development of a planar low cost inertial measurement unit for uavs and mavs,” Master’s

thesis, Swiss Federal Institute of Technology Zurich, 2008.

[46] Y. Bar-Shalom, X.-R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and

Navigation: Theory, Algorithms and Software. John Wiley and Sons, Inc., 2002. [Online].

Available: http://dx.doi.org/10.1002/0471221279.fmatter indsub

[47] E. Foxlin, “Pedestrian tracking with shoe-mounted inertial sensors,” Computer Graphics and

Applications, IEEE, vol. 25, no. 6, pp. 38 –46, nov.-dec. 2005.

[48] B. Khaleghi, S. Razavi, A. Khamis, F. Karray, and M. Kamel, “Multisensor data fusion: An-

tecedents and directions,” in Signals, Circuits and Systems (SCS), 2009 3rd International Con-

ference on, nov. 2009, pp. 1 –6.

[49] J. Roecker and C. McGillem, “Comparison of two-sensor tracking methods based on state vec-

tor fusion and measurement fusion,” Aerospace and Electronic Systems, IEEE Transactions on,

vol. 24, no. 4, pp. 447 –449, jul 1988.

[50] J. Gao and C. Harris, “Some remarks on kalman filters for the multisensor fusion,”

Information Fusion, vol. 3, no. 3, pp. 191 – 201, 2002. [Online]. Available: http:

//www.sciencedirect.com/science/article/pii/S1566253502000702

[51] D. Willner, C. B. Chang, and K. P. Dunn, “Kalman filter algorithms for a multi-sensor system,” in

Decision and Control including the 15th Symposium on Adaptive Processes, 1976 IEEE Conference

on, vol. 15, dec. 1976, pp. 570 –574.

http://www.mdpi.com/1424-8220/10/4/2472

http://dx.doi.org/10.1002/0471221279.fmatter_indsub



BIBLIOGRAFIA 67

[52] Q. Gan and C. Harris, “Comparison of two measurement fusion methods for kalman-filter-based

multisensor data fusion,” Aerospace and Electronic Systems, IEEE Transactions on, vol. 37, no. 1,

pp. 273 –279, jan 2001.

[53] Y. Bar-Shalom, “On the track-to-track correlation problem,” Automatic Control, IEEE Transac-

tions on, vol. 26, no. 2, pp. 571 – 572, apr 1981.

[54] Y. Bar-Shalom and L. Campo, “The effect of the common process noise on the two-sensor fused-

track covariance,” Aerospace and Electronic Systems, IEEE Transactions on, vol. AES-22, no. 6,

pp. 803 –805, nov. 1986.

[55] Y. Bar-Shalom, Tracking and data association. San Diego, CA, USA: Academic Press Profes-

sional, Inc., 1987.

[56] Y. Bar-Shalom and X. Li, Multitarget-multisensor Tracking: Principles and Techniques,

1995. Yaakov Bar-Shalom, 1995. [Online]. Available: http://books.google.com.mx/books?id=

NmXGQgAACAAJ

[57] P. S. Maybeck, Stochastic Models, Estimating, and Control. New York: Academic Press, 1979,

vol. 1.

[58] G. Welch and G. Bishop, “An introduction to the kalman filter,” 1995, updated july 2006.

http://books.google.com.mx/books?id=NmXGQgAACAAJ

http://books.google.com.mx/books?id=NmXGQgAACAAJ

68 BIBLIOGRAFIA

Referencia Grafica

Figura 1.1 Fuente: Autor.

Figura 1.2 Fuente: Autor. Adaptada de Formulation and Structure of the SLAM Problem [26].

Figura 1.3 Fuente: Autor. Adaptada de General Formulation of Visual SLAM Problem [27].

Figura 1.4 Fuente: Autor. Adaptada de Process Model [27].

Figura 1.5 Fuente: Autor. Adaptada de Process Model [27].

Figura 1.6 Fuente: Autor. Adaptada de Measurement equation [25].

Figura 2.1 Fuente: [32].




Figura 2.5 Fuente: [32].



Figura 3.2 Fuente: Autor. Adaptada de Reference Frames and Transformations [36].


Figura 3.4 Fuente: Autor. Adaptada de Reference Frames and Transformations [36].

Figura 3.5 Fuente: Autor. Adaptada de Matrices de Rotacion [37].







Figura 3.12 Fuente: Autor. Adaptada de Earth’s Magnetic field [33].

Figura 3.13 Fuente: Autor. Adaptada de Complimentary Kalman Filter [43].




Figura 4.1 Fuente: Autor. Adaptada de Measurement fusion models [50].

Figura 4.2 Fuente: Autor. Adaptada de The track-to-track fusion models [50].

69

70 BIBLIOGRAFIA

Figura 4.3 Fuente: Autor. Adaptada de The track-to-track fusion models [50].






Figura B.1 Fuente: [27].

Figura B.2 Fuente: Autor. Adaptada de Perspective Projection [27].

Figura B.3 Fuente: Autor. Adaptada de Pose [27].

Apendice A

PROBLEMA GENERAL DE

ESTIMACION SECUENCIAL

Considerando un sistema con un vector de estados x, n−dimensional, la idea consiste en determinar

la distribucion de probabilidad del estado en el instante k, dado un valor inicial particular para el

estado, x(0), y las secuencias de medidas y entradas de control posteriores. Lo anterior puede ser

representado como una funcion de densidad de probabilidad conjunta:

p(x(k)|z(0 : k), u(0 : k), x(0)) (A.1)

Donde z(0 : k) y u(0 : k) son el conjunto completo de observaciones y entradas de control en todos

los instantes de tiempo. Con el objetivo de realizar este procedimiento eficientemente en tiempo real, se

busca una solucion recursiva de tal manera que si tenemos una estimacion de la distribucion del estado

para el instante de tiempo anterior, k−1, podemos utilizar esta con la ultima entrada de control, u(k),

y observacion, z(k), para calcular la distribucion del estado para el instante de tiempo actual, k.

p(x(k − 1)|z(0 : k − 1), u(0 : k − 1), x(0)) (A.2)

Estableciendo que la distribucion del estado puede ser controlado por dos procesos. Entonces tomando

el estado anterior, x(k − 1), y una entrada de control conocidos, u(k), una funcion de proceso predice

la nueva distribucion del estado en el instante de tiempo k.

p(x(k)|u(k), x(k − 1)) (A.3)

Luego la distribucion del estado basado sobre todas las entradas de control y observaciones anteriores

se pueden actualizar de forma recursiva, para tomar en cuenta el efecto de las nuevas entradas de control

marginando la dependencia en el estado anterior.

p(x(k)|z(0 : k − 1), u(0 : k), x(0)) =

∫p(x(k)|u(k), x(k − 1))

p(x(k − 1)|z(0 : k − 1), u(0 : k − 1), x(0))dx(k − 1)

(A.4)

71

72 APENDICE A. PROBLEMA GENERAL DE ESTIMACION SECUENCIAL

Una funcion de medida usa la distribucion del estado actual x(k) para definir la distribucion del

conjunto de observaciones generados en el intervalo de tiempo actual.

p(z(k)|x(k)) (A.5)

Esta permite que la distribucion del estado pueda ser recursivamente actualizada usando el teorema

de Bayes teniendo en cuenta el efecto de las nuevas observaciones.

p(x(k)|z(0 : k), u(0 : k), x(0)) =p(z(k)|x(k))p(x(k)|z(0 : k − 1), u(0 : k), x(0))

p(z(k)|z(0 : k − 1), u(0 : k))(A.6)

Esta secuencia recursiva de marginalizacion y acondicionamiento de la distribucion del estado es

el proceso fundamental seguido por los filtros de estimacion secuencial y es independiente del tipo

de distribucion. Si se asume que el estado x tiene una distribucion Gaussiana, entonces se puede

representar sus estadısticas usando su media x y covarianza Pxx. Si lo anterior es posible, esto permite

el uso del filtro Kalman como un estimador secuencial para el problema.

A.1. Filtro Kalman

El filtro Kalman es un estimador optimo del primer y segundo momento estadıstico de una variable.

Optimo, en el sentido que garantiza la mınima varianza del error en la estimacion (error cuadratico

medio) dadas ciertas condiciones [57] [58]. Es un algoritmo recursivo de procesamiento de informacion

y es considerado un metodo de fusion directa de mediciones. Utiliza de manera eficiente toda la infor-

macion disponible para estimar el valor de las variables de interes aun con la presencia de mediciones

ruidosas. Principalmente usa:

Modelos dinamicos del sistema o proceso en cuestion y del o los dispositivos de medicion.

La descripcion estadıstica del ruido en el sistema, el error en la medicion y la incertidumbre en

los modelos dinamicos.

Cualquier informacion disponible sobre las condiciones iniciales de las variables de interes.

El problema general de estimar el estado de un proceso en tiempo discreto esta gobernado por la

siguiente ecuacion en diferencias estocastica.

xk = Axk−1 +Buk−1 + wk−1 (A.7)

A.1. FILTRO KALMAN 73

Donde xk es el estado prededido, xk−1 es el estado anterior, uk−1 es la entrada de control y wk−1

es una variable aleatoria que representa el ruido del proceso. Las variables A y B son las matrices de

transicion de estados y la de entrada, respectivamente. Con el modelo para las medidas se pretende

obtener el valor real de la medida o algo que sea una buena aproximacion de este.

zk = Hxk + vk (A.8)

Donde vk es una variable aleatoria que representa el ruido de la medida. H es la matriz de medidas.

El ruido presente en el proceso wk y vk se asumidos estadısticamente independientes, blancos y con

una distrubucion de propabilidad normal.

p(w) ∼ N(0, Q) (A.9)

p(v) ∼ N(0, R) (A.10)

Las ecuaciones para el Filtro Kalman pueden ser divididas en 2 grupos. Las ecuaciones de time update

y las de measurement update. Las primeras son las responsables de la proyeccion hacia adelante del

estado actual y la estimacion de las covarianzas del error, para obtener las estimaciones a priori para

el proximo paso. Las ecuaciones de measurement update son las encargadas de la retroalimentacion,

por incorporar una nueva medida dentro de la estimacion a a priori para obtener una mejor estimacion

a posteriori [58].

Las ecuaciones de time update pueden ser pensadas como las ecuaciones de prediccion, mientras

que las measurement update como las ecuaciones de correcciones. las ecuaciones de prediccion son las

siguientes:

xk = Axk−1 +Buk−1 (A.11)

Pk = APk−1AT +Q (A.12)

Las ecuaciones de correccion son:

Kk = PkHT (HPkH

T +R)−1 (A.13)

xk = xk +Kk(zk −Hxk) (A.14)

Pk = (I −KkH)Pk (A.15)


A.2. Filtro Kalman Extendido

Si el proceso a ser estimado o las realciones de medida son no lineales es necesario usar el Filtro

Kalman Extendido o EKF . Este filtro realiza una linealizacion respecto a la medida actual y su

covarianza. Pare este caso el proceso es gobernado por una ecuacion de diferencias estocastica no

lineal.

xk = f(xk−1, uk−1, wk−1) (A.16)

zk = h(xk + vk) (A.17)

De manera similar las variables w y v representan el ruido en el modelo del proceso y en el de la

medida respectivamente. En la practica no se requiere conocer los valores individuales de ruido wk y vk

en cada intervalo de tiempo. Por tanto el vector de estados y de medida puede ser aproximado como:

xk = f(xk−1, uk−1, 0) (A.18)

zk = h(xk, 0) (A.19)

Donde xk es una estimacion del estado aposteriori para el intervalo de tiempo anterior. Las ecuaciones

de prediccion son las siguientes:

x−k = f(xk−1, uk−1, 0) (A.20)

P−k = AkPk−1ATk +WkQk−1W

Tk (A.21)

Donde x−k es notacion apriori y xk a posteriori. Por su parte las matrices A, W , H y V son las

matrices jacobianas de las derivadas parciales de las funciones no lineales con respecto a los estados y

al ruido que deben ser calculadas para cada tiempo k.

A[i,j] =∂f[i]

∂x[j](xk−1, uk−1, 0) (A.22)

W[i,j] =∂f[i]

∂w[j](xk−1, uk−1, 0) (A.23)

H[i,j] =∂h[i]

∂x[j](xk, 0) (A.24)

V[i,j] =∂h[i]

∂v[j](xk, 0) (A.25)

A.2. FILTRO KALMAN EXTENDIDO 75

Donde xk es el vector de estado aproximado obtenido de la ecuacion A.18. Las ecuaciones de co-

rreccion son:

Kk = P−k HTk (HkP

−k H

Tk + VkRkV

Tk )−1 (A.26)

xk = x−k +Kk(zk − h(x−k , 0)) (A.27)

Pk = (I −KkHk)P−k (A.28)


Apendice B

MODELO DE FORMACION DE LA

IMAGEN

En el sistema SLAM basado en vision, el principal sensor es una camara que proporciona las medidas

de las caracterısticas visuales en las imagenes. En la mayorıa de los sistemas, el modelo de la camara

de agujero de alfiler (pinhole camera model) estandar ofrece un marco matematico que es usado para

modelar la formacion de la imagen. Este modelo es una simplificacion del real.

Figura B.1: Camara monocular.

B.1. Perspectiva de proyeccion

El modelo de la camara de agujero de alfiler representa la apertura de la camara en un solo punto

comunnmente llamado como el centro optico O de la camara. Un punto en 3 dimensiones en el marco

de referencia euclideano, es proyectado sobre el plano de imagen de la camara a lo largo de un a lınea

a traves del centro optico de la camara, tal como se muestra en la figura B.2.

La longitud focal f , es la distancia mas corta entre el centro optico O y el plano de la imagen en

donde se observa la vista proyectada del mundo en 3 dimensiones. El punto p = [px, py, f ]T es una

imagen de un punto en 3D P = [Px, Py, Pz]. El centro de la imagen Oi se encuentra intersectando el

77

78 APENDICE B. MODELO DE FORMACION DE LA IMAGEN

X

Y

Z

u

v

Oif

O

eje óptico

Punto en 3D

Punto proyectado

Rayo de proyección

Figura B.2: Geometrıa del modelo de la camara de agujero de alfiler.

plano de la imagen con el eje optico z. Por lo tanto, usando el teorema de triangulos semejantes se

obtienen las ecuaciones fundamentales de la perspectiva de proyeccion:

px = fPxPz

py = fPyPz

(B.1)

La manera como se representa una proyeccion de un punto en 3D a otro en 2D es usando la funcion

de proyeccion Π: <3 7→ <2 :

p =

[px

py

]= Π(P ) =

[f Px

Pz

fPy

Pz

](B.2)

Las coordenadas de la imagen son establecidas a partir de los ejes u y v. Donde u toma valores

positivos de izquierda a derecha y v de arriba hacia abajo y su centro u origen es el punto de interseccion

entre el plano de la imagen y el eje optico u0 = (u0, v0)T . La transformacion entre coordenadas

requiere conocer el tamano de cada pixel en unidades del sistema coordenado de la camara. El tamano

de un pixel rectangular (sx, sy) puede ser establecido como un elemento del arreglo CCD. El punto

p = (px, py, f)T en las correspondiente coordenadas de imagen (u, v) tienen las siguientes relaciones:

px = −(u− u0)· sx py = −(v − v0)· sy (B.3)

B.2. POSICION 79

u = u0 −pxsx

v = v0 −pysy

(B.4)

El signo menos en la ecuacion B.3 aparece porque los ejes u y v tienen direcciones opuestas a los ejes

X y Y del sistema de coordenadas de la camara. Por consiguiente, la ecuacion de proyeccion final de

un punto P es:

Π(P ) =

[u0 − fPx

sxPz

v0 − fPy

syPz

](B.5)

Los parametros f, (u0, v0)T , (sx, sy)T son llamados parametros intrınsecos de una camara los cuales

son obtenidos de un procedimiento de calibracion [27].

B.2. Posicion

Para proyectar un punto en 3D que representa la posicion de una marca en la imagen y adicionalmente

este se encuentre definido en el marco de referencia de navegacion n, es necesario describir la posicion

de la camara en este mismo marco. De esta manera sera posible formular la transformacion euclideana

que tome un punto 3D en el marco de navegacion n para determinar su equivalente en el marco de

referencia de la camara o del cuerpo b. Los marcos de referencia son presentados en la seccion 3.1.

u

xin

xcn

xcin

Xn

Yn

Zn

Xb

Yb

Zb

Cámara

Referencia

Plano deimagen

Figura B.3: Transformaciones entre el marco de referencia n y del cuerpo b.

80 APENDICE B. MODELO DE FORMACION DE LA IMAGEN

Tomando como referencia la figura B.3, es posible escribir que la distancia entre la camara y la marca

en la imagen xci dada en el marco de referencia del cuerpo b (equivalente al marco de referencia de la

camara) es igual a la diferencia entre la posicion la caracterıstica xi y de la camara xc, en el marco de

navegacion n, y multiplicada por la correspondiente matriz de rotacion Cbn:

xbci = Cbn(xni − xnc ) (B.6)

Ahora sabiendo que en realidad disponemos de la informacion correspondiente a la posicion de la

camara xc en el marco n y de la distancia existente entre la camara y la marca xci en el marco de

referencia del cuerpo b, se puede deducir que:

xni = Cnb xci + xnc (B.7)

Debido a que la ecuacion B.6 representa la posicion de la marca en el marco de referencia de la

camara b, es posible formular la nueva ecuacion de proyeccion:

Π(xbci) = Π(Cbn(xni − xnc )) (B.8)

Documents

Fusi on de SLAM Monocular´ con Sensores Inerciales para la ... · Aplicando Filtro Kalman por el Ingeniero Electr onico´ Carlos Alberto Vianchada Est evez´ Tesis sometida como