Post on 01-Feb-2021
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.