GPS/MEMS INS Data Fusion and Map Matching in Urban Areas

This paper presents an evaluation of the map-matching scheme of an integrated GPS/INS system in urban areas. Data fusion using a Kalman filter and map matching are effective approaches to improve the performance of navigation system applications based on GPS/MEMS IMUs. The study considers the curve-to-curve matching algorithm after Kalman filtering to correct mismatch and eliminate redundancy. By applying data fusion and map matching, the study easily accomplished mapping of a GPS/INS trajectory onto the road network. The results demonstrate the effectiveness of the algorithms in controlling the INS drift error and indicate the potential of low-cost MEMS IMUs in navigation applications.


Introduction
A critical component of a navigation system is the Global Positioning System (GPS). However, outages and multi-path phenomenon of GPS signals frequently occur in urban areas. Another navigation system, the inertial navigation system (INS), is an interesting complementary navigation system [1]. GPS has long-term stability with homogeneous accuracy, while the short-term stability with high navigation accuracy of the INS is excellent, but stand-alone INS positioning accuracy deteriorates rapidly with time. Thus, the integration of GPS and INS provides a high data rate of complete navigation solutions (i.e., position, velocity, and attitude) with superior accuracy. Advances in Micro-Electro-Mechanical

OPEN ACCESS
System (MEMS) inertial sensor technology have rendered the integrated GPS and INS a low-cost option for positioning and navigation. In data fusion, the Kalman Filter (KF) suffers from divergence in the approximations of the linearization process during GPS outages. Inertial sensor biases are not well estimated leading to residual drifts, especially when using MEMS-based inertial measurement units (IMUs) [2,3]. Previous studies applied KF to combine GPS and IMU data in vehicle and aircraft navigation [4,5]. Bevly [4] demonstrated the ability of a low-cost GPS receiver to reduce errors inherent in low-cost ground vehicle IMUs. Wendel et al. [5] addressed the development of an integrated navigation system based on MEMS IMU and GPS for an unmanned aerial vehicle (UAV).
To obtain a robust navigation system, mapping the position onto a spatial road map is necessary [6]. This spatial mismatch is particularly severe at junctions and built-up areas with complex routes. Quddus et al. [7] reviewed the literature on map matching and listed the advantages and drawbacks of the various algorithms. The commonly used methods are point-to-curve matching and curve-to-curve matching. The point-to-curve matching algorithm attempts to identify the curve (link) that is closest to a point. The approach uses a local or incremental algorithm that maps current positions onto a road line on a map. Cossaboom et al. [8] used KF to integrate GPS and INS in a loosely coupled scheme to enhance navigational solution while further improvement is achieved by the map matching. However, the approach only considers the current position and the matching accuracy is generally low. A more novel approach is curve-to-curve matching, which considers the estimated location as a curve consisting of a set of points. This algorithm achieves better accuracy based on certain distance measures and aligns an entire trajectory with a road map.
The main purpose of this study is to conduct KF and map matching by integrating GPS and INS data to identify the right trajectory and robust navigation, and to improve the matching navigation accuracy. In addition, this study compares the point-to-curve and curve-to-curve matching approaches in the process of using a road network map. Result shows that the curve-to-curve matching approach is more effective in navigation applications.

Data Fusion Algorithm
KF is used for data fusion in this study. KF addresses the general problem to estimate the state x of a discrete-time controlled process that is governed by the state equation as follows: (1) where is the state vector at time k including position error, velocity error, attitude error, accelerometer bias, gyroscope bias, accelerometer scale factor, and gyroscope scale factor, respectively, Ф k−1;k is the state transition matrix from time k−1 to k, and w k is the system noise.
The measurement model is: Since the aiding measurements from GPS are position and velocity, Equation (2) can be rewritten as: where are mapping matrices, is the position error vector; is the velocity error vector expressed in the Earth-centered Earth-fixed frame (ECEF or e-frame); and are the position and velocity noise, respectively. and are the positions in the e-frame obtained by INS and GPS, respectively. V e INS and V e GPS are the velocities in the e-frame obtained by INS and GPS, respectively.
In a KF-based system, the system noise (w k ) and the measurement noise (ε k ) are assumed to be independent normal distributions with zero mean and variances (Q k and R k ), respectively: , In fact, the system noise and measurement noise are not usually correctly estimated but can be modeled by the representation of a system noise model and measurement noise model. In practice, Q k is modeled based on sensor calibration and R k is modeled based on the GPS position and velocity uncertainty. Now, let's define as the a priori state and associated variance matrix estimated at time k given the knowledge of the process prior to time k−1, where: is the a posteriori state estimate at time k given the measurement z k . A priori and a posteriori estimate errors and associated covariance matrices are: , ] The linear combination between the a posteriori state estimate , a priori state estimate , and a weighted difference between actual measurement z k and measurement prediction is: The associated covariance matrix P k is determined as: where the equation ) is called measurement innovation or the residual. K is called as Kalman gain matrix and can be shown by: The KF estimates a process by using a form of feedback control-the filter estimates the process state at some time and then obtains feedback in the form of measurements. As such, the KF updates in two steps each stage: time update and measurement update. The time update estimates the current state and error covariance for the next time step. The measurement update incorporates a new measurement into the a priori estimate to obtain an improved a posteriori estimate.

Map-Matching Algorithm
The purpose of developing the curve-to-curve algorithm was to reduce the position error drift during GPS outages. The method used in the study is modified from [8]. Figure 1 shows the processes in the algorithm. In the data processing, signal points are ordered in time and then point ID are attached. A link represents a road in the map and the road azimuth is known. The steps are described as follows: Figure 1. The map-matching algorithm.
1. Integrate GPS and IMU data using the KF: Extended KF (EKF) is used to integrate GPS and INS data based on a loosely coupled scheme. The details of EKF can be found in previous researches [3]. In this study, a linear approximation of a nonlinear function is proposed. The EKF can be simplified to the KF algorithm. The equations of the algorithm are shown in Equations (1)-(11). 2. Search candidate road links in a 30-m trajectory buffer and project points to links: The purpose of this step is to determine all link candidates and project signal points to candidate links for map matching. Firstly, the buffer is created based on the GPS/ INS trajectory. All links within the buffer will be defined as the candidate road links. 3. Cluster point sets considering projected link azimuth and point ID continuity: Signal points and candidate road links are stored in a database of a GIS. When the position errors in some points are large, the projected link azimuth and point ID are vital information to facilitate dividing links into point clusters. Figure 2 illustrates the clustering process. Point sets in each road link can be separated when considering projected link azimuth and point ID information. G i and G ij are defined as the clustering point sets based on link azimuth i and the ones based on link azimuth i and section j. A two-step clustering approach is shown. For example, the point sets are firstly separated into G 2 (p4, 5) and the other cluster G 1 (p1, 2, 3, 6, 7, 8) based on the projected link azimuth, and then are separated into G 11 (p1, 2, 3) and G 12 (p6, 7,8) according to point ID continuity. 4. Determine the closest link in each point set: The objective is to determine the minimum distance from a point set to a link as the map matching indicator. If the minimum distance from a point set to a candidate link is determined, the link with the minimal distance is identified as the matching link: where D ij is the distance from a point set i to a link j; Li is the matching link in a point set i. For example, in Figure 2, the link with the minimal average distance is the best-matched link such as L1 link 1 in G 11 , L2: link 4 in G 2 , and L3: link 6 in G 12 . However, the point-to-curve algorithm skips step 4 and directly determines the closest link in each point. 5. Reduce redundancy: When the testing motorcycle stops, the redundancy is a large amount of duplicated random points. In the model, redundancy can be identified and removed automatically based on velocity information: , where v i is velocity of the vehicle at point pi; v thres is the threshold velocity in motorcycle stopping.

Experiments and Materials
Tests were performed in Tainan City to assess the efficacy of both algorithms. The test trajectory length was approximately 6 km and the total experiment time was about 1,500 s. The MIDG II low-cost IMU and single frequency GPS receiver were mounted on a motorcycle to collect data (Figure 3). Table 1 shows the MIDG II specifications. The MIDG II is a GPS-aided INS for use in obtaining attitude, position, velocity, acceleration, and angular rates for navigation.

GPS Receiver
MEMs IMU  Figure 4 shows the raw GPS point data (Left panel) and GPS/INS data integrated by KF (Right panel). Although GPS signals are often unavailable due to signal outages in urban canyons, the MEMS-based inertial sensor technology has high-frequency signals without outages for low-cost integrated vehicle navigation systems [9]. An integrated GPS and INS will overcome the outages, but the KF cannot compensate all the drift errors caused by both sensors on the motorcycle platform. The resulting trajectory doesn't match well the road map used as the reference. The average error of each integrated point to a matching line is 5.09 m (right panel in Figure 4). Compared to the average errors of only GPS from each point to the matching road, an INS/GPS system with an average position error is still large because the platform is a motorcycle. Motorcycles are not as stable as vehicles. In the system, we don't consider the nonholonomic constraint (NHC) that limits the velocity of the vehicle in the plane perpendicular to the forward direction. Thus, map matching is used directly to overcome the problem.   Figure 5). However, the curve-to-curve algorithm mitigates most of the errors and improves the navigational solution (Right panel of Figure 5). Figure 6 shows the details of matching result comparisons of the two algorithms. The matched links are separated into three sub-links using the point-to-curve algorithm (Left panel of Figure 6), but can be improved using the curve-to-curve algorithm (Right panel of Figure 6).  Most of the limitations of existing algorithms have been explained in [7]. Two major limitations of map matching are incorrect matches at junctions or parallel roads (Equation (1)) and redundant points when the motorcycle stops (Equation (2)). The difficulty of identifying the correct link at a junction using the traditional map-matching procedures is observable in the left panel of Figure 6. The point-to-curve algorithm assigns an incorrect link because the algorithm does not take all azimuth (heading) information of the trajectory into account. In our approach, the heading information for each point set is considered in the map matching. This information implies that the trajectory is the same link. The algorithm accurately matches the correct track with the road map (Right panel of Figure 6). In the system, KF has not considered the constraint of zero velocity update, however, many redundant points occurred when the motorcycle stopped in waiting areas and crossroads. The problem of redundant points will be solved by using map matching. According to the model, redundancy can be removed using the curve-to-curve algorithm when the motorcycle stops (Right panel of Figure 7). The algorithm can overcome the redundant point problem (Left panel of Figure 7) by considering point ID continuity and road network information, therefore, the research develops a smart map matching algorithm for road networks. The algorithm can reduce the drift errors associated with the GPS outages, incorrect matches, and redundant points when the motorcycle is stationary for the precise identification of the correct link.

Conclusions
This study applies data fusion and a map-matching model in identifying the relationship between GPS/INS measured data and road map data for a robust navigation solution. The algorithm is suitable for integrating GPS and INS data into map matching in urban areas, and improves the incorrect routes based on the point-to-curve map matching. Therefore, even using a low-cost IMU or an unstable platform, the curve-to-curve map-matching algorithm can identify the right routes. Consequently, the proposed scheme can provide consistent navigation solutions with improved sustainability in GPS-denied environments. The best routes of map matching can be determined by reducing the irrational routes in GPS-denied environments. Future studies will be conducted to extend the procedure from 2-D to 3-D in land vehicular navigation systems. In addition, further study of personal mobile devices for real-time pedestrian navigation will be undertaken.