Recursive Estimation of Vehicle Position by Using Navigation Sensor Fusion

Shun-Hung Chen, Chan Wei Hsu, and Shih Chieh Huang Research and Development Division

Automotive Research and Testing Center Changhua, Taiwan

shungchen@artc.org.tw

Abstract—In this paper, a sensor fusion scheme is employed to reduce positioning error of a vehicle since the GPS signal is fail. The vehicular information, such as position, heading direction, and velocity, can be obtained through GPS signal. Generally, the positioning accuracy of commercial GPS module is within the 3 meters, however, the GPS module may disconnect the signals from satellites since the vehicle is maneuvered under shelters, e.g. parking garage, tunnel, high dense urban, etc. Therefore, our proposed methodology is able to improve the estimation accuracy of vehicle position based on dead reckoning method. The first step, the Kalman filter is utilized to reject the noise of velocity measurement which is captured from gearbox and wheel speed sensor and also predict the velocity and displacement of vehicle in next sample time. The second step is to construct the displacement model of the vehicle by adopting ARMA model, which is able to estimate the state of vehicle. Digital map information which is applied to correct the positioning result of ARMA model is addressed in the last step. A real time experiment result of GPS signal lost in a tunnel is carried out to demonstrate the performance of our proposed method.

Keywords-Kalman filter; ARMA model; GPS; Recursive structure; Vehicle positioning.

I. INTRODUCTION The localization or navigation technique is an important

and interesting topic in intelligent transportation systems (ITS). Recently years, the commercial navigator is more and more popular and a number of functions have been integrated in this device, such as path planning, real-time traffic information, localization, navigation, and so on. These functions are even packed as mobile applications (app) to help the smartphone users to operate conveniently. All of these apps, the function of localization is the fundamental of all applications. The localization technique is relied upon the information from GPS module which is equipped in commercial navigator and smartphone. The GPS module has an advantage of small dimension to be integrated in circuit. It is capable of receive the signal constantly from satellites and thus the moving displacement is able to be estimated by receiving at least four know satellite signals. According to the theory of time of arrival, the position is computed finally by using trilateration [2]. After executing these procedures, the longitude, latitude, altitude, velocity and heading direction is able to be calculated. Unfortunately, GPS receivers may be not the best solution in the following cases, e.g. the vehicle is maneuvered in the

indoor environment or in urban areas where is unable to receive the signal from satellites directly or since the localization accuracy is worse to 20 or 30 meters. For the reason of improving the localization accuracy, some localization techniques such as path integration [11], dead reckoning [1, 4], cellular localization [14], cooperative localization [6] and map localization [9], etc., are utilized to accomplish the requirement of localization. The dead reckoning method is used to calculate the vehicle position based on the last know position of the vehicle. The current position of the vehicle is inferred from some movement information, such as velocity, acceleration, heading direction, time, etc. However, the major drawback of dead reckoning method is unable to be used over long periods of time due to the accumulative errors. The same reason also exists on path integration method. Without assisting by fixed external reference trajectory, the localization result of path integration method is inaccurate highly by accumulating the errors with time. Another sort of localization technique is utilizing a number of fixed infrastructures to compute the position of vehicle, such as cellular localization and image localization [15, 17], and so on. The advantage of cellular location method is presented in mobile cellular base stations to estimate the position of the vehicle. By applying received signal strength indicator (RSSI) [12] or time difference of arrival (TDOA) [19] method or angle of arrival (AOA) [18], the distance from vehicle to base station is easy to be estimated. Since more than three distances from different base stations are obtained, the position of vehicle is able to be computed through trilateration. However, the cellular localization method is executed under the cellular communication networks only. Besides, the position accuracy of cellular location method is generally less than GPS and the localization error is averagely between 90 to 250 meters [5]. The image location method encounters the same problems as cellular localization method.

In this paper, a sensor fusion system scheme, which includes an accelerometer, gyroscope, wheel speed sensor, and gearbox speed sensor, is proposed to solve the localization problem of GPS signal lost. By using Kalman filter [7, 10, 13], the velocity of vehicle can be estimated precisely and the moving distance is thus computed. Based on the method of dead reckoning, the vehicle position can be estimated. To avoid the accumulative error over long periods of time, the autoregressive moving average (ARMA) [3, 8, 16] method is utilized to construct the vehicle displacement model since the

GPS signal is lost. This ARMA model enables to provide more precise estimation results than dead reckoning method. However, the localization error of ARMA model might be increased due to the terns of error, a compensated strategy is required to be designed. In our control strategy, map-based information is employed to be the reference trajectory, which is separated into several segments, is composed of fixed values of longitude and latitude. After using proportional control strategy, the localization error of ARMA model is thus decreased.

The organization of this paper is summarized as follows. In section 2, the main localization strategy is addressed. The Kalman filter is employed to estimate the velocity and moving displacement of vehicle as precise as possible. The next step is to construct ARMA model according to the state of Kalman filter and than a promotional controller is applied to correct the position of vehicle via matching a map. In section 3, the experimental platform is exhibited and the experimental results are shown in section 4. In the remainder of this paper, the conclusions are given.

II. LOCALIZATION METHOD

A. Kalman Filter The Kalman filter is a kind of recursive structure for

estimating least squares errors and is composed of measurement and time update, respectively. In this paper, the vehicle velocity is calibrated by means of Kalman filter to reduce the estimating error while the GPS signal is unavailable. By applying the physical relationships, the vehicle velocity is given by

vkkkk Tuvv 1 (1)

From (1), the next step vehicle velocity is represented by the instant velocity and the acceleration during interval time T,

vk is the velocity noise. According to the relationship between time and distance, the mathematical formulation of position can be written as

pkk

kkkuTTvpp 2

2

1 (2)

where pk is the position noise. From (1) and (2), the vehicle model can be formulated as follows.

kkk

kkkkzCxy

wBuAxx 1 (3)

where k is the time index, A, B, C are the system, control observation matrices, respectively. T

kkk vpx ][ is called the state of the system, uk is the input to system, yk is the measurement output, T

vkpkkw ][ and zk are noise and both of the average of wk and zk are all zero. The noise covariance of wk and zk are defined as follows.

)( Tkk wwEQ (4)

)( Tkk zzER (5)

Figure 1. The flowchart of localization process.

where )(E means the excepted value.

Based on Eq. (3) to (5), the measurement update step of Kalman filter can be formulated as follows.

RCCPCPK T

k

Tk

k

1

1 (6)

)( 11 kkkkk CxzKxx (7)

1)( kkk PCKIP (8)

where Pk is called the covariance matrix of estimation error, and Kk is the Kalman gain. Therefore, the Kalman filter recursive structure contains the time update in Eq. (3) and measurement update in Eq. (6)-(8) to renew the covariance matrix Pk and correct the state xk.

B. ARMA Model The ARMA model method can describe a time series

model accurately and has an advantage in its practicability inherently. The positioning signal from GPS and the positioning error can be included in ARMA model as the states

Prior knowledge of state

xk-1|k-1 Pk-1|k-1

Physical model

xk|k-1 Pk|k-1

Update Measurement

xk|k Pk|k

Output estimate of state

ARMA model

Digital map Controller

k+1→k

Output

Error

and the coefficients and the order of this model are needed to be determined. Hence, the ARMA model is formulated with the previous state and errors are shown as below.

m

iikkkik

n

ikk eeyy

110 (9)

where n and m are the index of lagged terms on state and estimated error, respectively. The k is the analogous to the regression coefficient, k is the moving average coefficient. After finishing three standard processes for ARMA modeling, the coefficients in Eq. (9) could be identified.

According to the information from digital map, the longitude and latitude of reference path can be obtained in advance and the reference model is thus determined. Therefore, the control system concept can be applied to correct the state of ARMA model in (9).

C. Hybrid Scheme Our proposed method is combined with the techniques,

which are described in aforementioned methodologies. Two strategies are crucial to the result of localization. The first key point is the accuracy of the measurement of velocity and the second one is the compensation of ARMA model. In our proposed methodology, the localization process is separated into three steps; the first step is to calibrate the speed of vehicle and calculate the cruse displacement in the time interval. The second step is to infer the coefficient of the ARMA model of time series model. After obtaining the ARMA model, the digital map information is utilized to be matched the state ARMA model to correct the actual position of vehicle in third step. The localization process could be illustrated by means of flowchart and is shown in figure 1.

III. EXPERIMENTAL PLATFORM The vehicle localization problem is basically solved by

receiving GPS signal. However, the GPS signal is not available in anywhere and anytime, the other sensors are intervened to estimate the state of vehicle when GPS signal is lost. Hence, some sensors is applied to measure the attitude of vehicle is required and the experiment in this paper is carried out by the sensor fusion system and the structure of this system is illustrated in figure 2. The sensor fusion system consists of a GPS receiver, accelerometer, gyroscope, wheel speed sensor, and gearbox speed sensor. The GPS receiver can receive the signals from satellites and provides the information including the vehicle position, altitude, heading direction, and velocity. The positioning error of the GPS module is not greater than 5 meters in general case. The accelerometer is used to provide the vehicle acceleration in (3) to vary the state of Kalman filter. The wheel speed sensor and gearbox speed sensor are used to measure the velocity of vehicle. In low speed driving, the gearbox speed sensor can capture the vehicle speed more accurate than wheel speed sensor, and vise versa. Therefore, the velocity measurement is separated into two regions. If the velocity of vehicle is less than 10 kph, the measurement of velocity is adopted the measurement result of gearbox speed sensor, otherwise, the velocity measurement is relied on wheel

Figure 2. The architecture of sensor fusion system.

Figure 3. The digital map of Baguashan Tunnel.

Figure 4. The experimental platform.

Vehicle

GPS

Gyroscope

Accelerometer

Wheel speed sensor

Gearbox speed sensor

Digital Map

speed sensor. Hence, the velocity measurement error is less than 5 kph. After filtering the noise of vehicle velocity by (6)-(8), the cruse distance pk and velocity vk in (3) can be estimated. Similar to the manner of velocity estimation, the gyroscope integrated yaw rate into heading angle and compared with GPS course. The role of gyroscope in the sensor fusion system is to detect the yaw rate of the vehicle since the GPS signal is lost. After integrating yaw rate, the vehicle yaw angle is easy to be determined. To accomplish the experiment, all the sensors of sensor fusion system are calibrated in advanced. The wheel speed sensor and gearbox speed sensor is calibrated according to GPS velocity signal and the gyroscope measurement result is also aligned in terms of GPS heading signal.

IV. EMPIRICAL RESULTS The target of this paper is to develop a methodology to

tackle the problem of GPS signal lost. In order to demonstrate our proposed localization method, a vehicle is driven to pass through a tunnel is scheduled. The scheduled tunnel is named Baguashan Tunnel, which is located at Changhua, Taiwan. The total length of Baguashan Tunnel is 4928 meters which connects east and west side of Bagua Mountain so that it can save more than half hours of traveling time from Nantou to Changhua. The roadmap of Baguashan Tunnel is shown in figure 3. In our experiment, the experimental platform is maneuvered from western entry to eastern entry of Baguashan Tunnel in advance and than is driven back from eastern entry to west. Our experimental platform, named Mitsubishi Savrin, is shown in figure 4 and the sensor fusion system is equipped on this platform. In this section, our localization results are compared to the results of using dead reckoning methodology, so that the comparisons are easy to show the improvement of the performance of vehicle localization. Figure 5 shows the experiment result that the vehicle is maneuvered from western entry to eastern exit. The true trajectory of Baguashan Tunnel is depicted by spot line. The solid line is the result of our proposed method and is compared to the dash line of using integrator only. Figure 6 shows that the localization result of our proposed method is more close to the actual trajectory than the result of using dead reckoning methodology. It is easy to make the comparison that the result of our proposed method is more match the actual trajectory of Baguashan tunnel. The localization error is expressed in figure 6. The localization error of using dead reckoning methodology exhibits the tendency to diverge and the maximum error is greater than 500 meters. However, the maximum localization error of our proposed method is not greater than 50 meters. Thus, our proposed method is able to reduce 90 % error of the result of using dead reckoning methodology.

Due to the terrain of Bagua Mountain, the localization result from eastern entry to western exit might be affected and generally make the results inaccurate. Figure 7 is the localization results of the maneuver of vehicle from eastern entry to western exit of Bagrashan Tunnel. The localization result of using dead reckoning methodology (dash) will leave the actual trajectory (spot) further with the time. By Contrast, the result of our proposed method (solid) is more match the actual trajectory. The localization errors of these two methods are illustrated in figure 8. After travelling about 5000 meters,

the localization error of using dead reckoning method presents the tendency to diverge and the maximum error is almost 1000

120.6 120.61 120.62 120.63 120.64 120.65 120.6623.94

23.945

23.95

23.955

23.96

Longitude

Latit

ude

ResultReferenceDead Reckoning

Figure 5. Experiment results (from western entry to east)

0

500

1000Lo

ngitu

de e

rror (

met

er)

0 20 40 60 80 100-200

0

200

400

Sampling(time)

Latit

ude

erro

r(met

er)

ResultDead Reckoning

Figure 6. Comparison of localization error between proposed method and dead reckoning method.

120.6 120.61 120.62 120.63 120.64 120.65 120.6623.93

23.935

23.94

23.945

23.95

23.955

23.96

23.965

Longitude

Latit

ude

ResultReferenceDead Reckoning

Figure 7. Experiment results (from eastern entry to west)

-1000

-500

0

500

Long

itude

erro

r (m

eter

)

ResultDead Reckoning

0 20 40 60 80 100-500

0

500

1000

Sampling(time)

Latit

ude

erro

r(met

er)

Figure 8. Comparison of localization error between proposed method and dead reckoning method.

meters, whereas, the maximum localization error of our proposed method is not exceeded 100 meters whatever the error of longitude and latitude. Obviously, our proposed method enables to improve the localization performance of dead reckoning methodology.

V. CONCLUSIONS This paper presents a recursive methodology of vehicular

position estimating since GPS signal is fail. Based on dead reckoning method, three steps are proposed to improve the result of vehicle localization. First of all, the Kalman filter is utilized to tackle of filtering the measurement noise of velocity which is obtained from gearbox and wheel speed sensors, so that the estimative vehicle velocity is more matching the actual velocity. Therefore, the status of vehicle in next sample time can be predicted. Secondary, the ARMA modeling technique is employed to describe the relationship between the previous and present states of vehicle so that the vehicular displacement model is thus formulated. And the last step, the position error of ARMA model is corrected by means of digital map matching.

To demonstrate the efficacy of our proposed methodology, a vehicle is driven to pass through Baguashan tunnel in Changhua, Taiwan. Outside the tunnel, the position of vehicle is measured through GPS signals and the accuracy is better than 10 meters; on the contrary, the localization problem is relied on sensor fusion system since the vehicle is driven in the tunnel. According to the experiment results, the vehicle positioning error is not exceeding 100 meters after traveling 5000 meters in the tunnel. By contrast, our proposed methodology is able to reduce more than 80% of the positioning error of dead reckoning method. Consequently, the experiment results can evidence that our proposed methodology is able to improve the performance of dead reckoning method.

ACKNOWLEDGMENT 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.

REFERENCES [1] A. Agarwal and S. R. Das, “Dead reckoning in mobile ad hoc network,”

in Proc. of Wireless Communications and Networking, vol. 3, 2003, pp. 1838-1843.

[2] K. Arshak and F. Adepoju, “Adaptive linearized methods for tracking a moving telemetry capsule,” in Proc. of IEEE International Symposium on Industrial Electronics, 2007, pp. 2703-2708.

[3] M. Athineos and P.W. Ellis, “Autoregressive modeling of temporal envelopes,” IEEE Trans. on Signal Processing, vol. 55, no. 11, 2007, pp. 5237-5245.

[4] W. Cai, F. B. S. Lee, and L. Chen, “An auto-adaptive dead reckoning algorithm for distributed interactive simulaiton,” in Proc. of the 13th Workshop on Parallel and Distributed simulation, 1999, pp. 82-89.

[5] M. Y. Chen, T. Sohn, D. Chmelev, D. Haehnel, J. Hightower, J. Hughes, A. LaMarca, F. Potter, I. Smith, and A. Varshavsky, “Partical metropolitan-scale positioning for GSM phones,” in Proc. of the 8th International Conf. on Ubiquitous Computing, 2006, pp. 225-242.

[6] A. Conti, M. Guerra, D. Dardai, N. Decarli, and M. Z. Win, “Network experimentation for cooperative localization,” IEEE Journal on Selected Areas in Communications, vol. 30, no. 2, 2012, pp. 467-475.

[7] C. Hu, W. Chen, Y. Chen, and D. Liu, “Adaptive Kalman filtering for vehicle navigation,” Journal of Global Positioning System, vol. 2, no. 1, 2003, pp. 42-47.

[8] M. Jachan, G. Matz, and F. Hlawatsch, “Time-frequency ARMA models and parameter estimators for underspread nonstationary random process,” IEEE Trans. on Signal Processing, vol. 55, no. 9, 2007, pp. 4366-4381.

[9] G. R. Jagadeesh, T. Srikanthan, and X. D. Zhang, “A map matching method for GPS based real-time vehicle localization,” Journal of Navigation, vol. 57, 2005, pp. 429-440.

[10] T. Jiang, N. D. Sidiropoulos, and G. B. Giannakis, “Kalman filter for power estimation in mobile communicaiton,” IEEE Trans. on wireless communication, vol. 2, no. 1, 2003, pp. 151-161.

[11] T. Kimchi, A. S. Etienne, and J. Terkel, “A subterranean mammal uses the magnetic compass for path integration,” Proc. of the National Academy of Science, vol. 101, no. 4, 2004, pp. 1105-1109.

[12] X. Li, H. Shi, and Y. Shang, “A sorted RSSI quantization based algorithm for sensor network localization,” in Proc. Of the 11th International Conf. on Parallel and Distributed Systems, 2005, pp. 557-563.

[13] S. Rezaei and R. Sengupta, “Kalman filter based integration of DGPS and vehicle sensors for localization,” in Proc. of the IEEE International Conf. on Mechatronics & Automation, 2005, pp. 455-460.

[14] S. S. Saab, “An RSS based localization algorithm in cellular networks,” in Proc. of International Conf. on Advances in Information and Communication Technologies, 2011, pp. 167-171.

[15] M. Saedan, C. W. Lim, and M. H. Ang, Jr., “Omnidirectional image matching for vision-based robot localization,” in Proc. of the 2006 IEEE International Conf. on Mechatronics, 2006, pp. 17-22.

[16] T. I. Salsbury and A. Singhal, “A new approach for ARMA pole estimaiton using higher-order crossing,” in Proc. of the 2005 American Control Conf., 2005, pp. 4458-4463.

[17] S. Se, D. G. Lowe, and J. J. Little, “Vision-based global localization and mapping for mobile robots,” IEEE Trans. on Robotics, vol. 21, no. 3, 2005, pp. 364-375.

[18] G. D. Stefano and A. Petricola, “A distributed AOA based localization algorithm for wireless sensor networks,” Journal of Computers, vol. 3, no. 4, 2008, pp. 1-8.

[19] P. Xing, H. Yu, and Y. Zhang, “An assisting localization method for wireless sensor networks,” in Proc. of the 2nd International Conf. on Mobile Technology, 2005, pp. 1-6.