IMACS Multiconference

on

"Computational Engineering in Systems Applications"(CESA), October 4-6, 2006, Beijing, China.

1

GPS/INS Data Fusion for Land Vehicle Localization

C.Cappelle, D.Pomorski and Y.Yang Laboratoire d'Automatique, Genie Informatique et Signal (CNRS UMR 8146) Universite des Sciences et Technologies de Lille, Cite Scientifique, Polytech'Lille, Bat. D 59655 Villeneuve d'Ascq Cedex, FRANCE E-mail: Cindy.Cappelle@polytech-lille.fr, denis.pomorski@univ-lillel .fr, yanqin.yang @ed.univ-lillel .fr
-

Abstract For the land vehicle positioning, we need an accuracy that the actual sensors can't provide singly. So, we classicaly integrate two or more complementary sensors. In this paper, we present an algorithm based on Kalman filter, which integrates data from an Inertial Navigation System and a Global Positioning System receiver.
-

Keywords- Data Fusion, Inertial Navigation System (INS), Global Positioning System (GPS), Kalman Filter.
I.

: a GPS receiver and an INS. The second part introduces the theory of the Kalman filter. The third part develops the GPS/INS data fusion: we compare the different structures of filter and the inertial navigation model used. Then, in the section IV, we explain how we process the GPS data before filtering those data with the INS data. The fifth part is dedicated to the obtained results. And finally, in the sixth part, we expose our future prospects, objectives.

INTRODUCTION

There are two types of methods for the localization: the relative and the absolute localization. The relative localization provides the position and orientation of a vehicle with the successive displacements from the known initial configuration. Dead reckoning sensors (velocity sensor, gyrometer, accelerometer, ...) acquire information from the vehicle itself. They have a high acquisition frequency. But, the drawback is the position drift with the time of measure.
The absolute localization acquires information from the exterior of the vehicle, the environment. The absolute sensors are, for example, GPS, camera, ultrasound sensor, telemeter, With this method, there is no drift, but the principal inconvenient of the GPS is the lost of visibility of the satellites.
...

The present research work has been supported by Science and Technology for Safety in Transportation, and founded by the European Union, the 'Delegation Regionale 'a la Recherche et 'a la Technologie', the 'Ministere Delegue 'a l'Enseignement Superieur et 'a la Recherche', the 'Region Nord Pas de Calais' and the 'Centre National de la Recherche Scientifique'.
II.

THE GPS AND INS PRINCIPLE

[SUK 00] reminds the principle of the GPS and the INS and their errors. A. The GPS and the satellitary navigation
A. 1 The GPS description The satellitary navigation system are now used in a lot of applications and all over the world. Two systems currently
operate:

Absolute Sensors Absolute localization Low acquisition frequency Average accuracy in short term Very good accuracy in long term

Dead Reckoning Sensors Relative localization High acquisition frequency Very good accuracy in short term Bad accuracy in long term

. the American Global Positioning System (GPS), . the Russian Global Navigation Satellite System (GLONASS).
In
a

few

years,

the

european

system Galileo will operate

too.

This comparison shows that those two types of sensors are complementary. So, as said in [BER 04], an intuitive solution is the use of an absolute sensor to correct the data provided by a deadreckoning sensor.
The six sections following in this article are : first, a presentation of the sensors, we used for our experimentation

Those two systems are originally developed for military applications. So, the civilians can't be sure, they have the signals in the critical periods. The GPS satellites are equally spaced on six orbits. So that, an user everywhere on the globe can see at least four satellites (without consideration of surrounding structures). Each satellite transmits navigation and range data simultaneous on two frequencies LI (1575,42MHz) and L2(1227,60MHz). The initial purpose of implementing

21

IMACS Multiconference on "Computational Engineering in Systems Applications"(CESA), October 4-6, 2006, Beijing, China.

two frequencies is to remove the ionospheric errors associated with the transmission through the atmosphere.

The principle is based on measuring the distance, that means the difference between the moment of the transmission and the moment of the reception multiplied by the speed of light represents the range from the satellite to the receiver. Three satellites are required to calculate the three unknowns of position. But, the synchronization between the user and the satellites is required. As this synchronization is expensive for the user (we can't put an atomic clock in each receiver), an extra unknown of clock synchronization must be used. So, four satellites are necessary to determine the position.
To locate the satellite signals, the receiver generates an internal code which correspond to the GPS code. A phase lock loop tries to correlate the signal producted by the receiver to that of an incoming satellite signal. The maximum correlation means that the satellite is catched. The information from this satellite can then be read.

. High frequency faults. The high frequency faults appear when the GPS signals undergo multipath errors, because of a tree or a building, ... We have an another fault, when the receiver use an another set of satellites. Those high frequency errors can't be reduced by using a DGPS, because they depend on the local environment.

A.3 The used GPS : Thales SagittaO2 For our experimentation, we use a GPS sensor from the Thales Navigation firm : the SagittaO2 system. His acquisition cadency is 1OHz. The SagittaO2 has different modes for users : classical GPS, differential GPS, (DGPS, WAAS/EGNOS), and the Real Time Kinematic technique (LRK, KART/EDGPS).

B. The INS and the inertial navigation

B. 1 The INS description The inertial sensor measures the angular velocity and the acceleration of the vehicle: . the gyrometers measure the rotation, . the accelerometers measure the acceleration but they don't measure the acceleration due to the gravity. An inertial sensor is normally one of this three groups . ISA (Inertial Sensor Assembly) the raw data from the inertial sensor is the only output data, . IMU (Inertial Measurement Unit): an ISA but the raw data are corrected (bias, scale factors), * INS (Inertial Navigation System): an IMU but the output data is fed to navigation algorithms in order to obtain the position, velocity and attitude.

With the classical GPS, the accuracy is about 5 to 30 meters. In order to have a submeter accuracy, the differential mode GPS (DGPS) is required. A base station is installed on a known position. The position calculated for the base station is then compared with the known position. So, we obtain an error value, which is supposed to be the same for the receivers close to the base station. The base station transmits this error to all the receivers, which can correct their calculated position. The accuracy can be further improved if we observe the change in phase of the incoming signal, because the phase changes can directly be linked with the position changes. If six satellites are available, we can have a centimeter accuracy with this 'Real Time Kinematic'approach. A.2 The GPS errors For the GPS data, the principals errors are the following
ones:

. Selective avaibility error. Before the first of May 2000, the selective avaibility was a mecanism adopted by the defensive department of the USA to deteriorate the navigation for the non military GPS. . Ionospheric propagation error. The ionosphere is composed of gas ionized by the solar radiation. So there are electrons, which act on the GPS signals as a dispersive environment. So the velocity of the GPS signals changes in the ionosphere. . Tropospheric propagation error. The inferior part of the atmosphere is composed of gas and water vapour, which increases the propagation way.

B.2 The INS errors As said in [GRE 01], the error sources of INS are: * Initialization errors. In order to have the position and velocity, the inertial sensors integrate the acceleration, so we need the more accurate initial values of the position and the velocity. * Alignment errors. There are the alignment errors of the gimbals (for gimbaled systems) or of attitude direction cosines (for strapdown systems) * Calibration errors. We calibrate the sensors to estimate the parameters of models used in sensor error compensation. . Gravity model errors. The influence of unknown gravity must be modeled. In our case, we ignored this because we completed our tests in a small zone and on a plate surface area. . Schuler damping. * Coriolis effect.

22

IMACS Multiconference on "Computational Engineering in Systems Applications"(CESA), October 4-6, 2006, Beijing, China.

B.3 The used INS: XSens MT9

We used an inertial sensor from the XSens firm: the MT9. It is a strapdown system. It measures the rate of turn, the accelerations and the earth magnetic field. Then an algorithm calculates absolute orientation : the measurement of gravity and magnetic north compensate for otherwise unlimited increasing errors from the integration of rate of turn data.

Xk+1lk

E[Xk+l - k/k I Ax /k + Buk + GE[vk/x-41] Axck/k + BUk

Xk+1 -X+11k

Xk+l/k

Ax/k + GVk

Pk+llk

E[-4+11k k+1 /kI APIkAT + GQGT

The sample frequency is 1OOHz.

III. KALMAN FILTER

We correct this prediction with the new measure Yk+1:

T is +1ht t+1o + Kk+y [Yk+-:C+1k]

That is to say :

In 1960, Kalman gave a recursive solution to the linear fil-

tering problem ([KAL 60],[WEL 02]).

Consider the linear discrete system:
Xk s =
=

Xk+1lk+1
and

Xk+1l

Xk+llk-+

K+1+k+1

kC+llk + Wk+l]

lYk

CkXk + Wk

AkXk1 + BkUk + GkVk

Pk+lk+l

E[x+i11+14i+I

The random variables Vk and Wk represent the process and measurement noise. They are supposed to be indepedent, white, with normal probability distributions and with process noise covariance Q and measurement noise covariance R.

[(I-Kk+l C)Pk+llk(I Kk+ C)T] +Kk+lRKkT+l

The estimated error must be statistically orthogonal to the innovation Yk+ - C4+11k .

E[Vk

E[Wk]

E[x+1k+1 (Yk+1

k+1k]

P(v)

N(0, Q) and P(w)

=

N(0, R)
Uk

i.e.

This discrete random process verify:

E[Xk+1l
So:
Pk+1

E[Xkl +B

Kk+

Pk+lk CT (CPk+lk CT + R)

So, we minimize the estimated error variance when v(k) and w(k) are gaussian.

E[(Xk+l- E[Xk+l)(Xk+l E[Xk+1])T]

APkAT + GQGT
=
=

P+ll+l = P+1k Kik+lCP+llk

Such a filter varies with time because the Kalman gain Kk depends of time. But it tends towards an invariant filter, if the system is stable and observable. So the gain Kk can be deduce from the solution of the Ricatti equations system:
Pp

E[Yk+1]
CYYk+1

E[Xk+l]

E[(Yk+l -E[Yk+l])(Yk+l-E[Yk+l])']

CP+1CT + R

The objective of the Kalman filter is to estimate the state at the current step n : xc(n), with all the signal values until the step k. Such an estimation is a filtering of the system if n = k, a prediction if n > k and a smoothing if n < k. Here, we interest to the case n = k + 1. The system is supposed to be Markov, so with the values y mesured until the step k, we can estimate the state at the step k + 1.

PE K(+OO)

APEAT
PP

PPCT(CPPCT + R) 1
IV. GPS/INS DATA FUSION

-PPCT(CPP CTR)-1CPp

GQGT

A. Aided Inertial Navigation System Structures

x1 minimize the variance of the prediction error:

[SUK 00] proposes 3 implementations to integrate data from an inertial system and a GPS.

23

IMACS Multiconference on "Computational Engineering in Systems Applications"(CESA), October 4-6, 2006, Beijing, China.

INT
GPS

Acceleration alnd Rotation Rates

IF

KALMIAN FILTER
Obsetvations

NON LINEAR

P6sitiun

N and Atttuide
Estll1ationf

INS

Positioit, VNloul

Estimated
erors

+1 Ve_
KALMAN FILTER
LINEAR

Fig. 1. Direct filter

(IPS
A. 1 The direct structure The drawback of such an implementation is that the prediction equations have to be evaluated at each sample of the inertial data. This requires substantial processing due to the high samples rates of IMUs. To overcome this, an INS should be employed so that, without the data from the GPS, the data of the vehicle state can be calculated external to the filter.

(Obserationls
Fig. 3. Direct feedback filter

e [ap av a]T The process model is

5p

c6v

~M
6fb:

S;. Auu

fJna + C 6fb
-C~b

A.2 The indirect feedback

INS
O Ivi-V1

PoItIo, N eocity anid Attitude r11

Etinaed enors

c&Wb: rotation rate error mesured by the gyroscopes, Cb : transformation matrix from the body frame to the navigation frame.
So, we have:
0 0 0 0 0 0
U

fn: acceleration in the navigation frame, acceleration error in the body frame,

t)Obse
Obsel ratioii

d eTls

KLALMAN FILTIER

Fig. 2. Indirect feedback filter

The observation which is delivered to the filter is the observed error, which is the difference between the inertial system and the GPS system. The weakness of this implementation is the unbounded error growth of the INS which causes an unbounded growth in the error observation delivered to the filter. This poses a problem to the linear filter because only small errors are allowed due to the linearisation process. A.3 The direct feedback In this structure, the estimated errors are fed back to the INS, thus minimising the growth of the observed error that is delivered as an observation to the filter. We choose the third implementation the direct feedback
structure.

0 0 0 0 0 0
U

0 0 0 0 0 0
U

1 0 0 0 0 0
U

0 1 0 0 0 0
U

0 0 1 0 0 0
U

-fy
U

fz
0 0

0 0 0 0

-fz
0
U

0 0 0

fx
0 0

-fx
0
U

0 0 0 fy

0 0 0 0 0 0 0 0 0 0 0 0

0 0

The accelerations are fed directly in the matrix F, so there is no control vector u = 0. The process model A is composed of time-varying terms. The state transition matrix is given by:
I + FAt +O(At)

Fk

and the state transition is

B. Inertial Navigation Model

x(k/k -1) Fkx(k -l/k -1) The system noise covariance is given by:

Qkc =
with:

1/2(FkGkQckG/T FT + GkQckGTc)At
Q ck =

The state model is based on the inertial sensor error ([SUK 00]). So the state vector x is composed by position, velocity and acceleration errors:
x

[=px Spy 6pz 5vx 5vy 6vz 6ax day 6az]T

[6p O

LO

6fb
O

6Wb

24

IMACS Multiconference on "Computational Engineering in Systems Applications"(CESA), October 4-6, 2006, Beijing, China.

6p is the noise in the position error evaluation. It depends on the uncertainty of velocity estimation. We obtain:
0
Q

The raw GPS data are given in the figure 4. There are a few isolated points far from the actual trajectory, they are unuseful to find the trajectory. So, we must eliminated those points.
In the figure 5, the data in latitude according to time are given. For the GPS receiver in DGPS mode, the GPS qual5 041 5 04 0 5 03 8

Qk

Cb
0

Cb'

For the observations, we have:

inertial Z Zk-Zk [

kZ
g

gps

5 03 8 5 03 7

inertial Zk inertial

ZUk

zpgps ZPk
kgp

I
0]

50H
5 03 5 5034 5 033 61 21 1 81 2 41 3 01 3 61

The observation matrix is given by:

421

48i

54
rre

60

661

721

781

841

90

9 61

02

Fig. 5. The latitude path without correction

H(k)

-13*3

-I3*3 0

The observation covariance matrix is:

Rk

ity indice and the number of visible satellites must be more than 4. The first and third segment are in good condition. But, in the second segment, the environment doesn't satisfied the conditions of DGPS : the base station signal was lost. That's why, there is a shift in this period.

Rk

-3*36P2 Oc~p2

I33*v23

02

V. DATA PROCESSING

A. GPS Data Processing

In order to obtain the trajectory from the GPS data, we need

to:

detect and correct the high frequency faults, transform the frame, use the derivative filter to find the velocity vector and to filter the position.

308 8

308so b
308A4

308 2

303

307 .
307 4

5035 5 5036 6036 5 5037 5037 5

longitudelJOOl deg)

038 5038 6 5039 5039 5 5040 5040 5

Fig. 4. The raw GPS data in latitude and longitude

Fig. 6. Correction of high frequency default

25

IMACS Multiconference on "Computational Engineering in Systems Applications"(CESA), October 4-6, 2006, Beijing, China.

In order to detect and correct this default, the method described in figure 6 is used. We try to have 'in-continu segment'. The e threshold detects if the point is isolated. If it is far from the adjoining points, this isolated point is removed. Otherwise, we search the first point of the 'in-continu segment' as the beginning of the second segment in figure 5. Then, we calculate the correction corresponding to the previous segment. Finally, we correct all the points to have the more continu path. After the correction, the latitude and longitude are given in figure 7.
r, nI Ch Ah

* Calculate the acceleration in the navigation frame

fn

Cb fb

where fb is the acceleration in the mobile frame, i.e. the acceleration measured by the sensor. * Remove the initial bias * Consider the gravity . Transform the data in the ENU frame * Integrate two times to obtain the velocity and position

5036 44 F

5036 42 F
5036 4 F

With the figures 9 and 10, we compare the velocity on X-axis calculated by the inertial sensor and the GPS sensor respectively. We constate the huge drift of the inertial sensor. This inertial sensor can only be used to the navigation in short-term.
17 7'7

503638

X\

5036J 6
budbso4
308.38

-50

308A4

308.42

Ibngitude(Ml0 deg)

308644

3 S048 308A6 4

308&5

Fig. 7. GPS trajectory after the high frequency default correction

To compare the GPS data to the inertial data, we transform the latitude and the longitude of the GPS in the local frame. The result is presented in figure 8.

_~~~~~~~~~ieO0
-2UU

Fig. 9. The velocity on X-axis measured by the inertial sensor

4U l
30

20

X
X
4

10

-10
-20

<
-

30
-40

-30

-20

-10

10

east(m)

20

30

40

50

60

-81 0

20

00

Fig. 8. Corrected GPS trajectory in the local frame B. INS Data Processing

timneip]1s)

600

800

1000

12

oD

Fig. 10. The velocity on X-axis measured by the GPS

VI.

EXPERIMENTATIONS

The different steps to compute the INS data are:

Calculate the direct cosine matrix with the quaternion
vector

The result obtained after the Kalman filtering tial data and GPS data is given in figure 11.

on

the iner-

26

IMACS Multiconference on "Computational Engineering in Systems Applications"(CESA), October 4-6, 2006, Beijing, China.

C. Real time application

Land navigation has led to the creation of many research projects and platforms like TRAVEL and OPSYDE. The purpose of this project is the development of land navigation techniques to increase the security of highway transports. The TRAVEL platform considers the creation of an autonomous convoy of vehicles while the OPSYDE project consists of data fusion techniques to solve the problem of multisensor navigation in order to increase the accuracy and reliability.
Fig. 11. Filtered Inertial Sensor and GPS

The test platform installed is composed of several ROBUCAR, electric vehicles and their on-board sensors where the most important are GPS and attitude sensors.
We are developing a real time application using the real time environment RTAI (RealTime Application Interface) for Linux, which lets us write applications with strict timing constraints and the software Syndex. So, we will get the data from the sensors, implement our data fusion algorithm, and control the vehicles in real time.
REFERENCES

The dotted line is the trajectory of the inertial sensor helped by the GPS receiver and the continuous line is the GPS trajectory that we have already seen. We notice that the drift from the inertial sensor is corrected by the integration of the GPS data. At the begining, the two trajectories are similar but after the second corner, the difference is growing. After the third corner, the difference seems to be borned. The GPS sensor has well represented the path except in the part of the high frequency fault. If we suppose that the GPS trajectory is the reference path, there is about 6 meters of difference at the end of the trajectory for the inertial sensor. In comparison with the distance of the trajectory, the error is 2.5%.

VII. FUTURE PROSPECTS

A. Additional sensors and data base

In order to have better results, we can add other sensors. For example:

. Odometer: in the long absence of GPS signal, we could integrate the INS data with an another dead-reckoning sensors, for example an odometer. See [ABU 05] . Camera: in [CHA 05], a vision system is used to detect the distance from the vehicle to the lines of the road, and the orientation on the lane. . Map-matching in [NAJ 05], the vehicle is locate on a numeric road map. Extended Kalman Filtering and Belief theory are used to integrate all the data.

B. Filtering methods

The Kalman filtering is the basic method of sensors fusion, but another types of filters or approach to integrate the sensor data could be used. For example Extended Kalman Filter, Particle Filter ([CAR 05]), Belief theory, Fuzzy logic, or Information Theory ([GRO 02],[BER 04]).

[ABU 05] ABUHADROUS I., Systeme embarque temps reel de localisation et de modelisation 3D par fusion multi-capteur, These de Doctorat de l'Ecole des Mines de Paris, 2005. [BER 04] BERDJAG D., POMORSKI D., DGPS/INS data fusion for land navigation, vol. II, p. 881-887, International Society of Information Fusion, Jun 2004. [CAR 05] CARON F., DAVY M., DUFLOS E., VANHEEGHE P., Fusion de capteurs potentiellement defaillants parfiltrage particulaire, GRETSI 2005, Louvain-la-Neuve Belgium, september 2005. [CHA 05] CHAUSSE F., LANEURIT J., CHAPUIS R., Vehicle Localization on a Digital Map Using Particles Filtering, Proceedings of the IEEE Intelligent Vehicles Symposium, 2005. [GRE 01] GREWAL M., WEILL L., ANDREWS A., Global Positioning Systems, Inertial Navigation and Integration, John Wiley and Sons, 2001. [GRO 02] GROCHOLSKY B., Information Theoretic Control of Multiple Sensor Platforms, Australian Center for Fields Robotics, University of Sydney, march 2002. [KAL 60] KALMAN R. E., A new approach to linear filtering and prediction problems, Transactions of the ASME, Journal of Basic Engineering, vol. 82, march 1960. [NAJ 05] NAJJAR M. E., BONNIFAIT P., A Road-Matching Method for Precise Vehicle Localization using Kalman Filtering and Belief Theory, Journal of Autonomous Robots, S.I. on Robotics Technologies for Intelligent Vehicles, Kluwer Academic Publishers, september 2005. [SUK 00] SUKKARIEH S., Low cost, high integrity, aided inertial navigation systems for autonomous land vehicles, Australian Center for Fields Robobtics, University of Sydney, march 2000. [WEL 02] WELCH G., BISHOP G., An Introduction to the Kalman Filter, University of North Carolina, Chapell Hill, TR 95-041, march 2002.

27