A Real Time Localization System for Vehicles Using Terrain-Based Time Series Subsequence Matching

: Global Navigation Satellite Systems (GNSSs) are commonly used for positioning vehicles in open areas. Yet a GNSS frequently encounters loss of lock in urban areas. This paper presents a new real-time localization system using measurements from vehicle odometer data and data from an onboard inertial measurement unit (IMU), in the case of lacking GNSS information. A Dead Reckoning model integrates odometer data, IMU angular and velocity data to estimate the rough position of the vehicle. We then use an R-Tree structured reference road map of pitch data to boost spatial search e ﬃ ciency. An optimized time series subsequence matching method matches the measured pitch data and the stored pitch data in reference road map for more accurate position estimation. The two estimated positions are fused using an extended Kalman ﬁlter model for ﬁnal localization. The proposed localization system was tested for computational complexity with a median runtime of 12 ms, and for positioning accuracy with a median position error of 0.3 m.


Introduction
Autonomous vehicles and connected vehicles have emerged in recent years. This development is linked to the rapid development of intelligent transportation systems based on advances in computing speed, and 5G and sensor technologies. One of the key issues in all these systems is the ability to localize a vehicle precisely. Their effectiveness is heavily dependent on lane-level or decimeter-level ego-localization. Furthermore, the localization of the vehicles must be highly reliable to prevent failures of these systems during operation.
Global Navigation Satellite Systems (GNSSs) are commonly used for positioning vehicles. Given a sufficient number of satellites, these systems can achieve centimeter-level precision by taking advantage of Continuously Operating Reference Stations (CORS) in an open area. The BeiDou Navigation Satellite System (BDS) in particular, has been on the rise in recent years and shows great potential in terms of positioning and timing [1]. Combining multiple satellite systems improves the availability of signals, gives operators more access, and increases accuracy [2]. However, GNSS frequently encounters loss of lock in urban areas with high buildings and trees, which is averse to GNSS geometry and visibility, degrades positioning accuracy, and can even lead to positioning failure.
An Inertial Navigation System (INS) is an attractive augmentation of GNSS [3] that can solve this issue. An INS has Dead Reckoning (DR) capability and can provide position, velocity and attitude estimation even when loss of lock occurs in GNSS. At the same time, INS error can be mitigated using proposed a new interest point method with multi-scale extrema features, to extract feature vectors from the pitch information of road regions and created a KD-tree in the preprocessing phase. In the online phase, the feature vectors for a query signal were tested for a match within the feature tree to find vehicle's position estimation. Laftchiev et al. [38] proposed a linear model based on autoregressive models with an exogenous input to segment the pitch data and compare model output instead of original pitch data, which accelerates the matching process. However, in all these methods, finding a balance between time consumption and accuracy is an elusive goal. Therefore, new approaches must be developed to improve the viability of using advanced time-series subsequence matching methods for vehicle localization. In this work, we present a new real-time localization system that combines INS measurements, odometer data and an advanced DTW method to achieve both accuracy and computation efficiency without recourse to GNSS signals.

Problem Formulation
Previous works attempt to localize a vehicle based on terrain information using either particle filter-based methods or solely time series subsequence (TSS) matching methods, but both have limitations. Particle filter represents the posterior distribution of the state using a set of particles which evolve recursively as new information becomes available [39]. Particle degeneracy is a common problem when implementing a particle filter, which reduces the performance of the particle filter. Previous research on particle filter-based localization [4,[11][12][13] using terrain information addressed particle degeneracy by increasing the particle numbers, which increased the computation burden. Moreover, only observation information at a current moment was used to update the particles. As in [12], pitch or pitch difference measurements have similar trends at different speeds, but at a specific location, the values of measurements at various speeds may be at different orders of magnitude. Therefore, using information from the current moment only is not sufficient to ensure accuracy. Consequently, we choose to use TSS matching methods to find the path of the vehicle and current position using a sliding window of a certain length incorporating historical data.
DTW has been shown to be the most precise measure for time-series data queries in most domains [16,40]. Even though previous localization research [11,34,37,38] has argued that DTW is not computationally efficient, nevertheless authors in [16] proposed an exact DTW algorithm whose sequential search is much faster than most alternatives. In our work, we adopted the DTW algorithm from [16] to do TSS matching on pitch data. Furthermore, localization processes discussed in previous research depend solely on time-series matching, regardless of accuracy, which is not robust as pitch data varies with speed and increases the chance of matching failures.
Another unsolved issue with pitch-based localization is that IMU error is neglected. IMU gyros exhibit drift performances from 0.1 • /h at a tactical grade to 100 • /h at a typical Micro Electromechanical System (MEMS) grade [41]. A tactical IMU alone can accomplish accurate localization but is way too expensive to be applied broadly on consumer vehicles. A new pitch-based localization system needs to be designed for low grade IMUs.

Contributions
We designed a new pitch-based localization system, which has several advantages over the methods described in the literature: 1.
An advanced DTW algorithm is used and the road map with terrain information is stored in an R-Tree to speed up the online localization process, which makes our system truly real-time; 2.
To ensure the accuracy at the same time, an integrated localization model is proposed to fuse in-vehicle multisensory data and time-series data matching results; 3.
Our proposed model is insensitive to dynamics of the vehicle velocity while most existing methods require steady speeds; Remote Sens. 2020, 12, 2607 4 of 18 4.
Our system is robust to abrupt changes in single-pitch data since it uses time-series subsequence pitch data instead of single time-point pitch data.
The remainder of this paper is organized as follows. The methods are described in Section 2. Experimental implementation and the results are presented in Section 3. Section 4 provides the conclusions.

Methods
In our integrated model, only two in-vehicle sensors are used: an odometer and a low-grade IMU, which are easily installed on almost all vehicles. The model includes an offline mapping and an online localization process. In Figure 1, we illustrate our system in the online localization phase at time step k. Measurements from IMU and odometer are combined by a Dead Reckoning (DR) model to predict the navigation state (latitude, longitude, height, roll, pitch, yaw) of a vehicle. The predicted pitch value is stored in a vector together with all pitch values in previous t d seconds, to generate a pitch TSS x k . Accumulated driving distance in the t d seconds is calculated by the odometer measurements, and if it is less than a threshold, the predicted navigation state is returned as the result at the current time step. From a predefined map which includes accurate positions and pitch values of road points, a closest road point to the vehicle can be found based on the vehicle navigation state. Given a search window, a search area in the road map can be found based on the closest road point. Then a series of pitch TSS Y k from the map is generated based on the search area. A time series subsequence matching model is applied for x k and Y k to find the best-fit matching subsequence in Y k . An estimation model is used to estimate the vehicle position from the matching model. Finally, an integrated model is used to integrate the predicted position from the DR model and the estimated position from the matching model, and return a final vehicle position. Detailed explanations of key models with grey background in Figure 1 are shown in the following text. The remainder of this paper is organized as follows. The methods are described in Section 2. Experimental implementation and the results are presented in Section 3. Section 4 provides the conclusions.

Methods
In our integrated model, only two in-vehicle sensors are used: an odometer and a low-grade IMU, which are easily installed on almost all vehicles. The model includes an offline mapping and an online localization process. In Figure 1, we illustrate our system in the online localization phase at time step k. Measurements from IMU and odometer are combined by a Dead Reckoning (DR) model to predict the navigation state (latitude, longitude, height, roll, pitch, yaw) of a vehicle. The predicted pitch value is stored in a vector together with all pitch values in previous td seconds, to generate a pitch TSS x k . Accumulated driving distance in the td seconds is calculated by the odometer measurements, and if it is less than a threshold, the predicted navigation state is returned as the result at the current time step. From a predefined map which includes accurate positions and pitch values of road points, a closest road point to the vehicle can be found based on the vehicle navigation state. Given a search window, a search area in the road map can be found based on the closest road point. Then a series of pitch TSS Y k from the map is generated based on the search area. A time series subsequence matching model is applied for x k and Y k to find the best-fit matching subsequence in Y k . An estimation model is used to estimate the vehicle position from the matching model. Finally, an integrated model is used to integrate the predicted position from the DR model and the estimated position from the matching model, and return a final vehicle position. Detailed explanations of key models with grey background in Figure 1 are shown in the following text.

Offline Road Map Generation
A road map including terrain information is generated for the driving region, and all sensors are synchronized before mapping. The vehicle must travel slowly and smoothly to gather a sufficient number of discrete points in the test region, because the higher the resolution of the map, the higher the positioning accuracy. The ith point of the road map is defined as where id i is the index of the point i, the values lat id , lon id i , h id i , and θ id i are its latitude, longitude, height and pitch data, respectively; ∆s id i is the distance traveling from the last point, as recorded by the odometer.
After data acquisition, the road points are stored in an R-Tree. R-Tree is a dynamic index structure for spatial searches [42], and can index multi-dimensional information such as geographical coordinates in a highly efficient manner. When the point data is organized in an R-Tree, for a point, the nearest neighbor p k i within a given distance r can efficiently be queried. Even though we structure the R-Tree by latitude and longitude information, tree nodes include all features of p i . Nodes that are in the same bounding box are stored in a list and have consecutive index numbers. Therefore, to find a previous neighbor whose index is id i−1 , merely requires a search for the previous node in the bounding box, or if p i is the first node in the bounding box, a search for the last node in the previous child bounding box.

Dead Reckoning Model
In our DR model, real-time measurements from odometer and IMU are integrated to provide a position estimation of a vehicle. A low-cost IMU can estimate the position, attitude, and velocity of a vehicle from the raw data: velocity increments as measured by an accelerometer and angular increments as measured by a gyroscope, in the navigation frame. Because of the drift of the accelerometer and gyroscope in the IMU, the velocity and angular increments from the IMU contain errors that cause the navigation results to drift rapidly. In [43], the author elaborated estimation techniques for low-cost inertial navigation; we however, focus on integrating an IMU and odometer, we call the DR model.
We define the vehicle frame (v-frame) as forward-transversal-down, originating at the vehicle center, the navigation frame (n-frame) as north-east-down, originating at the earth mass center, and body frame (b-frame) expressed as forward-transversal-down, originating at the IMU center. More details about the frames can be seen in [43]. The odometer is installed on the rear wheel, as it is not used for steering. We assume that the vehicle does not have either a cross track or vertical speed but only an along-track speed v at time step k, as measured by the odometer, so that the speed of the vehicle at the The relationship between the velocity of vehicle at the n-frame v n,k IMU and v v,k wheel can be expressed as: where δv n,k IMU and δv n,k wheel are the error of v n,k IMU and v n,k wheel , respectively. The value C n,k b and C b,k v express the rotation matrix from the b-frame to the n-frame and from the v-frame to the b-frame, while, ω b,k nb × is the cross-product form of the rotation rate of the b-frame with respect to the n-frame, and δι b,k wheel is the lever-arm vector of the odometer in the b-frame.
Considering this relationship, the Extended Kalman Filter (EKF) is used to integrate the measurements from the IMU and the odometer. A state vector X k is constructed as: Remote Sens. 2020, 12, 2607 6 of 18 where superscript T is the transposition, δϕ n,k IMU is the error in attitude as estimated by INS using the IMU at the n-frame, δp n,k IMU is the error of position, δε b,k a is the drift error of the accelerometer at the b-frame, and the drift error of gyroscope is δε b,k g , and δκ k wheel is the scale factor error of the odometer. The value W k is the noise in X k including the white noise generated by the specific force from the accelerator w b,k a whose deviation is the velocity random walk of the accelerator, and the white noise of the angular rate from the gyroscope w b,k g whose deviation is the angular random walk of the gyroscope. The EKF state transformation is shown in Equation (4), where F k and G k are coefficient matrix. Then, Equation (4) is discretized in time so that a coarse estimation of state vector X k− can be derived from X k−1 by the identity matrix I, F k−1 , G k−1 and W k−1 : The EKF observation function is constructed as follows: where H k is coefficient matrix and V k ∼ N 0, R k is observation error covariance matrix. From Equation (1) we can get: so that: Afterwards, X k− is updated by Z k via EKF to get X k : where Q k is the spectral density matrix, P k the error covariance of X k , and K k the Kalman gain of EKF. The estimated error X k is fed back to the navigation state of position, velocity, and attitude in the n-frame: If the v k measured by the odometer drops below a certain predefined threshold, a zero-velocity update (ZUPT) is applied instead of the wheel sensor velocity, v v,k wheel . More details about ZUPT implementations can be found in [44].
f is the sensor frequency and ∆s is the odometer increment. If d k t d is less than a predefined distance threshold d thre1 , then the navigation state estimated by the DR model is assumed to be the final vehicle state. As an illustrative example, consider a vehicle stopping when a traffic light turns red, in this condition d k t d would be small and the DR model is applied for final navigation state estimation at this time step.

Time Series Subsequence Matching Model
After the vehicle navigation state is calculated using the DR model and exploiting the efficiency of an R-Tree spatial search, we calculate the closest point p k i in the road map to the estimated point p k . Given a search window [−d w , d w ], we find all road points Y k from id i−m to id i+n in the map whose distances to p k i are within this window: Because of the drift error of an IMU, p k is the possible vehicle position, so we assume that the accurate position drops in the set of all possible road points, Y k . We use a time series matching method DTW to estimate the optimum position. More details about DTW are in [16]. Algorithm 1 describes the algorithm.

Algorithm 1: Application of DTW in Localization
Initialize Variables: Set the distance vector D k to empty Initialize Constant: Cutoff frequency for a low-pass filter: f c Algorithm Loop: FOR l = i − m: i + n Similar to Equation (14), from the road map find a point cluster B k such that: Get the pitch data vector y k = θ id l−u , . . . , θ id l−1 , θ id l from B k Normalize y k and x k using Z-Score method Interpolate or sample y k to y k to have the same length with x k Low-pass filter y k and x k with Fast Fourier Transform (FFT) by f c Calculate the distance d l between filtered sequences y k and x k by DTW Store d l in the distance vector D k END FOR RETURN D k Remote Sens. 2020, 12, 2607 8 of 18 Normalization is necessary before executing DTW as y k and x k may be on different magnitudes. Moreover, to reduce the noise in the measurement related to velocity, we use FFTW [45] to perform Fast Fourier Transform (FFT) in our work. FFTW is a C subroutine library for computing the discrete Fourier transform in one or more dimensions and is thought to be the fastest Fourier Transform tool among all the open source FFT tools.

Estimation of Navigation State from the Matching Model
In almost all DTW applications, the best-fit matching subsequence has the smallest distance value. In the pitch-based localization, pitch angle measurements contain bias noise and are affected by vehicle speed. There even might be different vehicles or sensors when generating the road map and processing online localization. Moreover, most parts of a road have slight slopes, so the magnitude of pitch angle measurements is small. Thus, the smallest value in the D k does not necessarily mean that we can find the best subsequence and the right location. Instead, we choose n minima and choose the one whose position is closest to navigation state estimation from DR, which can constraint time series matching results. This principle makes the final navigation state more robust and precise. We use an algorithm detecting the valleys in data to calculate M k and m k , shown in Algorithm 2.

Integration Model
We obtain the road point m k 's lat id m , lon id m , and h id m from the map. Equation (2) is augmented to: Remote Sens. 2020, 12, 2607 9 of 18 including an error δp n,k TM of position (lat id m , lon id m , and h id m ) estimated by TSS matching model. Equation (7) is also augmented as: so H k becomes: F k and G k are changed to the following: Then, EKF is applied again by repeating Equations (9)-(12), but here Q k and P k should also include the error density information and error covariance of δp n,k TM . Finally, we get the integrated navigation state like Equation (13).

Experiments
In our work, field experiments are carried out to test our integrated navigation system. Our system and algorithms are implemented in C++ on a Windows10 PC with 3.70 GHz CPU and 16 GB RAM. The test vehicle (see Figure 2a) we use is a Chery Tiggo car equipped with a Novatel SPAN-CPT, which couples GNSS and IMU to provide consistent navigation support. An RTK-GPS (centimeter-level accuracy) is used to gather ground truth positions for evaluation. The IMU components are comprised of a Fiber Optic Gyros (FOG) and a MEMS accelerometer, its performance metrics are shown in Table 1. An encoder is used to measure the speed of a vehicle. In our experiments, we travelled twice on a 6.5-km round route (see Figure 2b) on the Huashan Road in Wuhan, China. From Figure 2c, we can see that in our test area there are both sharp and slight changes in height; these various features help test the robustness of our system.
The vehicle travelled along the route (called reference trip) at about 20 km/h to generate the reference map, where 3D positions (latitude, longitude, height), attitudes (roll, pitch, yaw) and the encoder measurements (distances between two adjacent positions) were recorded at 100 Hz. Then, the reference map was stored in an R-Tree as described above in Section 2.1. In the second trip (called measurement trip), the vehicle travelled on the route at various speeds and recorded raw IMU measurements (angle increments, velocity increments), 3D positions, attitudes, and the distances at 100 Hz. The 3D positions and attitudes were used as ground truth for evaluation. Then, the encoder's distance measurements were converted to velocity and integrated with IMU for the DR model as described above in Section 2.2. Figure 3 shows the vehicle's velocity at n-frame in the measurement trip. Around time 3 min and 5 min, the vehicle decelerates to about 0 m/s, stops for a while, and accelerates again. This is because of the traffic lights and the U-turn in the route (see Figure 2b). Unlike most existing studies, which assume that the vehicle travels at a steady speed, the vehicle's various speed features in our work also help prove the robustness of our navigation system. The vehicle travelled along the route (called reference trip) at about 20 km/h to generate the reference map, where 3D positions (latitude, longitude, height), attitudes (roll, pitch, yaw) and the encoder measurements (distances between two adjacent positions) were recorded at 100 Hz. Then, the reference map was stored in an R-Tree as described above in Section 2.1. In the second trip (called measurement trip), the vehicle travelled on the route at various speeds and recorded raw IMU measurements (angle increments, velocity increments), 3D positions, attitudes, and the distances at 100 Hz. The 3D positions and attitudes were used as ground truth for evaluation. Then, the encoder's distance measurements were converted to velocity and integrated with IMU for the DR model as described above in Section 2.2. Figure 3 shows the vehicle's velocity at n-frame in the measurement trip. Around time 3 min and 5 min, the vehicle decelerates to about 0 m/s, stops for a while, and accelerates again. This is because of the traffic lights and the U-turn in the route (see Figure 2b). Unlike most existing studies, which assume that the vehicle travels at a steady speed, the vehicle's various speed features in our work also help prove the robustness of our navigation system.
To test the repeatability of terrain features of the two trips, pitch values are shown in Figure 4. Note that the pitch values in the measurement trip are the ground truth from the integrated SPAN-CPT and RTK-GPS. Because of the good performance of FOG and centimeter-level accuracy of RTK-GPS, the pitch values in the two trips are almost the same in Figure 4a. In Figure 4b    To test the repeatability of terrain features of the two trips, pitch values are shown in Figure 4. Note that the pitch values in the measurement trip are the ground truth from the integrated SPAN-CPT and RTK-GPS. Because of the good performance of FOG and centimeter-level accuracy of RTK-GPS, the pitch values in the two trips are almost the same in Figure 4a. In Figure 4b we can see the effect of the vehicle's stops in the measurement trip, pitch values remain unchanged around time 3 min and 5 min.  In the experiments, we conduct DR calculations at 100 Hz, DTW matching at 1 Hz, and record the final integrated navigation state at 1 Hz. In our system, there are several predefined parameters, like duration d t , distance threshold 1 thre d and 2 thre d in Figure 1. Table 2 shows the predefined parameters in our system and their values in our experiments. In the experiments, we conduct DR calculations at 100 Hz, DTW matching at 1 Hz, and record the final integrated navigation state at 1 Hz. In our system, there are several predefined parameters, like duration t d , distance threshold d thre1 and d thre2 in Figure 1. Table 2 shows the predefined parameters in our system and their values in our experiments.  Figure 5 illustrates the comparison results of the horizontal vehicle position estimation from our integrated model and the pure DR model without being integrated by DTW matching during the measurement trip. We can see that both our system output and the DR model output have similar pattern as the ground truth, but our system bias is much smaller than that of the DR model. In the beginning of the measurement trip in Figure 5e, our system and the DR model have good correspondence with the ground truth. After a while, in Figure 5d the DR model gradually deviates from the ground truth trajectory and in Figure 5b,c it completely deviates from the lane. In Figure 5b there is a U turn where the DR model overshoots the lane during position calculation. This phenomenon is common in DR, which is caused by lever arm effect and accumulated error in INS and odometer. At the end of the trip, we can see from the Figure 5d,e that the bias of the DR model output decreases and leads to a better correspondence with the ground truth. Our system has a much better performance than the DR model, no matter if on a straight lane or U turn, and our system output keeps good positioning accuracy. integrated model and the pure DR model without being integrated by DTW matching during the measurement trip. We can see that both our system output and the DR model output have similar pattern as the ground truth, but our system bias is much smaller than that of the DR model. In the beginning of the measurement trip in Figure 5e, our system and the DR model have good correspondence with the ground truth. After a while, in Figure 5d the DR model gradually deviates from the ground truth trajectory and in Figure 5b,c it completely deviates from the lane. In Figure 5b there is a U turn where the DR model overshoots the lane during position calculation. This phenomenon is common in DR, which is caused by lever arm effect and accumulated error in INS and odometer. At the end of the trip, we can see from the Figure 5d,e that the bias of the DR model output decreases and leads to a better correspondence with the ground truth. Our system has a much better performance than the DR model, no matter if on a straight lane or U turn, and our system output keeps good positioning accuracy.   Figure 6 shows a boxplot of the vehicle positioning error in the DR model and our system, and a boxplot of the runtime of our system. The data for the boxplot is at 1 Hz for both the DR model and our system. Our system's median error was about 0.3 m while that of the DR model is about 100 m. Moreover, the median run time of our system is 12 ms, and for all the time steps (cycles) the runtime is less than 100 ms. Therefore, our system meets real-time requirements as the cycle is 1 s. Figure 7 shows the time series of vehicle state errors in the DR model and our system. In Figure 7a the DR model's position error is increasing with time in the first half of the trip, after reaching a peak, it keeps decreasing. However, Position Dilution of Precision (PDOP) for the DR model is reflected by the error covariance matrix P k in Equation (12), increasing over time. The DR model position error, as calculated by the ground truth, decreased unexpectedly in the second half of the trip, possibly due to random values in the EKF. Dilution of Precision (DOP) is not consistent with the error calculated from the ground truth, nor the error calculated from the velocity and attitude. Figure 7b shows the velocity error in the driving direction. The DR model's velocity error is much larger than that of our system. In Table 1, we can see that the accelerometer in SPAN-CPT is at a MEMS level, its large drift error leads to deviation quickly during operation. However, in Figure 7d-f, errors of pitch, roll and yaw angles of the DR model are slightly larger than those of our system, which is due to the good quality of FOG in SPAN-CPT. As the pitch data is matched in our work, Figure 7c shows the time series of pitch data for the DR model, our system and the ground truth. We can see that both the DR model and our system achieved a similar pattern with the ground truth, and pitch data from our system is slightly closer to the ground truth than the DR model. The similarity and accuracy of the pitch data from our system benefit our matching process, help find a precise estimation of the vehicle's position, and lead to a positive constraint on the drift errors of the gyroscope and accelerometer in the integration model. We can see that it is a positive recursive procedure and makes our system more robust.
Remote Sens. 2020, 12, x FOR PEER REVIEW 13 of 19 Figure 6 shows a boxplot of the vehicle positioning error in the DR model and our system, and a boxplot of the runtime of our system. The data for the boxplot is at 1 Hz for both the DR model and our system. Our system's median error was about 0.3 m while that of the DR model is about 100 m. Moreover, the median run time of our system is 12 ms, and for all the time steps (cycles) the runtime is less than 100 ms. Therefore, our system meets real-time requirements as the cycle is 1 s. Figure 6. The error of localization from the pure DR model and our integrated model, compared with the ground truth from RTK-GPS, and the runtime of our integrated model. Figure 7 shows the time series of vehicle state errors in the DR model and our system. In Figure  7a the DR model's position error is increasing with time in the first half of the trip, after reaching a peak, it keeps decreasing. However, Position Dilution of Precision (PDOP) for the DR model is reflected by the error covariance matrix P k in Equation (12), increasing over time. The DR model position error, as calculated by the ground truth, decreased unexpectedly in the second half of the trip, possibly due to random values in the EKF. Dilution of Precision (DOP) is not consistent with the error calculated from the ground truth, nor the error calculated from the velocity and attitude. Figure 7b shows the velocity error in the driving direction. The DR model's velocity error is much larger than that of our system. In Table 1, we can see that the accelerometer in SPAN-CPT is at a MEMS level, its large drift error leads to deviation quickly during operation. However, in Figure  7d-f, errors of pitch, roll and yaw angles of the DR model are slightly larger than those of our system, which is due to the good quality of FOG in SPAN-CPT. As the pitch data is matched in our work, Figure 7c shows the time series of pitch data for the DR model, our system and the ground truth. We can see that both the DR model and our system achieved a similar pattern with the ground truth, and pitch data from our system is slightly closer to the ground truth than the DR model. The similarity and accuracy of the pitch data from our system benefit our matching process, help find a precise estimation of the vehicle's position, and lead to a positive constraint on the drift errors of the gyroscope and accelerometer in the integration model. We can see that it is a positive recursive procedure and makes our system more robust. These results indicate that DR model is vulnerable to the biases of the gyroscope, accelerometer and odometer. Our system compensates for these biases by integrating position estimation with higher accuracy for improved positioning performance. The FFT and DTW processes in our work are implemented with FFTW and fast DTW, furthermore, we extensively optimize their codes to make them even faster for real-time computing. Our system delivers high performance and efficiency with a median position error of 0.3 m and a median runtime of 12 ms.

Discussion
In our system, we use a TSS-matching model in a DTW algorithm to find the possible positions of a vehicle. We choose n positions where the DTW distance is minima instead of the smallest These results indicate that DR model is vulnerable to the biases of the gyroscope, accelerometer and odometer. Our system compensates for these biases by integrating position estimation with higher accuracy for improved positioning performance. The FFT and DTW processes in our work are implemented with FFTW and fast DTW, furthermore, we extensively optimize their codes to make them even faster for real-time computing. Our system delivers high performance and efficiency with a median position error of 0.3 m and a median runtime of 12 ms.

Discussion
In our system, we use a TSS-matching model in a DTW algorithm to find the possible positions of a vehicle. We choose n positions where the DTW distance is minima instead of the smallest distance of D k in Algorithm 2. Then through the constraint in Algorithm 2, we find the best positioning estimation from n minima. The smallest DTW distance means that its corresponding subsequence on the reference map matches our calculated pitch subsequence the best. But it may not be the right position. This is reasonable because compared with other applications of TSS matching, amplitudes of pitch values are small, pitch values are vulnerable to disturbances, and for flat road or straight slope the pitch curve may have less features, so subsequences of pitch data at different positions on the reference map may have similar patterns and have similar DTW distances. In this situation, we need an extra constraint, as in Algorithm 2, to find the right positioning estimation. Figure 8 supports our assumption by presenting the position error of using the smallest DTW distance of D k instead of minima in our system. We can see that there is a wrong positioning estimation at around 7 min. The error caused by this erroneous positioning estimation accumulates and affects all cycles afterwards. , because we think it is large enough in our work. Figure 9 shows the position error e of the tests and the control group. We can see that if we increase w d from 10 to 20 m ot of the position error slightly changes, but the runtime increases a lot. w d increases m he number of road points Y k in Equation (15) increases and the iteration in the loo ithm 1 also increases, which increases time consumption. Even though w d is increased fro m, considering our system's positioning error which is less than 2 m, the search window We also test how these parameters affect the performances of our system by control variates method. A series of tests named Testk, k = 1, 2, ...5 are implemented with parameters values shown in Table 3. Parameter values of the control group are from Table 2. The parameter values with gray background are different from the corresponding values in the control group. Note that d thre2 is not tested, because we think it is large enough in our work. Figure 9 shows the position error and runtime of the tests and the control group. We can see that if we increase d w from 10 to 20 m, the boxplot of the position error slightly changes, but the runtime increases a lot.d w increases mean that the number of road points Y k in Equation (15) increases and the iteration in the loop in Algorithm 1 also increases, which increases time consumption. Even though d w is increased from 10 to 20 m, considering our system's positioning error which is less than 2 m, the search window (−10 m, 10 m) is large enough to find the right position. Therefore, Test1 results in a similar boxplot of position error with the control group. We also test how these parameters affect the performances of our system by control variates method. A series of tests named Test , 1, 2,...5 k k = are implemented with parameters values shown in Table 3. Parameter values of the control group are from tested, because we think it is large enough in our work. Figure 9 shows the position error and runtime of the tests and the control group. We can see that if we increase w d from 10 to 20 m, the boxplot of the position error slightly changes, but the runtime increases a lot. w d increases mean that the number of road points Y k in Equation (15) increases and the iteration in the loop in Algorithm 1 also increases, which increases time consumption. Even though w d is increased from 10 to 20 m, considering our system's positioning error which is less than 2 m, the search window (−10 m, 10 m) is large enough to find the right position. Therefore, Test1 results in a similar boxplot of position error with the control group.  In Test2, the cutoff frequency f c in the low-pass filter in Algorithm 2 decreases from 8 to 5 Hz; from Figure 9 we can see that the positioning result is slightly worse than the control group, and the running time is almost the same. f c has no effect on runtime. Lower f c means a smoother pitch subsequence, but less small features, which is not good for finding the best position estimation.
From Figure 1 and Equation (14), we can see that t d controls the length of the pitch subsequence. In Test3, t d increases from 2 to 5 s, which means more pitch data are involved in the matching procedure so that the DTW algorithm takes more time. The positioning accuracy is similar to that of the control group, which means that pitch data in the duration of 2 s are enough to find the best position estimation.
In Figure 1, d thre1 is a distance threshold; if the accumulated distance that the vehicle moves within t d is less than d thre1 , the navigation state from the DR model is assumed to be the vehicle's final state at the current time step. This constraint is for low speed situations, like waiting for traffic lights or obstacle avoidance. In Figure 3, we can see that the vehicle stops twice, and the second stop lasts for about 1 min. During these situations, the vehicle uses the DR model directly to calculate its navigation state. If d thre1 is increased from 5 to 10 m (the situation lasts for longer for the vehicle), our integration model works less, and the vehicle takes the direct output from the DR model in more cycles. From Figure 9, we can see that in Test4 there is a larger position error and the runtime is similar to the that of the control group.
From Algorithm 2, we can see that the number n of minima has a direct effect on the choice of possible position estimations in matrix M k . Larger n means more possible estimations using the constraint in Algorithm 2 to obtain the best estimation m k . In Test 5, we decrease n from 8 to 4.
From Figure 9, we can see that 8 minima can produce a more accurate position estimation and n has a very limited effect on runtime.

Conclusions
This paper proposed an integrated model for real-time localization. This integrated model deploys a DR model and a DTW algorithm. The DR model calculates the vehicle's navigation state (position, velocity and attitude) by IMU and odometer. However, the DR model is vulnerable to the drift error of gyroscope and accelerometer in the IMU and the accumulated error of odometer. Using the prediction of the vehicle's navigation state from the DR model, we take advantage of R-Tree to find the closest road point in the reference terrain map efficiently. Given a search window centering on this road point, we match our calculated pitch data over a period with the pitch data stored in the reference map using DTW algorithm to find the best-fit position estimation. This position estimation is integrated with the DR model by EKF to get final navigation state. The integration constrains IMU drift error and accumulated odometer error for improved positioning accuracy. Field experiments were conducted to test the integrated system, the terrain map covered a route more than 6 km and the measurement trip lasted for more than 10 min. In our work, we used a high-performance MEMS-level IMU sensor which helped the DR system provide good estimations of positions. In the future, we will try low-cost MEMS-level IMU sensors in our system to make our system more practical. Experimental results show that our integrated system can accurately (mean position error: 0.3 m) and efficiently (median runtime: 12 ms) localize the vehicle.