Kinematic and Dynamic Vehicle Model-Assisted Global Positioning Method for Autonomous Vehicles with Low-Cost GPS/Camera/In-Vehicle Sensors

Real-time, precise and low-cost vehicular positioning systems associated with global continuous coordinates are needed for path planning and motion control in autonomous vehicles. However, existing positioning systems do not perform well in urban canyons, tunnels and indoor parking lots. To address this issue, this paper proposes a multi-sensor positioning system that combines a global positioning system (GPS), a camera and in-vehicle sensors assisted by kinematic and dynamic vehicle models. First, the system eliminates image blurring and removes false feature correspondences to ensure the local accuracy and stability of the visual simultaneous localisation and mapping (SLAM) algorithm. Next, the global GPS coordinates are transferred to a local coordinate system that is consistent with the visual SLAM process, and the GPS and visual SLAM tracks are calibrated with the improved weighted iterative closest point and least absolute deviation methods. Finally, an inverse coordinate system conversion is conducted to obtain the position in the global coordinate system. To improve the positioning accuracy, information from the in-vehicle sensors is fused with the interacting multiple-model extended Kalman filter based on kinematic and dynamic vehicle models. The developed algorithm was verified via intensive simulations and evaluated through experiments using KITTI benchmarks (A project of Karlsruhe Institute of Technology and Toyota Technological Institute at Chicago) and data captured using our autonomous vehicle platform. The results show that the proposed positioning system improves the accuracy and reliability of positioning in environments in which the Global Navigation Satellite System is not available. The developed system is suitable for the positioning and navigation of autonomous vehicles.


Introduction
Continuous, accurate and high-integrity positioning information is critical for the operation of autonomous vehicles, including path planning and motion control functions. Several concerns and challenges remain regarding the positioning and navigation systems of autonomous vehicles, including the following: (1) Perception in extreme weather: It is difficult for visual or light detection and ranging (LiDAR) sensors to identify lane lines when the road is covered with water or snow. Furthermore, in open areas, a sufficient number of road markers cannot be detected. A high-precision Global Navigation Satellite System (GNSS) is not affected by these factors and can thus provide improved perception to some extent. information. In this paper, two novel algorithms, image singular value decomposition (ISVD) and statistic filter of feature space displacement (SFFSD), were proposed to ensure the robustness and accuracy of the V-SLAM procedure in the local environment. Next, V-SLAM and a GPS were calibrated with regression analysis and error correction, and the calibration result was used to update the vehicle model-based positioning information. To account for various driving conditions, a kinematic vehicle model and a dynamic vehicle model were considered to refine the calibration result. Finally, we assessed the performance of the proposed algorithm from a practical perspective using simulations, KITTI benchmarks and experiments.
This paper is organised as follows. Section 2 provides a brief overview of related work. Section 3 discusses the problem framework and proposed positioning strategies, including the vehicle model set, V-SLAM and its calibration with a GPS and the interacting multiple model filter. Section 4 details the experiments and evaluations, and Section 5 presents the conclusions.

Related Work
Several positioning methods have been developed over the last five decades. Some of these methods are summarised below.

Global Navigation Satellite System (GNSS) Localisation
GNSS refers to all satellite navigation systems, including global and regional systems (e.g., the American GPS, the Russian Glonass system, the European Galileo system and the Chinese Beidou satellite navigation system) and related enhancement systems (e.g., the American Wide-Area Augmentation System, the European Geostationary Navigation Overlay System and the Japanese Multi-functional Transport Satellite Augmentation System) [9]. The GPS is currently the most common GNSS system due to its low cost, wide applicability and weather resistance. However, the GPS has several limitations. First, the satellite signals may be blocked in tunnels, mountain roads and streets surrounded by tall buildings, preventing the GPS receiver from receiving satellite signals. Second, satellite-based localisation performance is affected by several factors, including satellite clock drift, transmission through the atmosphere and multipath propagation. The typical localisation accuracy of the GPS is between 10 and 20 m, significantly larger than the width of a typical traffic lane. To reduce the error caused by clock drift and atmospheric transmission, a ground-based differential GPS (DGPS) was proposed. DGPS can significantly reduce the position estimation error, improving the localisation accuracy to several metres. However, the error caused by multipath propagation cannot be mitigated, and the accuracy and reliability of localisation remain insufficient for applications, such as collision warnings, platooning and automatic parking.

DR Localisation
DR is a classic localisation technique that is independent of the GNSS. For an object moving in two-dimensional space, if its initial position and all displacements at previous time points are known, the current position of the object can be calculated from the initial position and the cumulative displacement obtained by inertial sensors (e.g., odometers, gyroscopes, accelerometers and electronic compasses). There are two requirements for the implementation of DR localisation: (1) the initial position of the moving target must be known and (2) the distance and direction of the moving target at all moments must be obtained [10]. DR systems have several advantages, including high autonomy, high security, good resistance to radio interference and weather resistance. Furthermore, DR systems use only stand-alone inertial measurement components to calculate the position, speed and other navigation parameters. However, since DR localisation is an accumulative process, each estimated position of the target depends on the localisation of previous movements. Thus, the measurement and calculation errors accumulate over time, leading to continuous deterioration in accuracy. As a result, DR localisation is unsuitable for long-term operation. In addition, initial alignment in a DR system requires time, especially for position measurements [11].

Map Matching Localisation
Map matching is a software-based localisation correction method. By associating GPS localisation with road information in a digital map database, the vehicle position relative to the map can be determined [12]. Map matching applications are based on two assumptions: (1) all vehicles travel only on roads and (2) the accuracy of the digital map is higher than that of the estimated position of the vehicle on the road. When these conditions are met, the localisation trajectory is compared with the road information via an appropriate matching process to determine the road sections that the vehicle is most likely to travel along with the vehicle's most likely position within these road sections. The performance of a map-matching algorithm depends strongly on the resolution of the digital map [13]. The digital map must have the correct network topology and high accuracy; otherwise, false matches will occur [14].

Mobile Radio-Based Localisation
Mobile radio-based localisation is a process of locating an object using a land-based wireless infrastructure. Generally, this process involves measuring the transmission parameters of radio waves (e.g., the difference in wave arrival time or phase or the variation in amplitude or frequency) travelling from known stationary objects to a moving target. Based on these parameters, the difference in distance between the known stationary objects and the target object, along with the moving direction of the target object, can be determined [15]. A common application of mobile radio localisation is the localisation of the mobile phone user by the American 911 telephone system. Other mobile radio-based localisation methods including localisation based on ultra-wideband and WiFi signals and cooperative localisation based on a Vehicular Ad-Hoc Network (VANET) [16][17][18]. However, mobile radio-based localisation requires an infrastructure with many roadside base stations, resulting in a high cost. Some researchers have incorporated SLAM into mobile radio-based localisation applications [19].

Vision/LiDAR-Based Localisation
Vision/LiDAR-based positioning has attracted considerable attention in recent years. Vision-based positioning has been widely explored because of its low cost and immunity to electromagnetic interference [20][21][22]. However, the resulting images are affected by changing light and image blurring (images captured during vehicle vibration or sharp turns), which break the projection relationship between the scene points and the image pixels. Most existing visual positioning methods suffer from a large cumulative error and non-real-time performance. Many studies [23][24][25] have reported the advantages of LiDAR-based positioning, including robustness in bad weather (variable light, rain, fog and snow). However, its cost and time-consuming nature restrict the large-scale application of LiDAR-based positioning. Global or local optimisation and loop-closure detection are tricky problems that need to be solved.

Multiple Sensor-Based Localisation
Approaches combining multiple sensor types are becoming the norm in practical positioning and navigation systems. Common multi-sensor systems combine a GPS and inertial navigation systems in loose-coupled or tightly coupled architectures [26]. However, professional and high-grade devices come with a high overall cost. In Jo et al. [8], a rule-based filter update method with a validation gate was adopted. The system did not update the GPS measurement when the number of satellites in use was four or less. That study completely ignored the fact that a positioning assessment based only on the number of satellites is not reliable, and the positioning result was affected by the GPS signal reflection and the multipath effect. Al Hage et al. [27] designed a tightly coupled GPS/odometer with faults diagnosis to mitigate GPS errors. Model-aided positioning was first applied in spaceflight position estimation, and its performance was evaluated. Some studies [5,8] have combined different in-vehicle sensors to improve the accuracy of the positioning system; however, most of these were designed for manned vehicles rather than driverless vehicles. In manned vehicles, metre-or sub-metre-level error, which results in occasional discontinuity or drift, is acceptable. In contrast, these situations are not acceptable in autonomous vehicles as they will seriously affect reliability and safety.

Proposed Fusion Positioning Strategy
In this section, we detail the proposed positioning strategy. Figure 1 shows the system structure. The vehicle models, which include kinematic and dynamic vehicle models, are explained first. Next, the V-SLAM algorithm and its calibration using a GPS track algorithm are presented. The, the calibration result used to update measurements are given. Finally, the in-vehicle sensors and vehicle models and how they were employed to refine the calibration result are discussed.

Vehicle Modelling
The incorporation of kinematic and dynamic vehicle models in the positioning method can compensate for a GPS signal loss. The kinematic vehicle model is suitable for low-speed and low-slip driving conditions, while the dynamic vehicle model is suitable for high-speed and large-slip motion. Herein, a vehicle motion model that combines the kinematic vehicle model and the dynamic vehicle model is established to account for numerous driving conditions.

Kinematic Vehicle Model
Although a rigid dynamic vehicle model is optimal for practical situations, the many degrees of freedom make the model complex, and the multi-parameter coupling is unfavourable for use in integrated navigation systems. According to the structure and characteristics of a front-wheel-steering vehicle, a four-wheeled vehicle can be simplified into a two-wheeled bicycle model [28]. As shown in Figure 2, in the bicycle model, the front and rear wheels are expressed by single wheels. In Figure 2, G is the vehicle's centre of gravity; O is the centre of vehicle current motion; V G is the velocity of G; δ is the front wheel deflection; β is the corresponding slip angle of G; ψ and ψ + β are the heading and course angle of the vehicle, respectively; and l f and l r are the distance from G to the front (point F) and rear (point G) wheel, respectively; r F , r G , and r R are the radius from the motion centre O to the front wheel centre F, the gravity centre G and the rear centre R, respectively. The kinematic vehicle model assumes that no slip occurs between the ground and the wheels, which is accurate for vehicles moving at low speeds. In this case, the velocity directions at points F and R (V F and V R , respectively) are consistent with the directions of the front and rear wheels, respectively. The kinematic relationship described above can be described using Equations (1)- (5): . . .

Dynamic Vehicle Model
Considering vehicle slip and lateral velocity caused by changes in road conditions, vehicle load, wind speed/direction, etc., a dynamic vehicle is introduced to account for cases of high-speed motion in which the vehicle slides laterally but does not completely roll [29,30]. As shown in Figure 3, the tyre-slip angle is the angle between the wheel speed vector and the longitudinal wheel axis. Here, we assume that the tyre-slip angle is proportional to the lateral force acting on the tyre. Equations (6) and (7) were derived according to the force and motion state of G: where m is the mass of the vehicle; r is the velocity of heading angular ψ; I z is the inertial yaw moment; and V x and V y are the longitudinal and lateral velocities of the vehicle, respectively. As shown in Figure 3, the dynamic vehicle model assumes that the tyre-slip angle is proportional to the lateral force that acts on the tyre: where α f and α r are the slip angles relative to the front and rear wheels, respectively; and C f and C r are the cornering stiffnesses of the front and rear tyres, respectively. By combining Equations (6) and (7) with Equations (8) and (9), the vehicle motion can be estimated by the dynamic vehicle model as follows: . . . . .

V-SLAM Algorithm
Herein, two novel algorithms are introduced into V-SLAM: ISVD, an anti-blurring algorithm, and SFFSD, an algorithm to remove feature outliers [31]. ISVD ensures that the system chooses the less-blurred images. SFFSD removes false matches from the initial putative feature correspondences. In this paper, these two algorithms are employed to enhance the performance (e.g., robustness, efficiency and accuracy) of the V-SLAM module.

ISVD Algorithm
The ISVD algorithm was designed to mitigate the effect of image blurring. ISVD is based on the principal component analysis algorithm, which selects the top k eigenvectors with the highest eigenvalues to represent an image matrix. The quality of an image can be judged by the image singular value.
For an image matrix I ∈ R m×n , there exists orthogonal matrixes U ∈ R m×m and V ∈ R n×n that compose I = UWV T , where W = diag(δ 1 , δ 2 , . . . , δ n ). δ 1 ≥ δ 2 ≥ . . . ≥ δ n are the non-zero singular values. After the decomposition of the image matrix, the singular value histogram can be established with the non-zero singular values. The definition of d ISVD is given by Equation (16): where δ i is the singular value of W, C thres is a singular value threshold, which was set to 100 in this study.
To verify the idea proposed above, Figure 4a-e with increasing degrees of blurring were obtained from the LIVE2 database [32] and are shown in Figure 4. According to ISVD, the image singular value curves of images a-e are depicted in Figure 5, in which the threshold value C thres is marked with a red line.
The calculation of the degree of image blurring using ISVD is summarised in Algorithm 1. The original image matrix is too large to decompose; to speed up this procedure, the anti-blurring process was applied only to key frames, which is more efficient that the corresponding frame-by-frame process. Input: RGB image I Output: Blurred degree d ISVD of image I Initialisation: d ISVD = 0, num = 0 1: Convert colour I to grayscale I gray 2: Calculate a singular value W = diag(δ 1 , δ 2 , . . . , δ n ) with singular value decomposition on I gray 3: For δ i in W 4: If δ i ≥ C thres 5: num + + 6: End 7: End 8: Return blurred degree d ISVD = num/n

SFFSD Algorithm
In Badino et al. [34], the tracking error in local invariant features between adjacent images was found to follow a Laplacian distribution. Similarly, we found that the feature-matching error of successive image frames (I l,i−1 , I l,i ) or (I r,i−1 , I r,i ) along with the feature-tracking error of the left and right matched image frames (I l,i−1 , I r,i−1 ) or (I l,i , I r,i ) in the stereo model, conform to a Laplacian distribution. Verified with the experiments, the pixel shift of matched features, including inliers and outliers, along the x-axis or y-axis was fitted with a Laplacian distribution, which had a long tail, as shown in Figure 6. In contrast, the pixel shift of matched features considering only inliers followed a Laplacian pattern with a short tail. In Figure 7, the line fitting was conducted only with inliers; the fitted line had a short tail and a peak bin. In these two figures, the x-axis indicates the displacement in pixels, and the y-axis is the number of features collected by histogram statistics.  Based on the above findings, we propose a SFFSD to remove the outliers and retain the inliers. Algorithm 2 briefly describes the SFFSD. Here, we present only the statistic filtering between two candidates matched images I 1 and I 2 . In our experiments, the bin number n was set to 5. In this case, the corresponding features included 80% inliers. However, the value of n was allowed to vary; it depended on the diversity of the surrounding environment. To further refine the matches after statistic filtering, circle matching was employed to verify the matches [35]. Output: Good feature matches VM good 1: Detect features I 1 , I 2 to obtain descriptors M des1 , M des2 and key points M keys1 , M keys2 2: Match M des1 , M des2 to obtain the original matches VM matches with brute force matcher and hamming distance 3: Calculate the key-points displacements for VM matches in x and y components ∆u = u 1 − u 2 , ∆v = v 1 − v 2 4: Create a 2D histogram with ∆u and ∆v to confirm the highest bins for mode approximation 5: Use the sample within the radius to perform parameter estimation of the Laplacian distribution in x and y 6: Determine the min and max boundary values to include a certain percentage ratio = 0.9 of inliers, assuming a Laplacian distribution 7: Find the matches VM u_mats according to the boundary in Step 6 8: Repeat Steps 6 and 7 to find the matches VM v_mats 9: Calculate the common element V com from VM u_matches and VM v_matches 10: For id in VM matches 11: If id in V com 12: Pushback the corresponding element into VM good 13: End 14: End

V-SLAM Track and GPS Track Calibration
In this part, the V-SLAM track p i is used to the correct GPS trajectory q i via regression analysis. This process included three main steps: (1) coordinate system conversion, (2) time alignment and improved iterative closest point (ICP) calibration, and (3) inverse coordinate system conversion.
In common applications, the GPS value is defined in the geocentric coordinate system. To perform regression analysis, the GPS value needs to be converted to the local spatial coordinate system. In this paper, we converted from the geocentric coordinate system to the "east, north, up" coordinate system via the universal transverse mercator (UTM) projection, as shown in Equations (17) and (18): where: M = a(m 0 r + m 1 sin(2r) + m 2 sin(4r) + m 3 sin(6r)), where k 0 is a scale factor (0.9996 in this paper); x and y are the longitude and latitude, respectively; a (6,378,245.0) and b (6,356,863.0188) are the equatorial radius and polar radius, respectively; L 0 is the central meridian; e 1 is the first eccentricity (0.0818191908); f e is the false easting value (500,000); and f n is the false northing value (10,000,000). We improved the original ICP by accounting for the weight w i at each timestamp. First, we calculated the centroids of three-dimensional points generated using the V-SLAM algorithm and the centroids of GPS after the coordinate conversion. Next, the covariance matrix between the measurements of V-SLAM and GPS was calculated. A singular value decomposition was carried out to obtain the rotation and translation matrices. This process is summarised in Algorithm 3.

Output: Rotation matrix and translation vector that minimises
Centred vectors x i = q i − q, y i = p i − p 3: Covariance matrix S = XWY T , where X and Y have x i and y i as columns, respectively, and W is a diagonal matrix with w i on the diagonal 4: Singular value decomposition S = UΣV T 5: R = UV T and t = q − Rp 6: V-SLAM track after calibration in global coordinates p i = Rp i + t The traditional ICP method based on least squares is not robust against outliers (i.e. this method tends to be affected by bad GPS signals, which are common in challenging outdoor environments). To improve the robustness of calibration, least absolute deviations is used to eliminate the effect of outliers. The problem can be solved via iteratively re-weighted least squares. The result of iteration converges to least absolute deviations; that is, The final weights are used as a measurement of credibility, which is part of the weight assigned to the timestamps. The credibility for timestamp c i is calculated using: where R and t are the rotation matrix and translation vector obtained in Algorithm 3, respectively; and φ is the credibility distance bound. We updated the weight using w i = s i c i . The loop continues until it meets the condition given by The last operation in this process is the inverse coordinate system conversion (i.e., inverse UTM projection). At this point, we could obtain the accurate global position information as follows: where:

Interacting Multiple Model (IMM) Filter
The IMM filter is designed to calculate the weight of each model under changing external driving conditions and estimate the vehicle position. The IMM filter is composed of four parts: (1) interaction, (2) two extended Kalman filters (one for the kinematic vehicle model and a second for the dynamic vehicle model), (3) a model probability update and (4) the fusion of state and covariance estimation from the two models.

Interaction
The states from the kinematic and dynamic vehicle models are mixed with each other using a predicated probability. The mixing probability u j|i k−1 is expressed as: where u j k−1 is the probability of the model j; and π ji is the probability for the transition from model j to model i, which is calculated based on a prior knowledge using a statistical method [32]. The index i, j = 1 refers to the kinematic vehicle model, while i, j = 2 represents the dynamic vehicle model. The predicated model probability u i k|k−1 is given as: The mixing state X i k−1|k−1 and their covariance P i k−1|k−1 of the model i are respectively computed using: In Equations (45) and (46), ∆X

Extended Kalman Filter
The extended Kalman filter was adopted to predict and update the state and covariance of each model. The prediction and update steps were carried out based on the equations shown in Figure 8. The state vector is represented as X k = (x k , y k , h k , ψ k , β k , v k ), while the input vector is u k = (v k_wheel , δ k ).

Model Probability Update
Each model probability is updated based on the model innovation. Assuming Gaussian statistics, the probability of model i at time k for the observation is calculated using: with where S i k is the residual covariance matrix.

Estimation Fusion
The output state X k|k and its covariance P k|k are computed according to the Gaussian mixture equation: with where ∆X i k = X k|k − X i k|k . Figure 9 shows the experimental platform. An autonomous vehicle, "Sinda," was equipped with a high-resolution stereo camera rig (Basler Ace1600 GigE, Basler, Ahrensburg, Germany, image size 960 pixels × 720 pixels), LiDAR (Velodyne HDL-32E, Velodyne, CA, USA) and GPS (NovAtel OEM718D, NovAtel, Calgary, Canada) and a micro-electro-mechanical system (MEMS)-based IMU (ADIS16465, Analog Devices, Norwood). The parameters of the vehicle are shown in Table 1.

Simulation
To ensure consistency with the simulation, we applied the same vehicle parameters in the experiment. To verify the reasonableness of the established vehicle model, two typical scenes were simulated: a circular trajectory ( Figure 10) and a U-turn trajectory (Figure 11). In the first simulation, the vehicle speed was set to approximately 55 km/h. Due to a high level of tyre slip, the dynamic vehicle model was adopted for position estimation. In the second simulation, the vehicle speed was maintained at approximately 45 km/h. In a straight line, the vehicle kinematic model was consistent with the assumption of no tyre slip; however, this assumption broke down for sharp turns. When the vehicle drove in a straight line, the model probability returned from the dynamic vehicle model to the kinematic model. The simulations demonstrate that the IMM could adapt to several driving conditions, which is vital for application in real-world situations.

Benchmark Dataset
Due to a lack of in-vehicle sensor data in the KITTI benchmark dataset, verification experiments were conducted using only camera (Point Grey Flea 2 FL2-14S3C-C, FLIR System, Arlington, VA, USA) and GPS (OXTS RT3003, OXTS, Middleton Stoney, UK) data (No.2011_10_03_drive_0027, Figure 12; No.2011_10_03_drive_0034, Figure 13) from residential scenarios. When the vehicle made a sharp turn or moved through a cluttered environment, the GPS-based trajectory (marked by the blue line) had an unacceptable positioning error, while the trajectory of our method (marked in red) was closer to the real trajectory.     After comparisons, the basic information is shown in Table 2. The distance of the ground truth from OXTS RT3003 on the KITTI benchmark was different from the distance estimated using our proposed method. The extra distance was naturally caused by losing the GPS signal or a multipath effect. To specify the relative errors between these methods, the root mean square errors (RMSEs) are shown in below Table 2.

Real Data
To evaluate the accuracy and reliability of the developed algorithm, outdoor tests were conducted using various methods: normal DGPS (green), DGPS/IMU (cyan), DGPS/IMU/in-vehicle sensors (blue) and DGPS/IMU/in-vehicle sensors/SLAM calibration (red). Moreover, DGPS/IMU/in-vehicle sensors was regarded as the "ground truth" to calculate the RMSE of different methods. The experiments conducted with real data were divided into three parts. First, experiments were conducted in bad GPS conditions corresponding to the most challenge case regularly encountered by an autonomous vehicle. Second, a short-distance experiment including different driving behaviours was conducted to test the adaptability of the system. These driving behaviours were selected to introduce unstable factors into the vehicle positioning system. Finally, a long-distance trajectory experiment was performed as a full test. The proposed global optimisation model eliminated the cumulative error derived from the incremental characteristic of the methods (e.g., odometry and DR methods). The abscissa axis and ordinate axis denote the longitude and latitude values, respectively.

Bad GPS Conditions
The most challenging driving conditions for GPS localisation are those where the GPS signal is blocked or reflected. We experimentally tested the special case where an autonomous vehicle stopped in a place where the GPS signal was blocked or reflected by buildings, tunnels or bridges, as shown in Figures 17 and 18. In these cases, the GPS signal was not stable or precise, and the radius of the circle that covered all GPS points was large when using normal DGPS, DGPS/IMU and DGPS/IMU/in-vehicle sensors. Not surprisingly, the proposed method exhibited absolute stability resulting from the local accuracy of the SLAM module. The comparison results in the bad GPS condition are shown in Table 3. Figure 17. Tunnel scenario. To highlight the stability of the proposed positioning method, the vehicle moved slowly and stopped briefly at the end of the tunnel. The GPS signal was blocked or reflected. All GPS-based methods were affected, resulting in drift of several meters. In contrast, the proposed method was stable and accurate. Figure 18. Interchange bridge scenario. The GPS signal was clearly blocked when the vehicle passed through the interchange bridge. Because of the IMU and in-vehicle sensors, the DGPS/IMU and DGPS/IMU/in-vehicle sensors methods could handle this challenge. In contrast, the DGPS-only method was inaccurate, as marked with a rectangle (The abscissa axis and ordinate axis denote the latitude and longitude values, respectively). To exhaustively explore the adaptability of the system, experiments involving different driving behaviours (e.g., quick starts and stops, sharp turns, reversing the car and driving in challenging conditions, such as on a rough road or in an indoor parking lot) were conducted. Quick starts and stops along with sharp turns resulted in tyre slip for vehicles driving on road surfaces with low adhesion coefficients. As shown in Figure 19, the vehicle passed through an indoor parking lot and reversed into an empty parking space to test the algorithm performance in a realistic scenario. With the exception of the proposed method, all positioning methods performed badly. Violent shaking introduced error in the IMU-based method, as shown in Figure 20. Unexpectedly, the DGPS-only method performed almost equal with IMU-based positioning methods. The comparison results of the short-distance trajectory with various driving behaviors are shown in Table 4.

Long-Distance Trajectory in a Cluttered Environment
A long-distance experiment including many of the scenarios found on urban and suburban roads (e.g., traffic lights and waiting zones) was performed as a final complete test. The main purpose of the long-distance experiment was to evaluate the effect of the SLAM module on the positioning result. Because SLAM is a locally accurate and incremental positioning method, cumulative error was inevitable. The proposed global optimisation strategy could eliminate this error, as shown in Figure 21. After the vehicle stopped at the stop line, we verified that the positioning point and the stop line on the map were coincident. The Comparison results of long-distance trajectory in a challenging environment are shown in Table 5.

Conclusions
The positioning and navigation system is a critical component of an autonomous vehicle. We have proposed a real-time, precise and low-cost vehicular positioning method. Common GPS and MEMS-based IMUs are affordable for use in commercial autonomous vehicles. For vehicles equipped with automatic braking systems and electronic stability control, the steering angle and the speeds of the four wheels can be directly obtained via the vehicle's CAN bus. Thus, the proposed strategy can be applied in autonomous vehicles.
The main contribution of this paper is the combination of V-SLAM for local coordinates and GPS for global coordinates considering their complementary properties. ISVD and SFFSD algorithms were proposed to ensure an accurate and reliable V-SLAM. The fusion was performed using an improved weighted ICP and least absolute deviations. The appropriate vehicle or kinematic model was selected according to the current state of the moving vehicle.
The performance of the proposed method was sufficiently verified by simulations, benchmark tests and experimental scenarios. The relationship between the driving state and vehicle model was analysed through various simulations. Using the KITTI benchmark database, we compared the ground-truth data with our proposed calibration method. The proposed method performed better than conventional methods in some special scenarios. In the experimental scenarios, an autonomous vehicle was driven through challenging environments (e.g., a tunnel and interchange bridge). The results showed that the developed algorithm satisfied the accuracy and reliability requirements for autonomous vehicle positioning and navigation. In future work, the algorithm will be extended to consider more extreme driving conditions, such as icy pavements and long tunnels. The developed positioning system will be applied to the applications of an autonomous vehicle indoor parking system or truck platoon. In addition, to promote the development and application of embedded positioning systems, the efficiency should be comprehensively considered.