Passive Sensor Integration for Vehicle Self-Localization in Urban Traffic Environment †

This research proposes an accurate vehicular positioning system which can achieve lane-level performance in urban canyons. Multiple passive sensors, which include Global Navigation Satellite System (GNSS) receivers, onboard cameras and inertial sensors, are integrated in the proposed system. As the main source for the localization, the GNSS technique suffers from Non-Line-Of-Sight (NLOS) propagation and multipath effects in urban canyons. This paper proposes to employ a novel GNSS positioning technique in the integration. The employed GNSS technique reduces the multipath and NLOS effects by using the 3D building map. In addition, the inertial sensor can describe the vehicle motion, but has a drift problem as time increases. This paper develops vision-based lane detection, which is firstly used for controlling the drift of the inertial sensor. Moreover, the lane keeping and changing behaviors are extracted from the lane detection function, and further reduce the lateral positioning error in the proposed localization system. We evaluate the integrated localization system in the challenging city urban scenario. The experiments demonstrate the proposed method has sub-meter accuracy with respect to mean positioning error.


Introduction
Vehicle self-localization in urban environment is a challenging but significant topic for autonomous driving and driving assistance. Both motion planning and vehicle cooperation need accurate localization. Localization methods can be categorized into two types based on the sensors used: passive sensor-based and active sensor-based. Passive sensors such as Global Navigation Satellite System (GNSS) receivers and vision sensors collect signals, radiation, or light from the environment, while the active sensors have transmitters, which can send out light waves, electrons or signals. The reflection bounced off the target will be collected by the active sensor. Velodyne, a type of Light Detection and Ranging (LIDAR) sensor is a representative active sensor for vehicle self-localization [1][2][3][4][5][6][7]. Active sensor-based localization has a weakness from a practical point of view-the price of these sensors is currently very high. Although we expect that the cost of these sensors will be reduced in the near future, they suffer from another critical problem, which is the high energy consumption. Therefore, it is necessary to consider low-cost solutions such as GNSS receivers, cameras and inertial sensors-i.e., passive sensors-as an alternative or compensation. Different with those related works, this paper focuses on the problem of vehicle self-localization in the most challenging environment, a city urban environment, and improves the positioning performance from two aspects: both GNSS positioning technology and sensor integration. The main contribution of this paper is to propose a multiple passive sensor-based localization system for the lane-level localization, which integrates the innovative 3D-GNSS positioning technique with an inertial sensor and onboard monocular camera. The flowchart of the proposed system is shown in Figure 1. In the integrated system, 3D-GNSS provides the global positioning result. The motion of vehicle is described via speedometer data from a Control Area Network bus, and heading direction from the Inertial Measurement Unit (IMU). The vision-based lane detection is firstly used for controlling the drift of the IMU. Moreover, the lane keeping and lane changing behaviors are obtained from the lane detection function. These behaviors describe the relative movement of the vehicle, which can further improve the lateral positioning error. Finally, we use a partite filter to integrate this information and the 2D lane marking map.
Sensors 2015, 15, page-page lane-level localization, which integrates the innovative 3D-GNSS positioning technique with an inertial sensor and onboard monocular camera. The flowchart of the proposed system is shown in Figure 1. In the integrated system, 3D-GNSS provides the global positioning result. The motion of vehicle is described via speedometer data from a Control Area Network bus, and heading direction from the Inertial Measurement Unit (IMU). The vision-based lane detection is firstly used for controlling the drift of the IMU. Moreover, the lane keeping and lane changing behaviors are obtained from the lane detection function. These behaviors describe the relative movement of the vehicle, which can further improve the lateral positioning error. Finally, we use a partite filter to integrate this information and the 2D lane marking map. Our previous works [19][20][21] focused on the development and evaluation of 3D-GNSS for pedestrian applications. In addition, we only combined 3D-GNSS and inertial sensors for improving the localization performance in our previous works [27,28]. This paper further improves the localization performance by additionally integrating vision-based lane detection. This paper is an extension of the work presented in our conference paper [49]. Compared to the conference work, an idea of heading direction rectification is additionally described and evaluated, and more experimental results and discussion are presented in this paper as well.
We organize the remainder of the paper as follows: the 3D-GNSS positioning method and vision-based lane detection are presented in Sections 2 and 3, respectively. The drift rectification method for an inertial sensor is described in Section 4. Section 5 explains the integration algorithm. The experimental results are demonstrated in Section 6. Finally, we will end the paper in Section 7 with conclusions and proposals of future work.

3D-GNSS
The integrated localization system considers the GNSS positioning result as the main information source. Generally speaking, GNSS positioning provides an estimation for localization, and the role of other sensors is to optimize the position around the GNSS result. A more accurate GNSS result makes it easier for the integrated system to achieve lane-level performance. Therefore, the performance of the GNSS positioning technique should be improved first. The satellite positioning techniques suffer from NLOS and multipath effects in urban canyons, as shown in Figure 2. This paper proposes to utilize 3D-GNSS to reduce both the multipath and NLOS effects. We briefly describe the idea of 3D-GNSS in this section, while the details of 3D-GNSS have been published in our previous works [19][20][21]. Figure 3a shows the flowchart of 3D-GNSS method. In order to analyze and correct the pseudorange error caused by multipath and NLOS, a 3D building map is needed. 3D-GNSS recognizes the reflected path of satellite signals using ray-tracing, and further estimates the reflection delay. Theoretically, the pseudorange simulation at the position of receiver should be the same as the pseudorange measurement. Thus, 3D-GNSS employs the candidate distribution algorithm to rectify the positioning error. Firstly, a set of position hypothesis, named candidates, are distributed around Our previous works [19][20][21] focused on the development and evaluation of 3D-GNSS for pedestrian applications. In addition, we only combined 3D-GNSS and inertial sensors for improving the localization performance in our previous works [27,28]. This paper further improves the localization performance by additionally integrating vision-based lane detection. This paper is an extension of the work presented in our conference paper [49]. Compared to the conference work, an idea of heading direction rectification is additionally described and evaluated, and more experimental results and discussion are presented in this paper as well.
We organize the remainder of the paper as follows: the 3D-GNSS positioning method and vision-based lane detection are presented in Sections 2 and 3 respectively. The drift rectification method for an inertial sensor is described in Section 4. Section 5 explains the integration algorithm. The experimental results are demonstrated in Section 6. Finally, we will end the paper in Section 7 with conclusions and proposals of future work.

3D-GNSS
The integrated localization system considers the GNSS positioning result as the main information source. Generally speaking, GNSS positioning provides an estimation for localization, and the role of other sensors is to optimize the position around the GNSS result. A more accurate GNSS result makes it easier for the integrated system to achieve lane-level performance. Therefore, the performance of the GNSS positioning technique should be improved first. The satellite positioning techniques suffer from NLOS and multipath effects in urban canyons, as shown in Figure 2. This paper proposes to utilize 3D-GNSS to reduce both the multipath and NLOS effects. We briefly describe the idea of 3D-GNSS in this section, while the details of 3D-GNSS have been published in our previous works [19][20][21].

30201
Sensors 2015, 15, 30199-30220 Figure 3a shows the flowchart of 3D-GNSS method. In order to analyze and correct the pseudorange error caused by multipath and NLOS, a 3D building map is needed. 3D-GNSS recognizes the reflected path of satellite signals using ray-tracing, and further estimates the reflection delay. Theoretically, the pseudorange simulation at the position of receiver should be the same as the pseudorange measurement. Thus, 3D-GNSS employs the candidate distribution algorithm to rectify the positioning error. Firstly, a set of position hypothesis, named candidates, are distributed around the previous positioning result and the position given by conventional GNSS receiver. Next, 3D-GNSS generates the pseudorange simulation from each candidate position using the ray tracing, as shown in Figure 3b.   [19]; (b) NLOS effect [19]. There are three types of satellite conditions. The LOS signal is not affected by the buildings, and does not include the reflection delay. In the NLOS case, as shown in Figure 2b, the reflection delay is equal to the difference between the direct path and the reflection path. However, the third condition is more ambiguous, which is the multipath effect shown in Figure 2a. This research assumes that the signal in the multipath condition is about 6 dB weaker than the LOS signal, and the strobe correlator [50] with 0.2 chip spacing is adopted in the commercial receiver based on the experience. The research uses these principles to estimate the pseudorange delay in the multiple path condition. By using ray tracing, both the satellite condition and the reflection delay can be estimated.
The 3D-GNSS technique additionally checks the consistency of the satellite conditions by considering the signal strength. If the satellite conditions estimated using the signal strength conflict   [19]; (b) NLOS effect [19].   [19]; (b) NLOS effect [19]. There are three types of satellite conditions. The LOS signal is not affected by the buildings, and does not include the reflection delay. In the NLOS case, as shown in Figure 2b, the reflection delay is equal to the difference between the direct path and the reflection path. However, the third condition is more ambiguous, which is the multipath effect shown in Figure 2a. This research assumes that the signal in the multipath condition is about 6 dB weaker than the LOS signal, and the strobe correlator [50] with 0.2 chip spacing is adopted in the commercial receiver based on the experience. The research uses these principles to estimate the pseudorange delay in the multiple path condition. By using ray tracing, both the satellite condition and the reflection delay can be estimated.
The 3D-GNSS technique additionally checks the consistency of the satellite conditions by considering the signal strength. If the satellite conditions estimated using the signal strength conflict  There are three types of satellite conditions. The LOS signal is not affected by the buildings, and does not include the reflection delay. In the NLOS case, as shown in Figure 2b, the reflection delay is equal to the difference between the direct path and the reflection path. However, the third condition is more ambiguous, which is the multipath effect shown in Figure 2a. This research assumes that the signal in the multipath condition is about 6 dB weaker than the LOS signal, and the strobe correlator [50] with 0.2 chip spacing is adopted in the commercial receiver based on the experience. The research uses these principles to estimate the pseudorange delay in the multiple path condition. By using ray tracing, both the satellite condition and the reflection delay can be estimated.
The 3D-GNSS technique additionally checks the consistency of the satellite conditions by considering the signal strength. If the satellite conditions estimated using the signal strength conflict with the result from ray tracing, then the 3D-GNSS excludes the satellite from the following process. The probability of each position hypothesis is evaluated based on the similarity between the pseudorange measurement and the pseudorange simulation. Figure 3c indicates the different likelihood values of candidates using different colors. It is easy to see that the candidates around the ground truth position have higher weighting. The black dots are the invalid candidates, whose pseudorange similarity exceeds the defined threshold in this research of 10 m.
Finally, the 3D-GNSS provides the final rectified result by weighted averaging the positions of all the valid candidates. In this paper, we adopt the multiple satellite systems in 3D-GNSS, which includes not only GPS, but also GLObal NAvigation Satellite System (GLONASS) and Quasi Zenith Satellite System (QZSS).

Lane Detection from Monocular Camera
The lane detection method used in this research is highly inspired by Aly's work [38]. However, different from the lane detection research described in [38], this research does not focus on the lane detection itself, but rather employs the vision-based lane detection to improve the accuracy of localization.

Inverse Perspective Mapping (IPM)
Before the lane detection, a top view of the road surface is generated, which is named Inverse Perspective Mapping (IPM). This has two advantages: the first one is that the perspective effect can be eliminated from the images, which makes the lanes become parallel [38]. The other one is that the road surface is represented in meters by the IPM. Thus, the prior information about the lane markings can be used easily.
In order to generate the IPM, a flat road is assumed. In addition, for conducting this transformation, camera intrinsic parameters and camera extrinsic parameters are also used. As shown in Figure 4a, an image coordinate {F i } = {u, v}, a camera coordinate {F c } = {X c , Y c , Z c } and a world coordinate {F w } = {X w , Y w , Z w } are defined, respectively. The world coordinate is centered at the optical center of the camera, and Z w is perpendicular to the road surface. The camera coordinate X c axis is assumed to stay in the world coordinate X w -Y w plane. It means that the systems has a pitch angle α and yaw angle β for the optical axis, but the roll angle is zero. In addition, focal length of camera is (f u , f v ), and optical center is (c u , c v ). h is the height of the camera coordinate above the road plane. Thus, any point in the image coordinate can be projected to the road plane by the homogeneous transformation [38]: where Equation (1) represents the transformation from the camera coordinate to the image coordinate, and Equation (2) is the transformation from the world coordinate to the camera coordinate. Therefore, any point on the ground can be projected back to the image coordinate. Figure 4b shows an 30203 Sensors 2015, 15, original image from onboard camera, and the region of interest (ROI) is marked by the red rectangle. Figure

Lane Detection
For enhancing lane marking and reducing other disturbances, the IPM image is firstly smoothed by a Gaussian filter along the vertical direction, and then filtered by a second-derivative of Gaussian for the horizontal direction. The algorithm considers 2.5% pixels as the road marking and thresholds the filtered IPM [38], as shown in Figure 5a. In addition, the lane detection is limited in a rectangular area centered at the vehicle center. The width along the lateral direction is 7 m (two-lane width), and the farthest distance along the heading direction is 30 m. Figure 5b shows the lane detection ROI. One lane consists of two line boundaries. Therefore, the lane detection problem is converted into a line detection one in the ROI. The line detection algorithm consistz of a Hough transform to locate the rough areas of lines, followed by a Random Sample Consensus (RANSAC) fitting to optimize the line detection results [38]. In fact, the two lines are represented by two local maxima in the Hough transform result, which is visualized in Figure 5c. Thus, the rough area of the first line is located by finding the global maximum in Hough transform space, as indicated by the red rectangle in Figure 5d. After that, the RANSAC line fitting is conducted in the red rectangle for detecting the first line. Figure 5e shows the result from RANSAC line fitting. In this paper, the lane width is assumed as 3.5 m. The

Lane Detection
For enhancing lane marking and reducing other disturbances, the IPM image is firstly smoothed by a Gaussian filter along the vertical direction, and then filtered by a second-derivative of Gaussian for the horizontal direction. The algorithm considers 2.5% pixels as the road marking and thresholds the filtered IPM [38], as shown in Figure 5a. In addition, the lane detection is limited in a rectangular area centered at the vehicle center. The width along the lateral direction is 7 m (two-lane width), and the farthest distance along the heading direction is 30 m. Figure 5b shows the lane detection ROI.

Lane Detection
For enhancing lane marking and reducing other disturbances, the IPM image is firstly smoothed by a Gaussian filter along the vertical direction, and then filtered by a second-derivative of Gaussian for the horizontal direction. The algorithm considers 2.5% pixels as the road marking and thresholds the filtered IPM [38], as shown in Figure 5a. In addition, the lane detection is limited in a rectangular area centered at the vehicle center. The width along the lateral direction is 7 m (two-lane width), and the farthest distance along the heading direction is 30 m. Figure 5b shows the lane detection ROI. One lane consists of two line boundaries. Therefore, the lane detection problem is converted into a line detection one in the ROI. The line detection algorithm consistz of a Hough transform to locate the rough areas of lines, followed by a Random Sample Consensus (RANSAC) fitting to optimize the line detection results [38]. In fact, the two lines are represented by two local maxima in the Hough transform result, which is visualized in Figure 5c. Thus, the rough area of the first line is located by finding the global maximum in Hough transform space, as indicated by the red rectangle in Figure 5d. After that, the RANSAC line fitting is conducted in the red rectangle for detecting the first line. Figure 5e shows the result from RANSAC line fitting. In this paper, the lane width is assumed as 3.5 m. The  One lane consists of two line boundaries. Therefore, the lane detection problem is converted into a line detection one in the ROI. The line detection algorithm consistz of a Hough transform to locate the rough areas of lines, followed by a Random Sample Consensus (RANSAC) fitting to optimize the line detection results [38]. In fact, the two lines are represented by two local maxima in the Hough transform result, which is visualized in Figure 5c. Thus, the rough area of the first line is located by finding the global maximum in Hough transform space, as indicated by the red rectangle in Figure 5d. After that, the RANSAC line fitting is conducted in the red rectangle for detecting the first line. Figure 5e shows the result from RANSAC line fitting. In this paper, the lane width is assumed as 3.5 m. The hypothesized area of the second line can be determined based on this assumption. Then the RANSAC line fitting is used for the line detection again. Figure 5f,g demonstrate the hypothesized area of the second line and the result of the RANSAC line fitting, respectively. Thus, the position of the vehicle relative to the two line boundaries can be estimated.

Lane Keeping and Lane Changing Detection
One lane is represented by two line boundaries on the road surface. After the line boundaries are detected, we employ the particle filter to track each line. The purpose of the line tracking is to detect the lane keeping and changing behaviors. Figure 6 shows the line detection and tracking results in a lane changing process from frame t to t + 2k. In Figure 6, the blue rectangle is the ROI of lane detection, the red color lines are the detected lines. The attached number, e.g., "Line-1", "Line-2" and "Line-3", indicate the tracking results. The small green rectangle at the bottom of images denotes our vehicle position. From frame t to t + k, the Line-2 becomes close to the center of the vehicle, which is indicated in the Figure 6a,b. As time increases, the Line-2 gradually appears at the left side of the vehicle, as shown in Figure 6c. In this process, the position of the tracked Line-2 changes from the right side to the left side of the vehicle, which indicates the vehicle performs right-direction lane changing.
Sensors 2015, 15, page-page hypothesized area of the second line can be determined based on this assumption. Then the RANSAC line fitting is used for the line detection again. Figure 5f,g demonstrate the hypothesized area of the second line and the result of the RANSAC line fitting, respectively. Thus, the position of the vehicle relative to the two line boundaries can be estimated.

Lane Keeping and Lane Changing Detection
One lane is represented by two line boundaries on the road surface. After the line boundaries are detected, we employ the particle filter to track each line. The purpose of the line tracking is to detect the lane keeping and changing behaviors. Figure 6 shows the line detection and tracking results in a lane changing process from frame t to t + 2k. In Figure 6, the blue rectangle is the ROI of lane detection, the red color lines are the detected lines. The attached number, e.g., "Line-1", "Line-2" and "Line-3", indicate the tracking results. The small green rectangle at the bottom of images denotes our vehicle position. From frame t to t + k, the Line-2 becomes close to the center of the vehicle, which is indicated in the Figure 6a,b. As time increases, the Line-2 gradually appears at the left side of the vehicle, as shown in Figure 6c. In this process, the position of the tracked Line-2 changes from the right side to the left side of the vehicle, which indicates the vehicle performs right-direction lane changing.     Figure 7 shows the lane keeping scenario. The green rectangle denotes our vehicle position, which is located between Line-1 and Line-2 during the driving period. In Figure 7a-c, Line-1 always appears at the left and Line-2 always appears at the right side of the vehicle. Unlike the lane changing scenario, there is no line cross from one side to the other side of the vehicle in this period. Based on the relative position of the vehicle and the tracked lines, the lane keeping can be detected. This information will be used in the following integration. Figure 7 shows the lane keeping scenario. The green rectangle denotes our vehicle position, which is located between Line-1 and Line-2 during the driving period. In Figure 7a-c, Line-1 always appears at the left and Line-2 always appears at the right side of the vehicle. Unlike the lane changing scenario, there is no line cross from one side to the other side of the vehicle in this period. Based on the relative position of the vehicle and the tracked lines, the lane keeping can be detected. This information will be used in the following integration.

Heading Direction Rectification for an Inertial Sensor
In this research, an Inertial Measurement Unit (IMU) sensor is employed to detect the heading direction of the vehicle. The IMU sensor outputs the acceleration, the angle rate and the value of angle along each axis. According to the IMU sensor specification, the pitch and roll angles are the difference from the body of the IMU sensor to the horizontal plane perpendicular to the gravity direction. In addition, the yaw angle is the accumulated angle changes since the sensor is reset or connected. Suppose that the heading direction has been calibrated relative to the true north direction, the vehicle velocity can be calculated by following equations: where, λ and ω are the roll and pitch angle of the IMU, respectively. θ is the heading direction from the true north direction. The vehicle velocity V CAN can be represented by V north´east , and V altitude in Equation (3). The speed V north´east is further decomposed by Equation (4). Because we conducted the experiments in a flat area, the simplified North-East plane is adopted for the positioning. The GNSS positioning result at first epoch is defined as the original point of the North-East plane. The movement of the vehicle can be calculated by Equation (5): where, px north,k , x east,k q denotes the vehicle position at time k. Figure 8 shows the trajectory generated from Equations (5). The trajectory is indicated by blue points and the ground truth is shown by cyan line. It is clear to see that the blue points are parallel with the ground truth at the beginning, but it is drifting with the increase of time.
, , 1 north k east k xx denotes the vehicle position at time k. Figure 8 shows the trajectory generated from Equations (5). The trajectory is indicated by blue points and the ground truth is shown by cyan line. It is clear to see that the blue points are parallel with the ground truth at the beginning, but it is drifting with the increase of time. This paper proposes to use the vision based lane detection to control the drift effect. When the lane keeping manner is detected, the vehicle heading direction is rectified as the direction of the occupied lane, which could be estimated from the 2D lane marking map. Otherwise, the heading direction is accumulated by the yaw angle rate provided by IMU sensor. This idea is formulized as follows: where, θ k is the heading direction at time k, θ lane k is the lane direction obtained from 2D lane marking map. ∆θ I MU k denotes the angle changing obtained from IMU sensor.

Integrated Localization of 3D-GNSS, Inertial Sensor and Lane Detection
In this research, multiple passive sensors are simultaneously adopted for positioning. 3D-GNSS, inertial sensor and lane detection are integrated in the particle filter. The particle filter is a nonparametric version of the Bayes filter, and has widely been applied. The particle filter represents a posterior using a set of particles , x i k is the two dimensional positioning, and i is the particle index, k is the time, n means the number of the particles. Each particle has an importance weight w i k . The vehicle position is recursively estimated by the following steps [51]:

2.
Correction: an weight of each particle in x i k , w i k ( n i"1 is evaluated with the new observation z k , according to certain observation model w i k " ppz k |x i k q. In this paper, z k includes two observation G k for 3D-GNSS positioning result and V k for lane detection result. 3.
Resampling: the particle set x i k , w i k ( n i"1 will be resampled based on the importance weight. Figure 9 demonstrates the process of the weight evaluation for one particle. The yellow point is the particle i. The distance between the GNSS positioning result and the particle i is consist of the longitudinal distance D i,k GNSS,longitudinal and the lateral distance D i,k GNSS,lateral by referring to the direction of the occupied lane. Therefore, the probability computed thanks to 3D-GNSS measurement ppG k |x i k q is represented as follows: ppG k |x i k q " ppG k,lateral |x i k q¨ppG k,longitudinal |x i k q

GNSS positioning result
Particle i

Distance to GNSS result
Distance to two lines of lane Lane direction Figure 9. Distance estimation for a particle.
This paper set the value of σ 2 GNSS as nine square meters, which is tuned empirically. Figure 10a  In addition, the distance from each particle to right and left lines´D i,k lane,right , D i,k lane,le f t¯c an be estimated from the prepared 2D lane marking map, as shown in Figure 9. The probability computed thanks to lane detection measurement ppV k |x i k q can be calculated as follows: where, probability ppV k,right |x i k q and ppV k,le f t |x i k q correspond to the right line and left line, respectively. This paper empirically sets the variance σ 2 lane as 0.25 m 2 . Figure 10b visualizes the probability ppV k |x i k q estimated from the measurement of the lane detection. When the lane keeping is detected, the particles outside the occupied lane of the previous result, will be excluded in the calculation, and visualized as black dots in Figure 10b. The particles around the lane center have higher probability, because the vehicle runs along the lane center in experiments. It is important to note that the lane detection can sense the lateral position. It cannot perceive the position difference along the longitudinal direction. Therefore, we propose to integrate ppV k |x i k q into ppG k,lateral |x i k q. Thus, the integrated probability is represented as: where, γ is the importance weight of the lane detection measurement, which is set as 0.5 empirically. Figure 10c visualizes the integrated probability of all particles by different colors. Comparing to Figure 10a, the high weighting particles are not around of GNSS positioning result, but appear in the correct lane. ppV k |x i k q in Equation (13) leads to this improvement. In addition, when the system detects lane changing, the operation for the particle exclusion follows the lane changing direction. Figure 10d demonstrates the valid particles corresponding to the lane detection measurement in the the lane changing case. 10 along the longitudinal direction. Therefore, we propose to integrate  (13) where,  is the importance weight of the lane detection measurement, which is set as 0.5 empirically.

Experiments
We chose the Hitotsubashi area in Tokyo for experiments due to the tall building density. Figure 11 shows the developed 3D map for 3D-GNSS and 2D map for integration. We used two kinds of data to construct the 3D map. The first one is the 2-dimensional building footprint, which is provided by Japan Geospatial Information Authority. The other one is the Digital Surface Model (DSM) data, obtained from Aero Asahi Corporation. The DSM data includes the height information of the building [20]. The 2D map is generated from high resolution aerial images provided by NTT-geospace.

Experiments
We chose the Hitotsubashi area in Tokyo for experiments due to the tall building density. Figure 11 shows the developed 3D map for 3D-GNSS and 2D map for integration. We used two kinds of data to construct the 3D map. The first one is the 2-dimensional building footprint, which is provided by Japan Geospatial Information Authority. The other one is the Digital Surface Model (DSM) data, obtained from Aero Asahi Corporation. The DSM data includes the height information of the building [20]. The 2D map is generated from high resolution aerial images provided by NTT-geospace. In the experiment, a u-blox EVK-M8 GNSS model, a commercial level receiver was adopted. It is produced by u-blox Company (Thalwil, Switzerland), and can receive signals from multiple-GNSS (GPS, GLONASS, and QZSS). We placed the u-blox receiver on the top of our vehicle to collect pseudorange measurements. In addition, an IMU sensor (AMU-3002A Lite, Silicon Sensing, Amagasaki-shi, Hyogo Prefecture, Japan) and speedometer recorder were used to measure the angle attitude and the vehicle velocity, respectively. Moreover, an onboard camera was installed in the vehicle, which captured the front view images when driving. These images are the input of the lane detection algorithm. In addition, we manually distinguished the ground truth trajectory of our vehicle from these images, and manually decided the occupied lane for the result evaluation. The driving distance is approximate 1500 m in each test.
In the vehicle self-localization, it is more important to distinguish which lane the vehicle is in  In the experiment, a u-blox EVK-M8 GNSS model, a commercial level receiver was adopted. It is produced by u-blox Company (Thalwil, Switzerland), and can receive signals from multiple-GNSS (GPS, GLONASS, and QZSS). We placed the u-blox receiver on the top of our vehicle to collect pseudorange measurements. In addition, an IMU sensor (AMU-3002A Lite, Silicon Sensing, Amagasaki-shi, Hyogo Prefecture, Japan) and speedometer recorder were used to measure the angle attitude and the vehicle velocity, respectively. Moreover, an onboard camera was installed in the vehicle, which captured the front view images when driving. These images are the input of the lane detection algorithm. In addition, we manually distinguished the ground truth trajectory of our vehicle from these images, and manually decided the occupied lane for the result evaluation. The driving distance is approximate 1500 m in each test.
In the vehicle self-localization, it is more important to distinguish which lane the vehicle is in compared to the positioning accuracy. Therefore, both the lateral error and the correct lane rate are employed to estimate the performance of the localization system. Figure 12 shows the definition of the lateral error and the heading direction error. The lateral error Error P k is the minimum distance from P k to the ground truth. The heading direction error Error θ k is the direction difference from the estimated trajectory to the ground truth trajectory. The heading direction error is used for the evaluation of the heading direction rectification.

11
In the experiment, a u-blox EVK-M8 GNSS model, a commercial level receiver was adopted. It is produced by u-blox Company (Thalwil, Switzerland), and can receive signals from multiple-GNSS (GPS, GLONASS, and QZSS). We placed the u-blox receiver on the top of our vehicle to collect pseudorange measurements. In addition, an IMU sensor (AMU-3002A Lite, Silicon Sensing, Amagasaki-shi, Hyogo Prefecture, Japan) and speedometer recorder were used to measure the angle attitude and the vehicle velocity, respectively. Moreover, an onboard camera was installed in the vehicle, which captured the front view images when driving. These images are the input of the lane detection algorithm. In addition, we manually distinguished the ground truth trajectory of our vehicle from these images, and manually decided the occupied lane for the result evaluation. The driving distance is approximate 1500 m in each test.
In the vehicle self-localization, it is more important to distinguish which lane the vehicle is in compared to the positioning accuracy. Therefore, both the lateral error and the correct lane rate are employed to estimate the performance of the localization system. Figure 12 shows the definition of the lateral error and the heading direction error. The lateral error P k Error is the minimum distance from k P to the ground truth. The heading direction error k Error  is the direction difference from the estimated trajectory to the ground truth trajectory. The heading direction error is used for the evaluation of the heading direction rectification.

Evaluation for Lane Detection
This section evaluates the lane detection by comparing the estimated lane with the hand-labeled ground truth. The cases of correction detection, partial non-detection, completed non-detection and false detection are shown in Figures 13-16, respectively.

Evaluation for Lane Detection
This section evaluates the lane detection by comparing the estimated lane with the hand-labeled ground truth. The cases of correction detection, partial non-detection, completed non-detection and false detection are shown in Figures 13-16 respectively.            As shown in Figure 13, the developed lane detection can locate the lane boundary in different scenarios. The first row of Figure 13 shows a lane changing case, the crossed line is detected accurately. The second and the third rows demonstrate that there are many other vehicles around our vehicle in the experiments. Because the occupied lane is visible, the lane detection still works for finding the occupied lane. However, this kind of urban environment is challenging for detecting other lanes, because of the occlusions. Figure 14 shows two examples of partial non-detection. In the two examples, the right lane boundaries are not detected and the left boundaries are correctly detected. The reasons are the occlusion of other vehicles in the first example, and the degradation of the lane markings in the second example. This case is not serious for the localization system. In the case of partial non-detection, the detected lane boundaries can still be used for integration. In the quantitative evaluation, the cases of Figures 13 and 14 are considered as correct detections. As shown in Figure 13, the developed lane detection can locate the lane boundary in different scenarios. The first row of Figure 13 shows a lane changing case, the crossed line is detected accurately. The second and the third rows demonstrate that there are many other vehicles around our vehicle in the experiments. Because the occupied lane is visible, the lane detection still works for finding the occupied lane. However, this kind of urban environment is challenging for detecting other lanes, because of the occlusions. Figure 14 shows two examples of partial non-detection. In the two examples, the right lane boundaries are not detected and the left boundaries are correctly detected. The reasons are the occlusion of other vehicles in the first example, and the degradation of the lane markings in the second example. This case is not serious for the localization system. In the case of partial non-detection, the detected lane boundaries can still be used for integration. In the quantitative evaluation, the cases of Figures 13 and 14 are considered as correct detections.
The complete non-detection means both left and right boundaries cannot be detected as shown in Figure 15. In the first example, the reason for the non-detection is the unclear lane marking on the road surface. In the second example, the lost detection of the left boundary is because the detection algorithm excludes the line, which does not cross the bottom of the ROI. The reason for the right boundary is that there are so few pixels of the line in the ROI. In the case of the completed non-detection, the localization system uses the GNSS positioning result for integration because of the lack of lane detection results. In addition, Figure 16 shows an example of false detection. A part of the right lane boundary is occluded by the front vehicle. The body of the vehicle presents a line feature, which causes the false detection.
The quantitative evaluation of the lane detection is illustrated in Table 1. The total number of frames including lanes is 5920. The developed lane detection algorithm can detect lanes in 95.3% of the frames. About 4.7% of the frames are complete non-detection ones, where the non-detection happens around curved road areas and on roads including unclear lane markings, such as Figure 15. In the detections, 99.6% of the frames are correct, such as the cases of Figures 13 and 14. Only 0.4% are false detections, such as the case of Figure 16. Table 1 indicates that the lane detection displays high reliability.

Evaluation for Heading Direction Rectification
As presented in Section 4, the IMU sensor suffers from the drift problem. The drift effect will generate errors in the propagation of particles. This paper proposes to rectify the heading direction, when lane keeping happens. Two types of heading direction errors in one experiment are plotted in Figure 17. The green line indicates the error of the IMU sensor, and the yellow line shows the error after the proposed rectification. The positive value means the difference in the counterclockwise direction. It is clear to see that the heading direction of the IMU sensor has about 5˝drift by the end of the experiment. The error of the rectified heading direction changes around zero. This result proves that the vision-based lane detection can solve the drift problem. Actually, the experiment demonstrated in Figure 17 corresponds to the one discussed in Figure 8. In order to make the effectiveness of heading rectification more comprehensive, we plot the inertial trajectory using the rectified heading direction in Figure 18. After the rectification, the inertial trajectory is parallel to the ground truth trajectory. In Figure 17, the areas denoted as 1 to 6 are the turning areas, which are also indicated in Figure 18. The large error is caused by unconfident ground truth trajectory, because it is difficult to manually describe the accurate curve of the turning trajectory. demonstrated in Figure 17 corresponds to the one discussed in Figure 8. In order to make the effectiveness of heading rectification more comprehensive, we plot the inertial trajectory using the rectified heading direction in Figure 18. After the rectification, the inertial trajectory is parallel to the ground truth trajectory. In Figure 17, the areas denoted as 1 to 6 are the turning areas, which are also indicated in Figure 18. The large error is caused by unconfident ground truth trajectory, because it is difficult to manually describe the accurate curve of the turning trajectory.

Evaluation for Localization
In this section, we focus on the evaluation of the positioning performance. In order to understand the benefit of the proposed 3D-GNSS in urban canyon environments, this paper compares the Weighted Least Square (WLS) GNSS-based integration and the proposed 3D-GNSS-based integration. Figure 19a shows the positioning results of WLS-GNSS and WLS-GNSS-based integration using purple dots and green dots, respectively. The 3D-GNSS and 3D-GNSS-based integration in the same test are shown in Figure 19b, using red dots and yellow dots. The ground truth is represented by the cyan line. In Figure 19a, the purple dots corresponding to the WLS-GNSS result are randomly spread over a wide area. On the contrary, the 3D-GNSS is much more accurate compared to the WLS-GNSS. Moreover, the 3D-GNSS-based integration also indicates better performance than WLS-GNSS-based integration, which can be understood by comparing the green dots and yellow dots in Figure 19c. Except for the GNSS positioning method, the two integration systems use the same algorithm. This proves that the more accurate the GNSS method is, the higher performance the integrated system has. 14 demonstrated in Figure 17 corresponds to the one discussed in Figure 8. In order to make the effectiveness of heading rectification more comprehensive, we plot the inertial trajectory using the rectified heading direction in Figure 18. After the rectification, the inertial trajectory is parallel to the ground truth trajectory. In Figure 17, the areas denoted as 1 to 6 are the turning areas, which are also indicated in Figure 18. The large error is caused by unconfident ground truth trajectory, because it is difficult to manually describe the accurate curve of the turning trajectory.

Evaluation for Localization
In this section, we focus on the evaluation of the positioning performance. In order to understand the benefit of the proposed 3D-GNSS in urban canyon environments, this paper compares the Weighted Least Square (WLS) GNSS-based integration and the proposed 3D-GNSS-based integration. Figure 19a shows the positioning results of WLS-GNSS and WLS-GNSS-based integration using purple dots and green dots, respectively. The 3D-GNSS and 3D-GNSS-based integration in the same test are shown in Figure 19b, using red dots and yellow dots. The ground truth is represented by the cyan line. In Figure 19a, the purple dots corresponding to the WLS-GNSS result are randomly spread over a wide area. On the contrary, the 3D-GNSS is much more accurate compared to the WLS-GNSS. Moreover, the 3D-GNSS-based integration also indicates better performance than WLS-GNSS-based integration, which can be understood by comparing the green dots and yellow dots in Figure 19c. Except for the GNSS positioning method, the two integration systems use the same algorithm. This proves that the more accurate the GNSS method is, the higher performance the integrated system has.

Evaluation for Localization
In this section, we focus on the evaluation of the positioning performance. In order to understand the benefit of the proposed 3D-GNSS in urban canyon environments, this paper compares the Weighted Least Square (WLS) GNSS-based integration and the proposed 3D-GNSS-based integration. Figure 19a shows the positioning results of WLS-GNSS and WLS-GNSS-based integration using purple dots and green dots, respectively. The 3D-GNSS and 3D-GNSS-based integration in the same test are shown in Figure 19b, using red dots and yellow dots. The ground truth is represented by the cyan line. In Figure 19a, the purple dots corresponding to the WLS-GNSS result are randomly spread over a wide area. On the contrary, the 3D-GNSS is much more accurate compared to the WLS-GNSS. Moreover, the 3D-GNSS-based integration also indicates better performance than WLS-GNSS-based integration, which can be understood by comparing the green dots and yellow dots in Figure 19c. Except for the GNSS positioning method, the two integration systems use the same algorithm. This proves that the more accurate the GNSS method is, the higher performance the integrated system has. To explain the reason for the improvement in the 3D-GNSS method, the satellite conditions in this experiment are illustrated in Figure 20. In the experiment, the GNSS receiver can receive the signals from nine satellites on average. About five of the satellites are in LOS condition, and other satellites are NLOS. In the conventional WLS, the NLOS signal is used directly. Thus, the pseudorange error of the NLOS signal will lead to the positioning error. In the proposed 3D-GNSS positioning method, the 3D map information is adopted for distinguishing NLOS and multipath effects, and correcting the pseudorange errors. Therefore, the 3D-GNSS achieves better performance. It also provides an accurate basis for the integrated localization system.  To explain the reason for the improvement in the 3D-GNSS method, the satellite conditions in this experiment are illustrated in Figure 20. In the experiment, the GNSS receiver can receive the signals from nine satellites on average. About five of the satellites are in LOS condition, and other satellites are NLOS. In the conventional WLS, the NLOS signal is used directly. Thus, the pseudorange error of the NLOS signal will lead to the positioning error. In the proposed 3D-GNSS positioning method, the 3D map information is adopted for distinguishing NLOS and multipath effects, and correcting the pseudorange errors. Therefore, the 3D-GNSS achieves better performance. It also provides an accurate basis for the integrated localization system. We repeat three tests along the driving route. Table 2 shows the quantitative comparison based on the multiple tests. The comparison includes "WLS-GNSS", "WLS-GNSS&IMU&Speedometer integration", "WLS-GNSS&IMU&Speedometer&Lane detection integration", "3D-GNSS", "3D-GNSS&IMU&Speedometer integration", and "3D-GNSS&IMU&Speedometer&Lane detection integration". WLS-GNSS&IMU&Speedometer integration and 3D-GNSS&IMU&Speedometer integration exclude the lane detection function. As demonstrated in Table 2, the 3D-GNSS method also shows much better performance compared to WLS. About 63.2% of results are in the correct lane. Although 3D-GNSS cannot be directly used for vehicle self-localization, it has potential as the main source in the integration. After integrating with IMU and speedometer, the correct lane rate is increased and the mean error is improved to 1.2 m. Table 2 indicates the integration of GNSS, IMU, speedometer and lane detection has 93% correct lane rate and sub-meter accuracy. To demonstrate the performance of the 3D-GNSS based integration, Figure 21 shows the positioning error of the three types of localization methods in the experiment that has been discussed in Figure 19. Obviously, the 3D-GNSS&IMU&Speedometer&lane detection integration has the best performance. Most of the time, the positioning error is maintained under 1.5 m, but there are some areas where the positioning error is larger than 1.5 m. Those areas are indicated in Figures 21 and 19b, correspondingly. The areas 1, 2, 3, 5, 6 and 7 in Figure 21 denote the intersections. In the intersections, the lane detection function does not work because of the lack of lane markings. Therefore, the positioning error cannot be reduced compared to 3D-GNSS&IMU&Speedometer integration. This result indicates that it is necessary to develop other road marking recognition functions for intersection areas, which is expected to improve the positioning error in those intersection areas.
WLS-GNSS&IMU&Speedometer integration and 3D-GNSS&IMU&Speedometer integration exclude the lane detection function. As demonstrated in Table 2, the 3D-GNSS method also shows much better performance compared to WLS. About 63.2% of results are in the correct lane. Although 3D-GNSS cannot be directly used for vehicle self-localization, it has potential as the main source in the integration. After integrating with IMU and speedometer, the correct lane rate is increased and the mean error is improved to 1.2 m. Table 2 indicates the integration of GNSS, IMU, speedometer and lane detection has 93% correct lane rate and sub-meter accuracy. To demonstrate the performance of the 3D-GNSS based integration, Figure 21 shows the positioning error of the three types of localization methods in the experiment that has been discussed in Figure 19. Obviously, the 3D-GNSS&IMU&Speedometer&lane detection integration has the best performance. Most of the time, the positioning error is maintained under 1.5 m, but there are some areas where the positioning error is larger than 1.5 m. Those areas are indicated in Figures 21  and 19b, correspondingly. The areas 1, 2, 3, 5, 6 and 7 in Figure 21 denote the intersections. In the intersections, the lane detection function does not work because of the lack of lane markings. Therefore, the positioning error cannot be reduced compared to 3D-GNSS&IMU&Speedometer integration. This result indicates that it is necessary to develop other road marking recognition functions for intersection areas, which is expected to improve the positioning error in those intersection areas.
Area 4 is in the road link. This area is enlarged and shown in Figure 22.     Figure 10. The lane detection reduces the lateral positioning error by correctly adjusting the probability of particles, which is the reason for the improvements indicated in Table 2 and Figure 21.     Figure 10. The lane detection reduces the lateral positioning error by correctly adjusting the probability of particles, which is the reason for the improvements indicated in Table 2 and Figure 21.   Figure 10. The lane detection reduces the lateral positioning error by correctly adjusting the probability of particles, which is the reason for the improvements indicated in Table 2 Figure 10. The lane detection reduces the lateral positioning error by correctly adjusting the probability of particles, which is the reason for the improvements indicated in Table 2 and Figure 21.

Conclusions
This research extended our previous 3D-GNSS work for vehicle positioning in city environments. As indicated in the experimental results, the innovative 3D-GNSS positioning technique achieved 1.5 m mean positioning error and a 63% lane recognition rate by reducing the multipath and NLOS effects. However, it is still difficult to satisfy the sub-meter requirement by only using 3D-GNSS. This paper proposed the integration of multiple on-board sensors, 3D-GNSS, inertial sensors and an on-board monocular camera for improving the accuracy of vehicle positioning. In the integration system, a lane detection algorithm is developed, in order to recognize the lane keeping/changing behavior. Finally, the particle filter integrates the three sources, 3D-GNSS, vehicle motion and lane detection for localization. In the integration, the drift problem of the inertial sensor is effectively controlled by using lane detection. Moreover, the lane detection function provides an additional measurement to optimize the positioning result along lateral direction. The experiments conducted in the urban traffic environment have demonstrated that the proposed system achieved a 93% correct lane recognition rate and sub-meter accuracy. In the near future, the sensing technology for the intersection scenario will be considered to improve the localization accuracy. In addition, more difficult scenarios will be discussed, such as under bridgea or in tunnels, where the GNSS signal is in outage conditions.