Improved Vehicle Positioning in Urban Environment through Integration of GPS and Low-Cost Inertial Sensors Pavel Davidson, Jani Hautamäki, Jussi Collin, and Jarmo Takala Department of Computer Systems, Tampere University of Technology P.O.Box 553, FIN-3310, Finland

BIOGRAPHY Pavel Davidson received his MSc in aerospace engineering from Technion, Israel Institute of Technology in 1996. From 1996 until 2006 he was employed with Israel Aircraft Industries, Tricom Technologies, and Spirit Corp. His main research interest is in navigation including multi-sensor integration and GIS. Currently he is a PhD candidate at Tampere University of Technology (TUT). Jani Hautamäki is studying towards MSc degree at the Department of Computer Systems at TUT. His area of specialization is programming of embedded systems. Jussi Collin is a senior research scientist at TUT. He received his Dr. Tech. degree from TUT in 2006. The topic of his dissertation was: Investigations of Self-Contained Sensors for Personal Navigation. His current research topic is sensor-aided navigation systems. Jarmo Takala received his MSc and Dr. Tech. degrees from TUT in 1987 and 1999, respectively. Currently he is Professor of Computer Engineering at TUT. His research interests are digital signal processing and parallel architectures for DSP INTRODUCTION Accurate and continuous position calculation is a key task for vehicle navigation and telematics applications. In most portable car navigation and telematics devices, the position is calculated based only on GPS data. However, in urban canyons stand-alone GPS suffers signal masking and reflections of the signal from buildings, large

vehicles, and other reflective surfaces. Driving tests in such metropolises as Hong Kong, Tokyo, and New York show that the chance of receiving four GPS satellites required for navigation can be as low as 20% of total driving time [1]. Even when four or more satellites are available, strong multipath effect might cause a positioning error of more than 100 m. In order to obtain uninterrupted navigation data in urban environment, GPS data can be augmented with a complementary navigation system that can work continuously in any type of urban environment [2]–[4]. This is known as an integrated navigation system. This article presents such an integrated GPS/DR (Dead Reckoning) navigation system. We consider a DR configuration consisting of one gyro for directional measurements and 3D accelerometer. Both the gyro and accelerometers satisfy the requirements for mass market portable consumer devices: low cost, light weight, and low power consumption. An odometer is not used in the proposed system as it requires additional car installation; this system is intended mainly for portable devices such as mobile phones, GPS-based peripherals, and handheld GPS navigation devices. This paper shows how GPS data can be augmented with DR sensor data using an Extended Kalman Filter (EKF) to achieve the required navigation performance in urban environment. In the proposed system, the DR sensors are calibrated when GPS is available. If GPS signal is not available or GPS position accuracy suffers from multipath, the calibrated DR sensors are used to determine the position of the vehicle. Field tests with real-time prototype show that the proposed method improves vehicle positioning performance in high multipath signal environment and during short GPS signal outages.

Sensor Parameter Compensation Gyro

3D Accelerometer

Stop Detection Module

Coordinate Transformation Matrix Computation

GPS Multi-Sensor Data Fusion (Extended Kalman Filter)

PN , PE ,V,ψ ,σ N ,σE ,σV ,σψ

Figure 2. Conceptual description of the navigation information sources and data flow for our portable car navigation system.

Figure 1. Typical urban environment in downtown Calgary, Canada. 1

INTEGRATION CONCEPT

The proposed car navigation system includes different types of navigation sensors and technologies. Our concept of data fusion from multiple sensor technologies is illustrated in Fig. 2. This methodology comprises several stages of data processing: − − − −

Inertial sensors data for stop detection; Inertial sensor data for position, velocity and heading computations (DR computations); Calibration of inertial sensors when vehicle is stationary; Integrated GPS/DR navigation solution;

This evaluation kit allows convenient real-time data fusion between the GPS and inertial sensors. In this arrangement the GPS receiver is the core of the system and is responsible for both making its own measurements, and for time tagging of the inertial sensor measurements. The integrated GPS/DR solution is computed at rate of 5 Hz and represented in NMEA 0183 format. 3

INTEGRATION OF GPS AND DR

Kalman filtering The real-time data fusion algorithm employs an extended Kalman filter (EKF) to combine computed GPS position, velocity, and heading with the acceleration and heading rate measurements provided by the 3D accelerometer and heading gyro. The EKF uses the values of the filter states to predict the future states through a dynamic model

The hardware parts of our navigation system are the shaded boxes. The above concept of integrating GPS and inertial sensors will be explained and represented mathematically in chapter 3. 2

DESCRIPTION OF REAL-TIME PROTOTYPE

To evaluate the performance of our integrated GPS/DR navigation system, a real-time prototype was built. One Analog Devices ADXRS150 yawrate gyro [5], and VTI Technologies SCA3000 3D accelerometer [6] were selected as the dead reckoning sensors to augment IT03 L1 GPS receiver by Fastrax [7], which is based on the Atheros chipset [8]. The inertial sensors are mounted on a separate sensor board which is connected to the Fastrax IT03 evaluation kit via SPI bus available on I/O card terminal connectors as shown in Fig. 3.

Figure 3. Evaluation kit hardware. GPS receiver chip is located below the sensor board.

which is based on dead reckoning equations to estimate position, velocity, and heading. The dynamic model is given by

 P&N  V cosψ       P&E  V sinψ  x& =   = f ( x ) =   & V   aL   &  ω    ψ 

(1)

where PN , PE are the vehicle north and the east positions, ψ is the vehicle heading, ω is the measured heading rate, V is the speed over ground, a L is the measured vehicle acceleration in the longitudinal direction. The vehicle frame longitudinal acceleration is calculated by transforming three-dimensional acceleration vector measured by accelerometers in the sensor frame into vehicle frame and calculating projection of the transformed vector on vehicle’s longitudinal axes. It also includes the effect of gravitational forces because of unknown road grade. It is defined as

a L = aT + bL + g ⋅ θ + n

(2)

where aT is the true vehicle longitudinal acceleration, g is the gravitational constant, θ is the road grade, bL is the longitudinal acceleration error and n is a wideband accelerometer noise. To properly model accelerometer and gyro measurement errors and unknown road grade, the state vector is augmented with two additional states: δω gyro bias, and δa acceleration bias and misalignment (including unknown road grade) as follows:

x = [PN

PE

V ψ

δw δa ]T .

(3)

The Kalman filter covariance propagation is given by

P (k + 1 | k ) = Φ (k )P (k | k )Φ (k ) + Γ(k ) T

(4)

where Φ is a discrete equivalent of the continuous transition matrix F and Γ is a discrete equivalent of the process noise power spectral density matrix. Since the dynamic model is nonlinear, it has to be linearized with respect to the state variables. The linearization is performed around the previously estimated values such that the continuous transition matrix F takes the following form:

0  0  0 F =  0  0  0

0 cosψ

− V sin ψ

0

0 sin ψ

V cosψ

0

0

0

0

0

0

0

0

1

0

0

0

− 1 / Tg

0

0

0

0

  0   1 . 0   0   − 1 / Ta  0

(5)

The states for the gyroscope and acceleration measurement errors were modeled as the first order Gauss-Markov processes with time constants Tg and Ta respectively. The values of these time constants and driving noise parameters are the tuning parameters of the Kalman filter. They can be derived from the accelerometer and gyro random walk and output noise characteristics. These values can be adjusted to approximate the Allan Variance plot of correlated noise in averaging time range we are interested. Since only one heading gyro is used, the road tilt influence on horizontal acceleration measurements cannot be separated from the actual vehicle acceleration. Nevertheless if the driving noise and time constant in Gauss-Markov process are chosen correctly the total acceleration measurement error including road tilts can be approximated quite well by this model. Compared to the bias error and unknown road tilts, the effect of the scale factor error is relatively small. Therefore, they are not included in the state vector. The EKF state vector is a 6-state vector. It was verified by numerous road tests that when GPS signals are available the integration algorithm for GPS+MEMS sensor measurement is robust to sensors bias variations and to moderate road tilts. The Kalman filter propagation of the state and covariance is done with approximately 10 Hz rate. This sampling rate guarantees no degradation for vehicles in regular urban dynamic environment. A 10 Hz propagation rate allows for a first order approximation of the continuous transition matrix F by its discrete equivalent Φ = I+F∆t, where I is identity matrix and ∆t is sampling period. The observation vector is calculated by taking the difference between GPS and DR corresponding positions and velocities, and in some cases heading. The measurement update can take the following forms: − − −

vehicle position, velocity and heading; vehicle position and velocity; velocity only, in the case of the zero velocity update;

The position accuracy of a single-frequency L1 GPS receiver is approximately 10 m in the horizontal axis and 15 m in the vertical axis. A single-frequency L1 GPS receiver determines velocity based on the Doppler shift of the GPS carrier wave. The velocity accuracy in the horizontal axis can reach 2–5 cm/s and in the vertical axis 4–10 cm/s 1σ standard deviation of the stochastic errors [9]. The accuracy of GPS depends heavily on satellite geometry and multipath errors. In this project, the update rate of the GPS receiver was set to 1 Hz to reduce the amount of computations in the processor. The GPS velocity measurements can be also used to determine vehicle heading. If there is no vehicle sideslip, the heading can be calculated as east north ψ GPS = arctan (VGPS / VGPS )

(6)

east where VGPS

V north are the east and north GPS , GPS velocity measurements, respectively. The standard deviation of the heading error can be approximated by [12]

σ (ψ GPS ) = σ (δVGPS ) / VGPS .

(7)

The GPS heading ψ GPS is calculated only when a vehicle has sufficient speed. This threshold is determined empirically and here a threshold of 2.5 m/s was used. Alignment and calibration In order to assist GPS with the gyro and accelerometers, initialization procedures have to be completed: initial alignment and calibration. The initial alignment procedure includes horizontal alignment based on the accelerometers outputs, yaw angle estimation using horizontal components of vehicle velocity, and azimuth alignment using external heading information. The initial calibration includes estimation of the accelerometer bias and misalignment, and also calibration of the gyroscope bias. The horizontal alignment includes the estimation of the sensor frame orientation with respect to the pseudo vehicle frame [2]. The pseudo vehicle frame can be defined as: X axis corresponds to longitudinal axis of vehicle, Z axis corresponds to vertical axis of the local level, and Y axis completes the triad. When vehicle is stationary the outputs from the horizontal accelerometers are caused by the tilt of the sensor frame with respect to the local horizon and accelerometer biases. This effect can be compensated in the following manner

a x  0    S  0  = DV a y      a z  g 

(8)

where a x , a y , a z are measured accelerations in sensor frame, g is the gravitational acceleration and DVS is a coordinate transformation matrix from sensor frame S to pseudo-vehicle frame V. Horizontal alignment gives two Euler angles for transformation matrix DVS . In reality the alignment is usually performed in a place which is tilted with respect to the local horizon by some small angle. In this case, the transformation matrix DVS will include this misalignment error which in turn will result in errors when calculating the projection of the acceleration vector on vehicle’s longitudinal axes. However, this misalignment error can be estimated quite accurately when DR computed velocity is compared with GPS velocity. Therefore, the detrimental effect of misalignment error can be reduced when the vehicle’s longitudinal acceleration is compensated with the estimated misalignment error. The yaw angle between sensor frame and vehicle frame can be estimated when vehicle is accelerating with constant direction [2] using the following kinematic relation:

 V yH V H  x

γ = arctan

   

(9)

H where V xH , V y are the horizontal components of

vehicle velocity. The calculation of the yaw angle gives the third angle necessary for computation of the transformation matrix DVS . The moments when vehicle is stationary can be also used for estimation of gyroscope bias error. In this case, the changes in heading are caused only by heading gyro bias. Therefore, the process of gyro bias estimation is straightforward. A real-time estimation algorithm can be based, for example, on recursive least squares method. In our navigation system, both the alignment and calibration are implemented in real-time and can be performed at any time when vehicle is stationary. It takes about 20 seconds to complete all these procedures in fully automatic mode. There is no

1200

10

1150

2

1100 1050 1000 950 900

10

850

17

800 17

18

19

20

21

22

23

Figure 4. Magnitude of the acceleration vector. Red dots show the acceleration (mg) during detected stop vs. time (min). need for the user to take any special actions. During and after the alignment the portable navigation device has to be fixed into a cradle. The system has the capability to re-initialize alignment and calibration parameters at any time when it is not moving. The need for re-computation of alignment parameters may arise due to change in orientation of the device during the drive which can be detected by the algorithm. This change in orientation can affect the gyro scale factor. The need for re-computation of accelerometer and gyro calibration parameters is caused by changes of accelerometer and gyro bias mainly due to temperature variations. Stop detection module

1

18

19

20

21

22

23

Figure 5. Mean quadratic deviation calculated in (10). Vertical axis is in logarithmic scale and time in minutes. The dashed line is the decision threshold. Red dots above the decision line are due to the time lag. measurement bias, and noise [10]. As it can be seen from the curve, during a complete stop the variance of the signal is noticeably smaller since it contains only the gravitational acceleration, accelerometer noise, and engine vibrations. The bias drift over short time is negligible. The sampling frequency of accelerometer in this example was 100 Hz.

r

During complete stops the variance of a does not exceed certain threshold which can be found during short initial learning phase. The mean and variance r of a during complete stop, µ s and σ s2 , respectively, are computed and stored.

The stationary state is detected by a stop detection module based on accelerometer information. Our stop detection algorithm is based on the analysis of the vehicle acceleration measured by the accelerometers and its variance. When GPS signals are available the velocity computed by a GPS receiver can be also used. Since this algorithm has to be independent of the mounting angle of the device, the original measurements at time t k from three accelerometers,

a x (t k ) , a y (t k ) , a z (t k ) , in sensor frame are r transformed to a acceleration vector, a (t k ) , since

the norm of acceleration vector does not depend on the angles to which the device is mounted [11]. A typical example of the magnitude of measured acceleration vector during a test drive is shown in Fig. 4. Vehicle acceleration in horizontal plane depends significantly on the movement of the vehicle including road imperfections,

The real-time implementation of this algorithm is based on the analysis of accelerometer data inside time window of predefined length. The time window length is one of the design parameters in this algorithm and usually chosen as 0.6–1.0 sec. Then the mean quadratic deviation of the acceleration samples from ߤ௦ inside this time window is calculated as

σn =

1 N

N

∑ (x

− µs )

2

k

(10)

k =1

r

where x k = a (t k ) is the magnitude of accelerations

r

[

]

at time t k , a (t k ) = a x (t k ) a y (t k ) a z (t k ) , and T

N is the number of samples inside the time window. The decision about vehicle being moving or stationary is made based on computed deviation, r σ n , and variance of a , σ s2 ; when σ n < p ⋅ σ s , we

Figure 6. Tampere road test. Position computed by DR navigation system during 30 sec GPS outage is shown by black dots. Map source – OpenStreetMap.org assume complete stop. The parameter p was chosen empirically based on the results of drive tests. This algorithm can work in all types of vehicles and has the ability to adapt to different conditions during short learning phase when GPS is available. The real-time version of stop detection algorithm employs the probabilistic approach. It analyses the accelerometers data within a moving window. Unlike the algorithm described in [10] where stops are reliably detected only after 15 sec delay, our algorithm detects stops almost immediately. 4

EXPERIMENTAL RESULTS

Our portable car navigation system was tested in road tests in urban environment that included tunnels, parking garages, urban canyons, and road interchanges. Typical performance during these road tests is presented in this section. The first experiment is driving test in Tampere, Finland, shown in Fig. 6. The 30 sec GPS outage was created artificially by unplugging the GPS antenna from the receiver. The position computed by our combined GPS/DR navigation system is shown by green dots. When GPS signals are not available the navigation system continues to compute vehicle position, velocity, and heading using MEMS gyro and accelerometers. Black dots for the trajectory on the map correspond to the deadreckoned position when GPS signals are not available. Magenta dots correspond to the trajectory

Figure 7. Portland tunnel road test. The tunnel is about 300 m long. The first GPS fix after the tunnel was 4 sec after car exit the tunnel. computed by DGPS. Since DGPS position error is only about 0.2 m, we consider this position our reference trajectory. The direction of car motion is shown by the arrow. This test shows that the combined GPS/DR solution provides significant improvement; accurate position, velocity, and heading information were available even in the absence of GPS signal. After 30 sec GPS outage the position error did not exceed 20 m. Figure 7 displays the system positioning performance during driving test in Portland, OR that included a 300 m long tunnel. It should be noted that there are no GPS signals in the tunnel. Magenta dots correspond to the standalone GPS receiver position measurements. Green crosses for the trajectory on the map correspond to the combined GPS+MEMS computed position. Black dots correspond to the dead-reckoned solution when GPS signals were not available. The first GPS position fix was received approximately 4 sec after car passed the tunnel making the total GPS outage about 18 sec. The error of GPS+MEMS computed position at that time was less than 20 m. The first GPS measurements after the tunnel have a quite large position error, about 30 m. This test shows that the combined GPS+MEMS solution provides significant improvement; accurate position, velocity, and heading information were available even in the absence of GPS signal. Figure 8 displays the test route in Portland, OR downtown drive test. This test route included high-

Figure 8. Portland urban canyon test. This test route goes between tall buildings. In some places GPS position error caused by multipath is about 35 m. multipath urban canyon environment in the vicinity of high-rise buildings. In some places, GPS position error caused by multipath was about 35 m. This test shows improvements of the combined GPS+MEMS system performance in high-multipath environment, which is explained by the ability of the combined GPS+MEMS algorithm to filter out GPS position and velocity outliers. 5

CONCLUSIONS

This paper has shown that low-cost inertial sensors can significantly improve GPS positioning by continuing to output position during short GPS outages with sufficient accuracy for most of car navigation applications. The integrated GPS+MEMS system has also demonstrated improvement of position and velocity accuracy in high multipath urban canyon environment and the ability to provide continuous output of the vehicle heading even when vehicle is not moving. This is useful if we apply the map-matching algorithm. This device does not require any installation in the vehicle. It works in all vehicles and can be easily transferred between vehicles. Finally, it should be noted that our design is suitable for portable navigation devices since the cost, size, and power consumption of inertial sensors meet the requirements for mass market consumer electronics. REFERENCES [1]

Zhang, X., Wang, Q., and Wan, D., “Mapmatching in road crossings of urban canyons

based on road traverses and linear heading change model,” IEEE Trans. Instrumentation and Measurement, vol. 56, no. 6, pp. 2795– 2803, Dec. 2007. [2] Chowdhary, M., Colley, J., and Chansarkar, M., “Improving GPS location availability and reliability by using a suboptimal, low-cost MEMS sensor set,” in Proc. ION GNSS Int. Technical Meeting Satellite Division, Fort Worth, TX, Sept. 25–28 2007, pp. 610–614. [3] Davidson, P., Hautamäki, J., and Collin, J., ”Using low-cost MEMS 3D accelerometer and one gyro to assist GPS based car navigation system,” in Proc. 15th Int. Conf. Integrated Navigation Syst., St. Petersburg, Russia, May 26–28 2008, pp. 279–285. [4] El-Sheimy, N., “Less is more – The potential of partial IMUs for land vehicle navigation,” Inside GNSS Magazine, vol. 3, no. 3, pp. 16– 25, Spring, 2008. [5] Analog Devices Inc., “ADXRS150 ±150°/s single chip yaw rate gyro with signal conditioning,” datasheet, http://www.analog.com/en/prod/0,2877,ADX RS150,00.html [6] VTI Technologies Oy, “SCA3000-D01 3-axis low power accelerometer with digital SPI interface,” datasheet, http://www.vti.fi/en/productssolutions/products/accelerometers/sca3000accelerometers/ [7] Fastrax Ltd., “IT03 development kit,” technical specification, http://www.fastrax.fi [8] Atheros Communications, http://www.atheros.com [9] Lätti, J., Leppäkoski, H., J. Collin, J., and Takala, J. “Accurate velocity determination using GPS: A comparison between three stand-alone receivers,” in Proc. ENC-GNSS, Geneva, Switzerland, May 29 – June 1 2007, pp. 55–66. [10] Basnayake, C, Mezentsev, O., Lachapelle, G., and Cannon, M.E., “An HSGPS, inertial and map-matching integrated portable vehicular navigation system for uninterrupted real-time vehicular navigation,” Int. J. Vehicle Inf. Commun. Syst., vol. 1, no. 1/2, pp. 131–151, 2005. [11] Hong, Y., “An algorithm to detect zerovelocity in automobiles using accelerometer signals,” MSc Thesis, Tampere Univ. Tech., Finland, Nov. 2008. [12] D. Bevly, “Global Positioning System (GPS): A Low-Cost Velocity Sensor for Correcting Inertial Sensor Errors on Ground Vehicles,” J. Dynamic Systems, Measurement, and Control, vol. 126, pp. 255–264, June 2004.