THE PERFORMANCE ANALYSIS OF A 3 D MAP EMBEDDED INS / GPS FUSION ALGORITHM FOR SEAMLESS VEHICULAR NAVIGATION IN ELEVATED HIGHWAY ENVIRONMENTS

In this study, a 3D Map Matching (3D MM) algorithm is embedded to current INS/GPS fusion algorithm for enhancing the sustainability and accuracy of INS/GPS integration systems, especially the height component. In addition, this study propose an effective solutions to the limitation of current commercial vehicular navigation systems where they fail to distinguish whether the vehicle is moving on the elevated highway or the road under it because those systems don’t have sufficient height resolution. To validate the performance of proposed 3D MM embedded INS/GPS integration algorithms, in the test area, two scenarios were considered, paths under the freeways and streets between tall buildings, where the GPS signal is obstacle or interfered easily. The test platform was mounted on the top of a land vehicle and also systems in the vehicle. The IMUs applied includes SPAN-LCI (0.1 deg/hr gyro bias) from NovAtel, which was used as the reference system, and two MEMS IMUs with different specifications for verifying the performance of proposed algorithm. The preliminary results indicate the proposed algorithms are able to improve the accuracy of positional components in GPS denied environments significantly with the use of INS/GPS integrated systems in SPP mode. * Corresponding author: Y.H. Lee INTRODUCTION Global Positioning System (GPS) has become an indispensable technology for land vehicular navigation applications. More importantly, the convergence of location, information and communication technologies have created a rapidly emerging market known as Location Based Service (LBS). However, GPS can provide good type of information only when there is a direct line of sight to 4 or even more satellites. In other words, the system does not work well in urban areas due to signal blockages and/or attenuation, which may deteriorate the positioning accuracy. This harsh environment makes GPS more difficult to be accurate for vehicular navigation because of signal blockages and multipath effects. Typical candidates for such an integrated navigation system are the GPS and Inertial Navigation Systems (INS). An INS/GPS integrated system provides superior performance in comparison with either a stand-alone GPS or an INS as it can overcome each of their individual limitations. However, the high cost and government regulations prevent the wider inclusion of high quality INS to augment GPS as a commercial system for many navigation applications. The grades of INSs differ from the performance of accuracy, which the price of the higher one is not reasonable to embed in vehicle navigation system. The progress of the low-cost INS that in Micro-Electro-Mechanical Systems (MEMS) technology enables the development of complete inertial measurement unit (IMU) composed of multiple MEMS-based accelerometers and gyroscopes. In addition to their compact and portable size, the price of MEMSbased system is far less than those of high quality IMUs. For navigation purpose, such a system can bridge the gap caused by frequent GPS signal blockages and provide seamless navigation solutions in GPS denied environment, however, their positional errors grow rapidly with time. Therefore, integrating other cost effective sensors for providing complementary navigation solutions to low-cost INS/GPS integrated land vehicular navigation systems is desired.


INTRODUCTION
Global Positioning System (GPS) has become an indispensable technology for land vehicular navigation applications.More importantly, the convergence of location, information and communication technologies have created a rapidly emerging market known as Location Based Service (LBS).However, GPS can provide good type of information only when there is a direct line of sight to 4 or even more satellites.In other words, the system does not work well in urban areas due to signal blockages and/or attenuation, which may deteriorate the positioning accuracy.This harsh environment makes GPS more difficult to be accurate for vehicular navigation because of signal blockages and multipath effects.
Typical candidates for such an integrated navigation system are the GPS and Inertial Navigation Systems (INS).An INS/GPS integrated system provides superior performance in comparison with either a stand-alone GPS or an INS as it can overcome each of their individual limitations.However, the high cost and government regulations prevent the wider inclusion of high quality INS to augment GPS as a commercial system for many navigation applications.The grades of INSs differ from the performance of accuracy, which the price of the higher one is not reasonable to embed in vehicle navigation system.The progress of the low-cost INS that in Micro-Electro-Mechanical Systems (MEMS) technology enables the development of complete inertial measurement unit (IMU) composed of multiple MEMS-based accelerometers and gyroscopes.In addition to their compact and portable size, the price of MEMSbased system is far less than those of high quality IMUs.For navigation purpose, such a system can bridge the gap caused by frequent GPS signal blockages and provide seamless navigation solutions in GPS denied environment, however, their positional errors grow rapidly with time.Therefore, integrating other cost effective sensors for providing complementary navigation solutions to low-cost INS/GPS integrated land vehicular navigation systems is desired.

PROBLEM STATEMENTS
Conventionally, the Kalman Filter (KF) approach has been widely recognized as the standard optimal estimation tool for current INS/GPS integration schemes.However, it has limitations, which have been reported by several researchers (Gelb, 1974;Brown and Hwang, 1992;Vanicek and Omerbasic, 1999).The major inadequacy related to the utilization of the KF for INS/GPS integration is the necessity to have a predefined accurate stochastic model for each of the sensor errors (Brown and Hwang, 1992).Furthermore, the prior information about the covariance values of both INS and GPS data as well as the statistical properties (i.e. the variance and the correlation time) of each sensor system has to be accurately known (Vanicek and Omerbasic, 1999).
Therefore, for INS/GPS integration applications (where the process and measurement models are nonlinear), the Extended Kalman Filter (EKF) operates under the assumption that the state variables behave as Gaussian Random Variables (GRV).Naturally, the EKF may also work for nonlinear dynamic systems with non-Gaussian distribution, except for heavily skewed nonlinear dynamic systems, where the EKF used for conventional INS/GPS integration schemes may experience problems (Shin and El-Sheimy, 2004).Since the quality of the MEMS INS sensors used for land vehicular navigation purpose cannot compare with those expensive ones used for mapping applications, the sustainability of such low-cost INS/GPS integrated navigation systems is limited as their positioning errors accumulate rapidly with time during GPS signal blockages.
Map Matching (MM) technologies have been applied as one of the core components of vehicular navigation systems because of the inclination of modern vehicular navigation systems in early 1980s.According to Saab (2000), the earliest conventional MM algorithm, a semi-deterministic method, was invented by two research groups from the US and one research group from the UK.Although numerous MM algorithms have been developed since late last century, their functionalities are rather limited as presentation tools.In other words, they are hardly used as one of the positional sensors in those commercially available products.While the 2D information of longitude and latitude from GIS database is almost completed for the MEMS grade INS/GPS navigation integrated system fusion map matching algorithm, the height information become an necessary element because of the condition between the developments of the metropolis transportation system, raised up these years by building highways such as MRT rail, high spend rail and expressway, and the roadway under highways.The navigation accuracy is affected by not only the GPS blockage and the unfinished map database of height information.Recognizing the highways above the roadways is an underdeveloped technique that will be discussed in the following sections.
Many of the IMU/GPS integration schemes are available for implementing seamless land vehicular navigation applications, such as loosely coupled integration and tightly coupled integration schemes (Titterton and Weston, 2004).The loosely coupled integration strategy is thus chosen here due to its simplicity of implementation and good performance.Therefore, the objective of this article is to facilitate similar ideas to propose novel algorithms that embed a 3D map matching algorithm in conventional loosely coupled INS/GPS integration scheme, the conceptual description of proposed algorithms is shown in Figure 1.In addition, the performances of proposed algorithms are validated using the measurements collected on and under highways with the use of a MEMS INS/GPS integrated land vehicular navigation system.
Figure 1 The conceptual description of proposed algorithm

THE LOOSELY COUPLED INTEGRATION SCHEME
To estimate navigation solutions optimally, the output of the INS mechanization needs to be integrated with the position and velocity solutions derived from GPS.The EKF is the most popular estimation technique for such integration.A simple form of the mechanization equations in the local level frame can be written as follows (El-Sheimy, 2002): (1) Where, For further discussions concerning the solutions and numerical implementations of the above differential equation, see El-Sheimy (2002).An INS mechanization algorithm by itself is seldom in good performance due to the inertial sensor biases and the fixed-step integration errors, and those errors will cause the PVA solution to diverge quickly.The navigation software must have some approach to account for these error sources to correct the estimated PVA (Chiang, 2003).The dynamic error model used in the KF for the navigation parameters (position, velocity and attitude) can be determined through the linearization of the INS mechanization equations and by neglecting insignificant terms in the resultant linear model.A simplified form is then obtained as following equations (Bar-Itzhack and Berman, 1988): (2) (3) Where, : the optimally estimated state vector Φ : the state transition matrix P : the variance-covariance matrix of inertia states Q : the system noise covariance matrix (-) : the estimated value after prediction (＋) : the estimated value after updating The measurement of update equations utilize new measurements into the a priori state estimate to obtain an optimized an optimized a posteriori state estimate.The measurement update equations are given as: Where, K: the Kalman gain matrix H: the design matrix R: the measurements variance-covariance matrix Z: the vector of updating measurements of position and velocity The update engine of KF is triggered at every GPS measurement using the difference between GPS and INS solutions as input.Hence, the KF generates an updated estimate for reducing the INS errors using measurement update equations.Whenever a GPS measurement is unavailable, the KF works in time prediction mode to estimate the error state vector.In this case, the KF equations need the statistical properties of the system to be stationary and well defined which cannot be guaranteed (Shin and El-Sheimy, 2004).
There are two schemes available for implementing seamless land vehicular navigation applications that are loosely coupled integration and tightly coupled integration, respectively.The choice of integration strategy primarily depends on the type of applications.Although tightly coupled integration has the advantage of high accuracy, updating of the GPS when the number of satellites is less than four, the complexity of computation still causes the heavy computation loads for the processor used for low-cost vehicular navigation applications (Chiang and Huang, 2008).Hence, due to increased fault tolerance and a decrease in numerical computation, a loosely coupled integration scheme is more dependent and easy to implement, which makes loosely coupled integration scheme more popular (Chiang and Huang, 2008).For this reason, the loosely coupled integration strategy is chosen by its simplicity of implementation and good performance.
Figure 2 depicts the architecture of a traditional closed loop and loosely coupled INS/GPS integration scheme implemented in this study (Shin and El-Sheimy, 2004).The GPS processing engine first calculates position fixes and velocities in the local level frame and then sends those solutions as measurements to the INS KF.By comparing the navigation solutions provided by INS mechanization with those solutions provided by GPS processing engine, the error states can be estimated optimally.For mobile mapping applications, the addition of odometer and magnetic compass to a traditional loosely coupled INS/GPS integrated systems is considered practical to guarantee their sustainability and accuracy, however, it does not hold true for land vehicular navigation applications as the price of the hardware used, the computation load of the onboard processor, and portability are considered as the important obstruction factors to develop low low-cost and seamless land vehicular navigation systems.Therefore, the addition of odometer and magnetic compass to loosely coupled INS/GPS integrated land vehicular navigation systems will deteriorate their portability, increase the cost of hardware and cause computational burden to onboard processor.

MAP MATCHING ALGORITHM
As mentioned previously, even with sensor fusion technologies, various errors are often inevitably accompanied with the different sensors.As a result, the estimated vehicle positions deteriorate from its actual positions especially in urban canyon with highways.To deal with this uncertainty, the 3D MM technology has been developed.

Figure 4 The flow sheet of 3D MM procedure
There are many studies focus on the algorithm of map matching (French, 1989;Taylor and Blewitt, 1999;Syed and Cannon, 2005;Yu et al., 2004).The main purpose of MM is to determine the most probable location of the vehicle on the map.A good MM algorithm can significantly enhance the positioning accuracy of the vehicle on a road network.In this article, the MM procedure is divided into six steps: position fix, boundary setting, heading constrained, nearest point searching, height constraint and error detection.Figure 4 describes the procedure of 3D MM applied.
In the position fix process, the absolute position is provided by the INS/GPS integrated system.Based on this information, the position of the vehicle is matched with the road network provided by the map database using the shortest distance principle.Position fix is applied at the beginning of the trajectory and failure in the error detection for cutting down the computation load.Once the position is located, the boundary setting is triggered.Due to the great amount of the data in the map database, the boundary setting is applied for avoiding wasting unnecessary time of searching the impossible point which is far away from the position of vehicle.In the boundary setting, the last coordinate obtained from an INS/GPS integrated system is considered as a center point.Depending on this center point, a limited region is set to cover the scope the vehicle may appear, which is called possibility area (Yu et al., 2004), as shown in Figure 5.The range of the region correlates with the accuracy of the navigation system.6.By comparing the heading of the vehicle and the orientation of the candidate roads in the possibility heading, the ideal ones will be calibrated, also shown in Figure 6.Bullock, 1995).
Bullock [1995] proposed a way of NFS on vector form.Based on the type of measurements used in this study, the NFS is modified and implemented in point to point mode, as shown below: ( 1 1 ) Where, d : the distance between a vehicle location and a coordinates of a point on the road : a coordinate representing the search point on north and east ,respectively : a coordinate representing the point in the map database on north and east ,respectively Therefore, the point with the minimum value of d is defined as the closest matching point.In the error detection procedure, the 'error' occurred in the trajectory should be defined first.
The height constrain is defined as the third axis coordinate, height information, which become another limitation for the map matching.While the height information is not as precise as the horizontal coordinates, the constraint is set to be at least 10 meters in cases so that the positioning can be much reasonable.The limit of pitch direction of the attitude angles is a part of height constraint.And so on there are many methods for checking the abnormal condition, see Yu et al. (2004) for details.Nevertheless, this procedure is time consuming thus increases the latency of navigation solutions significantly.Once the same matched point is continuously matched over the threshold set in advance while the vehicle is moving, the location of the vehicle will be sent to the position fix process and re-calculated by the INS/GPS integration system.On the other hand, if the results obtained by forward step pass the error detection, the coordinates are inherited to boundary setting for next 3D MM and all the steps mentioned above are carried out recursively.
Figure 7 shown implemented to modify the conventional loosely coupled scheme.The matched coordinates is acquired, and they are tagged with the timing information provided by those candidates.

RESULTS AND DISCUSSIONS
To validate the performance of proposed 3D MM embedded INS/GPS integration algorithm, a field test was conducted in the downtown area of Kaohsiung, the second largest city of Taiwan.In this test area, two scenarios were considered, paths on and under the freeways, and also the streets between tall buildings, see Figure 8 as detail, where the GPS signal is obstacle or interfered easily.The test platform was part mounted on the top of a land vehicle and in the car system shown in Figure 9.The IMUs applied includes SPAN-LCI (0.1 deg/hr gyro bias) from NovAtel, which was used as the reference system (in car system shown in Figure 9  The raw GNSS carrier phase measurements were processed differentially using the GrafNav 8.10 software for further processing. The test IMU data sets provided by two low-cost IMUs, C-MIGITS III and MIDG II were processed with GNSS raw measurement collected by ProPak V3OEM-V using the conventional loosely coupled scheme shown in Figure 2 and proposed 3D MM embedded loosely coupled scheme shown in Figure 7, respectively.In addition, those GPS raw measurements were processed in single point positioning (SPP) mode using pseudoranges measurements.The lever arms between GPS and IMUs were measured before the test and compensated during processing.The length of this field test is around 110 minutes and the availability of GPS is around 50%.
The reference trajectory was produced using the raw measurements of SPAN-LCI and GNSS carrier phase measurements in differential mode with RTS smoothing implemented in tightly coupled mode.After producing optimally smoothed navigations solutions, the MM procedure is applied with road centerlines stored in a digital map database with smoothed navigation solutions to generate the reference trajectories for future analysis.The accuracy of the road centerlines stored in the digital map database applied in this study is around 50 centimeters, which is provided by Kaohsiung city government.On the other hand, the accuracy of reference trajectory provided by the reference system is around 20 centimeters.Therefore, the accuracy of map matched reference trajectory applied in this study is less than 1 meter after error propagation during 3D MM procedure.
Figure 10 The 3D road line with the result of EKF (blue line), EKF+MM (green line) and reference trajectory (red line) Figure 10 shows trajectories from two scenarios with a 3D view in two different view angles.Generally speaking, the sustainability of C-MIGIT III system during long and frequent GPS outages is still better than MIDG II system in SPP mode.However, the proposed algorithm improves the sustainability and accuracy of MIDG II system.Therefore, the combination of MIDG II IMU with SPP and 3D MM using proposed algorithm is considered as the reasonable choice for developing a seamless land vehicular navigation system in terms of cost for future applications.frequent GPS outages significantly with the use of C-MIGIT III and MIDG II system in SPP mode, respectively.Those oscillations shown in Figure 11(a) and 11(b) were caused by the U-turn of vehicle under the overpass because there was no GPS signal reception before and after this maneuver.The length of GPS outage during the whole test is around 25 minutes.As shown in Table 1 shows the averaged improvement ratios of RMS errors for those two scenarios reach 90% and the averaged improvement ratios in term of maximum errors reach 92%.

Table 1 Numerical summary of positional errors
The accuracy requirement of map database varies with the accuracy of the positioning module applied and the overall accuracy of proposed algorithm relies on the accuracy of map database.The map database with sub-meter level accuracy combined with an IMU/DGPS integrated system with GPS carrier phase measurements is able to provide sub-meter level positioning accuracy.The positioning accuracy extends to 1 to 3 meters with an IMU/SPP integrated system with GPS pseudorange measurements.In addition, this study propose an effective solutions to the limitation of current commercial vehicular navigation systems where they fail to distinguish whether the vehicle is moving on the elevated highway or the road under it because those systems don't have sufficient height resolution.

CONCLUSIONS
This article exploits the idea of developing a 3D MM embedded loosely coupled INS/GPS integration scheme to provide sufficient sustainability for land vehicular navigation applications in GPS denied environments with reasonable positioning accuracy with the use of low-cost MEMS IMU/GPS integrated systems.Those map derived pseudo measurements can be sent back to a conventional loosely coupled INS/GPS integration scheme.Therefore, through this modified mechanization, a continuous measurement update mode can be carried out seamlessly during navigation through map derived information without additional loading of hardware and cost of sensors.
The preliminary results indicate the proposed algorithms are able to improve the accuracy of positional components in GPS denied environments significantly with the use of SPP mode.The averaged improvement of proposed algorithms exceeds 90% in terms of positioning accuracy and stability with the use of the MIDG/SPP integrated land vehicular system.Consequently, the modified loosely coupled INS/GPS integration scheme with map derived positions can provide the most consistent navigation solutions with sufficient sustainability.The innovative design is considered critical for the development future land vehicular navigation systems with 3D map information.

lr
: the position vector (latitude, longitude, height) l v : the velocity vector (e, n, u) l b R : the transformation matrix from the IMU body to local frame as a function of attitude components l g : the gravity vector in the local level frame b il  , b ib  : the skew-symmetric matrices of the angular velocity vectors , respectively : a 3x3 matrix whose non zero elements are functions of the user's latitude and ellipsoidal height error state vector in the local level frame : the velocity error state vector in the local level frame : the attitude error state vector in the local level frame : the error in the computed gravity vector in the local level frame : the accelerometer bias in the body frame : the sensor bias of accelerometers and gyros, respectively : the gyro drift vectors in the body frame : the scale factor of accelerometers and gyros, respectively E : a 3x3 matrix whose non-zero elements are a function of the vehicle's latitude and the Earth's radii of curvatures In the EKF, the INS errors are updated by the difference between GPS and INS solutions.The EKF applied in this study has 21 state vectors (Shin and El-Sheimy, 2004) ： ( 5 ) The equations of the KF are divided into two groups of equations; prediction and update.The time prediction equations are responsible for the forward time transition of the current epoch (k-1) states to the next epoch (k) states.The prediction equations are:

Figure 2 Figure 3
Figure 2 The traditional loosely coupled INS/GPS integration scheme (closed loop)

Figure 5
Figure5The possibility area of vehicle at t-1 epoch According toEl-Sheimy (2008), a partial IMU composed by one heading gyro (Gz) plus two horizontal accelerometers (Ax and Ay) can provide basic information for navigation.Therefore, heading information should be considered as a threshold to select the road candidates which fit in.Heading of vehicle means the direction of vehicle moving from t-1 epoch to t epoch, as shown in Figure6.By comparing the heading of the vehicle and the orientation of the candidate roads in the possibility heading, the ideal ones will be calibrated, also shown in Figure6.

Figure 6
Figure 6 The possibility heading of vehicle at t epoch and the diagram of heading constraint When the orientations of the candidate roads are in the range of the heading of vehicle, the candidate road is processed in the next step.Heading constrained only apply in the condition of GPS outage where heading of vehicle is available from INS.In this article, the Nearest Feature Search (NFS) algorithm is chosen for nearest point search because of its simplicity in terms of implementation (Bullock, 1995).

Figure 7
Figure 7 The proposed 3D MM embedded loosely coupled INS/GPS integration scheme The 3D MM procedure is implemented based on the availability of GPS.In the case of GPS outages, those predicted INS positions and headings provided by the EKF are used for 3D MM purpose to derive map matched positions.When more than four GPS satellites are in sight, GPS positions are used with MM mentioned above to derive map matched positions.Therefore, a robust and seamless continuous coordinate update can be supplied to the EKF to facilitate measurement update mode.
(b)), and two test IMUs, C-MIGITS III (5 deg/hr in run gyro bias) from BEI (in car system shown in Figure 9(b)) and MIDG II from Microbotics (150 deg/hr in run gyro bias) (on the top of the vehicle shown in Figure 9(a)).

Figure 8 Figure 9
Figure 8 Traditional view in the test area

Figure 11
Figure 11 The numerical performance comparison of each scenario in three different directions Figure 10(a) and 10(b) illustrate the results of SPP with the C-MIGIT III (SC), and Figure 10(c) and 10(d) are from the SPP with MIDG II (SM), respectively.Those results illustrate the improvements produced by proposed algorithm in the two conditions, and the matching of the height can turn into better solution from comparing with the EKF only solutions, especial show in the yellow dot line region in Figure 10(b) and 10(d), respectively.The height errors of the EKF solutions are corrected by the proposed algorithm with two systems.In other words, the proposed algorithm can provide sufficient resolution and accuracy in height thus it is easier to distinguish whether the vehicle is moving on the elevated highway or the road under it.Such capability can't be found from current commercial vehicular navigation systems.

Figure 11
Figure11lustrates detailed numerical comparison between a conventional loosely coupled scheme and proposed scheme in term of positional errors in comparison with the reference trajectory.In addition, Table1summarizes the Root Mean Square (RMS) errors as well as maximums of positional errors for two scenarios.As shown in Figure11(a) and Figure 11(b), the proposed scheme reduces the positional errors during