# Longitudinal error improvement by visual odometry trajectory trail and road segment matching

Dayang Nur Salmi Dharmiza Awang Salleh, Emmanuel Seignez

<figure><img src="image_1.png" /></figure>

## To cite this version:

Dayang Nur Salmi Dharmiza Awang Salleh, Emmanuel Seignez. Longitudinal error improvement by visual odometry trajectory trail and road segment matching. IET Intelligent Transport Systems, 2019, 13 (2), pp.313-322. 10.1049/iet-its.2018.5272. hal-04026569

HAL Id: hal-04026569

https://cnam.hal.science/hal-04026569

Submitted on 13 Mar 2023

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_2.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
IET Intelligent
Transport
Systems

Special issue
Call for Papers

Be Seen. Be Cited.
Submit your work to a new
IET special issue

Connect with researchers and
experts in your field and share
knowledge.

Be part of the latest research
trends, faster.

Read more

<figure><img src="image_3.png" /></figure>
IET Intelligent Transport Systems

Research Article

Journals
The Institution of
Engineering and Technology

# Longitudinal error improvement by visual odometry trajectory trail and road segment matching

Dayang Nur Salmi Dharmiza Awang Salleh¹✉, Emmanuel Seignez¹

¹SATIE, ENS Cachan, CNRS, Université Paris Sud, 91400 Orsay, France

✉E-mail: dayang-nur.salmi-dharmiza@u-psud.fr

ISSN 1751-956X
Received on 17th January 2018
Revised 5th September 2018
Accepted on 24th September 2018
E-First on 29th October 2018
doi: 10.1049/iet-its.2018.5272
www.ietdl.org

**Abstract:** As one of the key requirements in the intelligent vehicle, accurate and precise localisation is essential to ensure swift route planning during the drive. In this study, the authors would like to reduce the longitudinal positioning error that remains as a challenge in accurate localisation. To solve this, they propose a data fusion method by integrating information from visual odometry (VO), noisy GPS, and road information obtained from the publicly available digital map with particle filter. The curve of the VO trajectory trail is compared with road segments curve to increase longitudinal accuracy. This method is validated by KITTI dataset, tested with different GPS noise conditions, and the results show improved localisation for both lateral and longitudinal positioning errors.

## 1 Introduction

Accurate vehicle localisation has been immensely researched for years, in the development towards the autonomous vehicle. While the technology has started to develop and been in the market in recent years, it does not stop the motivation for further research. Besides its application for an autonomous vehicle, the current positioning and routing technology in vehicles should also be improved. There are still many issues that need to be addressed as the road network structure is becoming more complex with urban development and this frequently causes interruptions in localisation and path planning.

While low-cost GPS is widely used for localisation, it suffers from several conditions such as the multipath and non-line-of-sight effects especially in urban areas due to the dense buildings or other constructions like tunnels and bridges [1]. Zair *et al.* [2] proposed to overcome GPS signal problem to improve its accuracy by detecting and removing the outliers. This resulted in reliable GPS data, but the method consumes complex computation and the results are inconsistent particularly in biased GPS noise. Therefore, data fusion with other sensors is desirable to overcome this problem. Data fusion for vehicle localisation can be from several sensors and among those are LIDAR, GNSS receiver, camera sensor, Inertial Measuring Unit (IMU), and a radar sensor. In addition, the digital map can also be used as an input for the data fusion. These data and information can be used together, without requiring prior computation or data compensation, to provide new information of an estimated state.

For instance, Hata *et al.* [3] proposed curb detection by 3D-LIDAR fused with motion estimation by GPS/IMU. The paper presented a novel method for curb detection by using multilayer LIDAR to extract curb structure even with the existence of obstacles. Although the localisation performance is good, it still suffers from longitudinal error and the system is quite expensive. As our research motivation, we would like to reduce the localisation system cost by avoiding high cost sensors such as LIDAR and RTK GPS although they can easily provide accurate positioning for the vehicle [4, 5]. Generally, sensors used for localisation can be divided into two – passive and active sensors. LIDAR is an example of an active sensor since it transmits light pulse and detects the reflected light. Active sensors are not only more expensive, but they also consume more energy. Therefore, we would prefer passive sensors for cost optimisation in vehicle localisation.

Meanwhile, Gu *et al.* [6] have proposed a low-cost localisation method by passive sensors data fusion from 3D-GNSS, inertial sensor and camera sensor. The inertial sensor is used to smooth the positioning trajectory, but the drift makes it difficult to achieve accurate localisation. Thus, camera sensors are utilised for lane marking detection to reduce lateral positioning error while observing lane-keeping or lane-changing behaviour. The results show submeter positioning accuracy, but the method highly relies on the availability of 3D maps and it does not consider GNSS signal outage. On the other hand, Brubaker *et al.* [7] presented an interesting localisation technique by only using a camera for visual odometry (VO), matched with the digital map by a probabilistic model. It has an interesting approach that achieved a positioning error average of about 3 m, but it failed to perform well in ambiguous road networks. Besides, these works did not include further quantitative analysis on lateral and longitudinal errors. The lateral error can usually be improved because vehicles do not move vertically, and it can be compensated by using road width information obtained from lane marking on both sides of the road as presented in [8]. However, the longitudinal error remains a problem for localisation especially when the vehicle is moving on a straight path or road without intersections.

Most of the work in vehicle localisation [9–11] utilised stop lane marking or intersection detection to correct the longitudinal position and overcome this problem but there is a possibility of occluded lane markings or roads without lane markings that can degrade the localisation performance. Previously, the authors of [3, 6, 10, 11] proposed map matching approach by processing the image for road lane markings extractions and curb detection to compare with a road curve on the map. These localisation methods can reduce longitudinal error by profiting the vehicle's heading variation and slower speeds at intersections, which contribute to more accurate results. In fact, many studies performed an evaluation on residential road drive with intersections to compensate the VO drift after each turn [12–16]. This leaves a research gap of what will happen if the road network is a stretch of long straight road, with higher speed, or without intersections? Thus, a new strategy is required to address the longitudinal problem in such road condition.

Zeng *et al.* in [17] also presented a curve matching method whereby they performed curve comparison from GPS data with map roads. However, since GPS data typically contains noise – unless a high precision GPS device is used – it needs to be filtered to obtain a functional curve for comparison. Hence, using a similar concept, we utilise curve comparison with the road network on the
<figure><img src="image_4.png" /><figcaption>**Fig. 1** Localisation positioning error</figcaption></figure>

map but by using VO trajectory with the assumption that the VO trajectory curve often complies with road curve. Besides, by VO curve comparison, we can build a complimentary system to the lane markings detection that has disadvantages of obstructing objects, markings clarity and vehicles with high speeds that might not be able to capture the images clearly.

Our proposed approach is an extension of our previous idea in [18] where VO was integrated with noisy GPS by particle filter (PF) and road information obtained from OpenStreetMap (OSM) was used to manipulate particles weight that falls within the road boundary. Before, we only employed road information from OSM to obtain road width and mainly improved the lateral error. However, this time we also adopted VO trajectory curve comparison of the last few poses with road segments within searching radius aiming to evaluate the improvement of longitudinal localisation. Our method assigns probability weights to each road segment within search boundary as a factor for particles weight update to obtain a more accurate localisation and evaluate its positioning error. Different noise models are applied to the GPS data to test the robustness of our localisation. The details of our proposed method are explained in the next section.

# 2 Vehicle localisation

## 2.1 Positioning error

Vehicle localisation performance in accuracy and precision is generally measured by positioning error analysis which can be a relative distance error, heading error, lateral error, and longitudinal error. Distance error is the relative distance difference between ground truth data and fusion output while heading error is its angle difference. In order to reduce the distance error, we need to minimise the lateral and longitudinal errors.

The position difference in the lateral axis (left/right of the object) is denoted as a lateral error and longitudinal error is the distance difference in the longitudinal axis, which makes the localisation to appear ahead or behind the ground truth data (Fig. 1). While lateral error can be minimised by considering the road width factor, the longitudinal error remains difficult to be reduced especially for a drive with less heading variation and without intersection, stop or road markings. The principle of error reduction is that there must be a boundary or limit for the error range, like how the lateral error is limited by the road width. Thus, if we can limit the longitudinal range by using segments of the road, the error should also be decreased, with the condition that the true segment has been chosen correctly. For this reason, we propose a data fusion approach by matching the VO curve with road segments within a limited lookup range obtained from the first step of VO and GPS data fusion to find the road segment with the best fit.

## 2.2 System framework

System framework of our proposed method is depicted in Fig. 2. It mainly consists of three types of input for data fusion; a stream of images from camera, noisy GPS and map information. Firstly, the images are processed to generate a trajectory based on VO that detects and tracks moving feature points. It is fused with the GPS data to obtain the global position. Like our previous approach [18], road information such as road type and a number of lanes are extracted from the map database mainly for lateral error correction by road probability distribution factor (RPDF). Road type information determines the road width estimation, hence limiting lateral positioning error. Meanwhile, road lane distribution as depicted in Fig. 3 contributes in fusion level for further improvement in lateral error by probability factor $\alpha$ with value $p$ is pre-set based on trajectory direction. More details on multi-lane road probability distribution are explained in [18] for lane-level localisation.

In this paper, we utilised the output pose of VO and GPS fusion to act as a lookup centre for road segments curve estimation. These segments have a probability assigned based on their distance from the centre and similarity with VO curve. From VO trajectory output, we compared a specified length of latest trajectory curve with road segments from the map as a weighing factor for PF during the next step of data fusion. Then, different types of GPS noise were simulated to observe the performance and robustness of our proposed approach. According to the experiment conducted by Limsoonthrakul et al. [19], GPS signal noise can be divided into three categories: random GPS, shifted/biased GPS, and GPS signal loss and we adopted all types for method validation.

## 2.3 Visual odometry

VO is the displacement estimation of a moving vehicle based on images sequence obtained from stereo or single camera. It was first introduced for Mars Exploration Rovers to overcome trajectory drift produced from wheel odometry [20]. Since then it has been an interesting field of research for robot and vehicle localisation application.

VO is performed by tracking the detected features in sequential image pairs and translates the movement flow into the trajectory. The image pair captured at the time $k$ is referred to as $I_k$ and $I_{k+1}$. In our work, we applied a typical front-end monocular VO with its scale estimated from the ground truth during initialisation. Features from Accelerated Segment Test (FAST) algorithm is used for detecting interest points in the image as introduced by Rosten and Drummond [21] where it utilised 16 contiguous pixels around the candidate point. FAST is recorded to have better performance in detection error and speed compared with scale-invariant feature transform and speeded-up robust features [21–23]. Then, Kanade–
<figure><img src="image_5.png" /><figcaption>**Fig. 2** System framework of proposed approach</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>**Fig. 3** Full trajectory
(a) Single-lane, one-way road, (b) Bi-directional road</figcaption></figure>

Lucas method is adapted for optical flow calculation by estimating the movement from the similar motion of the 3 × 3 pixels patch around the feature point that matches with the previous image $I_{k-1}$ [24]. The translation and rotation information of the displacement is estimated by an essential matrix E based on five-point algorithm solver [25] and Random Sample Consensus is applied to select the fundamental matrix with the most inliers. Essential matrix E is decomposed by using singular value decomposition [26] to acquire the rotation matrix R and translation vector T shown as

$$E = [T]_x R, \qquad (1)$$

where $[T]_x$ is the matrix of image pair translation vector $T_k = [T_{xk}, T_{yk}, T_{zk}]^T$ at a time k, which can be expressed as

$$[T]_x = \begin{bmatrix} 0 & -T_{zk} & T_{yk} \\ T_{zk} & 0 & -T_{xk} \\ -T_{yk} & T_{xk} & 0 \end{bmatrix}, \qquad (2)$$

and the rotation matrix $R_k$ for the image $I_k$ is written as

$$R_k = \begin{bmatrix} \cos \theta_k & -\sin \theta_k & 0 \\ \sin \theta_k & \cos \theta_k & 0 \\ 0 & 0 & 1 \end{bmatrix}. \qquad (3)$$

The heading rotation of the vehicle is denoted as $\theta_k$ and the output trajectory for image sequence at a time k is generated from

$$\begin{bmatrix} x_k \\ y_k \\ z_k \end{bmatrix} = \begin{bmatrix} x_{k-1} \\ y_{k-1} \\ z_{k-1} \end{bmatrix} + \begin{bmatrix} \cos \theta_k & -\sin \theta_k & 0 \\ \sin \theta_k & \cos \theta_k & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} T_{xk} \\ T_{yk} \\ T_{zk} \end{bmatrix}. \qquad (4)$$

With $X_k^T = [x_k, y_k, z_k]^T$, the trajectory generation in (4) can also be rewritten as

$$X_k = X_{k-1} + R_k T_k. \qquad (5)$$

## 2.4 OpenStreetMap

In recent years, digital maps are widely available and easily accessible by anyone, provided by map service providers such as MapQuest, Google Maps, Bing Maps, and OSM. Among these, OSM is the only collaborative platform which has the Open Database License that allows users to freely use, share and modify the data. It is a community driven project where data can be contributed to GPS devices, aerial photography, manual survey or any mean and this data is available for public use. Freely editable OSM feature might cause data glitch in the event of the false edit but it has more advantages than disadvantages.

OSM is an open source platform while other map providers are commercial providers that hold a proprietary license, which restricts certain functions such as map editing for information update by users. Furthermore, these commercial providers only provide a full feature in selected countries. Aside from this, OSM contains detailed information that does not only include road network, but also road types, way distribution, road restrictions, vehicle maximum speed, and a number of lanes. Currently, we have only used road information such as ways connectivity, number of lanes and its direction for this research, but the abundant data provided by OSM creates a bright research prospect in future applications. OSM data structure is based on three core elements:

* **Nodes**: points with a geographic position that are usually used in ways and to specify the point of interest.

* **Ways**: lists of nodes in orders that have at least one tag or is included within a relation.

* **Relations**: ordered lists of one or more nodes, ways and relations and consist of one or more tags to describe geographic or logical relationships between other elements.

**Tags**, consisting ‘k’ (key) and ‘v’ (value) to store metadata about the map object properties, can exist in all elements in OSM. For our localisation method, we filtered out road network based on the tags and other map attributes are omitted to ease computation. These roads are then rendered with Mapnik rendering rules by using Maperitive to obtain roadmap with specific road width. Mapnik style is generally used by OSM to display raster images for the map and it is the most popular server-side 2D map renderer.

Roads filtered from the raw OSM file produces sets of ways constructed by sets of nodes where the distance between nodes is not consistent depending on the road shape. In some cases, the nodes are too distant from each other on a stretch of straight roads. Thus, we set the distance of a minimum node based on a threshold specified by road speed limit that will be explained in the next subsection.
<figure><img src="image_7.png" /><figcaption>Fig. 4 Algorithm for nodes generation in OSM data</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>Fig. 5 Angle parameters from three nodes used for curve matching</figcaption></figure>

## 2.5 Curve matching

Our proposed system utilises curve matching as a measure to compare the similarity between VO trajectory curve and map nodes from OSM. This requires the knowledge on curve length, orientation, and heading variation to optimise longitudinal positioning accuracy. Since OSM nodes are spaced mostly according to its shape linearity, we need to define the distance threshold between nodes to obtain sets of three nodes (node-trio) that has similar length with VO curve. This algorithm is illustrated in Fig. 4 and more details are as follows:

(a) Firstly, the value of *m* is determined to specify the required VO length. Thus, the curve matching is performed on the last *m* poses, $X_{k-m} \sim X_k$ where the duration of the curve fragment, *t* relies on VO's frame rate, *f*.

$$t = \frac{m}{f} \qquad (6)$$

(b) Knowing the vehicle average speed, $\bar{v}$ the distance threshold between nodes, $d_{th}^n$ is calculated as in (7). In order to obtain the distance in metre, vehicle speed (km/h) is converted by multiplying 10³/3600. Since we utilise sets of three nodes to define the OSM curve, the distance threshold between two nodes is divided by 2.

$$d_{th}^n = \frac{\bar{v}t}{2} \times \frac{10^3}{3600} \qquad (7)$$

(c) At the same time, the travelled distance of the VO curve, $d_{th}^{vo}$ within the last *m* poses is computed to find the difference with $d_{th}^n$.

$$d_{th}^{vo} = \frac{1}{2} \sum_{i=k-m}^{k} \sqrt{(x_i - x_{i-1})^2 + (y_i - y_{i-1})^2} \qquad (8)$$

(d) If the difference is negligible, the larger distance threshold is used to minimise new middle nodes generation. Hence, $d_{th} = \max(d_{th}^n, d_{th}^{vo})$. Else, VO trail length is chosen $d_{th} = d_{th}^{vo}$.

(e) With the fusion between GPS and VO, the vehicle position can be estimated on the map and the nodes around the location are found. Distances between nodes from the OSM data are recorded as $d_n$.

(f) Finally, in order to distribute road map probability with longitudinal accuracy optimisation, new nodes are replicated in between of nodes that are too distant with each other. The number of new middle nodes, $n_m$ is obtained by rounding results of nodes distance division with a threshold to the nearest integer

$$n_m = \left\lceil \frac{d_n}{0.5 \times d_{th}} + 0.5 \right\rceil \times d_{th}$$

The length of node-trio curve and VO travelled curve distance is not exactly equal all the time, but the difference is too small (<0.05%) and negligible. To compare the curve similarity between a fragment of VO trajectory curve and nodes of roads on OSM, the computation of curve similarity score, $S_{cs}$ relies on two key parameters: initial orientation $\theta_k^{in}$ and curve heading variation $\theta_k$. Parameters for VO trajectory trail are defined as

$$\theta_k^{invo} = \theta_{k-m}, \qquad (9)$$

$$\vartheta_k^{vo} = \sum_{i=k-m/2}^{k} (\theta_{i+1} - \theta_i). \qquad (10)$$

Three nodes curve from OSM data is illustrated in Fig. 5 and for the *n*’th node found within the searching radius, the initial orientation of the first two nodes $\theta_k^{in(n)}$ and curve change $\theta_k^{(n)}$ are calculated as

$$\theta_k^{in(n)} = \arctan\left(\frac{\Delta y_{0,1}^{(n)}}{\Delta x_{0,1}^{(n)}}\right), \qquad (11)$$

$$\vartheta_k^{(n)} = \arctan\left(\frac{\Delta y_{1,2}^{(n)}}{\Delta x_{1,2}^{(n)}} - \theta_k^{in(n)}\right) \qquad (12)$$

With these parameters, the curve similarity score $S_{cs}$ is obtained from the difference of initial orientation $\theta_k^{in}$ between nodes and VO curve and its heading variation $\vartheta_{theta_k}$ which can be written as

$$S_{cs} = \Delta\theta_k^{in(n)} \times \Delta\theta_k^{(n)} \qquad (13)$$

The curve matching steps from VO trajectory curve fragment to surrounding nodes are illustrated in Fig. 6, where it starts from

(a) The extraction of VO trajectory trail since the last *m* poses, followed by

(b) Connected nodes in ways are detected within a searching area from lookup centre.

(c) Then, the nodes are paired with neighbouring nodes to form ‘node-trios’. All sets found within the area are compared with the VO trajectory curve to obtain the most similar road segment to be matched.

(d) Finally, node-trio with the highest similarity score is identified and the road segment contains the highest probability that would affect particles’ importance weight during data fusion.

This approach allows the curve matching to be conducted in small fragments to obtain the most likely candidate of the road segment and also reduce computational cost. This is because by assessing only small fragments of trajectory within short travel
<figure><img src="image_9.png" /><figcaption>Fig. 6 VO and OSM node-trio curve matching steps</figcaption></figure>

<figure><img src="image_10.png" /><figcaption>Fig. 7 Double bend road in drive 34 with 2 s of VO trajectory in quadratic curve appended in the small box</figcaption></figure>

periods, the resulted curve is limited to second-order polynomial regardless of the overall road curve complexity. Generally, only double bend roads (as in Fig. 7) will result in third-order polynomial curve or above. Still, since there is a minimum radius limit for each road curve [27] and we are only assessing the last 2 s of pose sequence, the trajectory curve will not be long enough to exhibit third-order polynomial curve.

## 2.6 Data fusion

The input from VO, noisy GPS and OSM are fused by using PF in our proposed approach. The fundamental principle of PF is to use random samples (particles) to represent the posterior density of vehicle position in a dynamic state estimation framework such as road map information. It estimates the instate-space model's inference by expressing the posterior probability density in terms of random particles associated with importance weights. The filtering consists of several steps:

(a) *Initialisation*: At the time $k = 0$, $N$ particles are sampled around $X_0$ with equal weight $w_0^{(i)}$.

$$X_0^{(i)} \sim p(X_0), \qquad (14)$$

$$w_0^{(i)} = 1/N. \qquad (15)$$

(b) *Pose estimation*: For $i = 1, \dots, N$, particles are displaced based on VO trajectory vector. The displacement distance is denoted by $d_k$ and $\theta_k$ is VO heading rotation. Hence the position estimation is calculated as

$$\begin{bmatrix} x_k^{(i)} \\ y_k^{(i)} \\ \zeta_k^{(i)} \end{bmatrix} = \begin{bmatrix} x_{k-1}^{(i)} \\ y_{k-1}^{(i)} \\ \zeta_{k-1}^{(i)} \end{bmatrix} + d_k \begin{bmatrix} \cos \theta_k \\ \sin \theta_k \\ 0 \end{bmatrix} \qquad (16)$$

(c) *Weight update*: For a particle $i$, the importance weight update is based on its distance from GPS data $\beta_k^{(i)}$, position in/outside the road $\kappa_k^{(i)}$, and way candidate score $\gamma_k^{(i)}$ obtained from curve similarity $S_{cs}$. Aside from curve similarity, $\gamma_k^{(i)}$ is also determined by its

distance from the lookup centre $X_k^{(C)}$ to find the most suited nearby candidate by probability density function. The explanation for each factor is described below:

i. The relative distance of the particle $i$ from noisy GPS is measured as $d_{GPS}^{(i)}$, and it will exponentially affect the importance weight of the particle by noisy GPS factor $\beta_k^{(i)}$. This is obtained from an exponential distribution to indicate that the factor decreases with an increasing distance of the particle from the GPS position

$$\beta_k^{(i)} = e^{-(1/2)d_{GPS}^{(i)}}. \qquad (17)$$

ii. The road probability factor is defined as $\alpha = p$ when the particle is within road area that has the same direction with VO heading direction, and $\alpha = 1 - p$ if the particle falls on the road in the opposite direction (if any). For instance, for a simple single lane bi-directional road, $p$ is set as 0.8. As for the particles outside the road area, road probability factor is reduced exponentially by its distance. Thus, the factor $\kappa_k^{(i)}$ is calculated as

$$\kappa_k^{(i)} = \begin{cases} \alpha, & \text{if } X_k^{(i)} \text{ is inside road area,} \\ \alpha e^{-(1/2)d_k^{(i)}}^2, & \text{otherwise} \end{cases} \qquad (18)$$

where $d_k^{(i)}$ is the distance between the particle $x_k^{(i)}$ and the road boundary.

With the value of the factor in (17) and (18), these are multiplied with the current particle weight $w_{k-1}^{(i)}$ to obtain an updated weight after considering its position on the road map and distance from GPS. Then, the particle with the highest weight is selected as the best candidate to become the lookup centre, where its position is recorded as $X_k^{(C)}$.

$$X_k^{(C)} = \arg \max_{x_k^{(i)}} w_{k-1}^{(i)} \beta_k^{(i)} \kappa_k^{(i)} \qquad (19)$$

iii. From the lookup centre $X_k^{(C)}$, candidate ways are searched within a specified radius. From the candidate ways found in the searching area, the ways candidate score $\gamma_k^{(n)}$ is estimated based on its distance from the lookup centre $X_k^{(C)}$ and curve similarity, $S_{cs}$, is measured by the difference in key parameters between VO and OSM curves as shown in (13). Smaller $S_{cs}$ indicates a higher similarity between both curves and this will yield an increased probability of the way candidate score, $\gamma_k^{(n)}$. Assuming $d_c$ is the distance between $X_k^{(C)}$ and node $n$, $\gamma_k^{(n)}$ is calculated from Gaussian distribution as

$$\gamma_k^{(n)} = \frac{1}{\sqrt{2\pi}} e^{-(1/2)(d_c S_{cs})^2}. \qquad (20)$$

From the parameters, we will be able to obtain the best road segments candidates especially on curved roads or when the vehicle moves onto an intersection. It is still difficult to optimise longitudinal error on a straight road without heading
variation and intersections. However, our fusion technique will at least limit the longitudinal error from the probability distribution of candidate ways within the lookup range. This is illustrated in Fig. 8.

iv. Lastly, particle weight is updated by multiplying the way candidate score from the particle with other factors:

$$w_k^{(i)} = w_{k-1}^{(i)} \rho_k^{(i)} \kappa_k^{(i)} \frac{\gamma_k^{(n)}}{\sqrt{2\pi}}. \quad (21)$$

(d) Resampling: Before resampling, the particle weight is normalised from the sum of all particle weights as

$$\tilde{w}_k^{(i)} = \frac{w_k^{(i)}}{\sum_{i=1}^{N} w_k^{(i)}}. \quad (22)$$

To determine the suitable condition for resampling, the number of effective sampling sizes is calculated as

$$N_{ess} = \frac{1}{\sum_{i=0}^{N} (\tilde{w}_k^{(i)})^2}, \quad (23)$$

and when the effective sampling size is less than a certain threshold $N_{ess} < N_{thres}$, particles will be resampled. Those with lower importance weights are removed and replaced by duplicating equally distributed particles in the proximity of particles with higher weights. In order to overcome filtering degeneracy problem where only a few of the particles will have a significant weight, the threshold is set to balance multilevel fusion while at the same time does not eliminate the possible candidate particles. Therefore, for n level of fusion, we set the threshold as $N_{thres} \approx N/e^n$. Here, since we used N = 1000 for the particles size and four levels are applied for fusion (GPS distance, RPDF, distance from fusion output, and curve similarity score), our $N_{thres}$ value is 25.

## 3 Method validation

Our proposed method is validated by using KITTI dataset for localisation by VO. In this paper, our study is focused on longitudinal error correction so we analyse the localisation performance in two types of drives – highway and residential areas. Then, to test the localisation performance against noisy GPS, different noise models are applied in both drives – the extreme random noise of 10 m, biased GPS noise and lost GPS signal.

In our study, we implemented m = 20, which means only the last 20 poses (2 s of VO trajectory with a frame rate of 10 Hz) is considered as the curve fragment. The curve length will differ according to vehicle travel speed and this corresponds to the node-trios that are distanced based on threshold calculation from vehicle average speed or VO curve. In the case of VO trajectory with severe scale ambiguity, speed can be estimated from the road speed limit provided by OSM data. The pre-set m value was determined based on trade-off consideration between accuracy and computation cost. In order to obtain an accurate longitudinal position, the road segments should be as short as possible.

However, if we set m = 10 (last 1 s of VO trajectory), most of the resulting segments would be mostly found as straight trajectory trail. For instance, assuming the vehicle moves at 30 km/h, 1 s of displacement is only about 8.3 m in length. If it is a curve road, considering the least degree of a significant curve is 10°, this would result in a minimum radius of 55.6 m for the road. However, according to [27], the minimum road radius with a speed limit of 30 km/h is 60 m. Therefore, this would be sufficient for the curve analysis where the minimum curve length would be 10.5 m. Besides, this would degrade the efficiency of finding the best road segment candidate with the lack of information in a vehicle heading change. Hence, we concluded that 2 s of travel distance is ample to obtain accurate curve segment for analysis. On the other hand, if the m value is increased to 40, the trajectory curve length is longer and the distance between nodes will increase. As a result, longitudinal positioning would be less accurate with longer road segment containing similar probability distribution.

<figure><img src="image_11.png" /><figcaption>Fig. 8 Probability distribution of candidate ways from fusion output as lookup centre</figcaption></figure>

As for the search area size, candidate ways are searched within a specified radius of about 30 m from the lookup centre $X_k^{(C)}$ since we have an approximate location from the noisy GPS of up to 10 m accuracy. At the same time, we need to consider the possible bias or blunders, therefore an additional 20 m as a buffer zone is also considered in this study. Further distance is not considered since it might result in unnecessary road segment probability appearance in the ambiguous road network. Besides, expanding the search area to a larger size would only increase computational cost with a greater number of nodes found in the area for probability calculation. Therefore, we fixed it to 30 m while observing the effects of lost GPS signal on localisation performance.

### 3.1 KITTI dataset

KITTI dataset provided by Geiger et al. [28] consists of image sequences for VO with frame rate of 10 fps, data from Velodyne sensor (not utilised) and RTK-GPS data treated as ground truth for validation. In our study, we utilised two types of datasets where the first dataset consists of a highway drive with an instance of exiting the highway into a diverging road at the speed of around 60 km/h. The second dataset is a drive recorded in a residential area with many intersections. Trajectories for both tests are shown in Fig. 9 and zoomed trajectories with road segments probability distribution represented by nodes radius size are depicted in Fig. 10.

### 3.2 Noisy GPS

A system cannot be proved to be robust unless it has been tested under extreme conditions. A study on data fusion performance under different GPS noise conditions for both lateral and longitudinal errors was conducted by Limsoonthrakul et al. in [19]. The proposed localisation method integrates data from the camera sensor, GPS and wheel encoder. A camera is used to classify road from the image and GPS noise is manipulated to observe its effect on the data fusion trajectory. As expected, the lateral error is reduced significantly owing to the road classification while the longitudinal error was only moderately reduced. Overall performance comparison is made between Kalman filter (KF) and PF where the results show that PF performs better especially when GPS error is high and during the signal outage. This motivates us to develop a localisation method that can withstand various noise types while improving the localisation error.

To test the robustness of our system in different noise scenarios, we chose to model different types of noisy GPS as one of the fusion inputs during method validation. By doing that, we were able to evaluate the response of our filter on the common noises we found in low cost GPS in different environmental conditions. To achieve this, random noise was added to the RTK-GPS data which was down sampled to 12 Hz of frequency. Albeit GPS signal noise varies based on the receiver device specification and the surrounding environment, it generally can be characterised as a combination of flicker noise and white noise [29]. Thus, we initiated the noise with specific bias and variance with a 2D
<figure><img src="image_12.png" /><figcaption>**Fig. 9** Full trajectory
(a) Highway drive, (b) Residential drive</figcaption></figure>

Gaussian distribution that represents the white noise and a coherent Perlin noise was added to simulate the flickering noise behaviour. The noise is generated from (24) below. Assuming that $[\tilde{x}_k, \tilde{y}_k, \tilde{z}_k]^T$ is the ground truth position, noisy GPS $[\ddot{x}_k, \ddot{y}_k, \ddot{z}_k]^T$ is defined as

$$ \begin{bmatrix} \ddot{x}_k \\ \ddot{y}_k \\ \ddot{z}_k \end{bmatrix} = \begin{bmatrix} \tilde{x}_k \\ \tilde{y}_k \\ \tilde{z}_k \end{bmatrix} + \epsilon_G \begin{bmatrix} \cos \theta_p \\ \sin \theta_p \\ 0 \end{bmatrix} \quad (24) $$

where $\epsilon_G \sim N(\mu, \sigma^2)$ is a random Gaussian noise with mean $\mu$ and standard deviation $\sigma$. This Gaussian noise $\epsilon_G$ acts as the white noise amplitude and $\theta_p$ is generated from Perlin noise.

GPS noise can be caused by three main error sources which are noise, bias and blunders as depicted in Fig. 11 [30]. Noise error is the most common source that is the combined effect of pseudo random noise (PRN) with the noise within the receiver itself. In our validation, this noise error was implemented with random noise that has zero bias and extreme variance of up to 10 m, similar to a low-cost GPS receiver accuracy.

Meanwhile, bias error in GPS can be a result from Selective Availability (SA) or some other factors. SA is the intentional degradation of the standard positioning service (SPS) signals by a time varying bias that introduces delta error resulted from dithering the satellite clock. Since SA is a changing bias with low frequency (excess of a few hours), position solutions or individual SV pseudo-ranges cannot be effectively averaged over periods shorter than a few hours. Other factors that result in bias error include

* Ephemeris data errors that contain imprecise orbital data.

* Tropospheric delays which are affected by the atmospheric pressure, temperature and humidity experienced in the troposphere layer of the atmosphere.

* Unmodelled ionosphere delays where the ionosphere layer bends the GPS radio signal and changes its speed, and

* Multipath effect caused by reflected signals from surfaces near the receiver. This source is difficult to detect and avoid.

<figure><img src="image_13.png" /><figcaption>**Fig. 10** Fusion output trajectory during road divergence
(a) Zoomed trajectory, (b) Road segments probability distribution</figcaption></figure>

<figure><img src="image_14.png" /><figcaption>**Fig. 11** GPS error sources</figcaption></figure>

We simulated the bias error by shifting the GPS position 5 m in a global lateral position from the ground truth and with the same 10 m variance. This would result in a higher tendency of events where the GPS data falls into the road with the wrong direction or outside the road boundary.

Lastly, blunders are also one of GPS error sources where it can result in hundreds of meters of error due to user mistakes, including incorrect geodetic datum selection. This kind of large error can also be resulted from out of date GPS data due to signal unavailability due to blockades. To model this noise, the GPS data is turned off for a specific time range, so the localisation mainly relies on information from VO and map.

## 3.3 Results

Localisation performance is evaluated from the lateral and longitudinal errors of both datasets, comparing the performance of the proposed method as presented in this paper with the conventional fusion of GPS and VO by PF and our previous method without curve matching but with RPDF approach. Positioning error results for both highway and residential datasets are shown in Figs. 12 and 13, respectively.

### 3.3.1 Highway drive: For the highway drive during GPS with random noise (both unbiased and biased cases), it can be seen from Figs. 12a and b that the longitudinal error is improved significantly in the first 70 frames when the vehicle is driven in a straight road and facing road divergence. Regardless of the bias applied to the GPS noise, our method proved to be unaffected by the noise and able to select the best candidate road segment that matches with the true trajectory. As for the lateral positioning error, there is a brief improvement in random noise case especially during the road divergence in between frames 45 and 55, and it also shows that the lateral error tested in biased noise is significantly improved, similar to its longitudinal error.

Meanwhile, in the case of a lost GPS signal, as depicted in Fig. 12c, our previous approach without road segment curve matching suffered horrendous positioning error. In fact, the localisation failed to relocate its position even after the GPS signal is recovered, as can be seen from the results due to the travelled distance being too far from the ‘lost’ particle cloud. However, our proposed approach maintained minimal error owing to the curve matching of VO trajectory and road segments that enabled the localisation to detect the candidate road segment with the most probability. The longitudinal error result shows that our method
<figure><img src="image_15.png" /><figcaption>**Fig. 12** Lateral and longitudinal localisation positioning errors in highway drive for different noises</figcaption></figure>

(a) GPS with random noise, (b) GPS with biased noise, (c) GPS signal loss

managed to maintain an error below 5 m for the whole drive as opposed to the error of the previous method that could reach up to 20 m for both lateral and longitudinal errors.

### 3.3.2 Residential drive
Our second test was conducted on a longer drive at a residential area and error results are shown in Figs. 13a-c. During the random noise test, it did not show remarkable improvement in positioning error since both RPDF and our proposed method had the similar basic approach in localisation and there was no case of severe GPS error that could affect path selection. Since the vehicle speed in the residential drive was lower and the intersections were easily distinguished, the localisation showed a consistent performance even without the curve matching method. However, in overall, our proposed approach still achieved a smaller error compared with other methods in some instances where the RPDF longitudinal error peaked over 5 m.

In the biased error test for the residential drive, the results as displayed in Fig. 13b is particularly interesting for the amount of error degradation in the result of the method without curve matching in three events. The first event was during frames 70-120 when the vehicle moved along a straight road and there was a junction on the right where most GPS points were positioned due to the bias applied, the previous method assumed that the vehicle turned into the junction. While the second event happened around frame 520 to 590, where the vehicle was moving on a sharp curve that resembled making a U-turn twice, thus resulting the biased GPS falling into the previous lane. As a result, the previous approach mistakenly concluded that the vehicle made a U-turn while the approach proposed in this paper managed to cope with the situation by integrating the updated VO curve with road curve fragments for all occurrences.

Lastly, we simulated an instance of GPS signal loss to the residential drive and the results obtained are shown in Fig. 13c. The GPS data was turned off between frame 400 and 700 when the vehicle was moving along a curved road with multiple intersections on its sides, turning into an intersection and it travelled on a sharp curve resembling a U-turn. As shown in the error graph, the recent approach successfully achieved a consistent low positioning error and was unaffected by this lack of data compared with the results

<figure><img src="image_16.png" /><figcaption>**Fig. 13** Lateral and longitudinal localisation positioning errors in the residential drive for different noises</figcaption></figure>

(a) GPS with random noise, (b) GPS with biased noise, (c) GPS signal loss

of previous fusion methods. The RPDF method suffered severe error of over 40 m during the signal loss, but it was able to recover the localisation after two attempts. This is due to the slower vehicle speed and road curves, thus the travelled distance during GPS signal loss was still reachable by the particle cloud.

### 3.3.3 Comparison with other VO methods
To study positioning accuracy after data fusion and curve matching, we compared the lateral and longitudinal error of the trajectory results generated from several VO methods – Libviso2 (monocular and stereo) [31], ORB-VO [32] and ORBSLAM2 [32]. Note that these methods are purely based on VO, except for ORBSLAM2 that also performs mapping and loop closure for drift correction. Among these, ORBSLAM2 had recorded the best performance in
<table><caption>**Table 1 Positioning error comparison with other VO works**</caption><thead><tr><th rowspan="2">Method</th><th colspan="2">Drive 34 (residential)</th><th colspan="2">Drive 42 (highway)</th></tr><tr><th>ε<sub>lat</sub>, m</th><th>ε<sub>lon</sub>, m</th><th>ε<sub>lat</sub>, m</th><th>ε<sub>lon</sub>, m</th></tr></thead><tbody><tr><td>Libviso2 (M)</td><td>37.24 ± 62.62</td><td>23.91 ± 64.44</td><td>9.24 ± 41.48</td><td>10.34 ± 41.19</td></tr><tr><td>Libviso2 (S)</td><td>6.45 ± 19.93</td><td>6.83 ± 19.68</td><td>3.09 ± 18.71</td><td>3.00 ± 18.7</td></tr><tr><td>ORB-VO</td><td>2.68 ± 12.51</td><td>2.52 ± 12.49</td><td>3.90 ± 15.74</td><td>3.93 ± 15.86</td></tr><tr><td>ORBSLAM2</td><td>1.77 ± 6.66</td><td>2.22 ± 6.62</td><td>2.31 ± 14.83</td><td>2.38 ± 14.81</td></tr><tr><td>ours (random)</td><td>1.40 ± 1.06</td><td>1.37 ± 1.03</td><td>2.05 ± 1.62</td><td>2.08 ± 1.58</td></tr><tr><td>ours (bias)</td><td>1.66 ± 1.23</td><td>1.76 ± 1.32</td><td>2.27 ± 1.76</td><td>2.45 ± 1.89</td></tr><tr><td>ours (lost GPS)</td><td>1.41 ± 1.37</td><td>1.30 ± 1.48</td><td>1.34 ± 0.93</td><td>1.42 ± 1.25</td></tr></tbody></table>

<figure><img src="image_17.png" /><figcaption>**Fig. 14 Error summary for**
(a) Highway drive, (b) Residential drive</figcaption></figure>

translation error of only 1.15% that provides open source code for VO without fusion with LIDAR. The comparison of positioning error for each method with our proposed approach is presented in Table 1.

**3.3.4 Results summary:** The results of distance and positioning error for the compared approaches are summarised in Fig. 14 for both datasets and all noise simulations. In the random noise test, both PF and RPDF approaches suffered a larger error due to the lack of curve comparison, thus particles weight from data fusion was mainly affected by the GPS noise. This reduced the localisation accuracy for these approaches particularly during road divergence as can be seen from Fig. 12a. RPDF method shows a lower average in lateral error but due to the inaccurate localisation on road divergence, high error peaks resulted in higher error variation in comparison to standard PF approach which has higher mean and slightly less variation. However, our proposed approach for the highway drive shows significant improvement in error reduction to (2.05 ± 1.62) m in lateral error and (2.08 ± 1.58) m in longitudinal error as a result of successful path selection in diverging road and more accurate localisation. On the other hand, localisation performance in residential drive shows less significant improvement for random GPS noise because the vehicle speed is lower and it has less probability of resulting in wrong path selection.

Meanwhile, we managed to achieve further improvement in biased noise where both datasets were able to reduce the lateral error of (2.27 ± 1.76) m and (1.66 ± 1.23) m, respectively, for highway and residential drive, while the longitudinal error is reduced to (2.45 ± 1.89) m and (1.76 ± 1.32) m, respectively. Due to the GPS bias, PF method suffered worse positioning error where the average error almost doubled up to over 6 m from random GPS

test. However, RPDF method managed to decrease the error to below 4.2 m and our proposed approach with curve matching further improved positioning accuracy to <2.5 m in both lateral and longitudinal errors. This is owed to the final stage of data fusion that increased particles weight around lookup centre $X_k^{(C)}$ which limited the positioning error regardless of noise bias condition.

Finally, for the lost GPS test, positioning error with PF reached over 10 m and the RPDF method error was worse as a result of failed path recognition from VO + GPS fusion and persistent road probability distribution which continued to localise using the outdated GPS data during the absence of signal. However, our proposed method has an additional input which is the road curve matching that enabled localisation correction when GPS data is found outdated. The localisation performance was maintained with its average positioning error constantly below 2 m with best longitudinal performance of (1.43 ± 1.25) m for highway drive. This shows an interesting outcome where the error was smaller with the absence of noisy GPS data because the fusion output was mainly influenced by VO trajectory curve and road information from the OSM.

Performance comparison with other VO methods also shows improved positioning accuracy for both lateral and longitudinal. Regardless of the noise existence in GPS data, we achieved less error than other VO methods except for biased GPS test on drive 42 (highway road) where ORBSLAM2 performs slightly better. This is due to the effect of extreme GPS noise with bias that results in a tendency of biased localisation compared with highly accurate SLAM approach without noise input. However, our tests with noisy GPS are simulating the worst case scenario and even so, the positioning accuracy does not degrade much.

## 4 Conclusion

A method with data fusion of VO, GPS and digital map for vehicle localisation has been proposed to reduce longitudinal positioning error that has yet to be addressed carefully in this research area. It is a challenging task to ensure the longitudinal position is not affected by GPS noise and VO drift, thus our strategy was to match the VO trajectory trail with the road segments curve to find the most likely candidate path.

Segments matching reduced the computation complexity while increasing the probability of suitable candidate ways based on its curve similarity score. This resulted in a localisation system that could withstand various types of GPS noise and VO drift with the overall mean error <3 m and longitudinal error as low as 1.43 m. The longitudinal accuracy and precision were increased in our proposed approach compared with methods without curve matching, proving that the segmentation concept is able to compensate the longitudinal error. Not only that, it can also increase road selection accuracy immediately for swift path planning.

This method gives greater improvement in curved roads when compared to straight roads. However, the longitudinal error in both cases can be limited based on the weight distribution of the nearest candidate ways from the lookup centre. Later, the longitudinal drift can immediately be corrected whenever there is a slight heading variation of VO trajectory matched with road segments.

With the good performance in GPS signal loss during road diversion, we believe the localisation accuracy can be improved if
the GPS signal error is not too severe (variance <10 m). Besides, the dependency of fusion on GPS data can also be reduced to minimise the effect of GPS noise by only referring to the information during initialisation to estimate starting point and at a later time with less referral frequency. Although our approach requires additional computation for data fusion and curve matching, it is designed as a complementary system to the existing localisation by GPS. This is because there is still a possibility where VO cannot perform well due to lighting and weather conditions that would still require information from GPS and digital map for localisation. Therefore, to ensure robust and localisation, data fusion is indispensable.

This localisation method can be further improved with the use of lane or traffic sign detections. As discussed, our VO curve matching method is a complementary system for longitudinal position improvement in addition to the existing methods proposed by other researchers. It is desirable to fuse all the available information to obtain more accurate longitudinal localisation in the future.

## 5 References

[1] Kos, T., Markezic, I., Pokrajcic, J.: 'Effects of multipath reception on GPS positioning performance'. Proc. ELMAR-2010, Zadar, Croatia, September 2010, pp. 399–402

[2] Zair, S., Le Hegarat-Mascle, S., Seignez, E.: 'A-contrario modeling for robust localization using raw GNSS data', *IEEE Trans. Intell. Transp. Syst.*, 2016, **17**, (5), pp. 1354–1367

[3] Hata, A.Y., Osorio, F.S., Wolf, D.F.: 'Robust curb detection and vehicle localization in urban environments'. 2014 IEEE Intelligent Vehicles Symp. Proc, Dearborn, MI, 2014, pp. 1257–1262

[4] Kim, D., Chung, T., Yi, K.: 'Lane map building and localization for automated driving using 2D laser rangefinder'. 2015 IEEE Intelligent Vehicles Symp., Seoul, South Korea, 2015, pp. 680–685

[5] Jeon, D., Choi, H.: 'Multi-sensor fusion for vehicle localization in real environment'. 2015 15th Int. Conf. Control, Automation and Systems, Busan, South Korea, 2015, pp. 411–415

[6] Gu, Y., Hsu, L-T., Kamijo, S.: 'Passive sensor integration for vehicle self-localization in urban traffic environment', *Sensors*, 2015, **15**, (12), pp. 30199–30220

[7] Brubaker, M.A., Geiger, A., Urtasun, R.: 'Map-based probabilistic visual self-localization', *IEEE Trans. Pattern Anal. Mach. Intell.*, 2016, **38**, (4), pp. 652–665

[8] Lu, W., Seignez, E., Rodriguez, F.S.A., et al.: 'Lane marking based vehicle localization using particle filter and multi-kernel estimation'. 2014 13th Int. Conf. Control Automation Robotics & Vision, Singapore, 2014, pp. 601–606

[9] Schreiber, M., Hellmund, A.M., Stiller, C.: 'Multi-drive feature association for automated map generation using low-cost sensor data'. 2015 IEEE Intelligent Vehicles Symp. (IV), Seoul, South Korea, 2015, pp. 1140–1147

[10] Tao, Z., Bonnifait, P., Frémont, V., et al.: 'Mapping and localization using GPS, lane markings and proprioceptive sensors'. 2013 IEEE/RSJ Int. Conf. Intelligent Robots and Systems, Tokyo, Japan, 2013, pp. 406–412

[11] Suhr, J.K., Jang, J., Min, D., et al.: 'Sensor fusion-based low-cost vehicle localization system for complex urban environments', *IEEE Trans. Intell. Transp. Syst.*, 2017, **18**, (5), pp. 1078–1086

[12] Yu, Y., Zhao, H., Davoine, F., et al.: 'Monocular visual localization using road structural features'. 2014 IEEE Intelligent Vehicles Symp. Proc., Dearborn, MI, 2014, pp. 693–699

[13] Bak, A., Gruyer, D., Bouchafa, S., et al.: 'Multi-sensor localization – visual odometry as a low cost proprioceptive sensor'. 2012 15th Int. IEEE Conf. Intelligent Transportation Systems, Anchorage, AK, 2012, pp. 1365–1370

[14] Alonso, I.P., Llorca, D.F., Gavilan, M., et al.: 'Accurate global localization using visual odometry and digital maps on urban environments', *IEEE Trans. Intell. Transp. Syst.*, 2012, **13**, (4), pp. 1535–1545

[15] Schreiber, M., Königshof, H., Hellmund, A.M., et al.: 'Vehicle localization with tightly coupled GNSS and visual odometry'. 2016 IEEE Intelligent Vehicles Symp. (IV), Gothenburg, Germany, 2016, pp. 858–863

[16] Marouane, C., Maier, M., Leupold, A., et al.: 'Visual odometry using motion vectors from visual feature points'. 2016 Int. Conf. Indoor Positioning and Indoor Navigation (IPIN), Alcalá de Henares, Spain, 2016, pp. 1–8

[17] Zeng, Z., Zhang, T., Li, Q., et al.: 'Curvedness feature constrained map matching for low-frequency probe vehicle data', *Int. J. Geogr. Inf. Sci.*, 2016, **30**, (4), pp. 660–690

[18] Awang Salleh, D.N.S.D.A., Seignez, E.: 'Monocular visual odometry with road probability distribution factor for lane-level vehicle localization'. 2016 14th Int. Conf. Control, Automation, Robotics and Vision (ICARCV), Phuket, Thailand, November 2016, pp. 1–6

[19] Limsoothrakul, S., Dailey, M.N., Parnichkun, M.: 'Intelligent vehicle localization using GPS, compass, and machine vision'. 2009 IEEE/RSJ Int. Conf. Intelligent Robots and Systems, St. Louis, MO, 2009, pp. 3981–3986

[20] Cheng, Y., Maimone, M.W., Matthies, L.: 'Visual odometry on the Mars exploration rovers – a tool to ensure accurate driving and science imaging', *IEEE Robot. Autom. Mag.*, 2006, **13**, (2), pp. 54–62

[21] Rosten, E., Drummond, T.: 'Machine learning for high-speed corner detection', 9th European Conf. Computer Vision – Volume Part I (ECCV’06), Berlin, Heidelberg, 2006, pp. 430–443

[22] Rosten, E., Drummond, T.: 'Fusing points and lines for high performance tracking'. Tenth IEEE Int. Conf. Computer Vision (ICCV’05), Beijing, China, 2005, vol. 1, pp. 1508–1515

[23] Guerrero, M.: 'A comparative study of three image matching algorithms: sift, surf, and fast'. PhD thesis, Utah State University, 2011

[24] Tomasi, C., Kanade, T.: 'Detection and tracking of point features', *Int. J. Comput. Vis.*, 1991, **9**, (3), pp. 137–154

[25] Nister, D.: 'An efficient solution to the five-point relative pose problem', *IEEE Trans. Pattern Anal. Mach. Intell.*, 2004, **26**, (6), pp. 756–770

[26] Richard, H., Zisserman, A.: 'Multiple view geometry in computer vision' (Cambridge University Press, Cambridge, 2004, 2nd edn.)

[27] Kilinc, A.S., Baybura, T.: 'Determination of minimum horizontal curve radius used in the design of transportation structures, depending on the limit value of comfort criterion lateral jerk'. FIG Working Week, Rome, Italy, May 2012

[28] Geiger, A., Lenz, P., Stiller, C., et al.: 'Vision meets robotics: The KITTI dataset', *Int. J. Robot. Res.*, 2013, **32**, (11), pp. 1231–1237

[29] Mao, A., Harrison, C.G.A., Dixon, T.H.: 'Noise in GPS coordinate time series', *J. Geophys. Res., Solid Earth*, 1999, **104**, (B2), pp. 2797–2816

[30] Huston, C.D., Cornish, D.J.: 'Method and apparatus for calibration of a GPS receiver'. U.S. Patent No. 5, 751, 244, 12 May 1998

[31] Geiger, A., Ziegler, J., Stiller, C.: 'Stereoscan: dense 3d reconstruction in real-time'. IEEE Intelligent Vehicles Symp. (IV), Baden, Germany, June 2011, pp. 963–968

[32] Mur-Artal, R., Tardós, J.D.: 'ORB-SLAM2: an open-source SLAM system for monocular, stereo, and RGB-D cameras', *IEEE Trans. Robot.*, 2017, **33**, (5), pp. 1255–1262