1

A compensated system design for GPS failure using sensor fusion

system

Shun Hung Chen, Chan Wei Hsu, Shih Chieh Huang, Chun Hsiung Chen*

Automotive Research and Testing Center, Changhua Taiwan

TEL: 886-4-7811222 ext.2364

E-mail: stanley_chen@artc.org.tw

Abstract

This paper presents a sensor fusion system that can assist the vehicle to solve the localization

problem since the GPS signal is lost. Generally, the GPS signal is lost when the vehicle is

maneuvered under shelters, for example, in a tunnel, hillside, deep valley, indoor parking

garage, etc. In such situation, the inertial measurement sensors are applied to estimate the

position of vehicle, the accelerometer and gyroscope are used to calculate the longitudinal and

lateral state of the vehicle, respectively. Moreover, the position estimation of the vehicle on a

ramp is also considered in this paper. However, the estimation errors are accumulated so that

the bias errors of the sensor fusion system are thus increased. In order to increase the

estimation accuracy of vehicle position, the kinematic filter method is utilized to integrate the

information from sensor fusion system to correct the present and to predict the posterior

location precisely. The experimental results demonstrate that our sensor fusion system can

localize the vehicle since the GPS signal is lost.

Keywords: Kinematic Kalman Filter, GPS, Sensor Fusion System, Vehicle localization.

2

1. Introduction

To solve the problem of autonomous navigation, Leonard et al. [8] successfully used six

fixed sonars which are assembled on a mobile robot to detect the ambient environment and

figure out “Where I am?” in an indoor area. The sonar sensor have been applied in many

methodologies of localization, such as simultaneous localization and mapping (SLAM)

[2][4][11], Monte Carlo localization [9][13][15], etc. In outdoor environment, however, the

Global Positioning System (GPS) is easier and more reliable than inertial measurement units

(IMU) to localize the position of the vehicle [14]. The GPS module is enable to provide the

measurements of vehicle position, heading, and inertial velocity. Under the best condition, e.g.

greater than or equal to eight satellites, the GPS positioning error has a standard deviation of

less than 3 meters. Whereas, since the number of satellites is less than eight due to the shelter

from tunnel, vegetation or buildings, the average errors are increased even localizing failure.

In such situation, the vehicle is travelled in a place, which is similar to the indoor environment.

Therefore, to design a sensor fusion system to assist the GPS module for vehicle localization

is necessary.

The inertial measurement sensors, which have the function of determining the acceleration,

pitch rate, yaw rate of a vehicle are able to tackle the vehicle localization problem, for

example, the autonomous vehicle is equipped the inertial sensors including BEI C-MIGITS

III, Hall sensor, high-precision optical encoder to detect the velocity and steering angle,

respectively, to aid a GPS receiver for vehicle localization [7]. Walchko et al. [16] indicated

that the performance of GPS is restricted to the 1 Hz update rate which could not provide the

precise navigation information alone, therefore design of an inertial navigation system by

integrating IMU and digital compass to interlace the signal from GPS module could increase

the accuracy of localization result. Similar to the results of [16], Hemerly et al. integrated INS

and odometer to navigate a vehicle when GPS signal is lost [5]. The navigation process by

integrating INS and odometer is performed real time and is sampled at 100 Hz so that when

GPS signal is unavailable, the integrating navigation system is possible to obtain small errors.

The data synchronization, multi-rate operation, etc. are dealt with accordingly.

Although the inertial measurement sensors are able to solve the vehicle localization when

GPS signal is unavailable, the bias error inherently might cause the estimating results

divergent. Thus, using a reliable methodology to calculate the localization information and

reject those bias errors is important. In many investigations, the Kalman filter [1][3][6][10][17]

is the most popular method to account for trajectory prediction and noise rejection. The

Kalman filter applies dynamic model of a system and obtains the control inputs and

measurements of this model, and then the estimation of the system’s varying quantities is

formulated. This estimation result is better than the estimation implemented by using any

other measurement only. In our investigation, an estimator based on kinematic relationships,

3

named kinematic Kalman filter, is employed to integrate GPS and sensor fusion information.

Moreover, small road grade is also considered in this investigation, some gravity components

are compensated simultaneously.

The sensor fusion system in this paper is composed of accelerometer, gyroscope,

odometer, and gearbox speed sensor and the vehicular acceleration, heading direction,

velocity can be estimated respectively. Under the best conditions, i.e. more then eight

satellites, the sensor fusion information is calibrated by GPS signal, and vice versa. By fusing

GPS signal and sensor fusion information, a vehicle localization problem of entering a tunnel

is investigated. Consider the tunnel is constructed in suburban, the multipath effect in GPS is

reasonably to be ignored and the noise in the sensor fusion system is reasonably well-behaved.

In general, the requirements of lateral position error cause stringent effect on localization than

the requirements of longitudinal error [12]. Hence, a lateral kinematic model is formulated for

kinematic Kalman filter to estimate the position and eliminate the bias error of inertial

measurement sensors.

This paper is organized as follows: in section 2, a kinematic model of four-wheel and

front-steering vehicle is described. In the third section, a kinematic Kalman filter scheme is

adopted to estimate the vehicular position and dispose of the bias error of inertial

measurement sensors. The experimental results are demonstrated and discussed in the next

section. Finally, the conclusions of this paper are presented.

2. Kinematic Model A simple mechanism of a four-wheel vehicle can be described briefly in a bicycle model

and is shown in figure 1. In figure 1, the lateral kinemics of the bicycle model can be

formulated according to Newton’s second law, such as

−−

−+=

−+=

V

rlCl

V

rlClI

mgVrVmF

rfz

bankyy

22

11

)sin()(

βδβ

θ

αα

ɺ

(1)

where V is the velocity of vehicle and Vy is the lateral velocity, m is the mass of vehicle, r is

the vehicle yaw rate, δ is vehicle steering angle, β represents the sideslip angle, bankθ is vehicle roll angle, l1 and l2 represent the distances from the center of mass of vehicle to the

front and rear wheel, respectively. fCα and rCα are the total front and rear cornering stiffness.

Solving for the sideslip rate and yaw acceleration, we have

4

Figure 1 - The schema of bicycle model

−=

++=

z

yryf

bankyryf

I

FlFlr

mV

mgFF

21 2)cos(2

)sin(2)cos(2

δ

θδβ

ɺ

ɺ

(2)

Rewritten (2) into the form of state-space yields

δββ

α

α

αααα

αααα

+

−−+−

−+−−−

=

z

f

f

z

rf

z

rf

rfrf

I

ClmV

C

rVI

ClCl

I

ClClmV

ClCl

mV

CC

r 122

2121

221 1

ɺ

ɺ

(3)

By augmenting the states of the heading of vehicle ψ and the yawing bias on gyro bgyro, (3) could be reformulated as

[ ]gyrogy

z

f

f

gyro

z

rf

z

rf

rfrf

gyro

brIY

I

ClmV

C

b

r

VI

ClCl

I

ClClmV

ClCl

mV

CC

b

r

ψβ

δψ

β

ψ

βα

α

αααα

αααα

×=

+

−−+−

−+−−−

=

×44

122

2121

221

0

0

0000

0010

00

001

ɺ

ɺ

ɺ

ɺ

(4)

If the longitudinal and lateral velocities are known, the sideslip angle of vehicle can be

defined as

l1

l2

V Vy

r

β

δ αf

αr ψ

latitude

5

= −

x

y

V

V1tanβ (5)

Since the GPS signal is received, the orientation, longitudinal and lateral velocities can be

obtained. Consider the vehicle is maneuvered on a ramp, the acceleration measurements are

effected upon gravity components due to the attitude of vehicle is changed. The diagram of a

pitch center vehicle model with road grade is shown in figure 2. The kinematic relationship

between acceleration measurement and velocity components at sensor location of vehicle is

represented as

++⋅+=

+++⋅−=

noiseaVVa

noisegaVVa

biasyxymey

rbiasxyxmex

,,

,, sin

ψ

θψɺɺ

ɺɺ

(6)

where ax,me and ay,me are the longitudinal and lateral measurement accelerate of the vehicle,

respectively. vx,GPS and vy,GPS are the longitudinal and lateral velocity of GPS, respectively.

rθ is road grade angle, ax,bias and ay,bias are the longitudinal and lateral measurement bias of sensor, respectively.

Figure 2 – The pitch center vehicle model with road grade

3. GPS/sensor fusion system integration using Kalman filter

The conventional Kalman filter is composed of measurement update and time update. Due

to the low measurement update rate of GPS, the state estimating error and sensor bias would

be compensated by sensor fusion information. The measurement update is generally

formulated as follows

( )( )

( )

−=

=+=

−+=−−

)(ˆ)(~

)(~

)(ˆ)(ˆ

)(ˆ)()(ˆ)(~

11

tPKCItP

RCtPRCtPCCtPK

txCtyKtxtx

TTT (7)

θr g

ax,me

Vx

vx,GPS

Acceleromete

GPS

6

where x̂ and x~ represent the prior and update estimate of the system state at time t,

respectively. P̂ and P~

are the prior and update error covariance matrix at time t, respectively. K is the Kalman gain and y(t) is new measurement. C is the observation matrix and R

represents the measurement noise covariance. By integrating the sensor fusion system since

the GPS measurements are lost, the time update can be shown as follows

+=+

+∆+∆=+=+ ∑∑

∞

=

+∞

=

QAtPAtP

tuBTA

txTA

tuBtxAtx

Tdd

dd

)(~

)1(ˆ

)()!1(

)(~!

)()(~)1(ˆ0

1

0 ρ

ρρ

ρ

ρρ

ρρ (8)

where u is the input to the system and Q is the process noise covariance. By using (8), (3) can

be rewritten into the form of discrete time equation, such as

)(0

)(10

1

)()!1(

)(!

)()()1(0

1

0

tuT

txT

tuBTA

txTA

tuBtxAtx dd

∆+

∆−=

+∆+∆=+=+ ∑∑

∞

=

+∞

= ρ

ρρ

ρ

ρρ

ρρ (9)

Assume that the velocity measured by GPS module is employed to calibrate the measurement

velocity of sensor fusion system, the relationship between GPS measurement velocity and Vx

and Vy can be written as

==

yGPSy

rxGPSx

Vv

Vv

,

, cosθ (10)

where vx,GPS and vy,GPS represent the longitudinal and lateral velocity measurement from GPS.

When GPS signal is available, the position of vehicle can be measured by GPS and the

state space model can be written according to (10) as below

( )

[ ]Tyxav

GPSy

GPSx

r

y

x

y

y

x

x

yVxVIY

noisev

v

y

V

x

V

V

V

V

V

×=

+

+

=

×

−

44

.

,

1

00

10

00

0cos

0100

0000

0001

0000 θ

ɺ

ɺ

(11)

where the measurement output Yav is the state of the model, which includes the position,

longitudinal and lateral velocity of vehicle.

If the GPS signal is lost, the vehicle positioning problem is involved in gyroscope and

accelerometer measurements. The estimation of vehicle orientation is according to the

measurement of gyroscope. By using kinematic Kalman filter, the sideslip angle, yaw rate,

heading direction of the vehicle is obtained based on (4). For the measurement of vehicle

7

acceleration, an accelerometer is employed and then an acceleration Kalman filter is used to

tackle the velocities and sensor bias by combining (6) and (10), such as

[ ]Tbiasyybiasxxacc

r

mey

mex

biasy

y

biasx

x

biasy

y

biasx

x

aVaVIY

noise

g

a

a

a

V

a

V

a

V

a

V

,,44

,

,

,

,

,

,

0

0

0

sin

00

10

00

01

0000

100

0000

010

×=

+

+

+

−−

−

=

×

θ

ψ

ψ

ɺ

ɺ

ɺ

ɺ

ɺ

ɺ

(12)

The measurement output Yacc is the state of the longitudinal and lateral velocities and

measurement acceleration bias.

By summarizing the localization method in this paper, the GPS signal is utilizing to solve

the position problem. The grade angle is detected by gyroscope and then the position of the

vehicle is estimated by (11). If the GPS signal is lost, the kinematic Kalman filter method is

employed to estimate the attitude of vehicle and then by integrating the longitudinal and

lateral velocities to calculate the position of vehicle.

4. Experimental Results Figure 3 shows our experimental car, named Mitsubishi Savrin, and our experiments have

been carried out at the Pakuashan Tunnel1. When our experimental car is maneuvered out of

this tunnel, the vehicle localization problem is dealt with GPS signals. However, since our

experimental car is travelled in the tunnel or the GPS signal is too weak, the vehicle

localization problem is tackle by sensor fusion system. The sensor fusion system in this paper

consists of gyroscope and accelerometer and the block diagram is illustrated in figure 4. The

navigation system is operated by GPS data acquisition and INS correction to enhance the

navigation performance based on the integrated concept. To accomplish the experiment, the

odometer and gearbox speed sensor is used to compare the GPS velocity to calibrate the

accuracy of inertial measurement sensors. The odometer sensor has high feasibility to capture

the velocity of vehicle when the speed is greater than 10 kph. Whereas, the speed is less then

10 kph, the sensor calibration is tackled by gearbox speed sensor. Hence, the velocity

measurement error is less than 5 kph. Similar to the manner of velocity calibration, the

gyroscope integrated angular rate into heading comparing with GPS course.

1 Pakuashan Tunnel is located in Changhua, Taiwan. This tunnel connects two main highways in

Taiwan. The total length of this tunnel is 4.928 kilometers.

8

Figure 3 – Experimental car

Figure 4 – The block diagram of sensor fusion system

Figure 5 – The entry of Pakuashan Tunnel (left) and the experiment in the tunnel (right)

Figure 5 shows the Pakuashan tunnel and the view in this tunnel. The experimental results

are shown in figure 6. In figure 6, the dash line is the localization result via kinematic Kalman

filter (KKF) method and the dotted line represents the results by integrator only. Although

both of the estimation errors of these two methods are increased eventually, the performance

of kinematic Kalman filter method is more superior to that of integrator only. The longitudinal

position error of kinematic Kalman filter method is about 0.05 times less than the method of

integrator and the experimental results are illustrated in figure 7. Figure 8 shows that the

lateral position error of kinematic Kalman filter method is 13 times less than integrator

method. The experimental results demonstrate that our sensor fusion system can localize the

vehicle via kinematic Kalman filter method since the GPS signal is lost.

Gyroscope

Accelerometer

Attitude

propagation

Rotation from body

axis to NED axis

Euler angle

Kalman filter

y

y

x

x

V

V

V

V

ɺ

ɺ

9

Google_Map IntegratorKKF

23.930

23.935

23.940

23.945

23.950

23.955

23.960

23.965

23.970Latitude(deg)

120.62 120.64 120.66

Longitude(deg)

Figure 6 – The experimental results

KKF. Integrator.

-50

0

50

100

150

200

250

300

350

400

450

500

550

600

650

700Longitudinal_Error(m)

120.62 120.64

Longitude(deg)

Figure 7 - The experimental results of longitudinal position error

10

KKF. Integrator.

-0.0005

0.0000

0.0005

0.0010

0.0015

0.0020

0.0025

0.0030

0.0035

0.0040

0.0045

0.0050

0.0055

0.0060

0.0065

0.0070Lateral_Error(deg)

120.62 120.64

Longitude(deg)

Figure 8 - The experimental results of lateral position error

5. Conclusions

In this paper, the experimental results of vehicle localization in a tunnel are presented.

After calibrating the sensor fusion system, the localization problem can be solved by

integrating inertial measurement sensors when the vehicle is maneuvered in a tunnel. The

accelerometer and gyroscope can be employed to estimate the longitudinal and lateral attitude

of the vehicle, respectively. Due to the road grade in the tunnel, the estimating errors of those

inertial measurement sensors will be increased. Therefore, the kinematic Kalman filter

method which is considered the gravity components is employed to estimate the position of

vehicle on ramp. The experimental results show that the position estimation by kinematic

Kalman filter is more accurate than the measurement results of inertial measurement sensors

only.

Acknowledgement

This work is supported in research projects 101-EC-17-A-04-02-0803 by Department of

Industrial Technology, Ministry of Economic Affairs, Taiwan, R.O.C.

11

References

1. Batista, P. Silvestre, C. Oliveira, P. (2008). Kalman and ∞H optimal filtering for a class

of kinematic systems, In Proceedings of the 17th World Congress The International

federation of Automatic Control, pp. 12528-12533.

2. Caballero, F. Merino, L. Ferruz, J. Ollero, A. (2009). Vision-based odometry and SLAM

for medium and high altitude flying UAVs, Journal of Intelligent and Robotics Systems,

vol. 54, pp. 137-161.

3. Chen, B.C. Wu, Y.Y. Hsieh, F.C. (2010). Estimation of engine rotational dynamics using

Kalman filter based on a kinematic model, IEEE Transactions on Vehicular Technology,

vol. 59, no. 8, pp. 3728-3735.

4. Durrant-Whyte, H. Bailey, T. (2006). Simultaneous localization and mapping (SLAM):

part I the essential algorithms, Robotics and Automation Magazine, vol. 13, no. 2, pp.

99-110.

5. Hemerly, E.M. Schad, V.R. (2008). Implementation of a GPS/INS/odometer navigation

system, ABCM Symposium Series in Mechatronics, vol. 3, pp. 519-524.

6. Hu, C. Chen, W. Chen, Y. Liu, D. (2003). Adaptive Kalman filtering for vehicle navigation,

Journal of Global Positioning Systems, vol. 2, no. 1, pp. 42-47.

7. Jones, E. Fulkerson, B. Frazzoli, E. Kumar, D. Walters, R. Radford, J. and Mason, R.

(2006). Autonomous off-road driving in the DARPA grand challenge, In Proceedings of

2006 IEEE/ION Position, Location, and Navigation Symposium, pp. 366-371.

8. Leonard, J. J. Durrant-Whyte, H. F. (1991). Mobile robot localization by tracking

geometric beacons, IEEE Transactions on Robotics and Automation, vol. 7, no. 3, pp.

376-382.

9. Li, T. Sun, S. Duan, J. (2010). Monte Carlo localization for mobile robot using adaptive

particle merging and splitting technique, In Proceedings of 2010 IEEE International

Conference on Information and Automation, pp. 1913-1918.

10. Mosavi, M.R. Sadeghian, M. Saeidi, S. (2011). Increasing DGPS navigation accuracy

using Kalman filter turned by genetic algorithm, International Journal of Computer

Science, vol. 8, no. 6, pp. 246-252.

11. Mullane, J. Vo, B.N. Adams, M.D. Vo, B.T. (2011). A random-finite-set approach to

Bayesian SLAM, IEEE Transactions on Robotics, vol. 27, no. 2, pp. 268-282.

12

12. Rezaei, S. and Sengupta, R. (2007). Kalman filter-based integration of DGPS and vehicle

sensors for localization, IEEE Transactions on Control Systems Technology, vol. 15, no. 6,

pp. 1080-1088.

13. Rofer, T. Jungel, M. (2003). Vision-based fast and reactive Monte-Carlo localization, In

Proceedings of the IEEE International Conference on Robotics and Automation, pp.

856-861.

14. Weinstein, J. A. Moore, K. L. (2010). Pose estimation of ackerman steering vehicles for

outdoors autonomous navigation, In Proceedings of 2010 IEEE International Conference

on Industrial Technology, pp. 579-584.

15. Thrun, S. Fox, D. Burgard, W. Dellaert, F. (2001). Robust Monte Carlo localization for

mobile robots, Artificial Intelligence, vol. 128, no. 1-2, pp. 99-141.

16. Walchko, K.J. Nechyba, M.C. Schwartz, E. and Arroyo, A. (2003). Embedded low cost

inertial navigation system, In Proceedings of the Florida Conference on Recent Advances

in Robotics.

17. Welch, G. Bishop, G. (2001). An introduction to the Kalman filter, University of North

Carolina at Chapel Hill Department of Computer Science.