Sensor Fusion of Differential Gps and Inertial Measuring Unit To Measure State of A Test Vehicle

UNIVERSIDAD PONTIFICIA COMILLAS
ESCUELA TCNICA SUPERIOR DE INGENIERA (ICAI)

INGENIERO BELN ARRONTE ARROYUELOS
PROYECTO FIN DE CARRERA
SENSOR FUSION OF
DIFFERENTIAL GPS AND
INERTIAL MEASURING UNIT
TO MEASURE STATE OF A
TEST VEHICLE
AUTOR:
BELN ARRONTE ARROYUELOS
MADRID, junio de 2007
ii
Acknowlegdgements
First of all I want to thank my thesis director Tobias Hesse and supervisor Thomas
Sattel for giving me an opportunity as a foreign student to work in this project,
as well as, for giving me the means and help to develop my thesis.
Secondly to Sascha for helping me when I did not understand something, or just
for being there when I was a bad day.
My friends and family in Spain, for understanding my situation at the time.
And finally to Pablo, for calling me everyday, helping with my research, visiting
me... and everything else.
iii
Abstract
Driver assistance systems are being developed by most car brands to try to decrease the high number of traffic accidents in current times. This project is part
of an overall driver assistance project based on obstacle avoidance path planning.
Of course, in order to plan the path, as well as knowledge of the obstacles state,
the vehicles state is also necessary. This thesis will show the reader how the
vehicles state can be known accurately and at a high update rate (100Hz). To
achieve this goal, a Global Positioning System GPS and an Inertial Measuring
Unit IMU are fused by a Kalman filter.
After a general introduction is made in the first chapter, chapter 2 explains how
the two measuring devices involved in the project, GPS and IMU, work and why
one is of no use without the other. In order to know the vehicles kinematic state
both devices are necessary. The GPS is only able to provide position, while the
IMU provides only acceleration, orientation and turning rates, only by fusing both
of them the vehicles velocity estimation is accurate enough. Furthermore, the
GPS update rate is only 20Hz, while the IMU update rate is 100Hz. Kalman
filter fusion can also be a tool for estimating the vehicles position at the IMUs
update rate.
Chapter 3 describes the general structure of a Kalman filter both from a mathematical as from a control point of view. The Kalman algorithm is presented, a
recursive algorithm consisting on 2 time update equations and 3 measurement update equations. These equations, as well as the Kalman filter inputs and outputs,
are also explained in this chapter.
In Chapter 4 the Kalman algorithm is once more described, but this time for
the particular case of vehicle state estimation using the GPS and IMU measurements as Kalman filter inputs, and obtaining the vehicles position and velocity
as outputs. All matrices and vectors involved in the Kalman algorithm are clearly
deduced for this particular case.
Chapter 5 concerns all the practical work developed for this thesis, this is, working
with the measuring devices and programming the algorithm. The GPS used is
a differential GPS consisting on a rover station fixed on the roof of test vehicle
which receives corrections from a fixed base station. The IMU is also fixed on the
roof of the test vehicle next to the GPS antenna. A C + + implemented program
is in charge of logging the data from both the GPS and the IMU and saving it
on a file. The data is imported from the file to Matlab to be used as Kalman
filter inputs. These inputs have to be modified or adapted to fit the Kalman filter
equations that are programmed in Matlab. A pseudocode of the programmed
filter can also be found in this chapter.
Finally, chapter 6 presents the results obtained. Several test are taken, driving
in different ways to test how well the implemented Kalman filter works. A description of these test is made and a series of graphics show the behavior of the
variables of interest. Among these graphics, the most interesting one is that showing the vehicles position estimation at the IMUs update rate compared with the
iv
GPS position measurement. Another graphic that will have special importance
is the one were the results of the Kalman filter implemented in this thesis are
comparer to the ones obtained from a commercial platform IMAR, which already has an internal Kalman filter implemented. This comparison will serve as
a tool to validate the Kalman filter here implemented, it will be the graphic on
which the conclusions will be based. At the end of the chapter conclusions on how
accurate the measuring devices are and how good the Kalman implementation
is are reflected. Finally a brief note on the future challenges this thesis arises is
made.
Resumen
Sistemas de conduccion asistida estan siendo desarrollados por la mayora de las
marcas automovilsticas con el fin de disminuir los accidentes de trafico. Este
proyecto representa una parte de un proyecto de mayor alcance sobre conduccion
asistida basada en evitar un obstaculo a traves del planeamiento de la trayectoria
del vehculo. Para planear la trayectoria es necesario, a parte del conocimiento
del estado del obstaculo, el estado del vehculo. Este proyecto explicara al lector
como se puede calcular el estado del vehculo de manera precisa y a una frecuencia
de muestreo suficientemente alta (100Hhz). Para conseguir esto, se fusionara a
traves de un filtro Kalman un GPS (Global Positioning System) y un medidor de
inercia (Inertial Measuring Unit IMU).
Tras una introduccion general en el primer captulo, el captulo 2 explica como
los dos aparatos de medida utilizados en el proyecto, GPS e IMU, funcionan
y porque uno necesita del otro para que funcionen de manera correcta. Para
conocer el estado cinematico del vehculo son necesarios ambos aparatos. El
GPS solo proporciona posicion, mientras que el medidor de inercia proporciona
aceleraciones, orientacion y velocidades de giro; solo fusionando ambos aparatos
se podra estimar la velocidad del vehculo de una manera suficientemente precisa.
Por otra parte, la frecuencia de muestreo del GPS solo es 20Hz, mientras que para
el IMU es 100Hz. El filtro de Kalman tambien puede ser una potente herramienta
para estimar la posicion del vehculo a la frecuencia de muestreo del IMU.
El captulo 3 consiste en una descripcion general de la estructura del filtro Kalman
tanto desde un punto de vista matematico, como desde un punto de vista de control. Se presenta el algoritmo de Kalman, un algoritmo recursivo que consiste dos
ecuaciones de actualizacion del tiempo y tres de actualizacion de la medida. Estas ecuaciones, al igual que las variables entrada y salida del filtro, estan tambien
explicadas en este captulo.
En el captulo 4 nuevamente se explica el filtro Kalman, pero esta vez para el caso
particular de estimacion de variables de estado cinematico de un vehculo usando
las medidas del GPS e IMU como entradas del filtro, y obteniendo la velocidad
del vehculo como salida. En este captulo son deducidas todas las matrices y
vectores que aparecen en el algoritmo de Kalman.
En captulo 5 se centra en el trabajo practico realizado para llevar a cabo el
proyecto, principalmente, trabajar con los aparatos de medida y programar el
algoritmo. El GPS usado es diferencial, donde la estacion movil se sit
ua sobre el
techo del vehculo de pruebas y recibe las correcciones de la estacion base (fija).
El IMU tambien esta situado sobre el techo del vehculo, al lado de la antena
del GPS. Un programa implementado en C + + es el encargado de capturar
los datos tanto del GPS como del IMU y guardarlos en un archivo. Los datos
son importados del archivo a Matlab, donde se usaran como entradas del filtro.
Estas entradas han de ser modificadas o adaptadas para encajar en las ecuaciones
de Kalman que seran programadas en Matlab. Tambien sera mostrado en este
captulo un pseudocodigo del programa.
vi
Finalmente, el captulo 6 muestra los resultados obtenidos. Se hicieron una serie
de test, conduciendo de distintas maneras, para comprobar como de bien funciona
el filtro implementado. Se realizara una descripcion de estos test y una serie de
graficos mostraran las variables de interes. Entre estas graficas, la mas interesante
es en la que se ense
na una comparativa entre la posicion del vehculo estimada a
la frecuencia de muestreo del IMU y la medida del GPS. Otra grafica que tiene
especial interes es aquella en la que se comparan los resultados del filtro de Kalman
implementado en este proyecto con los de una plataforma comercial IMAR, que
ya tiene un filtro de Kalman internamente implementado. Esta comparacion
servira como herramienta para validar el filtro de Kalman implementado en este
proyecto, sera la grafica en la que se basen las conclusiones. Al final del captulo
se mostraran las conclusiones del proyecto, como de precisos son los aparatos de
medida, y como de bueno es el filtro Kalman implementado. Finalmente una
breve descripcion de los futuros desarrollos a tener en cuanta es planteada.
vii
CONTENTS
Contents
Part I: Memory
1 Introduction
2 Measuring Devices
2.1
2.2
GPS: Global Positioning System . . . . . . . . . . . . . . . . . . .
2.1.1
Single Point Solution . . . . . . . . . . . . . . . . . . . . .
2.1.2
Differential GPS Solution: DGPS . . . . . . . . . . . . . .
2.1.3
GPS Problems . . . . . . . . . . . . . . . . . . . . . . . .
IMU: Inertial Measurement Unit . . . . . . . . . . . . . . . . . . .
15
2.2.1
Xsens MTi . . . . . . . . . . . . . . . . . . . . . . . . . . .
15
2.2.2
How the IMU Helps Solve the GPS Problems . . . . . . .
17
3 The Kalman Filter
18
3.1
Need for Stochastic Estimation . . . . . . . . . . . . . . . . . . .
18
3.2
Statistic Concepts Needed to Understand Kalman Filters . . . . .
19
3.3
How Kalman Filters Work . . . . . . . . . . . . . . . . . . . . . .
21
3.3.1
Kalman Filter from a Controls Point of View . . . . . . . .
21
3.3.2
Kalman Filter from a Mathematical Point of View
24
. . . .
4 Kalman Filter to Estimate Position
28
4.1
Matrices Ad , Bd . . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
4.2
Matrix C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
33
4.3
Matrix R . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
33
4.4
Matrix Q . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
34
4.5
Initial Conditions + x[k 1] and + P[k 1] . . . . . . . . . . . . .
36
5 Practical Work
5.1
5.2
37
Working with the Measuring Devices . . . . . . . . . . . . . . . .
37
5.1.1
Hardware and Software used . . . . . . . . . . . . . . . . .
38
5.1.2
Setup of Tests . . . . . . . . . . . . . . . . . . . . . . . . .
41
5.1.3
Reference Frames . . . . . . . . . . . . . . . . . . . . . . .
44
Data Logging . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
47
5.2.1
47
Example of Logged Data . . . . . . . . . . . . . . . . . . .
viii
CONTENTS
5.2.2
5.3
Data Logging Pseudocode . . . . . . . . . . . . . . . . . .
49
Fusing the Data . . . . . . . . . . . . . . . . . . . . . . . . . . . .
51
5.3.1
Rotation Matrix RGI . . . . . . . . . . . . . . . . . . . . .
51
5.3.2
Defining the time steps . . . . . . . . . . . . . . . . . . . .
58
5.3.3
Defining Matrix C . . . . . . . . . . . . . . . . . . . . . .
58
5.3.4
Defining Matrix R . . . . . . . . . . . . . . . . . . . . . .
59
5.3.5
Defining Matrix Q . . . . . . . . . . . . . . . . . . . . . .
61
5.3.6
Defining the Initial Conditions + x[k 1] and + P[k 1] . .
62
Kalman Filter Implementation Pseudocode . . . . . . . . .
64
Comparing with IMAR Platform . . . . . . . . . . . . . . . . . .
65
5.3.7
5.4
6 Results
6.1
6.2
6.3
6.4
6.5
70
Test 1. Driving in a Large Circle . . . . . . . . . . . . . . . . . .
70
6.1.1
GPS Inputs . . . . . . . . . . . . . . . . . . . . . . . . . .
70
6.1.2
IMU Inputs . . . . . . . . . . . . . . . . . . . . . . . . . .
72
6.1.3
Kalman Filter Outputs . . . . . . . . . . . . . . . . . . . .
74
6.1.4
Comparing with IMAR Data . . . . . . . . . . . . . . . . .
79
Test 2. Driving in a Small Circle . . . . . . . . . . . . . . . . . .
82
6.2.1
GPS Inputs . . . . . . . . . . . . . . . . . . . . . . . . . .
82
6.2.2
IMU Inputs . . . . . . . . . . . . . . . . . . . . . . . . . .
84
6.2.3
85
6.2.4
88
Test 3. Driving in a Straight Path . . . . . . . . . . . . . . . . . .
91
6.3.1
GPS Inputs . . . . . . . . . . . . . . . . . . . . . . . . . .
91
6.3.2
IMU Inputs . . . . . . . . . . . . . . . . . . . . . . . . . .
92
6.3.3
93
6.3.4
97
Test 4. Driving Accelerating and Decelerating Rapidly . . . . . . 100

6.4.1
GPS Inputs . . . . . . . . . . . . . . . . . . . . . . . . . . 100
6.4.2
IMU Inputs . . . . . . . . . . . . . . . . . . . . . . . . . . 102
6.4.3
Kalman Filter Outputs . . . . . . . . . . . . . . . . . . . . 103
6.4.4
Comparing with IMAR Data . . . . . . . . . . . . . . . . . 106
Test 5. Driving Losing the GPS Signal . . . . . . . . . . . . . . . 109

6.5.1
GPS Inputs . . . . . . . . . . . . . . . . . . . . . . . . . . 109
CONTENTS
6.6
6.7
ix
6.5.2
IMU Inputs . . . . . . . . . . . . . . . . . . . . . . . . . . 110
6.5.3
Kalman Filter Outputs . . . . . . . . . . . . . . . . . . . . 111
6.5.4
Comparing with IMAR Data . . . . . . . . . . . . . . . . . 115
Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
6.6.1
Good GPS signal . . . . . . . . . . . . . . . . . . . . . . . 118
6.6.2
Bad GPS signal . . . . . . . . . . . . . . . . . . . . . . . . 118
Future Challenges . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
References
120
Annexes
121
LIST OF FIGURES
List of Figures
1.1
Kalman Filter Fusion . . . . . . . . . . . . . . . . . . . . . . . . .
2.1
GPS Segments . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2
GPS Triangulation . . . . . . . . . . . . . . . . . . . . . . . . . .
2.3
GPS Error Sources . . . . . . . . . . . . . . . . . . . . . . . . . .
2.4
Differential GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.5
Example of a GPS calculated velocity . . . . . . . . . . . . . . .
10
2.6
Real velocity versus calculated velocity . . . . . . . . . . . . . . .
11
2.7
Example of a GPS calculated acceleration . . . . . . . . . . . . .
12
2.8
Vehicles angles . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13
2.9
Vehicles orientation . . . . . . . . . . . . . . . . . . . . . . . . .
13
2.10 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
15
2.11 Sensor Fusion Algorithm . . . . . . . . . . . . . . . . . . . . . . .
16
3.1
Normal Probability Distribution Function fx (x) . . . . . . . . . .
19
3.2
Autocorrelation X1 , X2 . . . . . . . . . . . . . . . . . . . . . . . .
20
3.3
Autocorrelation of a White Noise Signal . . . . . . . . . . . . . .
20
3.4
Kalman Filter Block Diagram . . . . . . . . . . . . . . . . . . . .
21
3.5
Kalman Filter Algorithm . . . . . . . . . . . . . . . . . . . . . . .
25
3.6
Kalman Filter Block Diagram to observe the effect of matrix C .
27
4.1
Kalman Filter Block Diagram . . . . . . . . . . . . . . . . . . . .
28
5.1
Differential GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . .
38
5.2
GPS Receiver . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
38
5.3
GPS Antenna . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
39
5.4
Radio and Radio Antenna . . . . . . . . . . . . . . . . . . . . . .
39
5.5
IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
40
5.6
DGPS Set Up . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
41
5.7
Base Station . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
42
5.8
GPS and IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
42
5.9
Fixed IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
43
5.10 GPS and IMU position on car . . . . . . . . . . . . . . . . . . . .
43
5.11 GPS reference frame . . . . . . . . . . . . . . . . . . . . . . . . .
44
5.12 IMU reference frame . . . . . . . . . . . . . . . . . . . . . . . . .
45
5.13 Example of roll, pitch and yaw angles . . . . . . . . . . . . . . . .
45
LIST OF FIGURES
xi
5.14 IMAR reference frame . . . . . . . . . . . . . . . . . . . . . . . .
46
5.15 Logging Pseudocode . . . . . . . . . . . . . . . . . . . . . . . . .
50
5.16 Yaw Angle reference frame . . . . . . . . . . . . . . . . . . . . . .
52
5.17 Yaw Angle Estimation . . . . . . . . . . . . . . . . . . . . . . . .
53
5.18 Vehicles orientation . . . . . . . . . . . . . . . . . . . . . . . . .
54
5.19 Path Followed in G . . . . . . . . . . . . . . . . . . . . . . . . . .
54
5.20 Difference of Yaw Angles . . . . . . . . . . . . . . . . . . . . . . .
55
5.21 Path Followed in G . . . . . . . . . . . . . . . . . . . . . . . . . .
56
5.22 Difference of Yaw Angles . . . . . . . . . . . . . . . . . . . . . . .
57
5.23 GPS Position Errors in G . . . . . . . . . . . . . . . . . . . . . .
60
5.24 Acceleration Errors in I . . . . . . . . . . . . . . . . . . . . . . .
62
5.25 Kalman Filter implementation Pseudocode . . . . . . . . . . . . .
64
5.26 Coordinate conversion psuedocode . . . . . . . . . . . . . . . . . .
65
5.27 Earth Globe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
66
5.28 Local and Global Co-ordinates . . . . . . . . . . . . . . . . . . . .
67
5.29 R calculation . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
68
6.1
Path Followed . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
71
6.2
Horizontal Dilution of Precision . . . . . . . . . . . . . . . . . . .
71
6.3
Yaw Angle in I and G . . . . . . . . . . . . . . . . . . . . . . . .
72
6.4
Acceleration in x Direction in I and G . . . . . . . . . . . . . . .
73
6.5
Acceleration in y Direction in I and G . . . . . . . . . . . . . . .
73
6.6
x Position Measurement and Estimation . . . . . . . . . . . . . .
75
6.7
y Position Measurement and Estimation . . . . . . . . . . . . . .
76
6.8
Vehicle Kinematic Parameters in x Direction . . . . . . . . . . . .
77
6.9
Vehicle Kinematic Parameters in y Direction . . . . . . . . . . . .
78
6.10 x Position from IMAR and Estimation . . . . . . . . . . . . . . .
80
6.11 y Position from IMAR and Estimation . . . . . . . . . . . . . . .
81
6.12 Path Followed . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
82
6.13 Horizontal Dilution of Precision . . . . . . . . . . . . . . . . . . .
83
6.14 Yaw Angle in G . . . . . . . . . . . . . . . . . . . . . . . . . . . .
84
6.15 Acceleration in x Direction in G . . . . . . . . . . . . . . . . . . .
84
6.16 Acceleration in y Direction in G . . . . . . . . . . . . . . . . . . .
84
6.17 x Position Measurement and Estimation . . . . . . . . . . . . . .
85
6.18 y Position Measurement and Estimation . . . . . . . . . . . . . .
86
LIST OF FIGURES
xii
6.19 Vehicle Kinematic Parameters in x Direction . . . . . . . . . . . .
87
6.20 Vehicle Kinematic Parameters in y Direction . . . . . . . . . . . .
88
89
90
6.23 Path Followed . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
91
6.24 Horizontal Dilution of Precision . . . . . . . . . . . . . . . . . . .
92
6.25 Yaw Angle in G . . . . . . . . . . . . . . . . . . . . . . . . . . . .
92
6.26 Acceleration in x Direction in G . . . . . . . . . . . . . . . . . . .
93
6.27 Acceleration in y Direction in G . . . . . . . . . . . . . . . . . . .
93
6.28 x Position Measurement and Estimation . . . . . . . . . . . . . .
94
6.29 y Position Measurement and Estimation . . . . . . . . . . . . . .
95
6.30 Vehicle Kinematic Parameters in x Direction . . . . . . . . . . . .
96
6.31 Vehicle Kinematic Parameters in y Direction . . . . . . . . . . . .
97
98
99
6.34 Path Followed . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100

6.35 Horizontal Dilution of Precision . . . . . . . . . . . . . . . . . . . 101
6.36 Yaw Angle in G . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
6.37 Acceleration in x Direction in G . . . . . . . . . . . . . . . . . . . 102
6.38 Acceleration in y Direction in G . . . . . . . . . . . . . . . . . . . 102
6.39 x Position Measurement and Estimation . . . . . . . . . . . . . . 103
6.40 y Position Measurement and Estimation . . . . . . . . . . . . . . 104
6.41 Vehicle Kinematic Parameters in x Direction . . . . . . . . . . . . 105
6.42 Vehicle Kinematic Parameters in y Direction . . . . . . . . . . . . 106
6.43 x Position from IMAR and Estimation . . . . . . . . . . . . . . . 107
6.44 y Position from IMAR and Estimation . . . . . . . . . . . . . . . 108
6.45 Path Followed . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109
6.46 Horizontal Dilution of Precision . . . . . . . . . . . . . . . . . . . 110
6.47 Yaw Angle in G . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
6.48 Acceleration in x Direction in G . . . . . . . . . . . . . . . . . . . 111
6.49 Acceleration in y Direction in G . . . . . . . . . . . . . . . . . . . 111
6.50 x Position Measurement and Estimation . . . . . . . . . . . . . . 112
6.51 y Position Measurement and Estimation . . . . . . . . . . . . . . 113
6.52 Vehicle Kinematic Parameters in x Direction . . . . . . . . . . . . 114
LIST OF FIGURES
xiii
6.53 Vehicle Kinematic Parameters in y Direction . . . . . . . . . . . . 115

6.54 x Position from IMAR and Estimation . . . . . . . . . . . . . . . 116
6.55 y Position from IMAR and Estimation . . . . . . . . . . . . . . . 117
LIST OF TABLES
xiv
List of Tables
3
Logged data . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
48
NOTATION
xv
Reference Frames
Notation
G:
6
- (G*,
I: (I*,
6
-
ex ,
ey ,
ez )
ex , I ey , I ez )
E
6
-: (I*,
6
P
-:(P*,
M:(M*,
6
-
Description
ex ,
er ,
er ,
ey ,
e ,
e ,
GPS reference frame

IMU reference frame
ez )
Earth reference frame
e )
IMAR Platform reference frame
e )
IMAR with GPS base origen reference frame
Symbols
Symbol
Unit
Description
rn
(x, y, z)
(xn , yn , zn )
c
b
0
x
y
z
x,a
y,a
z
t
DOP
V DOP
HDOP
P DOP
T DOP
GDOP
rk
vk1,k
ak1,k
m
m
m
m/s
m
m
m
m
m
m
m
m
m
m
m
m/s
m/s2
pseudorange, distance from satellite n to GPS user

Cartesian position of GPS user
Cartesian position of GPS satellite n
speed of light
pseudorange bias
standard deviation
pseudorange standard deviation
longitudinal position standard deviation
lateral position standard deviation
vertical position standard deviation
longitudinal acceleration standard derivation
lateral acceleration standard deviation
vertical position standard deviation
time standard deviation
Dilution of Precision factor
Vertical Dilution of Precision
Horizontal Dilution of Precision
Positional Dilution of Precision
Time Dilution of Precision
Geometric Dilution of Precision
position at step k
velocity from step k-1 to k
acceleration from step k-1 to k
direction of velocity
sideslip angle
o
o
NOTATION
xvi
Symbol
Unit
Description
fx (x)
2
X
N (, 2 )
Rx ( )
E(x)
x[k]
x(t)
x(t)
x [k]
x[k]
+
x[k]
u(t)
u[k]
y(t)
y[k]
w(t)
v(t)
(w)
(v)
A
yaw angle
roll angle
pitch angle
probability density function
mean
variance
random variable
normal probability of mean and variance 2
time increment
covariance matrix
expected value of x
estimated state at step k
estimated state at time t
derivative of state x(t) at time t
real state at step k
a priori state estimate at step k
a posteriori state estimate at step k
input at time t
input at step k
measurement at time t
measurement at step k
process noise
measurement noise
process noise probability distribution
measurement noise probability distribution
matrix that relates a state x(t) with its derivative
x(t)
matrix that relates previous state x[k 1] with current state x[k]
matrix that relates the input u(t) with derivative of
the state x(t)

matrix that relates the current input u[k] with current state x[k]
matrix that relates the current state x(t) with the
measurement
process noise covariance matrix
measurement noise covariance matrix
a priori estimate error
a posteriori estimate error
a priori estimate error covariance matrix
a posteriori estimate error covariance matrix
Kalman gain matrix
acceleration vector measured by IMU
o
o
Ad
Bd
Q
R
e[k]
+
e[k]
P[k]
+
P[k]
K[k]
U
aIM
m
m/s2
NOTATION
xvii
Symbol
Unit
Description
S
rGP
m
rsens
xsens
ysens
x sens
ysens
xsens
ysens
U
ax,m aIM
x,m
IM U
ay,m ay,m
ax,bias
ay,bias
xGP S
y GP S
xIM AR
y IM AR
Xr,x
m
m
m
m
m/s
m/s
m/s2
m/s2
m/s2
m/s2
m/s2
m/s2
m
m
m
m
Xr,y
Xa,x
Xa,y
RGL
Rz
Ry
Rx
G
E
E G
R
R
latk
lonk
pd
Gd
P
G
P G
position vector measured by GPS

position vector at sensor (IMU) location
x position at sensor (IMU) location
y position at sensor (IMU) location
longitudinal velocity at sensor (IMU) location
lateral velocity at sensor (IMU) location
longitudinal acceleration at sensor (IMU) location
lateral acceleration at sensor (IMU) location
longitudinal acceleration measured by IMU
lateral acceleration measured by IMU
longitudinal acceleration bias (offset)
lateral acceleration bias (offset)
x position measured by GPS
y position measured by GPS
x position measured by the IMAR platform
y position measured by the IMAR platform
variable that describes the longitudinal position
measurement noise
variable that describes the lateral position measurement noise
variable that describes the longitudinal acceleration
measurement noise
variable that describes the lateral acceleration measurement noise
rotation matrix from local to global co-ordinate system
rotation matrix around z axes
rotation matrix around y axes
rotation matrix around x axes
yaw angle measured in G
yaw angle measured in E
yaw angle going from E to G
Earth radius
radius of the parallel where the GPS base is fixed
latitude of measurement k
longitude of measurement k
distance from vehicle to GPS base in P
distance from vehicle to GPS base in G
heading in P
heading in G
angle going from P to G
o
o
o
m
m
m
m
m
m
o
o
o
PART I: MEMORY
Introduction
Introduction
The incredible high number of traffic accidents in every day life is one of the main
concerns in our modern society. Statistics show that over 90% of car accidents are
due to human errors [K
uhnen 1995]. Traffic authorities are continuously throwing
strong campaigns that cause great impact to try to make people more conscious
of the dangers on the road and how important it is to drive savely. As for vehicle
development, the only thing that can be done is try to somehow help the driver
avoid these mistakes that lead to dangerous situations. Many vehicle brands
(BMW, VW, Mercedes) are investigating on the possibility of guiding the driver
on some level, this is what is known as driver assistance systems.
There are many types of driver assistance systems, the one studied in this project
consist of a control system which, considering the environment surrounding the
vehicle and its kinematics state, calculates the path the vehicle should follow to
avoid the environmental obstacles. Once the path is calculated, the path following
algorithm is in charge calculating the value of the torque that should be applied
to steering wheel in order to follow the path correctly. This algorithm calculates
the difference between the desired torque and the one the driver is applying. The
difference of torques provides the absolute value that is sent to the steering wheel,
helping the driver notice what he should be doing. It is named driver assistance
and not autonomous driving because it is always the driver that is in control.
The car will move following the drivers orders, but the driver will be able to
sense on the wheel if he is doing something wrong. If the driver turns the steering
wheel the same angle as the calculated one, he will not sense anything, but if he
turns more or less than the optimal angle, he will feel a small torque indicating
he should turn a bit more to the left or right.
To calculate the path the vehicle is supposed to follow, all environmental obstacles
are given a high potential value, while all safe driving spaces have a low potential
value. This way a potential field hazard map is given for every time instant. The
path is modeled as an elastic band; the potential acts as an external force on the
elastic band, forcing it to drive through low potential areas, safe driving spaces.
Information on environmental obstacles are given by sensors such as radar, lidar
and video cameras while information on the vehicle is given by sensors such as
GPS, inertial measuring units, wheel speed sensors and engine speed sensors
[Thomas Sattel 2005].
This project handles the part in charge of providing the vehicles position, velocity, acceleration and orientation. These variables are needed as an input for
the path following algorithm. To achieve this goal two sensors, a GPS and an
inertial measuring unit IMU are fused by a Kalman filter. A GPS is able to
provide position coordinates, while an inertial measuring unit provides acceleration, turning rates and orientation angles, but none of the two measuring devices
provides velocity directly. In addition, the GPS update rate is too low (20Hz)
for the update rates needed for the path following controller, while the inertial
measuring units update rate is high enough (100Hz).
Introduction
A Kalman filter is a powerful tool for sensor fusion; it can estimate certain parameters based on an input related to these parameters. In this case it is used
to estimate position and velocity based on acceleration (see figure 1.1), relating them both by integrating the acceleration once or twice, respectively. This
way the position and velocity are known at the inertial measuring units update
rate and every time a GPS position measurement is made, it is compared to the
estimation to correct the estimation errors.
Furthermore, the GPS measurements depend on the quality of the signal; they
are not always as accurate as they should. The Kalman filter also deals with this
problem. In addition to the acceleration input; there are two other inputs, the
process noise, which indicates how good the estimation is, and the measurement
noise, which indicates how good the GPS measurement is. The process noise is
represented by the process noise covariance matrix Q, which depends on the IMU
acceleration errors. The measurement noise is represented by the measurement
noise covariance matrix R, which depends on the GPS measurement errors. The
Kalman gain works as a weight factor based on these two noise inputs to keep
the optimal value of the estimated parameters. If the measurement noise is really
low, the value kept is the GPS measurement, if on the contrary, the signal is very
bad, the measurement noise will be very high and the value kept will be closer to
the estimation.
Figure 1.1 shows the basic scheme that is used. The IMU provides acceleration,
orientation and the process noise covariance matrix Q at 100Hz. The GPS provides position and the measurement noise covariance matrix R at 20Hz. The
Kalman fusion provides position and velocity at 100Hz.
Figure 1.1: Kalman Filter Fusion
Measuring Devices
Measuring Devices
This chapter is going to introduce the reader to the two measuring devices used,
the GPS and the IMU. A description of what they are, how they work and their
different operation modes will be made. Also an evaluation of what problems
each device may present and how they could be solved will be made.
2.1
GPS: Global Positioning System
In our modern society, GPS have acquired great popularity, especially in navigation systems, the information it provides is well known: position coordinates.
But what may not be so well known is how exactly it works. Being one of the
two measuring devices involved in this project, a brief description of each of the
possible operating modes a GPS can adapt and how they work will be made.
Finally some problems a GPS may not be able to deal with will be analyzed.
2.1.1
Single Point Solution
As can be seen in figure 2.1 there are three basic segments in a GPS system, the
space segment, the control segment and the user segment.
Figure 2.1: GPS Segments

The space segment, consisting of a constellation of satellites interacts with the
control segment, that tracks the satellites and updates their orbit information.
The user is a completely passive observer of the GPS signal sent from the space
station. How the users position is calculated is basic triangulation as can be seen
in figure 2.2 [Carlson 2004].
Measuring Devices
Figure 2.2: GPS Triangulation

The position of the user depends on the distance from the user to the satellite,
this distance is named pseudorange (r), being for every satellite n
rn =
p
(xn x)2 + (yn y)2 + (zn z)2 ,
(2.1)
where
rn = c t,
(2.2)
being c the speed of light, c 3 108 m/s.
To obtain 3 dimension coordinates (x,y,z) three satellites should be enough, the

problem is that very accurate time measurements are needed. Satellites use Cesium and Rubidium clocks which are very precise drifting up to 1010 ms per day,
but users use crystal clocks, which are not so precise, drifting about a ms per day.
This problem may easily be solved including a common bias for each pseudorange
rn =
p
(xn x)2 + (yn y)2 + (zn z)2 b,
a fourth satellite is needed to calculate the bias b.
(2.3)
Measuring Devices
2.1.2
Differential GPS Solution: DGPS
In theory the method seems perfect, but in practice it is not very precise, obtaining
accuracies with the most modern GPS receivers of about 2 meters [OEM ],p.64.
With that kind of error the measurement can not be considered reliable (at least
for collision avoidance purposes), research was made to know why there were such
errors, and how to correct them.
Figure 2.3 illustrates the principle source errors.
Figure 2.3: GPS Error Sources

These errors were found to be [Carlson 2004]:
Ionosphere and Troposphere lengthen the GPS signal path (figure 2.3 left)
reflected GPS signals have longer paths than direct satellite-GPS signals
(figure 2.3 middle)
satellites that are very close together and all oriented to the GPS user from
the same angle give a worse solution than those that are spread wider apart
(figure 2.3 right)
Measuring Devices
The best solution to avoid these errors is provided by a differential GPS (DGPS).
As can be seen in figure 2.4 in a DGPS there are two GPS antennas receiving
position data and are able to correct each other.
Figure 2.4: Differential GPS

Normally there will be a base station, this is, an antenna located in a known
position that does not move and a rover station, this would be the antenna located
on the car that does move. As the base station knows its position, it is able to
calculate the GPS signal errors (the difference between its known, real, position
and the supposed position the GPS is saying it has) so it can send the rover
station correction data on its position based on this difference. The accuracy
with this operation mode can go up to 2 cm within 200 m away from the base
station [OEMapage81].
Another method to obtain a differential GPS solution can be using a virtual
base station consisting of a mobile phone cell, instead of using a second GPS
antenna. This method consist on, through a GSM modem, making a call to a
phone company (ASCOS or SAPOS for Germany) sending our position. The
phone company can also see the position where the call was made from, so by
calculating the difference from the position sent and the one that the phone
company sees, the error can be calculated and the adequate correction data can
be sent back.
A wider description on working with the GPS and the different operating modes
(single point solution, DGPS using a real base station and DGPS using a virtual
base station) can be seen in Apendix1
Dilution of Precision DOP Factors
A parameter that can quantify the precision of the solution is the standard deviation of the measurements. The standard deviation of the measurement
depends on the pseudorange (distance from the satellites to the GPS antenna)
errors as well as on the geometry of the satellite constellation. The standard deviations of the pseudoranges 0 give a measurement that describes the geometry of
the satellites, they depend on the number of satellites in GPS view and their position with respect to each other and the GPS antenna. The physical meaning of the
Measuring Devices
DOP factors would be by what factor will the initial error (pseudorange error) be
multiplied to obtain the final error (position error), for the standard deviation of
the position measurement will be [Clynch 2006],p.3[Roger C. Hayward 1998],p.6
= 0 DOP.
(2.4)
The smaller the DOP factor is, the better the solution will be.
There are different DOP factors depending on the kind of error they want to
quantify [Clynch 2006],p.3[Roger C. Hayward 1998],p.6:
VDOP: Vertical Dilution of Precision
V DOP = z /0
(2.5)
HDOP: Horizontal Dilution of Precision

q

2
2
x + y /0
HDOP =
(2.6)
PDOP: Positional Dilution of Precision

P DOP =
q
x2
y2
z2
/0
(2.7)
TDOP: Time Dilution of Precision

T DOP = t /0
(2.8)
GDOP: Geometric Dilution of Precision

GDOP =
q
x2 + y2 + z2 + t2 c2 /0
(2.9)
For a DGPS the standard deviation of the pseudoranges 0 will be much smaller
than for a single point solution, so the standard deviation of the measurements
will also be smaller. Also notice that for a DGPS the DOP factors can be smaller
than 1 since the GPS corrections can make measurements error be even smaller
than the initial pseudorange errors.
Measuring Devices
2.1.3
GPS Problems
It may seem that a DGPS solution is a good measurement for our purpose, to
know the vehicles position, but it is not as simple as that. To know the vehicles
position is the main goal, but the knowledge of the vehicles velocity and acceleration is also important, as well as its yaw angle (orientation of the vehicle).
Another important issue is the update rate of these variables. Remember that
these measurements work as an input to calculate the trajectory of the car in
order to avoid the obstacle, therefore, the update rate needs to be high enough to
fit this use. Finally one more problem appears: The GPS measurements depend
on the quality of the signal, if the signal is not good, or it does not even exist
(driving though a forest for example) the measurements will not be at all reliable,
even when working with a DGPS.
A more detailed description of each problem will now be made to understand why
exactly they are problems and how they may be solved.
1. Knowledge of the vehicles velocity
GPS can provide a vehicles velocity, but it is calculated by difference of
position (distance traveled) in a period of time.
vk1,k =
rk rk1
.
t
(2.10)
This assumption is not very good, it only considers the case of constant
velocity during the period of time and the calculated velocity is always that
of the previous time increment, but not of the current time, there is a delay.
Figure 2.5 illustrates this problem. The first graphic of the figure shows
an example of a possible trajectory that could describe a vehicle. This
trajectory has been modeled by the polynomial function
s(t) = 0.06t5 1.66t4 + 16.44t3 68.55t2 + 106t.

The vertical axis represents the position of the vehicle, the co-ordinate system to which it is referenced is not important, it could be x, y or a combination of both.
The real velocity is represented by the derivative of the position, the
derivative of s(t), which is plotted in the second graphic of figure 2.5.
The third graphic of the figure represents the velocity calculated by equation
2.10 supposing the GPS samples were taken every second.
Measuring Devices
Figure 2.5: Example of a GPS calculated velocity
10
Measuring Devices
11
In figure 2.6 the real and the calculated velocity can be both seen in the
same graphic. As said before, the calculated velocity only considers the
case of constant velocity, when it is so, both graphics are very similar (for
example between 5 and 6 seconds). As the real velocitys slope increases,
the calculated velocitys error becomes higher.
Figure 2.6: Real velocity versus calculated velocity

2. Knowledge of the vehicles acceleration
The problem is the same as in the case of the vehicles velocity, the acceleration may be calculated by difference of velocity in a period of time, but
assuming the acceleration is constant.
ak1,k =
rk 2 rk1 + rk2
vk1,k -vk2,k1
=
.
t
t2
(2.11)
Figure 2.7 represents the real and calculated acceleration for the same
example polynom used in the previous section. It can be seen how if the
real accelerations slope is closer to zero, the similar the curves are.
Measuring Devices
Figure 2.7: Example of a GPS calculated acceleration
12
13
Measuring Devices
3. Knowledge of vehicles orientation

Just like the velocity, the orientation of this velocity can be calculated by
the trigonometric relations of the position, as can be seen in figure 2.8.
Figure 2.8: Vehicles angles

When referred to a global fixed coordinate system, the orientation of the
velocity can be expressed as
k = tan1
yk yk1
.
xk xk1
(2.12)
The main problem is that this is not the orientation of the vehicle , but
the orientation of velocity . In order to know the orientation of the vehicle
two GPS antennas would be needed. As can be seen in figure 2.9 the
vector going from one antenna to the other can be used to measure angles
in a certain reference frame.
Figure 2.9: Vehicles orientation
Measuring Devices
14
4. GPS update rate too slow

The update rate of the GPS that is going to be used is 20Hz, there is a
new measurement every 50ms. It may seem high enough, but remember
that when talking about vehicles and trying to avoid accidents, 50ms may
be too much. Think of how a vehicle moving at a a constant speed of
72km/h, 20m/s, in 50s it travels a distance of 1m, quite a long distance
for such a short period of time! If the update rate were to be 100Hz (new
measurement every 10ms), which is the goal for this project, the distance
traveled would have only been of 20cm.
To have a high update rate is also important to avoid stability problems
in the path following controller algorithm. This algorithm is programmed
assuming the input variables are continuous functions. If the update rate is
high enough, this assumption can be considered correct, but if on the contrary, it is too low, the path following controller may not work adequately,
even instabilities may appear.
5. GPS measurements depend on the quality of the signal
As seen before there were many factors that could influence the quality of
the signal. The main three were: Troposphere and Ionosphere lengthening
the signal, obstacles between the satellites and the user, and a bad satellite
constellation geometry.
A DGPS can help drastically to decrease these errors, but still there can be
problems due mainly to obstacles. When using a DGPS, the base station
can calculate the measurement errors and send them to the rover station,
but of course, both stations need a GPS signal to work with. The base
station will be located in a place were there are no obstacles (open sky
above the GPS antenna) but the rover station is located on top of the
vehicle, which is moving. The rover stations antenna can lose the GPS
signal when it goes under a bridge or in a forest for example. Therefore
no GPS position solution will be obtained. Furthermore both stations may
have a good GPS signal but there may be an obstacle in between the base
and the rover station, the corrections will not be sent, so there will be no
DGPS solution.
15
Measuring Devices
2.2
IMU: Inertial Measurement Unit
As indicated by its name, an Inertial Measuring Unit IMU is a measuring device

able to measure inertial data such as accelerations and turning rates.
2.2.1
Xsens MTi
The IMU that will be used for this project is the Xsens MTi - miniature gyroenhanced Attitude and Heading Reference Sensor (figure 2.10). The MTi can accurately calculate absolute real-time driftless orientation as well as calibrated acceleration, rate of turn and earth-magnetic field data in three-dimensional space.
Figure 2.10: IMU

As seen in figure 2.11 to provide these measurements it internally contains three
miniature 3D gyroscopes (rate of turn sensors), accelerometers and magnetometers. The accelerometers and magnetometers are very accurate since they can
correct their errors thanks to the knowledge of a reference value: gravity for
the accelerometers and the magnetic north for the magnetometers. By fusing all
three measuring devises, a control algorithm can use the accelerometers and magnetometers corrections to also correct the gyroscopes errors obtaining driftless
orientation data.
Measuring Devices
16
Figure 2.11: Sensor Fusion Algorithm

All these measurements can be obtained at an update rate up to 120Hz [MTi ],p.7
but since the default update rate value (the one it normally operates with) is
100Hz, this is the update rate that will be used.
The accuracy the Xsens MTi can provide is:
pitch and roll angles: less than 0.5o [MTi ],p.7
yaw angle: less than 1o [MTi ],p.7
rate of turn: less than 5deg/s [MTi ],p.12
acceleration: less than 0.02m/s2 [MTi ],p.12
magnetic field: less than 0.5mGauss [MTi ],p.12
Measuring Devices
2.2.2
17
How the IMU Helps Solve the GPS Problems
Now that a brief introduction on the IMU has been made, a description of how
all the problems mentioned in section 2.1.3 can be solved by fusing a GPS and
an IMU with a Kalman filter.
Remember there where 5 basic problems the GPS could not deal with:
1. Knowledge of the vehicles velocity
2. Knowledge of the vehicles acceleration
3. Knowledge of vehicles orientation
4. GPS update rate too slow
5. GPS measurements depend on the quality of the signal
The IMU itself, without the help of any fusion, can solve problems 2 and 3,
knowledge of the vehicles acceleration and orientation; fixing the IMU on the
vehicle it can directly measure its acceleration and its orientation, which will
be the yaw angle. As for the other three problems a Kalman filter must be
implemented.
As was briefly mentioned in the introduction, and will be seen in detail in chapter
3, the Kalman filter is able to estimate position and velocity based on the acceleration measurements. The position and velocity estimations can be updated
every time there is a new acceleration measurement. Comparing the position
estimation with a real GPS measurement when it is available, the estimation can
be corrected. The IMU has a much higher update rate than the GPS, 100Hz
compared to the 20Hz of the GPS, this means that the position update rate has
increased by a factor of 5.
By estimating the vehicles position the other three problems are implicitly being
solved. The knowledge of the velocity is not anymore calculated by the difference
of position in a period of time, but estimated. The update rate has been increased
by a factor of five, not only for the velocity estimation, but also for the acceleration
and orientation of the vehicle since now they are a direct IMU output, whose
update rate is higher. As for the dependence of the quality of the GPS signal
for the GPS measurements, the ability of the Kalman gain to work as a weight
factor assures that the best value (between the estimation and the measurement)
is kept; this way if the measurement is very bad due to a bad quality signal, the
estimation will be kept.
The Kalman Filter
18
The Kalman Filter
The Kalman filter, just like any other type of filter estimates a value of the variable
of interest as accurately as possible. This filter combines a set of mathematical
equations forming an optimal recursive data processing algorithm. Its main use
is providing stochastic estimation from noisy sensor measurements.
3.1
Need for Stochastic Estimation
Deterministic models are those that describe the behavior of a physical process
determining the values of certain variables. Stochastic models are those that do
not determine the value of the variable but the probability of that value to be, they
estimate states, they do not calculate values. Most physical systems are usually
described with deterministic models, much more simple than the stochastic ones,
and mainly because they are used so often, they are assumed to be correct.
Considering that neither deterministic nor stochastic models can strictly be the
same as the given physical system we ought to agree that many times, when the
deterministic models has an error that can be considered acceptable, a stochastic
model is not worth the effort.
On the other hand, deterministic models have several problems [Maybeck 1979]:
Only those effects of interest are taken into account.
No model can be perfect when certain effects are left out, in addition to
the fact that those variables of interest, the ones that really are modeled,
are just approximated by mathematical equations.
The only data available are the self-controlled inputs and the sensor measured outputs of the system.
Deterministic models cannot control the other inputs, those in the form of
disturbances.
Sensors used to measure the outputs cannot provide perfect readings.
Due to all these accuracy problems, sometimes there is a need to use stochastic
estimations, this is, using all the possible data provided by the sensors, and considering the noisy nature of these measurements, predict a state for the variables
of interest. In the case of knowledge of a vehicles position based on the IMU
and GPS measurements a stochastic model is needed, since the readings of these
measuring devices are not very accurate.
19
The Kalman Filter
3.2
Statistic Concepts Needed to Understand Kalman Filters
In order to understand how Kalman filters work, there are some statistic concepts
that need to be described. The Kalman filter has some noise inputs which will be
assumed to be white noise with a normal probability distribution. The probability
density function fx (x) of a normally distributed random variable X with mean
and variance 2 , X N (, 2 ), is calculated as [Greg Welch 2001][Maybeck 1979]
fx (x) =
1
2 2
2
1 (x)
2
e 2
(3.1)
The shape of this function can be seen in figure 3.1
Figure 3.1: Normal Probability Distribution Function fx (x)

For the particular case of Kalman filter estimation, the noise variables will have
mean zero, = 0 and the variance 2 will be expressed in terms of the covariance
matrix Rx ( ). A covariance matrix describes how a variable is autocorrelated
with itself over time. The autocorrelation of a random signal X(t) is defined as
Rx (t1 , t2 ) = E[X(t1 ) X(t2 )],
(3.2)
being E(x) the expected value of the random variable X
E(X) =
xfX (x)dx.
(3.3)
If the process is stationary (density function invariant with time) the autocorrelation depends only on the time difference = t1 t1 , the autocorrelation can
therefore be written as
Rx ( ) = E[X(t) X(t + )].
(3.4)
The Kalman Filter
20
In figure 3.2 an example of the autocorrelation of two random signals X1 and

X2 is shown. X1 is shorter an wider than X2 which indicates that X2 is less
correlated with itself than X1
Figure 3.2: Autocorrelation X1 , X2

When a random signal is completely uncorrelated with itself it is defined as white
noise. The autocorrelation function of a white noise signal is a dirac delta ( )
which is zero everywhere except at = 0 where it takes a value of A, as can be
seen in figure 3.3.
Figure 3.3: Autocorrelation of a White Noise Signal
The Kalman Filter
3.3
21
How Kalman Filters Work
As previously said, the Kalman filter is an optimal recursive data processing

algorithm used for providing stochastic estimations, it is a tool that processes all
available data from diverse measuring devices with a recursive algorithm (current
state depending on previous state) to obtain an optimal (minimum error) solution.
In order to obtain this optimal solution it needs to have knowledge of the nature
of the measuring devices, this is, the statistical description of measurement noise,
as well as knowledge of the process noise (how good the stochastic model is), and
the initial conditions.
3.3.1
Kalman Filter from a Controls Point of View
Figure 3.4 illustrates the basic structure of a Kalman filter. The plant represents
the real process where an input u(t) generates an output y(t). The observer is
a discrete model of the plant which will provide an estimation of a state vector
x(t). The state x(t) must contain at least the output y(t), though other variables
may also be part of the state vector.
Figure 3.4: Kalman Filter Block Diagram
22
The Kalman Filter
To obtain a model of the plant knowledge of the relationship between the input u(t) and the output y(t) is necessary. This relation can be described by
[Greg Welch 2001]
x(t)
= A x(t) + B u(t) + w(t),
(3.5)
being A the matrix relates the state x(t) to its derivative x(t)
and B the matrix
that relates the input u(t) to the derivative of the state x(t).
The variable w(t) represents the process noise. It is assumed to be white and
with a normal probability distribution
p(w) N (0, Q) ,
(3.6)
Where Q is the process noise covariance matrix.
Integrating the derivative of the state x(t)

the current state x(t) can be obtained.
The relationship between the output y(t) and the state x(t) is described by
y(t) = C x(t) + v(t),
(3.7)
where matrix C relates the state to the measurement. If there is no measurement

available C will be an empty matrix.
The variable v(t) represents the measurement noise. Like the process noise it is
assumed to be white and with a normal probability distribution
p(v) N (0, R) ,
(3.8)
being R the measurement noise covariance matrix.

The mathematical equations that govern the process have just been explained,
but the observer is a electronic model of the plant, a programmed algorithm,
and as any electronic tool it may only function with discrete time samples. This
discretization does not influence the structure of the equations, however it does
change matrices A and B to the discrete time matrices Ad and Bd . These matrices
can be calculated by [Ryu 2004]
Ad = eAK =
X
An K n
n=0
n!
(3.9)
The Kalman Filter
ZK
X
An K n+1
A
e dB =
Bd =
B.
(n
+
1)!
n=0
23
(3.10)
Matrix C does not change, the relation between the measurement and the state
is the same.
Obtaining now the discrete time equations for the observer:
x[k] = Ad x[k 1] + Bd u[k]
(3.11)
y[k] = C x[k]
(3.12)
Notice that the observer, being an electronic model has no noise. However the
noise will influence when calculating the Kalman gain K as will be seen in the
ongoing paragraphs.
Defining a a priori state estimate x[k], the state thought more likely to be,
considering the process prior to time step k and a a posteriori state estimate
+
x[k], the state more likely to be, after knowing the measurement y[k] we can
define a a priori and a a posteriori estimate error
e[k] = x [k] x[k]
(3.13)
e[k] = x [k] + x[k]
(3.14)
Where x represents the real state
The Kalman Filter
24
The a priori and a posteriori error covariance matrices are
P[k] = E[ e[k] e[k]T ]
(3.15)
P[k] = E[+ e[k] + e[k]T ]
(3.16)
The residual is defined as the difference between the measurement and the a priori
state estimate (y[k] C x[k]), the Kalman gain K works as a weight factor
minimizing the a posteriori error estimate covariance, modeling the a posteriori
state as a linear combination of a a priori and the residual
x[k] = x[k] + K[k] (y[k] C x[k])
(3.17)
The value of K that minimizes a posteriori error estimate covariance can be calculated considering the equations above and it is known to be [Greg Welch 2001],p.3:
P[k] CT
C P[k] CT + R
K[k] =
3.3.2
(3.18)
Kalman Filter from a Mathematical Point of View
A Kalman filter is recursive; it uses feedback to update itself as can be seen in

figure 3.5. This algorithm consists on 2 time update equations and 3 measurement
update equations.
The time update equations are responsible for the predicting a priori states and
error covariances
x[k] = Ad + x[k 1] + Bd u[k]
(3.19)
P[k] = Ad + P[k 1] ATd + Q
(3.20)
The Kalman Filter
25
The measurement update equations are responsible for obtaining the a posteriori
state estimate
P[k] CT
C P[k] CT + R
K[k] =
(3.21)
x[k] = x[k] + K[k] (y[k] C x[k])
(3.22)
P[k] = (I K[k] C) P[k]
(3.23)
Figure 3.5: Kalman Filter Algorithm
The Kalman Filter
26
Now that the algorithm has been presented it is easier to understand how the
Kalman gain works as a weight factor, as can be seen in equation 3.21 when the
process noise covariance matrix R tends to zero
lim K[k] =
R[k]0
1
C
(3.24)
the a posteriori estimate, equation 3.22, will be
x[k] = x[k] +
1
(y[k] C x[k]) = y[k].
C
(3.25)
On the other hand when the a priori error estimate covariance P [k] tends to
zero
lim K[k] = 0
P [k]0
(3.26)
the a posteriori estimate will be
x[k] = x[k] + 0 = x[k].
(3.27)
In conclusion, when the process noise covariance R approaches zero the actual
measurement y[k] is more liable than the predicted state while when the a priori
error estimate covariance P [k] tends to zero it is better to trust the predicted
measurement.
The Kalman Filter
27
After seeing the Kalman algorithm equations, taking another look at the Kalman
filter block diagram (figure 3.6) can help understand the process better how matrix
C influences the process depending on if it is empty or not.
Figure 3.6: Kalman Filter Block Diagram to observe the effect of matrix C
Observe how if there is no measurement available (C is an empty matrix, see
section 4.2) the a posteriori state estimate is equal to the a priori state estimate,
but once a measurement is available, the Kalman gain acts as a weight factor,
correcting the estimate by this now known value.
Kalman Filter to Estimate Position
28
In this chapter the Kalman algorithm will be described for the particular case of
vehicle state estimation. As seen in the introduction the inputs will be the IMU
U
acceleration measurement aIM
(t) and process noise covariance matrix Q, the
m
GP S
GPS position measurement rm (t) and measurement noise covariance matrix R
and the initial conditions x[k 1].
Figure 4.1 shows the Kalman filter block diagram for this particular case.
Figure 4.1: Kalman Filter Block Diagram

The general Kalman filter equations are once more shown, to know which variables
and matrices are needed in order to process the algorithm.
Time update equations
x[k] = Ad + x[k 1] + Bd u[k]
(4.1)
P[k] = Ad + P[k 1] ATd + Q
(4.2)
29
The measurement update equations

P[k] CT
C P[k] CT + R
K[k] =
(4.3)
x[k] = x[k] + K[k] (y[k] C x[k])
(4.4)
P[k] = (I K[k] C) P[k]
(4.5)
What is needed to process the Kalman filter are matrices Ad , Bd C, Q and R

and the initial conditions + x[k 1] and + P[k 1]. The subsequent paragraphs
will explain how each of these variables are obtained.
4.1
Matrices Ad , Bd
As said in the previous section the acceleration measured by the IMU will be used
to estimate the position of the vehicle every 10ms (100Hz update rate). This
acceleration can be expressed as
U
aIM
= asens + abias + noise,
m
(4.6)
where
U
aIM
represents the acceleration measured by the IMU,
m
asens represents the acceleration at sensors (IMU) location, and

abias represent the bias acceleration (offset)
U
Note that from now on the acceleration measured by the IMU aIM
will somem
times be expressed only as am , omitting the suffix IMU to simplify the notation,
but remember, that when talking about measured acceleration it will always be
measured by the IMU.
The acceleration at the sensors location can be expressed as the second derivative
of the position of the sensor, obtaining a relation between acceleration measured
and position:
ax,m = xsens + ax,bias + noise
(4.7)
ay,m = ysens + ay,bias + noise
(4.8)
30
These relationships can be expressed in a matrix form as:
x sens
y sens
x
sens
ysens
a x,bias
a y,bias
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0 0
0
xsens
ysens
1 0
0
0 1 0
x sens
0 0 1
y sens
0 0
0 ax,bias
0 0
0
ay,bias
{z
}
A

+

|
0
0
1
0
0
0
0
0
0
1
0
0
{z
B

ax,m
ay,m +noise
(4.9)
This matricial equation has the structure of a linear time equation, but there is
a need of a discrete form in order to apply a Kalman filter. As said before, the
only change needed is for matrices A and B to be discrete:
AK
Ad = e
X
An K n
n=0
n!
ZK
X
An K n+1
A
e dB =
Bd =
B
(n + 1)!
n=0
0
where
K represents the discrete time steps (time expressed in seconds)
(4.10)
(4.11)
31
After applying equations 4.10 and 4.11, matrix Ad and Bd are:
Ad =
Bd =
0 K
0
K 2 /2
0
1
0
K
0
K 2 /2
0
1
0
K
0
0
0
1
0
K
0
0
0
1
0
0
0
0
0
1
1
0
0
0
0
0
K 2 /2
0
0
K 2 /2
K
0
0
K
0
0
0
0
(4.12)
(4.13)
Obtaining the discrete equation x[k] = Ad x[k 1] + Bd u[k]
xsens
ysens
x sens
y sens
ax,bias
ay,bias
{z
xk
1
0
0
0
0
0
0 K
0
K 2 /2
0
1
0
K
0
K 2 /2
0
1
0
K
0
0
0
1
0
K
0
0
0
1
0
0
0
0
0
1
{z
K 2 /2
0
0
K 2 /2
K
0
0
K
0
0
0
0
{z
Bd
Ad

ax,m
ay,m
| {z }
ak
}|
xsens
ysens
x sens
y sens
ax,bias
ay,bias
{z
xk1
+(4.14)
32
Observe how if instead of simply applying equations 4.10 and 4.11 to obtain the
discrete matrices Ad and Bd the kinematic equations 4.7 and 4.8 would have been
integrated the solution would have been the same.
This method will be described for both equations being rsens the position vector
(xsens , ysens ). rsens will be integrated twice to obtain the position co-ordinates
between an initial state (x0 , t0 ) and a generic state (x, t).
am = rsens + abias
(4.15)
drsens
= am abias
dt
drsens = (am abias )dt
Zr
Zt
(4.16)
(4.17)
(am abias )dt
(4.18)
rsens = rsens,0 + am (t t0 ) abias (t t0 )
(4.19)
drsens =
drsens
= rsens,0 + am (t t0 ) abias (t t0 )
dt
drsens = (rsens,0 + am (t t0 ) abias (t t0 ))dt
Zr
drsens =
Zt
(rsens,0 + am (t t0 ) abias (t t0 ))dt
(4.20)
(4.21)
(4.22)
t2 t2
t2 t20
t0 t+t20 )abias ( 0 t0 t+t20 ) (4.23)
2 2
2 2
am
a
bias
= rsens,0 + rsens,0 (t t0 ) +
(t t0 )2
(t t0 )2
(4.24)
2
2
rsens = rsens,0 + rsens,0 (tt0 )+am (

rsens
Note that the initial state (x0 , t0 ) was denoted by zero to simplify the equations,
but this state could have been any state previous to the generic one (x, t). Also
notice that the time steps should be discrete, therefore the integration could have
been between a state k-1 and a state k obtaining the equation
rsens,k = rsens,k1 + rsens,k1 K +
abias
am
K 2
K 2 ,
2
2
(4.25)
which is the same equation as the one shown in the matrix form in equation 4.14
33
4.2
Matrix C
Matrix C relates the state to the measurement, which is the position x and y
xsens
ysens
| {z
= C
rsens
xsens
ysens
x sens
y sens
ax,bias
ay,bias
{z
x
k
(4.26)
where
C=
C1 when there is a new GP S measurement available

(4.27)
C2 when there is no new GP S measurement available
being
4.3
C1 =
1 0 0 0 0 0
0 1 0 0 0 0
C2 =
0 0 0 0 0 0
0 0 0 0 0 0
Matrix R
Going back to chapter 2, the measurement was described as:

y(t) = C x(t) + v(t)
(4.28)
where v(t) represented the GPS measurement noise and R will be the measurement noise covariance matrix. Denoting Xr,x and Xr,y as the variables that
describe, the GPS x and y position measurement noise, respectively, v(t) can be
expressed as [Kohler 2000]:
v(t) =
Xr,x
Xr,y
(4.29)
34
As seen in section 3.2 the covariance matrix is defined as:
R = E[v(t) v(t) ] =
E[Xr,x Xr,x ] E[Xr,x Xr,y ]

E[Xr,x Xr,y ] E[Xr,y Xr,y ]
(4.30)
2
E[Xr,x Xr,x ] = E[Xr,x
] represents the square of the standard deviation of the
2
variable Xr,x the same happens with y, E[Xr,y Xr,y ] = E[Xr,y
] represents the
square of the standard deviation of the variable Xr,y . Since there is no dependence
between the GPS error in the measurement of the x position and the y position
E[Xr,x Xr,y ] = 0. R can be expressed as
R=
4.4
2
r,x
0
2
0 r,y
(4.31)
Matrix Q
The deduction of matrix Q, is just the same as for matrix R but a bit more
complicated. The state estimation is described as:
x(t) = A x(t 1) + B u(t) + w(t 1)
(4.32)
where w(t) represented the process noise and Q will be the process noise covariance matrix. The input u is going to be the acceleration, so the noise will mainly
be due to the IMU acceleration noise. Looking back at equation 4.26 may help
understand the process better.
x sens
y sens
x
sens
ysens
a x,bias
a y,bias

=

|
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0 0
0
xsens
1 0
0 ysens
0 1 0
x sens
0 0 1
y sens
ax,bias
0 0
0
0 0
0
ay,bias
{z
}
A

+

|
0
0
1
0
0
0
0
0
0
1
0
0
{z
B

ax,m
(4.33)
ay,m + w(t)
35
Denoting Xa,x and Xa,y as the variables that describe, respectively, the IMU x
and y acceleration measurement noise, w(t) can be expressed as:
w(t) =
0
0
Xa,x
Xa,y
0
0
(4.34)
By definition the covariance matrix is:
T
Q = E[w(t) w(t) ] =
0
0
0
0
0
0
0
0
0
0
0
0
0 E[Xa,x Xa,x ] E[Xa,x Xa,y ]
0 E[Xa,x Xa,y ] E[Xa,y Xa,y ]
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
(4.35)
Like happened with the measurement noise, E[Xa,x Xa,x ] and E[Xa,y Xa,y ]
represent the square of the standard deviation of the variables Xa,x and Xa,y
and since there is no dependence between the noise in the x and y acceleration
E[Xa,x Xa,y ] = 0 for Q can be expressed as:
Q=
0
0
0
0
0
0
0 0
0
0 0
0
2
0 a,x
0
2
0 0 a,y
0 0
0
0 0
0
0
0
0
0
0
0
0
0
0
0
0
0
(4.36)
4.5
36
Initial Conditions + x[k 1] and + P[k 1]
The initial conditions are not as important as matrices Q and R since, in contrast
to these matrices, + x[k 1] and + P[k 1] will be updated by the algorithm, this
is, they will only be used for the first iteration, if they are well or not well chosen
will only affect the time the algorithm takes to converge into a good estimate.
The ideal will be to have some experimental measurements to know more or less
the initial state, being able to express + x[k 1] as
x0 =
xsens,0
ysens,0
x sens,0
ysens,0
ax,bias
ay,bias
(4.37)
It is important to notice as can be seen in equation 4.14, that even though the
state estimate is updated every estimation, the bias accelerations are never
updated, so this parameter in particular is important and here experimental test
are necessary (these test will be shown in section 5.3.6).
As for matrix + P[k 1] the same as said for + x[k 1] can be applied, the ideal
is to fix its value based on experimental knowledge.
Practical Work
37
Practical Work
The previous chapters have explained the theory of how the two measuring devices, GPS and IMU, work and what a Kalman filter is. In this chapter, all
practical tasks developed throughout this project will be shown. These task are
mainly divided in four subtasks:
1. Working with each of the measuring devices independently
Before the data that will be used to implement the filter can be logged, an
independent study of each of the two measuring devices, GPS and IMU,
must be done. The devices must be checked to see if the work correctly,
think which operating modes (if there are several) fit best the projects
needs and which are the outputs of interest. As well a draft of the logging
program must be developed and checked for each device.
2. Logging the data from the measuring devices
This will be done by a C++ implemented program synchronously for both
devices, GPS and IMU, and saved in an excel file. Data from an Inertial
Measuring and Surveying System, IMAR, will also be logged synchronously
to the one logged from the GPS and IMU. An IMAR platform is a commercial device which already has an internal Kalman filter implemented, it
provides position data based on high updated acceleration and slow position
measurements just like will be done in this paper. The data logged from
the IMAR platform is provided in an excel file, how it is logged does not
concern this paper.
3. Fusing the data
This task will be done offline, this is, once the data is logged and saved in
a file, it is fused through a Matlab implemented program, the fusion is not
real time.
4. Comparing the results with those from the IMAR platform to validate the
Kalman filter
The IMAR platform has already been tested and validated, so a comparison
between the position it obtains, and the estimation obtained from the GPS
and IMU fusion will ensure whether the Kalman filter here implemented is
good enough or not.
5.1
Working with the Measuring Devices
This section will teach the reader the how the work with the GPS and IMU
was done. The equipment needed, how the test were set up as well as a clearly
specified reference frame for each of the measuring devices are shown. A much
wider description of how the GPS works can be found in Annex A.
38
Practical Work
5.1.1
Hardware and Software used
GPS:
To obtain the best accuracy as possible a DGPS was used, remember that a DGPS
consists of a fixed base station and a moving rover station as can be seen in figure
5.1. The base stations receiver has knowledge of two positions for the base, the
fixed (or known) position and the one coming from the satellite constellation.
By difference of both positions the receiver calculates the errors and sends the
correction data to the rover station.
Figure 5.1: Differential GPS

The hardware and software needed to set up this operating mode is listed below:
2 GPS receivers (figure 5.2), one for the rover station and one for the base
station
Figure 5.2: GPS Receiver

The GPS receiver is the one that processes all the raw data coming form the
GPS antenna. The receivers used were Novatel ProPak-G2+DB9-RT2W for
the rover station and Novatel ProPak-G2+DB9-Generic for the base station
39
Practical Work
2 GPS antennas (one for each receiver) (figure 5.3)
Figure 5.3: GPS Antenna

Novatel was also the antenna provider
2 Radios (figure 5.4), equipped with good radio antennas to send the correction data from the base station to the rover station
Figure 5.4: Radio and Radio Antenna

2 12V battery sources for the receivers. The rover station can use the car
battery, but a portable one is needed for the base station.
A computer to send certain commands to the receivers and to log the data
Interface GPS-PC software program GPSconnect
40
Practical Work
IMU:
The equipment needed to work with the IMU is much more simple.
Xsens Inertial Measuring Unit (figure 5.5)
Figure 5.5: IMU

A computer to communicate with the IMU and to log the data
Interface IMU-PC software program MT software
41
Practical Work
5.1.2
Setup of Tests
The setup of a DGPS on top of a table is shown in figure 5.6. A 12V battery provides power for the receivers which are communicated by radio. A GPS antenna
is also communicated to each of the receivers and a computer is used to setup
the receivers mode as well as to log the data. The base station only needs to
be connected to a PC for the setup (it does not log any data), therefor only one
computer is needed (first setup the base station and then use that same computer
for the setting up and data logging of the rover station).
Radio 1
Battery
Antenna 2
Antenna 1
Radio 2
Receiver 1
Receiver 2
Figure 5.6: DGPS Set Up

Figure 5.7 shows the base station already setup outdoor. Notice that it is fixed on
a roof (high surface) to obtain a better signal avoiding trees and other obstacles.
The receiver can not be seen since it has been confined in a box to avoid contact
with water or leaves.
Figure 5.8 shows the GPS and the IMU fixed on the test vehicle, as well as the
radio antennas. There are two because one is for receiving data and the other
one for sending, there could have been only one if it were bidirectional. These
antennas are not physically the same one as the one shown in 5.4 because the
pictures are from different equipments, but the use is the same.
42
Practical Work
Figure 5.7: Base Station
Figure 5.8: GPS and IMU
43
Practical Work
To fuse both measuring devices the ideal is for them to be fixed in the exact same
point, that of course, is impossible. They will be fixed both on top of the test car,
close enough to suppose that the measuring point is the same, but not too close
to avoid a possible magnetic field distortion. Also, as will be seen in section 5.1.3
the IMU provides measurements in a sensor fixed coordinate system, so it should
be positioned so that the measuring units X-axis matches the cars longitudinal
axis and the measuring units Y-axis matches the cars lateral axis.
Figure 5.9: Fixed IMU

Figure 5.10 provides the measurements of where the GPS and IMU were located
on the top of the car.
Figure 5.10: GPS and IMU position on car
44
Practical Work
5.1.3
Reference Frames
It is important to clearly specify all reference frames for each of the measuring
devices involved. For the GPS, since a DGPS is being used, the easiest is to work
with a system fixed on the base station like shown in figure 5.11.
Figure 5.11: GPS reference frame

The reference frame G : (G , G ex , G ey , G ez ) has its origin in G which is the base
station and three orthogonal vectors G ex , G ey , G ez representing a measuring unit
of 1m. Only the x and y direction are going to be of interest, the vehicle is
supposed to be moving over a 2 dimension surface.
It is also important to know the IMU reference frame, as can be seen in figure
5.12 it is is fixed on the IMU itself, so all accelerations and turning rates will be
measured in this moving coordinate system I : (I , I ex , I ey , I ez ).
As for the angles they are measured based on a local magnetic North reference
frame E : (E , E ex , E ey , E ez ) as can be seen in figure 5.12. Each angle is measured
from the Earth fixed reference E frame to the IMU reference frame I; the positive
sign of these angles will follow the positive right hand rule [MTipage6].
45
Practical Work
Figure 5.12: IMU reference frame

The angle that rotates round the X axis will be denoted by roll angle (), the
one that rotates around the Y axis will be denoted by pitch angle () and the
one that moves round the Z axis will be the yaw angle (). Figure 5.13 shows
the three angles.
Figure 5.13: Example of roll, pitch and yaw angles
46
Practical Work
The IMAR platform has not been mentioned much yet since it is not part of
this project to know how exactly it works, only its outputs are of interest. Nevertheless, the reference frame these outputs are referred to is important. As
can be seen in figure 5.14 the IMAR platform uses a spherical reference frame
P : (E , P er , P e , P e ). The position of the vehicle is given in terms of latitude
with origin in the Greenwich meridian and longitude with origin in the Equator.
Figure 5.14: IMAR reference frame
Practical Work
5.2
47
Data Logging
This section will show an example of the data logged and explain the most important issues of the data logging program. It is important for the data from
both devices to be logged through the same program because they must be fused.
As said before the logging tool was a C + + program. The data must be taken
synchronously in order to compare the measurements in the different time steps,
logging with the same program allows the IMU (faster update rate) to work as a
trigger every time there is a new measurement, logging both devices at the same
time.
5.2.1
Example of Logged Data
Table 3 shows an example of the data logged.

The first column represents the seconds that have passed since the stat of the test,
this time is read directly from a very precise counter function the PC system has.
As can be seen the GPS data is logged every time the IMU data is logged, but as
its update rate is slower (5 times slower), the measurement does not change every
time. A new GPS measurement is in theory available every 5 IMU measurements,
but since there is no way of telling each of the devices when they should start
logging, sometimes the IMU can trigger immediately before the GPS measurement
has been updated, seeming that the IMU is 6 times faster, but then the next time
a GPS measurement is available it will be after only 4 IMU measurements.
Table 3 does not show all the data logged but only those variables of interest.
Apart from those variables the GPS also logged the latitude, the longitude,the
speed and the course of the vehicle as well as a measurement of the quality of
the signal (being 1 the worse and 5 the best). The IMU as apart from logging
acceleration in x, y and z and the roll, pitch and yaw angles, also measured the
turning rates and magnetic data in the same three directions.
48
Practical Work
Table 3: Logged data
Practical Work
5.2.2
49
Data Logging Pseudocode
Figure 5.15 represents a brief pseudocode of the C + + program1 that logs the
data and saves it on a file. The program is of course much more complicated than
it may seem in the pseudocode, but the most important issues are here exposed.
The most interesting part of the program that should be noticed is how (as has
already been said) the IMU works as a trigger for the GPS. The GPS will not
look to see if there is a new measurement available unless there has been a new
GPS update. Once the IMU has detected new data it will be logged and saved
in a variable. The CPU time and the GPS data will be logged and saved as well
and then they will all be saved in the excel file. Notice how the log GPS data is
always the latest, this is, the last known position will be saved, but maybe it has
not changed since the last loop. Also notice that when a new GPS measurement
is detected, the incoming data is raw, a conversion most be made to obtain
understandable data.
C:\Programs\ C++\C++ Brauschweig\Programs\GPSConnect\GPSConnect
50
Practical Work
Figure 5.15: Logging Pseudocode
Practical Work
5.3
51
Fusing the Data
Once all the data is logged the only thing left to do is to implement the Kalman
filter to check if it works adequately. This implementation is programmed in
Matlab, the main structure of the program is:
1. Import the data from the file2 .
2. Fixing the initial conditions.
3. Applying the Kalman algorithm for each time step. This will be done in a
recursive loop having a new state estimation in every loop.
4. Saving the vehicles state in a vector after each loop.
In chapter 4, what was needed to implement the Kalman filter and its structure
for the position estimation was shown. The variables of interest were matrices
Ad , Bd C, Q and R and the initial conditions + x[k 1] and + P[k 1]. This
section will show how to adapt the logged data to fit these variables and some
important aspects that affect how to implement the program.
5.3.1
Rotation Matrix RGI
The first thing that should be noticed from the data obtained from the GPS and
the IMU is that their reference frames are not the same. While the GPS has a
fixed reference frame on the base station G, the IMU uses a moving system I.
To apply the Kalman filter it is important that all measurements use the same
system. To know the position of an object a reference is always necessary, so the
best will be to have all the data in a fixed reference frame, use the GPS reference
frame G, this way the GPS data will not have to be changed.
To adapt the IMU data to G, a rotation matrix will be used. A rotation matrix
allows to express vectors measured in the IMU sensor reference frame I in a global
reference frame G and the other way round.
RGI rotates a vector in the IMU local reference frame (I) to the global GPS
reference frame (G)
RGI = RZ RY RX
RGI
2
(5.1)
1
0
0
cos 0 sin
cos sin 0
1
0 0 cos sin (5.2)
= sin cos 0 0
0 sin cos
sin 0 cos
0
0
1
C:\Programs\ Matlab\Matlab Mine\Data to Plot
52
Practical Work
RGI
coscos sinsincos cossin cossincos + sinsin

= cossin sinsinsin + coscos cossinsin sincos (5.3)
sin
sincos
coscos
So to know the IMU acceleration in a global system only the following rotation
is needed:
IM U
Ga
= RGI I aIM U
(5.4)
Since working only with a 2 dimension (X,Y) reference frame, the only rotation
needed is the one around the z axes, for this particular case
RGI
cos sin 0
= RZ = sin cos 0
0
0
1
(5.5)
The problem is that this rotation has to be applied being the angle from the
GPS reference frame G to the IMU local reference frame I, but this angle is not
directly known from the logged data. Remember that the yaw angle that the IMU
provided was that going from the Earth fixed reference frame E to the IMU local
reference frame I. In figure 5.16 the yaw angle in the different reference frames
can be seen.
Figure 5.16: Yaw Angle reference frame
53
Practical Work
Knowing the relative position between both fixed reference frames, this is, the
angle going from the Earth fixed frame E to the GPS fixed frame G (E G ), the
GPS yaw angle G can be calculated as:
=E E G
(5.6)
To obtain a good enough estimation of E G a test where the vehicle was moving
straight will be analyzed. If the vehicle is moving straight, no curves, its yaw
angle should be always constant measured from one, or another reference frame.
As can be seen in figure 5.17 the angle of the velocity can be calculated as
= tan1
y1 y0
y2 y1
y2 y0
= tan1
= tan1
.
x1 x0
x2 x1
x2 x0
Figure 5.17: Yaw Angle Estimation
(5.7)
54
Practical Work
In general this angle is not the same angle as the yaw angle (orientation of
the vehicle), but for the particular case of the vehicle moving straight the sideslip
angle is zero, so these two angles are the same, G as can be seen in figure
5.18
Figure 5.18: Vehicles orientation

In figure 5.19 the path followed in the test that will be analyzed can be seen
Figure 5.19: Path Followed in G

The first graphic of figure 5.20 shows the yaw angle measured by the IMU from
the Earth fixed reference frame, E , as expected, it is almost constant throughout
all the test. The second graphic shows the yaw angle measured by the GPS from
the GPS base fixed reference frame, G . To calculate this angle equation 5.7 was
applied every 50 GPS measurements to avoid bad GPS measurement errors (if
the equation were to by applied for very close measurements, the measurement
errors would lead to a wrong calculated angle).
55
Practical Work
= tan1
yk+50 yk
xk+50 xk
(5.8)
Like in the case of E , as expected, GP S , is almost constant throughout all the

test.
Figure 5.20: Difference of Yaw Angles

The third graphic of figure 5.20 shows the relative position between both reference
frames, E G , this angle is estimated to be 95o . This value was calculated by the
mean of differences of yaw angles.
56
Practical Work
This test was repeated in case there were for some reason big measurement errors.
Figure 5.21 shows the path followed while figure 5.22 the yaw angle measured from
the different reference frames as well as the relative position between both frames.
Figure 5.21: Path Followed in G
57
Practical Work
Figure 5.22: Difference of Yaw Angles

For this second test E G was also about 95o .
As said before the GPS yaw angle will be then calculated as:
=E 95o
(5.9)
58
Practical Work
5.3.2
Defining the time steps
As seen in chapter 4, matrices Ad , Bd and Q all depend of the time step K.

The ideal solution will be to have a constant defined time step increment. As the
IMU update rate is 100Hz the time increment should always be 0.01 seconds, but
in reality, as could be seen in table 3 it is not always constant. Since the Kalman
filter updates each estimation every time there is a new input, that is what shall
be done, even if the time increments are not the same, so for each estimation k,
the time step will be defined as:
K = tk tk1
(5.10)
Remember also that the estimation should be compared with the GPS measurement and that is why they were logged synchronously. If the estimations were
to be made every 0.01 seconds the 5th estimation would be at time 0.05s and if
the new GPS measurement were to be at time 0.052s then the comparison will
be made of two values at different times.
5.3.3
Defining Matrix C
Remember that matrix C related the state to the measurement, where
C=
C1 when there is a new GP S measurement available

(5.11)
C2 when there is no new GP S measurement available
being
C1 =
1 0 0 0 0 0
0 1 0 0 0 0
C2 =
0 0 0 0 0 0
0 0 0 0 0 0
So there is a need to define when there is or is not a new GPS measurement.

Since there is not always a new GPS measurement every 5 IMU measurements
(as seen in table 3) , C can not just simply be C2 for 4 loops and C1 every 5th
loop. The solution that will be adapted is for each loop to check the current and
the previous GPS measurement and compare them, if they are the same there is
no new measurement available, but if they are not, then there has been a GPS
measurement update, and C will not be empty.
59
Practical Work
if
gps
xgps
k xk1 = 0
then
C = C2 =
0 0 0 0 0 0
0 0 0 0 0 0
else
C = C1
5.3.4
1 0 0 0 0 0
0 1 0 0 0 0
Defining Matrix R
As was seen in chapter 4 matrix R is defined as
R=
2
r,x
0
2
0 r,y
(5.12)
where r,x and r,y represent the standard deviation of the x and y position
measurement error, respectively.
Going back to equation 2.4 the horizontal standard deviation of the position
measurement error h is the pseudorange standard deviation 0 times the HDOP
factor
h = 0 HDOP.
(5.13)
A possible approximation of the x and y standard deviation r,x and r,y , is

projection in each of the axes of the horizontal standard deviation h
r,x = h cos,
(5.14)
r,y = h sin.
(5.15)
60
Practical Work
The HDOP is obtained from the GPS, but the pseudorange standard deviation
0 will have to be calculated. Since there is no way of knowing the exact position
of the GPS antenna and there is no data available on the satellites position, the
pseudorange standard deviation 0 will be estimated from a test where the vehicle
just stood still for some time. Calculating the x and y standard deviation r,x ,
r,y for this test the horizontal standard deviation h is
q
h = (r,x )2 + (r,y )2 .
(5.16)
Obtaining now the pseudorange standard deviation as
0 =
h
HDOP
(5.17)
Figure 5.23: GPS Position Errors in G

Figure 5.23 shows the x and y position for this test, even though the car was
not moving, the measurements were not always the same, there was some kind of
error. The standard deviation is by definition
2
N
i (Xi )
,
N
being N the number of samples.

Applying equation 5.18 for the test, the x and y standard deviations are
(5.18)
61
Practical Work
r,x = 0.0023,
r,y = 0.0029.
The horizontal standard deviation is therefore
h =
p
(0.0023)2 + (0.0029)2 = 0.0037
Since the position of the GPS antenna was still throughout the test and it lasted
only about 1.5 minutes the HDOP was always the same, 0.9, obtaining an estimation of the pseudorange standard deviation of
0 =
0.0037
= 0.0041.
0.9
If the test had lasted longer maybe the HDOP factor would have changed, being
then a possible solution to use only those parts of the tests where the HDOP
factor is constant to estimate the pseudorange standard deviation 0 .
5.3.5
Defining Matrix Q
As seen in chapter 4 the process noise covariance matrix Q was
T
Q = E[w(t) w(t) ] =
0
0
0
0
0
0
0 0
0
0 0
0
2
0 a,x
0
2
0 0 a,y
0 0
0
0 0
0
0
0
0
0
0
0
0
0
0
0
0
0
(5.19)
where a,x and a,y represent the standard deviation of the x and y acceleration
measurement error, respectively.
Just as in the previous section, to calculate the pseudorange standard deviation
a test with the vehicle standing still for a while was considered, here the same
test will be analyzed to calculate the acceleration standard deviation.
62
Practical Work
Figure 5.24: Acceleration Errors in I

Being the acceleration standard deviation for this test (equation 5.18)
a,x = 0.0492
a,y = 0.0213
5.3.6
Defining the Initial Conditions + x[k 1] and + P[k 1]
As explained in chapter 4, for + x[k1] the ideal will be to have some experimental
measurements to know more or less the initial state vector
+
x[k 1] = x0 =
xsens,0
ysens,0
x sens,0
ysens,0
ax,bias
ay,bias
(5.20)
63
Practical Work
As for this first Kalman filter validation, the program is not working real time,
but with data saved on a file, the initial position can be known, being taken as
the first GPS position measurement. When working real time, it is advised to
begin every test at the same known spot, being able to initialize in advance the
vehicles position. On the contrary to the position, even if not working real time,
the velocity is not in anyway known till there is an estimation. Since this velocity
should be very small at the beginning of each test, it will be initialized to zero.
As for the acceleration biases, remember that they were the only parameters of
the state vector that were not updated, for it was, in this case, important to have
some test to know the biases values.
The test that will be used is the same one as for calculating the position and
acceleration standard deviations, the one where the vehicle was still for some
time. In figure5.24, the x and y acceleration means where already shown. Since
the vehicle was still, there should have been an acceleration of zero, but the means
were not zero, so these mean values are what will be considered the bias values.
+
x[k 1] =
S
xGP
0
GP S
y0
0
0
0.1077
0.1926
(5.21)
For + P[k 1] there is no way of knowing without previous test, and the its value
will probably change in every test, since its initialization is not so important, it
will be updated. It is going to be initialized as an identity matrix.
+
P[k 1] =
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
0
0
0
0
0
0
1
(5.22)
Practical Work
5.3.7
64
Kalman Filter Implementation Pseudocode
Like in the section 5.2.2 , here too a brief pseudocode of the Matlab program3 is
found.
Figure 5.25: Kalman Filter implementation Pseudocode
C:\Programs\Matlab\Matlab Mine\All graphs final
Practical Work
5.4
65
Comparing with IMAR Platform
As said in the introduction of this chapter, the IMAR platform data will be used
to validate the Kalman filter implemented. The study of this platform is not
concern of the project, therefore it has not been mentioned much before. The
results of the comparison will be shown in the results chapter. In this chapter
what will be done will be adapting the data obtained from the IMAR platform
to a form in which it is easier to compare with the results from the filter.
The data was logged from the three devices, GPS IMU and IMAR, synchronously, but some problems appear when wanting to compare the results. The
main problem is that while the estimation is referred to the GPS base G, the
IMAR platform provides position, but in the form of latitude and longitude in
the reference frame P. The problem can be solved since the GPS provided as well
as Cartesian coordinates x and y, latitude and longitude data coordinates, and
the latitude and longitude from the GPS base station G is also known. Therefore
the relation between both coordinates can be calculated, and that same relation
can be applied to obtain the Cartesian coordinates for the IMAR data. The
pseudocode of how the relationship between both cordinate systems is obtain is
shown in figure 5.26.
Figure 5.26: Coordinate conversion psuedocode
66
Practical Work
As can be seen in figure 5.27 the Cartesian coordinates x1 and y1 are referred to
a 2D system over a plane that cuts (x0 , y0 ), (x1 , y1 ) and (x0 , y1 ) which origin is
at latitude and longitude zero, x0 and y0 .
Figure 5.27: Earth Globe

x1 and y1 can be expressed in this system as
x1 lon1 R,
(5.23)
y1 lat1 R,
(5.24)
where the latitude and longitude are expressed in rad.

Note that equations 5.26 and 5.24 are only approximations since what x1 and
y1 really represent is the arc distance, which is a bit longer than the distance
projected on the plane.
Practical Work
67
In the case here studied, the vehicles position, which in figure 5.28 is noted by
x2 , y2 , is referred to a system which origin is at the GPS base station, x1 , y1 ,
expressed in a plane defined by the origin (x1 , y1 ) and tangent to the Earths
surface at the origin (x1 , y1 ).
Figure 5.28: Local and Global Co-ordinates

Referred to P,spheric coordinates, this is the same as saying that the plane is
defined by the normal vector that goes from the center of the Earth to the origin
n = (R 0, lon1 0, lat1 0), and the origin point (R, lon1 , lat1 ), obtaining the
plane equation
R R + lon1 lon2 + lat1 lat2 R2 lon21 lat21 = 0,
(5.25)
which is the same as
lon1 lon2 + lat1 lat2 lon21 lat21 = 0.
(5.26)
Here the approximation in equations 5.26 and 5.24 is really good since the distance
between the vehicle and the base station will be so much smaller than the Earths
radius, that the arc distance is practically the same as the projected distance.
The reference frame just described is not exactly the same as the IMAR platform
68
Practical Work
reference frame (P) (seen in section 5.1.3) expressed in Cartesian coordinates

instead of spherical since the origin changes, though also latitude and longitude
coordinates are considered. Therefore, it will be considered the IMAR with GPS
base origen reference frame denoted by M. The vehicles position in this frame is
expressed as M x and M y
Mx
= (lon2 lon1 ) R = (lon2 lon1 ) R cos(lat1 ),
(5.27)
My
= (lat2 lat1 ) R.
(5.28)
Note that for the M x coordinate the radius is no longer R but R , being R =
R cos(lat1 ) as can be seen in figure 5.29.
Figure 5.29: R calculation

As can be seen in figure 5.28, the GPS base fixed reference frame (G) has the
same origin as M, but the orientation of this system is unknown. What is known
is that the distance d, expressed either in G or M should be the same.
Md
Gd
Md
=G d
Mx
Gx
+M y 2
(5.29)
+G y 2
(5.30)
(5.31)
The system formed by equations 5.27, 5.28, 5.29, 5.30 and 5.32 provides the
exact radius R that the GPS receiver used to convert from longitude and latitude
spherical coordinates to the GPS base G Cartesian coordinates.
R2 =
+G y 2
(lon2 lon1 )2 cos2 (lat1 ) + (lat2 lat1 )2
Gx
(5.32)
69
Practical Work
This system was applied for the GPS data throughout several tests and the value
of R calculated was:
R = 6.3904 106 m
Now that the exact radius used is known, the angle that goes from each of the
systems to the vehicle M and G can be calculated as:
= tan1
= tan1
My
(5.33)
Mx
Gy
(5.34)
Gx
Being the angle between both co-ordinate systems

M G
M G
= M G
(5.35)
Once this angle is known, the IMAR data can be converted from latitude and
longitude coordinates to GPS based coordinates as:
IM AR
AR
AR
AR
= (lonIM
lonIM
) R cos(latIM
)
2
1
1
(5.36)
IM AR
AR
AR
= (latIM
latIM
)R
2
1
(5.37)
Mx
My
xG,IM AR
y G,IM AR
cos(M G ) sin(M G )
sin(M G ) cos(M G )
M,IM AR
x
(5.38)
y M,IM AR
This angle was calculated for the GPS data throughout several test just like when
R was calculated, which should be the same throughout all these tests, this angle
turned out to be M G =0.046o , which can be considered zero, but anyway the
rotation was made.
Results
70
Results
This chapter will take a look at several tests made, analyzing for each of them:
GPS inputs
Position measurements
Horizontal Dilution of Precision HDOP
IMU inputs
Acceleration measurements
Yaw angle
Kalman filter outputs
Estimated position
Estimated velocity
Estimated position compared with IMAR position
6.1
Test 1. Driving in a Large Circle
The first test to be analyzed is one just driving around an about 15m radius circle
for several laps as can be seen in figure 6.1. The speed was meant to be constant
throughout all the test.
6.1.1
GPS Inputs
The two GPS inputs are the position measurements and the HDOP factor, both
updated at 20Hz. Figure 6.1 shows the position measurements x and y in the
horizontal and vertical axis, respectively.
Figure 6.2 shows the HDOP for this test. Remember the HDOP was a parameter
that expressed how good the GPS signal was, it was the factor by which the input
error (pseudorange standard deviation 0 ) was multiplied to obtain the output
error (horizontal standard deviation h ) being better, the smaller it was. As can
be seen in figure 6.2, the HDOP varies from 0.9 to 1.9, which indicates a very
good GPS signal (as said in chapter 3, it may be a bit smaller than one, but it
will never get close to zero, it is impossible to have no output error if having an
input error).
71
Results
Figure 6.1: Path Followed
Figure 6.2: Horizontal Dilution of Precision
72
Results
6.1.2
IMU Inputs
The IMU provided, among many others, two inputs that were used for the Kalman
filter implementation, the yaw angle and the acceleration, both at an update
rate of 100Hz. As can be seen in first graphic of figure 6.3 the yaw angle for the
IMU reference frame I describes the 4 circles driven. The second graphic shows
the yaw angle but expressed in the GPS base fixed reference frame G, which is
rotated 95o with respect to I.
Figure 6.3: Yaw Angle in I and G

In figures 6.4 and 6.5 the acceleration in the x and y direction both in I (red) and
G (blue) coordinates can be seen. Note that for reference frame I the accelerations
in both x and y direction are almost constant to zero, as said before, driving at
constant speed. When expressed in G, these accelerations adapt the sin shape
form expected being the second derivative of a sin shaped position.
73
Results
Figure 6.4: Acceleration in x Direction in I and G
Figure 6.5: Acceleration in y Direction in I and G
Results
6.1.3
74
Kalman Filter Outputs
The two outputs obtained from the Kalman implementation were the vehicles
position and velocity. Figures 6.6 and 6.7 show both the GPS and the estimated
position for the x and y direction, respectively. As can be seen in the zoomed
graphics the estimation is quite good, observe how the estimated values adapt to
the GPS measurement curve quite smoothly but at a higher update rate. Every
5 estimations (IMU update rate 5 times higher than GPS update rate) the GPS
measurement and the estimation are practically the same, this is, because the
HDOP factor is so low (the measurement is very reliable) that the estimation is
almost the same as the measurement.
The estimation may seem to swing a little sometimes, but this deviation is very
small, maybe of about one centimeter. The estimation for this test has been
successful: the update rate is higher than that of the measurement and the estimation is good enough (or a lest it adapts to the GPS measurement curve very
good).
75
Results
Figure 6.6: x Position Measurement and Estimation
76
Results
Figure 6.7: y Position Measurement and Estimation
77
Results
In figure 6.8 the estimated position and velocity, as well as the IMU measured
acceleration for the x direction can be seen in the GPS reference frame G.
Figure 6.8: Vehicle Kinematic Parameters in x Direction

The estimated velocity can not be compared with any other measurement, so to
know if the results make sense, the relations between the kinematic parameters
will be analyzed.
As is well known, and has been repeated throughout this paper, the position,
velocity and acceleration are related by
vx = x
ax = v x = x
The results seem to be correct, for, as driving in a circle, the x position can be
expressed as:
x = R cos
being then
vx = x = R sin
ax = vx = R cos
which is precisely want can be seen in the graphics of figure 6.8.
78
Results
The kinematic parameters in the y direction are practically the same as for the x
direction but with a delay of as expected (if x = R cos then y = R sin)
Figure 6.9: Vehicle Kinematic Parameters in y Direction
Results
6.1.4
79
Comparing with IMAR Data
Figure 6.10 shows both the IMAR x position data as well as the Kalman filter
estimation in reference frame G. Both graphics seem very similar, but when
looking at the zoomed areas some jumps are noticeable in the IMAR data, these
jumps are larger when the graphic changes its slope rapidly (lower left), but also
exist when the slop is constant (lower right). On the other hand, not taking into
account the jumps, the IMAR graphic seems smother than the estimation when
the slope changes.
In the graphics in the lower part of figure 6.10 it is possible to see each data value
logged for the IMAR platform and each estimation. As just mentioned the IMAR
data is smoother (lower left) when having a change of slope till there is suddenly
a jump in the position, and then it continues being smooth again; in the case of
the estimation, the graphic is not so smooth, it has very little jumps, but this
avoids the big jumps. The estimation runs in the middle of the IMAR graphic
before and after its large jump, obtaining at the end a better value.
In the case of having a constant slope (lower right), the estimation is both smother
and has smaller jumps.
Something similar happens in the case of the y position (figure 6.11). When there
is a slop change (left part of the figure) the IMAR position data is a bit smoother
than the estimation, but it presents large jumps. On the other hand, when the
slop is constant (right part of the figure) both graphics present little jumps, but
the IMAR position data also sometimes presents a large jump.
This is clear proof that the Kalman filter implementation here developed is good
enough, it may even be said that it is better than that of the commercial IMAR
platform.
80
Results
Figure 6.10: x Position from IMAR and Estimation
81
Results
Figure 6.11: y Position from IMAR and Estimation
82
Results
6.2
Test 2. Driving in a Small Circle
The second test is very similar to the first, also driving in a circle at constant
speed but this time it is a small circle, of about an 8m radius, almost half the
one driven in the first test.
6.2.1
GPS Inputs
Figure 6.12 shows the GPS position measurements in the x and y direction in
each of the Cartesian axes, respectively.
83
Results
In figure 6.13 the HOP factor can be seen. For this test the value is constant to
0.9, indicating a very good GPS signal throughout all the test. The fact that this
value is so low and that it does not change implies that when a GPS measurement
is available, the estimation is going to practically be the same as the measurement
and this will happen throughout all the test.
84
Results
6.2.2
IMU Inputs
In figure 6.14 the Yaw angle already in global coordinates can be seen. Just
like in the case of the previous test, this figure illustrates the 4 circles driven.
Figure 6.14: Yaw Angle in G

Figures 6.15 and 6.16 represent the acceleration in the x and y direction, respectively. The acceleration shown, is already expressed in reference frame G for both
directions. The sin shape forms of the accelerations can be seen just like in the
first test. This shape is the expected one since the path followed is a circle.
Figure 6.15: Acceleration in x Direction in G
Figure 6.16: Acceleration in y Direction in G
85
Results
6.2.3
Figures 6.17 and 6.18 show both the GPS and the estimated position for the x
and y direction, respectively.
86
Results
The results are the same as for test 1, the estimated values adapt to the GPS
measurement curve quite smoothly at a higher update rate.

Notice that for this second test the position axis in the zoomed parts shows jumps
of only 5cm, therefore, even if little jumps are seen they are vary small, maybe
of about 0.5cm.
87
Results
The other Kalman filter output is the vehicles velocity. Figures 6.19 and 6.20
show the 3 kinematic parameters (position, velocity and acceleration) all in G
for the x and y direction, respectively. Like in the first test they are all shown
because there is nothing to compare the estimated velocity with to know if it is
correct or not. By comparing the velocity with the position and acceleration it is
possible to know if it is correct, since the velocity should be the derivative of the
position, and the acceleration should be the derivative of the velocity.
In the case of analyzing a circle it is easy to know since the derivative of a sin is
a cosin and the derivative of a cosin a sin like saw in the previous test.
This is the case for figures 6.19 and 6.20, so the velocity estimation is probably
correct.
88
Results
6.2.4
Figures 6.21 and 6.22 show both the IMAR position data as well as the Kalman
filter estimation for both x and y direction, respectively. What is seen is the
same as for the first test, when having a change of slope the IMAR data is much
smoother but presents vary large jumps compared with the estimation. These
jumps imply great position errors.
When the slope is constant the IMAR data presents smaller jumps than in the
case of having a changing slope, but the graphic drawn by the IMAR data seems
less smooth than that of the estimated position.
89
Results
90
Results
91
Results
6.3
Test 3. Driving in a Straight Path
This test consists only on driving in a straight path at constant speed, the interesting part is that the HDOP factor is quite high in some parts of the test, bad
GPS signal.
6.3.1
GPS Inputs
Figure 6.23 shows the path followed, the GPS position measurements in the x
and y direction in each of the Cartesian axes.

In figure 6.24 the HDOP factor can be seen. As said before, in during almost all
the test the GPS signal is quite good, with a HDOP factor between 1 and 2, but
there are some part of the test where the signal is not so good. After driving for
around 25s the HDOP factor doubles from about 2 to 4, and after about 83s it
is almost 4 times bigger than in the nearby times (form around 2 to almost 8).
92
Results
6.3.2
IMU Inputs
The yaw angle (figure 6.25) is basically constant throughout all the test. It
may not seem so when analyzing the graphic, but notice that the yaw angle
axis goes from -10 to -2, the highest angle difference is not even of 8o , therefore
the yaw angle can be assumed constant.
93
Results
Figures 6.26 and 6.27 show the other IMU input, the acceleration, in both x
and y direction, respectively. They are shown in in GPS G coordinates and are
basically zero for the test was driven at constant speed.
6.3.3
In figure 6.28 the GPS measurements and the estimation for the x direction
can be seen. The zoomed graphic shows the part where the GPS signal was
not so good. The estimation is quite good, smooth and adapted to the GPS
measurement. Around time 81.95s, where the HDOP factor jumped from 2 to
8, the Kalman gain effect can be seen: the signal was not so good, one GPS
measurement derived a little from the rest, so the estimation instead of trusting
this measurement trusted the a priori estimation, obtaining a smoother curve.
94
Results
95
Results
When analyzing the position in the y direction (figure 6.29) the estimation seems
much lees smoother than for the x direction, but taking a look at the position
axis for both directions notice that the scale for the y direction is much smaller
(5cm jumps compared to 50cm jumps in the x direction). In the y direction the
jumps seem more noticeable, but are of a maximum of 2cm.
96
Results
In figures 6.30 and 6.31 the 3 kinematic parameters can be seen. The velocity
estimation is considered to be correct since for both x and y direction the position
estimation draw a constant slope line, which derivative is a zero slope line at a
specific value, just like the one drawn in the graphics. The derivative of this type
of graphic is a zero slope line at value zero, which is exactly the acceleration
graphic obtained.
97
Results
6.3.4
For the x direction comparison of the IMAR data with the estimation (figure
6.32) both curves are very smooth, both are equally good.
In the case of the y direction both curves (the one obtained from the IMAR data
and the estimation) are less smooth than in the case of the x direction. But as
happened in test 1 and 2, while both curve present little jumps, the one coming
from the IMAR data also present some not so little jumps. Therefore, the Kalman
filter here implemented can be considered better than the IMAR platform.
98
Results
99
Results
100
Results
6.4
Test 4. Driving Accelerating and Decelerating Rapidly
This test does not follow any specific path, it was driven drawing different forms
and accelerating and decelerating rapidly.
6.4.1
GPS Inputs
Figures 6.34 and 6.35 show the two GPS inputs, the x and y position estimation
and the HDOP factor. Analyzing figure 6.35 it is seen that the GPS signal was
very good during the test, with a HDOP factor always smaller than 1.5.
101
Results
102
Results
6.4.2
IMU Inputs
The yaw angle (figure 6.36) shows how the test did not follow any specific
pattern, the orientation changes form one way to another throughout all the test.

The same happens with the acceleration in both directions (figures 6.37 and 6.38).
There is no specific pattern, but what can be notices with respect the previous
test is that it changes rapidly and goes up to quiet high values (almost 10m/s2 )
103
Results
6.4.3
Figures 6.39 and 6.40 show the GPS measurements and the estimation for the x
and y direction, respectively. As happened with the other test, the estimation is
quite good, smooth and following the curve drawn by the GPS measurement but
at a higher update rate.
104
Results
The estimation is a bit smoother when the slope is constant (right parts of the
figures) than when it is changing (left parts of the figures), but both can be
considered good enough estimations.
105
Results
Figures 6.41 and 6.42 show the position and velocity estimation as well as the
global acceleration for both direction, x and y, respectively. Like in all test
analyzed before, the relation between the 3 parameters, velocity and acceleration
the first and second derivative of the position, respectively, can be checked in
these figures. Since the graphics seem to adapt to these relations, the velocity
estimation can be considered correct.
106
Results
6.4.4
In figures 6.43 and 6.44 the IMAR data and the estimation for the x and y position
can be seen. Just like in the previous tests, when the slope changes (left part
of the figures) the IMAR data is smoother than the estimation but some large
jumps appear that imply some kind of error. When the slope is constant (right
part of the figures) the IMAR data is not so smooth anymore and there also some
small jumps.
107
Results
108
Results
109
Results
6.5
Test 5. Driving Losing the GPS Signal
This test was driven through areas where there was a bad GPS signal or even no
signal at all to see how well the Kalman filter reacted under these conditions.
6.5.1
GPS Inputs
As can be seen in figure 6.45 the GPS measurements are a complete chaos, not
smooth at all, and of course, this is not due to the fact that that the path followed
was a chaos, but to the huge errors in the GPS measurements due to the bad
signal.

Looking at figure 6.46 it can be seen how the HDOP factor is completely out of
scale, the HDOP axis goes from zero up to 9 108 . Of course, all the parts of the
graphic that do not seem to have a value of zero are parts where there was no
signal at all.
110
Results
6.5.2
IMU Inputs
The yaw angle (figure 6.47) graphic does not say much, only that the driver
was not following a specific path
111
Results
Figures 6.48 and 6.49 show the acceleration in the x and y direction, respectively.
This acceleration is around zero almost during all the test in both cases (x and
y direction), so all that can be extracted from these graphics is that the test was
done driving at constant speed.

The 2 IMU measurements did not show much, but what could be seen is that they
did not present strange shapes like in the case of the GPS measurement, obviously
the IMU measurements are not affected by the quality of the GPS signal
6.5.3
As can be seen both in figure 6.50 and figure 6.51 as soon as the GPS signal is
lost, the estimation is totally off, it is even worse than the GPS measurements,
that were already bad enough.
Though looking at the zoomed parts of the figures it seems that when the GPS
signal is not totally lost, but it just has bad quality, the Kalman estimation is
quite good. The Kalman estimation is smoother than the GPS measurements,
ignoring those that derive from the rest of the curve.
112
Results
113
Results
114
Results
Figures 6.52 and 6.53 show the 3 kinematic parameters in the x and y direction,
respectively. The velocity estimation, looks very bad, lots of jumps (especially in
the y direction), but of course, having such a bad position estimation, the velocity
estimation will be as bad.
115
Results
6.5.4
In this case the IMAR platform is clearly better than the Kalman filter here
implemented, even in the parts were the GPS signal was not totally lost, and
the estimation seemed to be better than the GPS measurement (center graphic
of figure 6.54) the IMAR curve is much smoother. When the GPS signal is lost
(lower graphic of figure 6.54) the IMAR solution is perfect, while the estimation
is even worse than the initial GPS measurement.
In figure 6.55 the same conclusions can be extracted for the y direction
116
Results
117
Results
Results
6.6
6.6.1
118
Conclusions
Good GPS signal
As was seen in section 6.1 to 6.4 when the GPS signal is good enough, the Kalman
filter implemented in this thesis is very accurate. The position estimations fit the
curve drawn by the GPS measurements perfectly, obtaining precisions of about
1cm, good enough to fit our purposes.
The update rate of these estimations is of 100Hz as desired. With this update
rate the signal can be considered continuous, as needed to serve as an input for
the path following controller.
Good results were also obtained when the signal was good, but lost a bit of precision (see section 6.5.3). In this case the estimations followed the position curve
more smoothly than the GPS measurements, which drifted from the expected
curve. The Kalman gain worked perfectly as a weight factor.
The filter was considered a success in this case (good GPS signal) since it was
even better than the one implemented in the comertial IMAR platform.
6.6.2
Bad GPS signal
On the other hand, when the GPS signal was bad (high HDOP) or if there was
none at all, the Kalman filter here implemented did not work at all (section 1.5.4).
The estimations drifted way too much. Being the HDOP factor so high (up to
9 108 ) the GPS measurements were never considered by K (Kalman gain), so
the signal did not correct its drifting errors. The acceleration measured by the
IMU is suppose to let the position know how it should react in the text time
interval, but our IMU is not good enough to do this alone (without the help of
the GPS measurements). For our purpose this is not so important because the
driver assistance system will be tried out in a test field were the GPS has already
been tested and the signal is perfect, but for the future it could be interesting to
used a IMU with a higher precision.
Results
6.7
119
Future Challenges
The main challenge is, now that the Kalman filter has been implemented and
works (at least with a good GPS signal), to implement the filter in C + + and
adjust it to work real time. This is, to add a module to the C + + program that
logged the data, so that once the data is logged, it directly enters the Kalman
algorithm and calculates the state estimations that will be passed on to the path
following algorithm. The driver assistance system can not be tried out till the
fusion is developed real time.
Another possible challenge, as mention in the previous section, is to enhance the
filter when having a bad GPS signal. A more precise IMU would be needed in
order to achieve this goal.
Other future challenges also appeared when trying out the GPS. They can be
found in Annex A.
REFERENCES
120
References
[MTi ] MTi and MTx User Manual and Tecnical Documentation
[OEM ] OEM4 Family Use Manual - Volue 1. Instalation and Operation
[Carlson 2004] Carlson, Christopher R.: GPS: Theory of Operations and Applications. 2004
[Clynch 2006] Clynch, James R.: The Global Positioning System. 2006
[Greg Welch 2001] Greg Welch, Gary B.: An Introduction to Kalman Filter.
University of North Carolina, 2001
[Kohler 2000] Kohler, Marjus: Using a Kalman Filter to Track Human Interactive Motion. Dortmund University, 2000
[K
uhnen 1995] K
uhnen, B
uhning E. Schepers A. Schmid M.: Unfallgeschehen
Auf Autobahnen-Structuruntersuchung. 1995
[Maybeck 1979] Maybeck, Peter S.: Stochastic Models, Estimation and Control.
Department of Electrical Engineering, Air Forse Institute of Tecnology, Ohio,
1979
[Roger C. Hayward 1998] Roger C. Hayward, J.David P.: GPS-Based Attitude for Aircraft. 1998
[Ryu 2004] Ryu, Jihan: State and Parameter Estimation for Vehicle Dynamics
Control Using GPS. 2004
[Thomas Sattel 2005] Thomas Sattel, Thorsten B.: Ground Vehicle Guidance Along Collision-Free Trajectories Using Elastic Bands. Heinz Nixdorf
Institute, University of Paderborn, 2005
ANNEXES
121
ANNEXES
Contents
1. Annex A: Novatel GPS Tryout...........................................................122
2. Annex B: Visit to Braunschweig.........................................................152
ANNEXES
122
Annex A
Novatel GPS Try Out (16.08.2006-18.09.2006).
This protocol will document everything done during the 4 weeks the GPS was rented,
explaining the different operating modes that were tried out, the problems that
appeared and how those problems were solved and what was left unsolved and will
been considered a future challenge. It is meant to be a quick guide for posterior
users of the GPS.
If there were any doubts or questions please contact Beln Arronte
b.arronte@gmail.com
All necessary documentation and software: GPS User Guide
1. Operating Modes
A single point solution, this is, one GPS antenna receiving position data from a
constellation of satellites is usually not very accurate due to the lengthen in the GPS
carrier path produced by Ionosphere and Troposphere, reflection in GPS signals
against objects and the relative position between the antenna and the satellites
(satellites positioned directly above the antenna give worse measurements than more
horizontal signals). Novatels GPS receivers can give accuracies of at best 1.8m with
a single point solution. This is why were we more interested in a differential GPS
solution.
A differential GPS (DGPS) is were there are two GPS antennas receiving position
data and are able to correct each other. Normally there will be a base station, this is,
an antenna located in a know position that does not move and a rover station, this
would be the antenna located on the car that does move. As the base station knows its
position, it is able to calculate the GPS signal errors (the difference between its
know, real, position and the supposed position the GPS is saying it has) so it can
send the rover station correction data on its position based on this difference. The
accuracy with this operation mode can go down to 2cm.
We tried two different modes to get a DGPS solution, using our own base station
with a second antenna and a second receiver and getting corrections from a virtual
base station, witch would be a mobile phone cell that is able to see our position. Both
methods will be better described in the on the ongoing paragraphs.
ANNEXES
123
1.1. Single Point Solution
Figure 1
Before testing the DGPS solution it is first necessary to check if a simple single point
solution is obtained and start getting familiar with the devises involved. To get a
single point solution the equipment needed is:
GPS receiver
Picture 1
We used Novatel ProPak-G2+DB9-RT2W
ANNEXES
124
GPS antenna
Picture 2
Picture 3
PC
Install software GPSolutoion
Car battery source
Antenna cable
ANNEXES
Source cable
Serial cable
125
The serial cable used is a regular RS232, though a virtual serial RS232 cable
with a serial port head on one end and a USB head on the other end (to
connect to the PC) can also be used.
If a USB-end-cable is going to be used it is necessary to install some software
so the computer can see the USB port as a virtual serial port.
USB software: USB Driver 3000
Installation guide: Release Notes 3000
Picture 4
In Picture 4 the basic single point setup can be seen. The receiver must be connected,
from left to right (see Picture 1) to:

Car battery source with the source cable through receivers output POWER
PC with serial (or virtual serial) cable though receivers output COM1,
COM2 or AUX, we have used output COM1
GPS antenna with a normal antenna cable though receiver output GPS
Outdoor, preferably open space with a clear view of the sky to get better satellite
signals open GPSolution and connect to the GPS receiver following the instructions
found on GPSolution Guide. If connected with a real serial port the com port will
normally be Com1, but if connected with a USB com it may be necessary to go
through all the com ports until the correct one is found. To find the correct USB com
ANNEXES
126
it is also helpful to look in START CONTROL PANEL SISTEMA

HARDWARE DEVICE ADMINISTRATOR PORTS
Note: it will only be possible to see the virtual com ports if the receiver powered and
is connected to the PC
Once the device is found remember to save the configuration setting with the correct
pot and baud rate desired, we have normally worked with 115200 bauds because this
is the fastest the GPS can support.
The GPS will be working correctly if in the GPSolution position window (View
position window) the position type is single (Figure 2), this is, there are enough
satellites to triangulate a solution (more than 4), if there are less than 4 satellites it is
necessary to go to a more open sky area to get a solution.
Figure 2
ANNEXES
127
1.2. DGPS Getting Corrections from a Virtual Base Station
Figure 3
In Figure 3 the basics of a DGPS solution getting corrections from a virtual base
station can be seen. What needs to be set in order to apply this method is the rover
station, the set up is just the same as for the single point position solution but we also
need to communicate with the virtual base station; that will be done through a GSM
modem, a call will be made to a phone company sending our position, they can also
see the position where the call was made from, so by calculating the difference they
can see the error and send us back the adequate correction data.
The equipment necessary is:
GPS receiver
Just like in the single point solution (Novatel ProPak-G2+DB9-RT2W)
GPS antenna
ANNEXES
128
GSM modem and GSM antenna
Picture 5
We used Wavecom-WM0D2 dual band modem
Sim card
Picture 6
In order to get correction data the sim card must be registered with either
ASCOS or SAPOS (when working in Germany) and the companies that have
been tested and work are O2 and T-Mobile, but it is necessary that these card
ANNEXES
129
are original from the phone company (a sim card from O2 or T-Mobile will
not work if it is purchased from The Phone House)
PC
Car battery source
Antenna, serial and source cables
Picture 7
Picture 7 shows how the connection should be made, it can be seen how the GSM
modem is connected to Com2 of the receiver being the rest of the connections
exactly the same as in the example for single point solution.
In Picture 8 and 9 the setup on the car can be seen
ANNEXES
130
Picture 8
Picture 9
ANNEXES
131
Once the connection has been made we are ready to try out the GPS.
1. Open GPSolution and check if the single point solution is working correctly.
2. Open the console window (View Console window)
The console window is the interface between the PC and the receiver.
As can be seen in
Figure 4 it is working correctly, connected to con USB1 and waiting for a
command
<OK
[USB1]
The commands must be typed in, and if valid they will appear on screen
followed by <OK (
Figure 5). If the message is invalid an error will appear and the commands
will not be passed on to the receiver, so it should be corrected and typed in
again (the invalid commands have no consequences, so no need there is no
need to start again)
Figure 4
ANNEXES
132
Figure 5
3. Type in the next commands to get corrections from the virtual base station
See Note1 on how to send commands via Hyper Terminal
All commands con be found in OEM4 User Manual Volume 2, the pages
have been identified on each command
FIX NONE
Makes sure that the receiver is not fixed any

specified position [Page 77]
UNLOGALL
Removes previous log commands from system

from all the ports [Page 131]
COM COM1 115200
Sets receivers com baud rates, note that we are

talking about the receivers coms, not the PCs
coms, so if the PC is connected to COM1 and we
want it to work at 115200 bauds this would be the
command needed [Page 59]
COM COM2 9600
COM2 baud rate, the modem is connected at

COM2, 9600 is normally the GSM radio baud
rate
INTERFACEMODE COM1 NOVATEL NOVATEL
ANNEXES
133
INTERFACEMODE specifies the type of data

the receiver can receive and transmit, COM1
interacts with the PC, so it transmits and receives
NOVATELs commands and logs [Page 87]
INTERFACEMODE COM2 RTCM NOVATEL OFF
COM2 interacts with the GSM modem, so it
should receive RTCM (Radio Technical
Commission for Maritime) corrections and
transmit NOVATELs commands and no
response is needed (OFF)
RTKSOURCE AUTO
Use RTK corrections that come in, no specified

station [Page 113]
PSRDIFFSOURCE AUTO Use DGPS corrections that come in, no specified

station [Page 104]
DYNAMICS AIR
The receiver is in a vehicle, so we should adjust

its
dynamics to those of its environment [Page
72]
ECUTOFF 5
The cut angle for satellites is 5, no satellites will

be tracked under 5 angle from the receiver [Page
73]
SEND COM2 "AT+CPIN=nnnn"

Sends the modem the sim cards pin number
(replace nnnn for the pin)
SEND COM2 "AT+CBST=7"
Lets the modem know that it is going to be
making an analog call (necessary for ASCOS that
work with analog, omit if working with SAPOS
that uses a digital protocol)
SEND COM2 "ATD020128982"
Dials the phone companys number to get
correction data. 020128982 is one of ASCOS
numbers to get corrections, other possible
ASCOS numbers can be found in Ascos Numbers
To be sure the modem is working correctly it can
be tested. See Note2
LOG COM2 GPGGA ONTIME 5
ANNEXES
134
Logs our position to the GSM radio every 5

seconds [Page 188]
LOG COM1 BESTPOSA ONTIME 0.05
Logs our best position every 0.05 seconds [Page
161]
Note1: It is also possible to send the commands via some kind of interface
program such as Hyper Terminal (START PROGRAMS
ACCESSORIES COMMUNICATIONS HYPER TERMINAL).
Hyper Terminal allows you to send all the commands on a text file
instead of having to type them in one by one and also the data logged
can be seen (but it is raw data), how to do this can be found in
Working with HyperTerminal. The problem is that since it is all sent
at a time, if it doesnt work it is difficult to detect in what command
the problem was, but once it has been tryout it is much more
comfortable. Another problem is that some of the commands are not
admitted by Hyper Terminal.
Hyper Terminal can not work simultaneously with GPSolution, so
after sending the text file it is important to disconnect.
Note2: To check the modem it is as simple as trying to call a cell phone,

instead of dialing ASCOS number try dialing your own, and see if the
phone rings, if so, the modem is working correctly, but it is also
important that the callers number is identified on the screen (if the
number cant be seen, the company wont know where to send back
the correction data). If the number is not identified try putting *31#
between ATD and the number (SEND COM2
"ATD*31#020128982")
4. Check if working correctly

To know if you are getting a differential solution check the Position Type on
the position window, if Narrow or Wide Integer, or Narrow or Wide Float
you are getting a differential solution, they main difference between each of
these solutions is the accuracy. When getting any other type of solution check
the manuals OEM4 User Manual Volume 1 and OEM4 User Manual Volume
1
ANNEXES
135
Figure 6
5. Log the data of interest

How to log the data can be found on GPSolution Guide
If using Waypoints PostProcessing software it may be interesting to log
BINARY data, but if this is not the case, and the user is going to process it
itself, then the best is to log ASCII data, that can later on be imported in excel
or matlab.
BESTPOSA is one of the logs most used, tracing parameters such as latitude,
longitude, height, standard deviations, number of satellites [Page161 OEM4
User Manual Volume 2]
For Waypoints PostProcessing it is also needed to get the RINEX file from
the phone company to have also data from virtual base station.
For more information on Waypoint: PostProcessing with Waypoint
ANNEXES
136
1.3. DGPS Getting Corrections from our Own Base Station
Figure 7
Figure 7 shows the basics of this method, just like in the case of the virtual base
station, the base station sends its known position and by difference with the real
position it can calculate errors and send correction data to the rover station. The setup
of the single point position is the same, but the base station must also be set. The
necessary equipment is listed below:
2 GPS receivers, one for the rover station and one for the base station
Just like in the single point solution we used Novatel ProPak-G2+DB9RT2W for the rover station and Novatel ProPak-G2+DB9-Generic for the
base station, but they can probably be exchanged
2 GPS antennas (one for each receiver)
2 Radios to send correction data from the base to the rover station
We used Satel 2As (Picture 10)
2 Radio antennas and cables (Picture 10)
ANNEXES
137
Picture 10
PC
Car battery source
In Picture 11 the connections can be seen, both receivers are set just as if two
independent single point solutions were going to be found but also connecting the
radios to COM2 (just like we did with the modems in the previous setup). It may be
important to connect the radios before giving power and seeing if their lights go on
(the ones we tried would not work if connected after the receiver was already
powered.
ANNEXES
138
Picture 11
Picture 12
ANNEXES
139
Picture 12 shows a more clear view of how each receiver is independently connected
to its own PC, antenna, radio and battery (in the picture the battery is common, but if
when the base and rover are separated two different batteries will be needed). A PC
in the base station is not necessary though out all the measurements, only when first
set, to send commands to set up the receiver, this can also be done with one single
PC for both receivers, GPSolution is able to open to devices at a time (see Picture 15)
After seeing the connections indoor we are going to have a look at them in a real
scenario, outdoor.
Picture 13
Picture 13 shows the base station, the PC may or not may be necessary throughout all
the measurements depending on the post processing method. When using this
method (communicating through radio) it is important not to interfere with other
users, so it is better to try it in a place far away from buildings, or if you have some
kind of permission you can only work where discussed.
In Picture 14 both types of antennas (GSM and radio) can be seen fixed on the car,
both of them were tryout, but of course not both at a time, only one of them is
connected.
ANNEXES
140
Picture 14
Once the connection has been made we are ready to try out the GPS.
1. Open GPSolution and check if both receivers are working correctly, both
getting a single point solution is.
2. Open the console window for each receiver.
3. Type in the appropriate commands for each receiver
a) Base Station
The base station is going to be the one on witch the correcting depend, so
it is important it is setup first.
UNLOGALL
Explained in previous method [Page

131]
COM COM1 115200

59]
ANNEXES
COM COM2 9600
141
Explained in previous method, maybe it

can work at a higher baud rate, but it
has not been tested

Explained in previous method [Page 87]
INTERFACEMODE COM2 NONE RTCA OFF
COM2 of base station does not receive
any data but is able to transmit RTCA
corrections
FIX POSITION latitude longitude height
Fixes the position from the base station.
After waiting some time at the location
where the base station is going to be
placed with a single point position
solution replace latitude, longitude and
height for the known latitude, longitude
and height given by the single point
position solution [Page 77]
ECUTOFF 5
Explained in previous method [Page 73]
LOG COM2 RTCAREF ONTIME 10

Log base station parameters [Page 252]
LOG COM2 RTCAEPHEM ONTIME 10 1
Log ephemeris and time information
[Page 252]
LOG COM2 RTCAOBS ONTIME 1
Log base station observations [Page
252]
Note2: Not all these logs are necessary, but they are the most used, and
the ones we have used when testing the method
b) Rover Station
FIX NONE
UNLOGALL

77]
131]
ANNEXES
142
COM COM1 115200

59]
COM COM2 9600
Explained in method a) Base station

[Page 59]

87]
INTERFACEMODE COM2 RTCA NONE OFF
COM2 of base station receives RTCA
corrections but does not transmit
anything
RTKSOURCE AUTO

113]
PSRDIFFSOURCE AUTO

104]
DYNAMICS AIR

72]
ECUTOFF 5

73]

161]

Just like in the previous method to know if you are getting a differential
solution check the Position Type on the position window.
For the base station the position type should be Fixed
For the rover station it should be Narrow or Wide Integer, or Narrow or Wide
Float.
ANNEXES
143
When getting any other type of solution check the manuals OEM4 User
Manual Volume 1 and OEM4 User Manual Volume 1
Note3: Before testing sending corrections by radio it may be interesting just

to see if you are able to get corrections communicating both receivers by
cable. In Picture 15 can be seen how a cable is joining both COM2 ports from
each receiver instead of the being there radios connected. It is important that
the cable is cross (TX connected to RX and vice versa), so that one receivers
and the other sends. The commands do not have to be changed, and if this
tryout works than with radio it should also work, and if not the problem has
to be with the radios.
Picture 15
5. Log the data of interest

As explained on the previous method how to log the data can be found on
GPSolution Guide
When logging ASCII data for later analysis in excel or matlab it is enough
just to log the rover stations data
If using Waypoints PostProcessing software it may be interesting to log
BINARY data, and it is necessary to also log data from the base station, so
two different PCs are needed. It is important that the data logged from the
base station covers that from the rover, this is, the base station must start
ANNEXES
144
logging data before the rover station, and must stop logging after the rover
station.
1.4. DGPS Getting Corrections from a Virtual Base Station for 2

Antennas
Figure 8
This is not a different method to get correction data; here we are just trying to get
position data from 2 different moving objects, for example 2 cars or 2 positions on
the same car as shown on Figure 8. Placing 2 antennas on the top of a car allows us
to measure yaw angles by difference of position from the 2 antennas.
That can be done measuring data independently for one antenna and the other, 2
single point position solutions, the problem is that is we want more accurate
measurements, to get correction data for both antennas we need either a third antenna
working as base station or 2 sim cards cards, one for each antenna. We did not have
any of those, so we had to think of something else.
We tried to get a correction data by GSM modem for one of the receivers and them
pass it on (the correction data) to the other receiver by cable (it could also maybe be
pass by radio, but we didnt try that).
The equipment needed would be:
2 GPS receivers
Just like in the single point solution we used Novatel ProPak-G2+DB9RT2W for one antenna and Novatel ProPak-G2+DB9-Generic for the other
ANNEXES
2 GPS antennas
GSM modem and GSM antenna
145
Just like on the other method we used Wavecom-WM0D2 dual band modem
Sim card
PC
Car battery source
The connections for the receivers will be:

Receiver 1 (the one that calls the modem to get correction data)
Just like the connection made to get to for the DGPS getting correction data
from a virtual reference station (Picture 7), but also a cross cable is connected
to receivers input AUX
Receiver 2
Like the connection made to get a single point solution (Picture 4) with the
cross cable coming from AUX of receiver 1 connected to AUX of receiver 2
To try this method we went through the same basic steps as in the previous ones,
after preparing the installation:
1. Open GPSolution and check if both receivers are working correctly, both
getting a single point solution is.
2. Open the console window for each receiver.
3. Type in the appropriate commands for each receiver
a) Receiver 1
It is the one in charge of making the call to the virtual base station, so the
commands are pretty much the same as in this method with only one
ANNEXES
146
antenna but with some little changes. The commands already commented
will not be mentioned again, but can be seen in the previous method.
Note that even though the cable is connected to receivers input AUX in
the commands we type COM3, there is no rule for knowing if internally it
is named COM3 or AUX (the label outside does not have to be the same
as the one inside). To know if you have to type in COM3 or AUX you
have to try, and see when the lights go on on the port.
FIX NONE
UNLOGALL
COM COM1 115200
COM COM2 9600
COM COM3 115200
COM3 baud rate, used 115200 because it

is the highest
INTERFACEMODE COM1 NOVATEL

INTERFACEMODE COM3 NOVATEL RTCM OFF
COM3 is the one that sends the data
from receiver 1 to 2, so from receiver 1
it does not receive any data but is
transmits RTCM corrections
RTKSOURCE AUTO
PSRDIFFSOURCE AUTO
DYNAMICS AIR
ECUTOFF 5
LOG COM3 PASSCOM3B ONCHANGED
ANNEXES
147
The pass command redirects data from

one port to another, the B indicates that
it is binary data [Page 226]
This command is made in the receiver
that is sending the data, in our case
receiver 1
b) Receiver 2
It is the one that is supposed to get a differential solution with the data
coming through AUX by cable, so the commands are mostly the same as
the ones for receiver 1 but simpler, none of the commands for COM2 are
needed, and the pass command is also unnecessary (only needed on the
receiver sending the data)
FIX NONE
UNLOGALL
COM COM1 115200
COM COM3 115200
COM3 is the one that receives the data
on receiver 2 from receiver 1, so
receiver 2 receives RTCM corrections
but does not transmits any data
RTKSOURCE AUTO
PSRDIFFSOURCE AUTO
DYNAMICS AIR
ECUTOFF 5

Unfortunately the method did not work; we got a differential solution for the
receiver connected directly to the modem (receiver 1) but only a single point
solution for receiver 2
ANNEXES
148
As the connection by cable with the pass command did not work we had to look for
another way to pass the data, another possibility was splitting the data coming from
the modem to both of the receivers. This was made with a Y cable (1 input and 2
outputs) the data is inputted from the modem and then split to two different cables,
one of them not crossed (1:1) that goes to the receiver making the call (receiver 1)
and the other crossed (X) that goes to receiver 2.
The connections can be seen in Picture 16; both of the cables coming from the
modem are connected to COM2 of the receivers.
Picture 16
The commands in this case are very similar to the ones just mentioned, when we
were joining the receivers by cable through AUX
a) Receiver 1
None of the commands for COM3 are now needed (since it is not used),
and the pass commands has no use know anymore
FIX NONE
UNLOGALL
COM COM1 115200
COM COM2 9600
ANNEXES
RTKSOURCE AUTO
PSRDIFFSOURCE AUTO
DYNAMICS AIR
ECUTOFF 5
b) Receiver 2
Just the same as before, no COM3 commands
FIX NONE
UNLOGALL
COM COM1 115200
COM COM2 9600
RTKSOURCE AUTO
PSRDIFFSOURCE AUTO
DYNAMICS AIR
ECUTOFF 5
149
ANNEXES
150
Unfortunately this second experiment also did not work, the same as before, receiver
1, the one connected with the 1:1 cable that made the call got correction data, but the
other still had a single point solution.
We went through the commands with our GPS provider (Novatel) and they all
seemed to be correct for both of the tests (joining both receivers through AUX and
with the Y cable), so we are not sure if the had some mistake that we were not able to
detect or perhaps it is just not possible to use correction both coming for the same
source for 2 different receivers.
ANNEXES
151
2. Future Challenges
Working with Jan Schomerus GPSconnect program
GPSconnect is a program coded in C++, able to log position data from the GPS to an
excel file in a simple and intuitive way. It has been designed to operate a DGPS
solution getting data from our own base station, though it could probably be adapted
to work with a virtual base station.
We were not able to work with the GPSconnect because the program was unable to
detect the receiver, so debugging those problems will be one of the main challenges
for the future as well as adapting the program to work with a virtual base station.
Getting Correcting for two Antennas with only one Modem

As explained before (1.4) we were not successful when trying to get correction data
for two different receiver with only one modem (one sim card), we are not sure if it
is possible, but it would be good to look over it a little bit more, as this would be a
really good method to measure yaw angles with good precision and at a lower cost
than having 3 antennas (2 for measurements and 1 as base station), or 2 different
modems (1 for each antenna measuring position)
ANNEXES
152
Annex B
Visit to Braunschweig (07-08 August 2006)
This document explains in details everything done in Braunschweig, where the data
from the IMU, GPS and IMAR platform was captured synchronously.
Monday 07 August 2006

10:00-12:00: Adapting the GPS data logger to the IMU
All the variables of interest both from the GPS and IMU are logged real time and
saved in an excel file as can be seen in the example below.
The most important variables logged from the GPS would be quality of the signal
(Quality-column 7) being 5 the best quality and 1 the worst, the position (Loc X, Loc
Y, columns 12-13) and the measurement error, that has something to do with
pdop(position dilution of precision), hdop (horizontal dilution of precision), vdop (vertical
dilution of precision) (columns 14-15-16).
From the IMU the variables of interest would be all the calibrated data: accelarations,
turn rates and location referenced to the magnetic north (X-Y-Z-acc, X-Y-Z-turn, XY-Z-mag) as well as the Euler angles: roll, pitch and yaw.
Total
Seconds
0,0193
0,0273
0,0446
0,0515
0,0595
0,0665
0,0844
0,0907
0,0984
0,1064
Y-acc
3,285
3,926
3,983
3,520
3,456
3,083
2,569
3,339
3,818
2,626
X-acc
-0,786
-0,984
-0,974
-0,822
-0,537
-0,153
0,103
0,256
0,127
-0,228
IMU data
8,888
9,528
10,749
10,241
10,208
11,234
10,429
10,078
10,953
10,280
Z-acc
GPS data
4
4
4
4
4
4
4
4
4
4
-0,033
-0,022
-0,027
-0,073
-0,036
-0,023
-0,044
-0,045
-0,035
-0,032
-0,012
-0,044
-0,060
-0,051
-0,042
-0,023
-0,037
-0,024
-0,025
-0,021
Y-turn
52,316
52,316
52,316
52,316
52,316
52,316
52,316
52,316
52,316
52,316
0,828
0,823
0,836
0,841
0,831
0,831
0,815
0,816
0,826
0,834
Z-turn
-0,400
-0,398
-0,397
-0,400
-0,399
-0,396
-0,398
-0,397
-0,396
-0,398
0,031
0,034
0,037
0,040
0,043
0,045
0,049
0,053
0,055
0,058
49,97
49,97
49,99
49,99
49,99
49,99
50,03
50,03
50,03
50,03
Loc X
-0,906
-0,908
-0,906
-0,907
-0,905
-0,908
-0,905
-0,906
-0,907
-0,908
Z-mag
177,9
177,9
174,4
174,4
174,4
174,4
172,6
172,6
172,6
172,6
Course
Y-mag
1,002
1,002
1,014
1,014
1,014
1,014
0,973
0,973
0,973
0,973
Speed
X-mag
10,564
10,564
10,564
10,564
10,564
10,564
10,564
10,564
10,564
10,564
Quality Latitude Longitude
X-turn
Total
System System System System
GPS
Seconds Time[h] Time[m] Time[s] Time[ms]
Time
0,0193
12
30
50
9
123049
0,0273
12
30
50
9
123049
0,0446
12
30
50
41
123049
0,0515
12
30
50
41
123049
0,0595
12
30
50
41
123049
0,0665
12
30
50
56
123049
0,0844
12
30
50
72
123050
0,0907
12
30
50
72
123050
0,0984
12
30
50
88
123050
0,1064
12
30
50
88
123050
Example
11,10
11,15
11,18
11,16
11,17
11,15
11,11
11,10
11,11
11,08
Roll
-7,65
-7,65
-7,81
-7,81
-7,81
-7,81
-7,98
-7,98
-7,98
-7,98
Loc Y
-5,75
-5,74
-5,74
-5,75
-5,77
-5,80
-5,84
-5,89
-5,93
-5,95
Pitch
1,4
1,4
1,4
1,4
1,4
1,4
1,4
1,4
1,4
1,4
Yaw
0,9
0,9
0,9
0,9
0,9
0,9
0,9
0,9
0,9
0,9
hdop
-149,20
-148,70
-148,21
-147,73
-147,25
-146,79
-146,35
-145,84
-145,32
-144,89
pdop
1,1
1,1
1,1
1,1
1,1
1,1
1,1
1,1
1,1
1,1
vdop
ANNEXES
154
12:00-13:00: Lunch
13:00- 18:00: First tests

We used a differential GPS, where there are two GPS receivers, a fixed (reference)
and the one that moves (car) obtaining very small errors (a couple of centimeters)
when moving close to the reference (200m)
Figure 9
Reference Receiver:
Picture 17
ANNEXES
155
Test car where the second GPS receiver is placed:
Picture 18
The GPS was already installed and had been tested; it worked correctly by itself, now
we had to fix the measuring unit to the car and see if it worked properly and logged
predictable (expected) values.
In order to apply a Kalman filter to fusion the IMU and GPS we need to log data
from the same point, so we fixed the measuring unit direct after the GPS on the top
of the car, trying to fix the measuring units X-axis on the cars longitudinal axis and
the measuring units Y-axis on the cars lateral axis.
ANNEXES
156
Picture 19
Picture 20
Picture 21
ANNEXES
157
Step 1: IMU Calibration

Before logging any data it is necessary to calibrate the IMU in order to correct the
error due to the magnetic fields that distort the measurements. This is done with the
magnetic field mapper software (included in the IMU kit). For a 2D calibration (we
didnt need 3D since we were going to move throughout a flat surface) the measuring
unit has to move thought at lest a full 360 circle, preferably at constant speed. We
drove in circles both ways to be certain to describe all possible directions. The
magnetic field mapper report provides information on how accurate your
measurements will be as can be seen in figure 2.
Figure 10
ANNEXES
158
Step 2: Checking if the IMU works correctly

The MT software 3D view is a useful tool to quickly check if the IMU is working.
Figure 11
We viewed it while driving, and it moved just like the car did, the sensor worked
correctly.
ANNEXES
159
Step 3: First log without movement to check if the logger program works correctly
File: gpslog060807121750_1minStillstand
Note: all the file names start with gpslog followed by a 12 digit number that
represents the date and time when the test was ran
(year/month/day/hour/minute/second).
Figure 12
As can be seen in figure 4 we were located, more or lees, 15m in Y and 40m in X
away from the reference receiver, consistent to where we were standing. The
variation both in X and in Y are from the furthest to the nearest measure about 10cm,
when we were standing still, a bit of an error, but the GPS seemed to be working.
ANNEXES
160
z-Accel [m/s2]
y-Accel [m/s 2]
x-Accel [m/s 2]
Accelerations
0.4
0.3
0.2
1000
2000
3000
4000
5000
6000
7000
8000
1000
2000
3000
4000
5000
6000
7000
8000
1000
2000
3000
4000
5000
6000
7000
8000
0.5
0
10.2
10
9.8
Figure 13
The accelerations measured from the IMU were close to zero in X and Y and gravity
acceleration in Z. The IMU also seemed to be working.
ANNEXES
161
Step 4: Real Tests driving though diverse paths and in different ways
Test 1. From the football field to the garage drawing a little curve in the
middle
File: gpslog060807122555_geradeausfahrt01
Test 2. From the garage back to the football field
Test 3. Drawing a complete circle in 1st gear with the steering wheel turned
all the way (360)
File: gpslog060807123308_kreis01
Test 4. Drawing a complete circle in 3rd gear with the steering wheel turned
all the way (360)
Test 5. Drawing a complete circle in 1st gear with the steering wheel turned
half way (180)
Test 6. Driving throughout a strait path accelerating and decelerating rapidly
at first, then a relax ride and near the end of the test we lost the some
quality of the GPS signal due to some obstacles
File: gpslog060807124352_FahrtumFlughafen
Test 7. Same ride as in test 1
Test 8. Same ride as in test 2
ANNEXES
162
Step 5: Reviewing the logged data

Looking at the data of one of the tests, for example test 3 (Drawing a complete circle
in 1st gear with the steering wheel turned all the way (360)) we can easily see if the
data is or isnt correct.
Figure 14
ANNEXES
163
z-Accel [m/s2]
y-Accel [m/s 2]
x-Accel [m/s 2]
Accelerations
2
1
0
-1
1000
2000
3000
4000
5000
6000
1000
2000
3000
4000
5000
6000
1000
2000
3000
4000
5000
6000
2
0
-2
-4
12
10
Figure 15
The location seemed correct as well as the accelerations, around 0.5 m/s2 in X and Y,
consistent to a 1st gear circle and gravity acceleration in Z.
The problem came when we looked at the turn rates (figure 8) and angles (figure 9).
The yaw-rate wasnt constant as expected, the roll and pitch angle werent constant
zero and the yaw angle didnt describe a circle. We had to revise the logging
program
ANNEXES
164
Yaw-Rate [rad/s]
Pitch-Rate [rad/s]
Roll-Rate [rad/s]
Turning Rates from Gyros

0.5
-0.5
1000
2000
3000
4000
5000
6000
1000
2000
3000
4000
5000
6000
1000
2000
3000
4000
5000
6000
-0.88
-0.9
-0.92
-0.94
0.5
-0.5
Figure 16
Yaw-ang [deg]
Pitch-ang [deg]
Roll-ang [deg]
Orientation
0.5
-0.5
1000
2000
3000
4000
5000
6000
1000
2000
3000
4000
5000
6000
1000
2000
3000
4000
5000
6000
0.5
-0.5
-0.88
-0.9
-0.92
-0.94
Figure 17
ANNEXES
165
Tuesday 08 August 2006

09:00-12:00: Debugging the data logger
We were sure the yaw angle measurements were wrong, but not so sure about the roll
and pitch angle measurements, they were not a constant zero as expected, but had
values near to zero, we were moving over a flat surface, but there could have been
bumps, the first thing we had to do was find out was if those values were also wrong
or not.
To find out, we ran the program and manually moved the IMU, making it roll and
pitch, that was enough to let us know that those values were also incorrect, it wasnt
only the yaw angle that was wrong.
Finally the problem was solved and we could do the car test again, hopping this time
we would obtain correct values.
12:00-13:00: Lunch
13:00-17:00: Second Tests

This time our test would be much more complete, because we would simultaneously
log data from the GPS and IMU as well as from a real device that already has a
Kalman filter installed and provides the state estimate, this way, we can implement a
Kalman filter to our data and check if the process is correct or not.
Step 0: Time Synchronizer
The device that provides the state estimate loges the data directly on a computer
placed inside the car, while the GPS and IMU data is logged on a laptop, in order to
compare them, once we have implemented our Kalman filter, we need them to have
the same time reference.
In picture 6 the time synchronizer can be seen, figure 7 shows the two computers that
will log the data.
ANNEXES
166
Picture 22
Picture 23
ANNEXES
Step 1: IMU Calibration
Just the same as in the first day we calibrated our magnetic field, in order to get the
best precision as possible.
Step 1: Object Reset

The first day we forgot to reset the object, this is necessary because the reference
station was located in a bit tilted plane, so then we would always have a pitch angle
even when the car was horizontal. When resetting we only changed the pitch angle
reference, the yaw angle reference was consistent with the magnetic north, and the
roll angle was fixed by the other two angles.
Step 2: Checking if the IMU works correctly
The same as in the previous day.
167
ANNEXES
168
Step 3: First log without movement to check if the logger program works correctly
File: gpslog060808122301_1minStillstand
Figure 18
The location seems to be correct, just like in the first tests a bit of movement can be
seen, but it is only about 10cm, it can be considered standing still.
ANNEXES
169
Accelerations
x-Accel [m/s2]
0.4
0.2
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
y-Accel [m/s 2]
-0.2
-0.4
z-Accel [m/s 2]
10
9.9
9.8
9.7
Figure 19
The accelerations are really close to zero in X and Y as they should since we are still
and Y suffers gravity acceleration, seems correct.
Yaw-Rate [rad/s]
Pitch-Rate [rad/s]
Roll-Rate [rad/s]

0.05
-0.05
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
0.05
-0.05
0.1
-0.1
Figure 20
The turn rates are all zero as they should.
ANNEXES
170
Orientation
Roll-ang [deg]
-0.5
-1
Pitch-ang [deg]
-1.5
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
1000
2000
3000
4000
5000
6000
7000
8000
9000
10000
-0.5
-1
-1.5
Yaw-ang [deg]
-2
-2.5
-3
Figure 21
The angles are constant, much better than in the first tests, the yaw angle seems to
change a little, but only about 0.5, we can assume it is constant.
The data logger seems to be working, we can start moving.
ANNEXES
171
Step 4: Real Test driving though diverse paths and in different ways
Test 1. Drawing a complete small circle turning right in 1st gear with the
steering wheel turned all the way (360, full turn)
File: gpslog060808122614_kleinerKreisGang1rechts
Test 2. The same as test 1 but in a different location
File: gpslog060808122746_kleinerKreisGang1rechts02
Test 3. Drawing a complete small circle turning left in 1st gear with the
steering wheel turned all the way (360, full turn)
File: gpslog060808122923_kleinerKreisGang1links
Test 4. Drawing a complete small circle in 3rd gear with the steering wheel
turned all the way (360, full turn)
File: gpslog060808123049_kleinerKreisGang3
Test 5. Drawing a complete large circle in 1st gear with the steering wheel
turned all the way (180, half turn)
File: gpslog060808123216_grosserKreisGang1
Test 6. Driving with no specific direction accelerating and decelerating
rapidly
File: gpslog060808123550_ChaosFahrt
Test 7. Driving throughout a strait path in 1st gear
File: gpslog060808123808_geradeausGang1
Test 8. Driving throughout a strait path stopping and driving again and some
extra turn (we forgot to stop the data logger)
File: gpslog060808124034_geradeaus(zurueck)Stop&Go
Test 9. Driving throughout a strait path in 3rd gear
File: gpslog060808124225_geradeausGang3
Test 10. Driving between building and trees to see signal how the signal
quality changed
File: gpslog060808124724_DLRGelaende
ANNEXES
172
Step 5: Reviewing the logged data

We are going to briefly look at a pair of test to see if the data was correct.
Test 1. Drawing a complete small circle turning right in 1st gear
File: gpslog060808122614_kleinerKreisGang1rechts
As can be seen the GPS works perfectly, providing the same positions the car drove.
Figure 22
Accelerations tending to zero, figure 15 (except for Z-axis, gravity), we were driving
at a constant speed.
Turn rates zero for roll and pitch and about 0.4rad/s for yaw rate, consistent with a 1st
gear circle (figure 16).
ANNEXES
173
z-Accel [m/s 2]
y-Accel [m/s 2]
x-Accel [m/s 2]
Accelerations
1
0
-1
-2
1000
2000
3000
4000
5000
6000
7000
1000
2000
3000
4000
5000
6000
7000
1000
2000
3000
4000
5000
6000
7000
2
0
-2
-4
11
10
9
8
Figure 23
Yaw-Rate [rad/s]
Pitch-Rate [rad/s]
Roll-Rate [rad/s]

0.2
-0.2
1000
2000
3000
4000
5000
6000
7000
1000
2000
3000
4000
5000
6000
7000
1000
2000
3000
4000
5000
6000
7000
0.1
-0.1
0
-0.2
-0.4
-0.6
Figure 24
ANNEXES
174
Orientation
Roll-ang [deg]
-2
-4
-6
1000
2000
3000
4000
5000
6000
7000
1000
2000
3000
4000
5000
6000
7000
1000
2000
3000
4000
5000
6000
7000
Pitch-ang [deg]
2
0
-2
-4
Yaw-ang [deg]
200
-200
Figure 25
In figure 17 we can see how 4 complete circles were drawn, and a pattern is shown in
the roll and pitch angles, that vary about 3, there where probably some little bumps
that made the car tilt a little.
Figure 18 shows the yaw angle, the first graphic counts only from -180 to180, we
can see how 4 circles are drawn, but it doesnt show a real evolution. The second
graphic doesnt have any value constrains, being easier to observe how the circles
were consecutively drawn.
ANNEXES
175
Yaw-ang [deg]
200
100
0
-100
Unwrapped Yaw-ang [deg]
-200
1000
2000
3000
4000
5000
6000
7000
1000
2000
3000
4000
5000
6000
7000
0
-500
-1000
-1500
-2000
Figure 26
ANNEXES
176
Test 6. Driving with no specific direction accelerating and decelerating

rapidly
File: gpslog060808123550_ChaosFahrt
Figure 27
In this test we drove through diverse paths accelerating and stopping the car when we
were on a straight path.
As can be seen in figure 20 high accelerations were obtain both in longitudinal
(5m/s2) and lateral directions (10m/s2)
There was also yaw rate when we drove in curves (figure 21).
ANNEXES
177
Accelerations
x-Accel [m/s 2]
-5
1000
2000
3000
4000
5000
6000
7000
8000
9000
1000
2000
3000
4000
5000
6000
7000
8000
9000
1000
2000
3000
4000
5000
6000
7000
8000
9000
y-Accel [m/s 2]
10
0
-10
-20
z-Accel [m/s 2]
15
10
Figure 28
-0.5
1000
2000
3000
4000
5000
6000
7000
8000
9000
1000
2000
3000
4000
5000
6000
7000
8000
9000
1000
2000
3000
4000
5000
6000
7000
8000
9000
0.5
-0.5
Yaw-Rate [rad/s]
Pitch-Rate [rad/s]
Roll-Rate [rad/s]

0.5
1
0
-1
-2
Figure 29
ANNEXES
178
Orientation
Roll-ang [deg]
20
0
-20
-40
1000
2000
3000
4000
5000
6000
7000
8000
9000
1000
2000
3000
4000
5000
6000
7000
8000
9000
1000
2000
3000
4000
5000
6000
7000
8000
9000
Pitch-ang [deg]
10
0
-10
-20
Yaw-ang [deg]
200
-200
Figure 30
We also road up and down little hills as can be seen in the roll and pitch angle in
figure 22.
Yaw-ang [deg]
200
100
0
-100
Unwrapped Yaw-ang [deg]
-200
1000
2000
3000
4000
5000
6000
7000
8000
9000
1000
2000
3000
4000
5000
6000
7000
8000
9000
200
0
-200
-400
-600
-800
Figure 31
ANNEXES
CONCLUSION:
We have achieved or targets, logged the data, and made sure that it was correct, now
we just need to implement the Kalman filter and check the process with the real
device that provides the state estimate.
179
PART II: BUDGET
ii
CONTENTS
Contents
1 Budget
1.1
Equipment Expenses . . . . . . . . . . . . . . . . . . . . . . . . .
1.1.1
IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.1.2
GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.1.3
Computers . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.1.4
Test Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . .
1.1.5
Total Equipment Expenses . . . . . . . . . . . . . . . . . .
1.2
Software Expenses . . . . . . . . . . . . . . . . . . . . . . . . . .
1.3
Human Labor . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.4
Other Expenses . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.4.1
Office Stationary . . . . . . . . . . . . . . . . . . . . . . .
1.4.2
Phone Bill . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.4.3
Trip to Braunschweig . . . . . . . . . . . . . . . . . . . . .
1.4.4
Total Other Expenses
. . . . . . . . . . . . . . . . . . . .
Total Budget . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.5
Annexes
10
Budget
Budget
This chapter will study the costs needed to develop this project, which will basically be divided in:
Equipment expenses (fixed tangible assets)
Software expenses (fixed intangible assets)
Human labor (current assets, salaries)
Others: office stationary, trips, phone bill (current tangible and intangible
assets)
Notice that in the case of the fixed assets the amortization most be considered in
order to calculate the real cost or the project. This amortization will be done in a
2 year basis, which is the duration of the overall driver assistance system project,
considering that this specific part of the project last 6 months; this is, the real
cost of the fix assets for this part of the project will be a forth of the total cost.
All cost are expressed in Euros except for the GPS expenses, which appear in US
Dollars. Once the total GPS cost is summed up, the change of currency will be
made.
Budget
1.1
1.1.1
Equipment Expenses
IMU
Product
Quantity Unit Price
Discount
Amount
MTi Attitude Heading Ref Syst 1
1.759,00
10
1.575,00
USB Cable MTi RS-232
100,00
10
90,00
Serial Cable MTi (ODU plug)
30,00
100
0,00
MT Software Development Kit
450,00
10
405,00
Suitcase MT DK
25,00
10
22,50
DEV Kit Discounts
-335,00
10
-301,50
Shipping Cost
35,00
35,00
TOTAL EUROS
1.826,00
2 year amortization
456,50
The IMU quotation can be fund in Annex C.

1.1.2
GPS
Product
Discount
Amount
11.495,00
11.495,00
DL4+L1L2 L1-/L2-GPS Re- 1

ceiver
9.995,00
9.995,00
University Discount
-10.745,00
-10.745,00
GPS 702 L1-/L2-GPS Antenna 2
995,00
1.999,00
Antenna Cable 5m,TNC-TNC
80,00
80,00
DL4+RT2
ceiver
Quantity Unit Price

L1-/L2-GPS
Re- 1
Budget
Antenna Cable 15m,TNC-TNC 1
160,00
160,00
Magnetic
/Bracket
230,00
460,00
Antenna
Support 2
2-way-radio Modem
1.500,00
3.000,00
GSM-Modem
750,00
750,00
1.350,00
1.350,00
Tariff, Charge and Additional 1

Cost
TOTAL
Discount
18.535,00
16,2
3.000,00
TOTAL US DOLLARS
TOTAL EUROS
15.535,00
20.195,50
2 year amortization
5.049,00
The GPS quotation can be fund in Annex D and E.
Budget
1.1.3
Computers
Product
Quantity Unit Price
Discount
Amount
Sony Vaio VGN-SZ1HP
2.000,00
2.000,00
Dell Inspiron 9400
1.000,00
1.000,00
TOTAL EUROS
3.000,00
2 year amortization
750,00
Two laptops were needed because data was captured from both the GPS base
station receiver as well as from the rover station receiver. Data was logged form
both receivers because an important part of this thesis was to understand how
the differential GPS works. Once the GPS has been studied and how it works is
totally clear to the developer engineer, only one laptop will be necessary.
1.1.4
Test Vehicle
In the case of this project the test vehicle was a donation for the university from
the vehicle brand, BMW. This car was out of circulation, it could only be used
for study inside the test field, and of course, it could not be sold afterward. This
is why in this particular case the vehicle has no cost associated, but it should
be an important cost to considered if wanting to develop a similar project in the
future.
1.1.5
Total Equipment Expenses
Equipment
Total Cost
2 year Amortization
IMU
1.826,00
456,50
GPS
20.195,50
5.049,00
Computer
3.000,00
750,00
Vehicle
0,00
0,00
TOTAL EUROS
25.021,50
6.255,50
Budget
1.2
Software Expenses
All software needed to use the GPS and IMU is already included in the equipment
expenses. It is also considered that the laptops come completely equipped with
Windows XP and Microsoft Office. Therefore, the only two software expenses are
Matlab and Microsoft C++ Visual Studio.
Product
Quantity Unit Price
Discount
Amount
Matlab 7.1 + signal processing 1

toolbox
3.100,00
3.100,00
Microsoft C++ Visual Studio
799,00
799,00
TOTAL EUROS
3.899,00
2 year amortization
974,75
Budget
1.3
Human Labor
The Human Labor has been calculated considering cost per hour of the engineer
developing the project, which is estimated to be 50 euros per hour and of the
engineer supervising the project, estimated 70 euros per hour. The duration of
the project was 6 months, considering 20 day months (no work on weekends), the
total days work sum up to be 120 days. The developing engineer dedicates all his
working time to the project, but the supervisor only dedicates 1 hour per day.
Considering all the mentioned above:
Type of Human Labor
Cost per Duration Hours

Hour
(days)
per Day
Total
Hours
Amount
Engineer
project
developing
the 50
120
960
48000,00
Engineer
project
supervising
the 70
120
120
8400,00
TOTAL EUROS
56400,00
Budget
1.4
Other Expenses
1.4.1
Office Stationary
Paper
Pens
DVDs
Printer
Total estimated cost 150 Euros
1.4.2
Phone Bill
When obtaining GPS corrections from a virtual GPS station through a GSM
modem, it was already explained that a phone call had to be done. Since these
calls were long, the cost has to be considered.
Total estimated cost 100 Euros
1.4.3
Trip to Braunschweig
The data used to implement the Klaman Filter was captured in a partner company
in Braunschweig (city about 200Km away). It was necessary to capture the data
in Braunschweig because it is where there was access to the IMAR platform. The
cost of the trip was:
Hotel: 2 rooms (one for the developer engineer and one for the supervisor
engineer) at an approximated cost of 100 euros per room, sums up to 200
euros
Car rent: 2 day rent cost approximately 200 euros
Gas: approximately 50 euros for the 400 Km
The whole trip sums up to a cost of 450 euros
Budget
1.4.4
Total Other Expenses
Expense
Total Cost
Office Stationary
150
Phone Bill
100
Trip to Braunschweig
450
TOTAL EUROS
700,50
Budget
1.5
Total Budget
Expense
Total Cost
Equipment Expenses
6.255,50
Software Expenses
974,75
Human Labor
56.400,00
Others
700,50
TOTAL EUROS
64.330,75
It can be observed that the cost that has the most weight in this thesis is the
human labor, but remember that in this case the university discounts were very
generous, furthermore, the car was donated for research purposes. It is important
to note this because the figures here presented can create a wrong opinion of the
real cost of this project.
ANNEXES
ANNEXES
Contents
1. Annex C: IMU Quote......................................................................11
2. Annex D: GPS Quote 1...................................................................12
3. Annex E: GPS Quote 2...................................................................15
10
University of Paderborn
Tobias Hesse
Heinz Nixdorf Institut
Frstenallee 11
33102 Paderborn
Germany
Quote
Page 1
Invoice Address
University of Paderborn
Tobias Hesse
Heinz Nixdorf Institut
Frstenallee 11
33102 Paderborn
Germany
Salesperson
E-Mail
Shipment Address
Marcel van Hak

info@xsens.com
Xsens Technologies B.V.

P.O. Box 545
7500 AM Enschede
The Netherlands
Phone No.
Fax No.
VAT Reg. No.
Bank
Account No.
IBAN
BIC
+31-53-4836444
+31-53-4836445
NL808707309B01
ABN Amro
565550675
NL94 ABNA 0565 5506 75
ABNANL2A
October 27, 2006

Quote No.
Line
20000
30000
35000
40000
50000
60000
70000
Product No.
Description
MTi DEVELOPMENT KIT

MTI-28A53G35
MTi Attitude Heading Ref Syst
standard (5g, 300 deg/s)
CA-USB2
USB Cable MTi
RS-232
CA-SERI
Serial cable MTi (ODU plug)
SW-SDK
MT Software Development Kit
CASE-MTI
Suitcase MT DK
DISC-DK
DEV KIT DISCOUNT
SHIPPING
Shipping Costs
10666
Quantity
Unit Price
D%
Amount
1,750.00
10
1,575.00
100.00
10
90.00
1
1
1
1
1
30.00
450.00
25.00
-335.00
35.00
100
10
10
10
405.00
22.50
-301.50
35.00
Extra sensor + USB cable + serial cable:

1695 Euro
Total EURO
Shipment Method
Shipment Date
1,826.00
CPT shipment address

November 22, 2006
Payment options:
- Payment in Advance (2% reduction)
- Credit card payment (VISA or MasterCard)
- Account (not eligible for first purchase or exceeding Euro 10,000.00)
This offer is 30 days valid. All our deliveries and orders will be executed in accordance with our general terms and conditions
as registered at the Chamber of Commerce Veluwe & Twente, no 08088230, available at www.xsens.com.

Sensor Fusion of Differential Gps and Inertial Measuring Unit To Measure State of A Test Vehicle

Enviado por

Dados do documento

Título original

Direitos autorais

Formatos disponíveis

Compartilhar este documento

Compartilhar ou incorporar documento

Opções de compartilhamento

Você considera este documento útil?

Este conteúdo é inapropriado?

Direitos autorais:

Formatos disponíveis

Sensor Fusion of Differential Gps and Inertial Measuring Unit To Measure State of A Test Vehicle

Enviado por

Direitos autorais:

Formatos disponíveis

UNIVERSIDAD PONTIFICIA COMILLAS

ESCUELA TCNICA SUPERIOR DE INGENIERA (ICAI)

PROYECTO FIN DE CARRERA

BELN ARRONTE ARROYUELOS

MADRID, junio de 2007

GPS: Global Positioning System . . . . . . . . . . . . . . . . . . .

Single Point Solution . . . . . . . . . . . . . . . . . . . . .

Differential GPS Solution: DGPS . . . . . . . . . . . . . .

IMU: Inertial Measurement Unit . . . . . . . . . . . . . . . . . . .

How the IMU Helps Solve the GPS Problems . . . . . . .

3 The Kalman Filter

Need for Stochastic Estimation . . . . . . . . . . . . . . . . . . .

Statistic Concepts Needed to Understand Kalman Filters . . . . .

How Kalman Filters Work . . . . . . . . . . . . . . . . . . . . . .

Kalman Filter from a Controls Point of View . . . . . . . .

Kalman Filter from a Mathematical Point of View

4 Kalman Filter to Estimate Position

Initial Conditions + x[k 1] and + P[k 1] . . . . . . . . . . . . .

Working with the Measuring Devices . . . . . . . . . . . . . . . .

Hardware and Software used . . . . . . . . . . . . . . . . .

Example of Logged Data . . . . . . . . . . . . . . . . . . .

Data Logging Pseudocode . . . . . . . . . . . . . . . . . .

Fusing the Data . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Rotation Matrix RGI . . . . . . . . . . . . . . . . . . . . .

Defining the time steps . . . . . . . . . . . . . . . . . . . .

Defining the Initial Conditions + x[k 1] and + P[k 1] . .

Kalman Filter Implementation Pseudocode . . . . . . . . .

Comparing with IMAR Platform . . . . . . . . . . . . . . . . . .

Test 1. Driving in a Large Circle . . . . . . . . . . . . . . . . . .

Kalman Filter Outputs . . . . . . . . . . . . . . . . . . . .

Comparing with IMAR Data . . . . . . . . . . . . . . . . .

Test 2. Driving in a Small Circle . . . . . . . . . . . . . . . . . .

Kalman Filter Outputs . . . . . . . . . . . . . . . . . . . .

Comparing with IMAR Data . . . . . . . . . . . . . . . . .

Test 3. Driving in a Straight Path . . . . . . . . . . . . . . . . . .

Kalman Filter Outputs . . . . . . . . . . . . . . . . . . . .

Comparing with IMAR Data . . . . . . . . . . . . . . . . .

Test 4. Driving Accelerating and Decelerating Rapidly . . . . . . 100

GPS Inputs . . . . . . . . . . . . . . . . . . . . . . . . . . 100

IMU Inputs . . . . . . . . . . . . . . . . . . . . . . . . . . 102

Kalman Filter Outputs . . . . . . . . . . . . . . . . . . . . 103

Comparing with IMAR Data . . . . . . . . . . . . . . . . . 106

Test 5. Driving Losing the GPS Signal . . . . . . . . . . . . . . . 109

GPS Inputs . . . . . . . . . . . . . . . . . . . . . . . . . . 109

IMU Inputs . . . . . . . . . . . . . . . . . . . . . . . . . . 110

Kalman Filter Outputs . . . . . . . . . . . . . . . . . . . . 111

Comparing with IMAR Data . . . . . . . . . . . . . . . . . 115

Good GPS signal . . . . . . . . . . . . . . . . . . . . . . . 118

Bad GPS signal . . . . . . . . . . . . . . . . . . . . . . . . 118

Future Challenges . . . . . . . . . . . . . . . . . . . . . . . . . . . 119

Kalman Filter Fusion . . . . . . . . . . . . . . . . . . . . . . . . .

GPS Error Sources . . . . . . . . . . . . . . . . . . . . . . . . . .

Example of a GPS calculated velocity . . . . . . . . . . . . . . .

Real velocity versus calculated velocity . . . . . . . . . . . . . . .

Example of a GPS calculated acceleration . . . . . . . . . . . . .

2.11 Sensor Fusion Algorithm . . . . . . . . . . . . . . . . . . . . . . .

Normal Probability Distribution Function fx (x) . . . . . . . . . .

Autocorrelation of a White Noise Signal . . . . . . . . . . . . . .

Kalman Filter Block Diagram . . . . . . . . . . . . . . . . . . . .

Kalman Filter Algorithm . . . . . . . . . . . . . . . . . . . . . . .

Kalman Filter Block Diagram to observe the effect of matrix C .

Kalman Filter Block Diagram . . . . . . . . . . . . . . . . . . . .

Radio and Radio Antenna . . . . . . . . . . . . . . . . . . . . . .