Print Article | NAVIGATION: Journal of the Institute of Navigation

Abstract

Reliable vehicle localization in global navigation satellite system (GNSS)-denied urban environments remains challenging owing to inertial drift during extended outages. This paper introduces waypoint-informed localization (WiL), a framework that leverages digital map information and onboard sensors to maintain navigational accuracy. The system employs the onboard diagnostic II (OBD-II) velocity integration framework (OVIF), which combines OBD-II vehicle speed with estimates from a semantic-aware stereo vision-based navigation system. During GNSS outages, WiL derives position and azimuth pseudomeasurements by integrating a vehicle kinematic model with two-dimensional road geometries, specifically road centerlines and edges. These estimates serve as aiding measurements for an extended Kalman filter, thereby bounding errors of a three-dimensional reduced inertial sensor system. Experimental validation demonstrates that the WiL-OVIF system achieves a 74.6% reduction in distance-traveled error compared with baseline methods, maintaining a localization error of 0.128% over a 2.8-km GNSS outage.

Keywords

NOMENCLATURE

View this table:

1 INTRODUCTION

Accurate and reliable vehicular localization is essential for optimizing traffic flow, reducing congestion, and improving road safety. With the global vehicle fleet exceeding 2 billion units (Gross, 2016), demand for robust navigation solutions continues to rise. Investments in advanced positioning and navigation systems are expected to reach $46.75 billion by 2030 (Siuhi & Mwakalonge, 2016), driving innovations in intelligent vehicular technologies.

These advancements enable applications ranging from smartphone-based navigation to the precise positioning required for advanced driver-assistance systems (ADASs) and autonomous driving (AD) technologies (Cheng et al., 2024). Accurate digital maps and real-time traffic data, as provided by services such as Google Maps, aid route planning and congestion mitigation (Chen et al., 2015). Standalone satellite navigation systems remain vital, especially in areas with limited connectivity. Additionally, ADAS features such as lane-keeping assistance and adaptive cruise control depend on high-fidelity positioning and navigation data to enhance vehicle safety (Ragab et al., 2025).

A significant challenge in current navigation systems stems from their reliance on global navigation satellite systems (GNSSs), which are vulnerable to signal attenuation and multipath effects in environments such as tunnels, parking garages, and urban canyons (Noureldin et al., 2013; Tang et al., 2024). Although inertial navigation systems (INSs) can provide temporary navigation solutions during GNSS outages, their performance degrades over time because of accumulated sensor errors, leading to position and velocity drift (Noureldin et al., 2013). Integrating odometry or exteroceptive sensor data can mitigate this drift (Rashed et al., 2024); however, optimally fusing onboard diagnostics II (OBD-II)-derived speed and exteroceptive sensor data remains an open challenge (Ragab et al., 2025). Furthermore, achieving lane-level localization in GNSS-denied environments requires effective use of digital map data (Ragab, 2024).

While topological map-matching can enhance GNSS-based navigation, its performance can be compromised in complex intersections and unstructured road layouts (Ragab et al., 2023). Visual map-based localization techniques offer higher precision but are susceptible to challenges posed by dynamic environments, adverse weather conditions, and inconsistencies between the sensed data and the reference map (Mounier et al., 2025; Siuhi & Mwakalonge, 2016; Zhu et al., 2022). From a practical standpoint, cost-effective navigation systems typically employ forward-facing stereo cameras and inertial measurement units (IMUs), which have become standard components in the automotive industry.

This research aims to develop a lightweight, multi-sensor positioning and navigation system using readily available onboard vehicle sensors and publicly accessible topological maps for urban environments characterized by limited satellite signal availability. Hence, we address the aforementioned limitations and propose a novel waypoint-informed localization (WiL) system. WiL integrates a reduced inertial sensor system (RISS), stereo visual odometry (VO) with semantic segmentation-based outlier rejection (SS-OR), and OBD-II-derived speed estimates to enhance localization accuracy in GNSS-challenged environments. The key contributions of this work are as follows:

A novel SS-OR scheme for vision-based navigation (VBN) systems.
An OBD-II velocity integration framework (OVIF) that optimally fuses stereo-vision-derived velocity measurements with OBD-II speed data.
The integration of WiL with two-dimensional (2D) topological maps to improve navigation performance during extended GNSS outages.

Figure 1 depicts the topological map employed in this study along with the reference integrated navigation solution. This research builds upon prior work focused on the limitations of OBD-II-reported automotive speed measurements (Ragab et al., 2025) and develops an SS-OR technique for enhanced VBN (Ragab, Elhabiby, et al., 2020). Although the proposed WiL system may not meet the stringent accuracy requirements for AD applications (Reid et al., 2019), it provides a robust and reliable backup localization capability for autonomous vehicles (AVs) when wireless positioning or visual map-matching becomes unavailable, ensuring continuous operation until other primary localization systems can be re-established.

FIGURE 1

Waypoints along the reference trajectory with the city of Toronto GIS; centerlines and edge points overlaid on Google Earth imagery

2 RELATED WORK

2.1 Visual and Wheel Odometry

Cameras offer a low-cost source of rich data for tasks like object detection, three-dimensional (3D) scene reconstruction, and odometry. VO has progressed from feature-based methods to direct intensity approaches (Aqel et al., 2016; Scaramuzza & Fraundorfer, 2011), with some works fusing both (Forster et al., 2017; Zhao & Vela, 2020) and others that jointly exploit camera placement and vehicle kinematic constraints (Scaramuzza, 2011). Stereo VO yields absolute scale and depth information, maintaining a distinct advantage over monocular techniques. This inherent capability drives its preferred adoption by automotive original equipment manufacturers (OEMs) for robust collision avoidance systems (“Subaru EyeSight - Canada”, 2025).

Although data sets exist that include speed-sensor measurements alongside other modalities, only a few provide OBD-II-derived vehicle speed in conjunction with low-cost perception and GNSS/IMU data (Ragab, 2024; Ragab et al., 2025). While these measurements can substantially complement inertial navigation, their rigorous use within multi-sensor fusion frameworks remains under-explored. In particular, the literature provides only a limited treatment of vehicle-specific error sources and wheel speed sensor (WSS) identification underlying the reported OBD-II speed, representing a critical gap in the field.

2.2 Addressing the Impact of Dynamic Objects on VBN

Conventional outlier rejection methods, such as random sample consensus (RANSAC), often struggle in dynamic environments, particularly when only a front-facing stereo camera is used in urban areas with shadows, traffic, and suboptimal lighting conditions (Fuentes-Pacheco et al., 2015). Features from distant objects are often discarded, reducing the pool of reliable correspondences for motion estimation. To overcome these challenges, researchers have introduced deep learning paradigms to enhance localization and mapping.

The integration of semantic segmentation models into VBN systems has been explored to enhance odometry and mapping. Wen et al. (2023) used SS-OR in outdoor dynamic scenes, outperforming DynaSLAM (Bescos et al., 2018) and ORB-SLAM2 (Mur-Artal & Tardos, 2017). However, the use of older segmentation models such as SegNet (Badrinarayanan et al., 2017) and the reliance on optimization methods like bundle adjustment introduced delays. DynaSLAM II (Bescos et al., 2021) improved outlier rejection in static scenes with a multi-object tracker, achieving state-of-the-art performance. Nevertheless, the study of Bescos et al. (2021) did not explicitly address ego-vehicle speed estimation or its integration with onboard systems.

This research builds upon that foundation by exploring the integration of OBD-II-derived vehicle speed, stereo VO, and semantic segmentation to enhance navigation in GNSS-denied environments.

2.3 Usage of Maps for Guidance, Navigation, and Control

Maps have become crucial for the guidance, navigation, and control (GNC) of AVs. In the 2007 DARPA Urban Challenge, winning teams relied on a simplified 2D map for route guidance (Urmson et al., 2009), marking a significant early use of maps in AVs. While 3D lane models, such as those used in the Bertha Drive project (Ziegler et al., 2014), provide highly accurate geometric data for navigation, the use of maps is not without challenges. These challenges include maintaining map accuracy over time, ensuring scalability across large areas, and handling discrepancies between map data and real-world conditions. Alternative solutions leveraging OpenStreetMap and VO have been proposed to improve vehicle localization (Cho et al., 2022; Ma et al., 2017); nevertheless, such methods either depend heavily on feature-specific visual matching or require high-end sensors such as 3D lidar, which is costly compared with camera-based systems.

In the context of GNC, precise localization is essential for safe vehicle operation (Reid et al., 2019). However, there has been limited research on the tight coupling of navigation and guidance for specific use cases. This study bridges that gap by integrating local planning with publicly available topological maps, offering a valuable aid during extended GNSS outages. By leveraging topological maps and integrating them with vehicle sensors, this work provides a scalable and resource-efficient navigation solution, particularly for long-term GNSS-denied navigation in autonomous and conventional vehicles.

3 TOPOLOGICAL MAP

The road map used in this research is a macroscale map of the City of Toronto (“City of Toronto Open Data”, 2017a). Macroscale, being the simplest of all, carries information that is sufficiently accurate for road-level positioning. The prototype map used in this work, produced via manual digitization of high-resolution aerial images, has a ground accuracy of ±30 cm. It is worth mentioning that in certain situations, such as when tall buildings or deep shadows obstruct the view, some features may not be accessible. It is crucial to keep this limitation in mind, as it may affect the functionality or accuracy of the system in use. The road map is stored as a set of shape files, each containing a layer corresponding to the features of interest.

The main shapefiles contain the following information:

Road centerlines: Geometry and attributes of road centerlines, useful for route identification and distance/orientation calculations (size: ∼12 MB).
Road edges: Road boundary information for determining width and checking if a point lies on the roadway (size: ∼87 MB).
Turn restrictions: Locations and types of restricted maneuvers (e.g., no right turn, no U-turn) (size: ∼44 MB).
Parking lots: Locations and attributes of parking areas (surface, garage) (size: ∼46 MB).

Each shape file contains a list of attributes used to identify individual objects, which are represented as polylines—sequences of shapepoints. The shape file structure adheres to a data convention commonly used in geographic information system (GIS) databases, facilitating efficient parsing and integration. The database content is organized according to the following key attributes:

Links: Polylines representing roads, annotated with length, speed limit, and functional class.
Nodes: Intersection points at which multiple links converge, including unique IDs and link connectivity.
Subtype and subtype ID: Classification attributes used to distinguish objects within the same category.
Elevation: Vertical positioning data used for slope and grade analysis.

Figure 2 presents the topological map overlaid with a land vehicle’s trajectory, where $r_{k}^{l}$ denotes the current 3D position in the local-level frame (LLF) using the east-north-up (ENU) convention and $u_{k}$ represents the control commands applied at epoch k.

FIGURE 2

Structure of the topological map (not to scale)

4 METHODOLOGY

4.1 System Overview

The proposed navigation system consists of three main blocks, each with a specific function. The overall system architecture is depicted in Figure 3, with each main block colored differently to enhance visualization.

FIGURE 3

Overall architecture of the navigation system (API: application programming interface; DTCM: dynamically tuned covariance matrix; PVA: position, velocity, and attitude)

To enable the system to independently handle issues such as error source determination and mitigation, it was designed in a decentralized fashion. This approach allows for a detailed understanding of the system characteristics and enhances the system’s adaptability to changing conditions. The navigation problem is divided into two distinct parts, with the first block responsible for obtaining the best possible longitudinal accuracy (i.e., along-track performance) and the second block responsible for estimating lateral motion (i.e., cross-track performance). By combining both blocks in a loosely coupled indirect extended Kalman filter (EKF), we show how a reliable navigation solution can be obtained even during extended GNSS outages.

4.2 Stereo VBN System

Our stereo VBN system architecture builds upon our previous stereo VO design, as detailed by Ragab, Elhabiby, et al. (2020). In this section, we provide a concise overview of the system and highlight the enhancements made in relation to our earlier work.

4.2.1 Inputs

The stereo VO system takes undistorted and rectified red-green-blue (RGB) stereo image pairs acquired at iteration $k \in Z^{+}$ , namely, $I_{l, k}$ and $I_{r, k}$ , with size $n \times m$ pixels (n columns and m rows). The corresponding grayscale stereo images of $I_{l, k}$ and $I_{r, k}$ are $I_{l, k}^{*}$ and $I_{r, k}^{*}$ . Using stereo vision, we compute the disparity images $D_{k}$ and $D_{k - 1}$ for two consecutive stereo grayscale image pairs by employing the semi-global matching algorithm, as it delivers denser depth images with superior structural details compared with correlation-based methods, which is advantageous for navigation (Stelzer et al., 2012).

4.2.2 Feature Detection and Association

In contrast to our previous work, we employ a deep-learning-based feature extraction and matching pipeline rather than classical handcrafted methods such as scale-invariant keypoints (SIFT) and binary descriptors (ORB), owing to their higher robustness under illumination changes, motion blur, and low-texture or low-light conditions (Singh et al., 2023). More specifically, we use SuperPoint (Detone et al., 2018) for keypoint detection and descriptor generation, coupled with SuperGlue (Sarlin et al., 2020) for context-aware feature association. SuperPoint extracts interest points directly from image intensities using a fully convolutional network trained via self-supervision, enabling repeatable and illumination-invariant features. SuperGlue then performs graph-based matching using attention mechanisms that exploit both spatial geometry and descriptor similarity, yielding more reliable correspondences than conventional descriptor matching. This combination significantly improves feature stability in challenging visual environments such as night-time urban driving or under varying headlight and shadow conditions.

FIGURE 4

Stereo VBN system (CDNN: convolutional deep neural network)

All feature detection and matching steps are performed on the left grayscale image stream, $I_{l, k}^{*}$ . World coordinates of these features are computed from the disparity map $D_{k}$ , and features with unknown disparity are discarded. The resulting set of 3D features is denoted as $F_{l, k}$ , and the same procedure is repeated over the previous frame $k - 1$ for temporal association.

4.2.3 Improved Outlier Rejection

The most typical outlier rejection algorithms are RANSAC, maximum likelihood estimation sampling consensus, and robust cost functions. In the specific case of urban environments, these outlier rejection schemes may not be sufficient to obtain suitable ego-motion estimates. This limitation is mainly due to the following reasons:

Suboptimal illumination levels can arise from high-rise buildings casting shadows.
The complexities of urban traffic can hinder accurate feature tracking, as illustrated in Figure 5(a).
Features triangulated at greater distances from the sensor are often discarded, as features at higher distances exhibit a higher reprojection error.
Triangulated features carry more uncertainty compared with 2D image plane features.

FIGURE 5

VO challenges and semantic segmentation improvements. (a) Stereo VO challenges in coherent slow-motion scenarios, (b) SS-OR with and without dilation.

In addition, it is highly likely that dynamic objects will be encountered in the scene. Figure 6 illustrates the semantic-segmentation-based probabilities of occurrence for a set of predefined outlier classes across three real-world trajectories, whose characteristics are summarized in Table 2.

FIGURE 6

Outlier class statistics across the tested trajectories

Dynamic object removal has become critical in land vehicle navigation. Semantic segmentation enables the exclusion of dynamic object features prior to ego-motion estimation, thereby reducing constraints on feature matching and improving motion estimation accuracy (Ragab, Elhabiby, et al., 2020). In this work, a DeepLabv3+-based semantic segmentation model (Chen et al., 2018), implemented as detailed by Ragab, Elhabiby, et al. (2020), is used to produce a segmentation map S from the resized input frame $I_{l, k}$ . A predefined set of class indices C, corresponding to dynamic object categories, is used to form an index array O of pixels in S that match any class in C. The SS-OR algorithm (Algorithm 1) takes as input the feature set $F_{l}$ , segmentation map S, and class set C to generate binary masks: $F_{m}$ , which marks the feature locations, and $B_{m}$ , which excludes features matching O. To mitigate segmentation noise, a dilation operation using a circular kernel $U_{m}$ with a radius of 20 px is applied to $B_{m}$ , yielding the mask $D_{m}$

ALGORITHM 1

Semantic segmentation-based outlier rejection

input: $F_{l}$ , S, C, U

output: f_l

1: begin

2: O = [(x, y), ∀S_x,y ∈ C]

3: F_m = zeros(n x m)

4: F_m(i, j)=1, ∀(i, j) ∈ F_l

5: B_m=ones(n x m)

6: B_m(i, j)=0, ∀(i, j) ∈ O

7: D_m=B_m ⊕ U_m

8: H_m=F_m ∧ D_m

9: f_l=(x, y), ∀ H_m(x, y)=1]

10:return f_l

(Figure 5(b)). A logical AND operation between $F_{m}$ and $D_{m}$ results in the final inlier mask $H_{m}$ , from which reliable features $f_{l}$ are selected and used for matching or tracking. The resulting matched pairs are denoted as $V = (f_{k - 1}, f_{k})$ .

4.2.4 Motion Estimation

We assume that the cameras are modeled as pin-hole models and that both cameras are intrinsically and extrinsically calibrated. The motion is estimated by minimizing the image reprojection error using the standard Levenberg-Marquardt least-squares minimization technique. Thus, we seek the homogeneous transform T_k that minimizes the reprojection error ϵ :

$ϵ = \sum_{(f_{k - 1}, f_{k}) \in V} {| p_{k - 1} - π (P T_{k} w_{k}) |}^{2} + {| P_{k} - π (P T_{k}^{- 1} w_{k - 1}) |}^{2}$ 1

where π is the perspective projection function that projects 3D points in the camera frame onto the image plane, |⋅|² represents the squared Euclidean distance, p and w are the homogeneous image and world coordinates, respectively, and P is the camera projection matrix. This minimization is carried out in a RANSAC loop in order to find the solution that best fits the model.

Considering camera axis conventions, the camera coordinate system must be transformed to the LLF to follow the right-handed ENU Cartesian coordinates that represent the position relative to a local origin (i.e., the initial position of the center of gravity of the vehicle). Let us assume that the rotation matrix from the camera frame to the body frame is denoted by $R_{c}^{b}$ , defined as follows:

$\begin{array}{l} R_{c}^{b} = [\begin{array}{l} 0 & 0 & 1 \\ - 1 & 0 & 0 \\ 0 & - 1 & 0 \end{array}] \end{array}$ 2

The 3D displacement vector $d_{k}^{l}$ in the LLF can be obtained as follows:

$d_{k}^{l} = R_{b}^{l} R_{c}^{b} t_{k}$ 3

where $R_{b}^{l}$ represents a transformation from the b-frame to the LLF, defined as follows:

$\begin{array}{l} R_{b}^{l} = [\begin{array}{l} \cos (α) \cos (β) & \cos (α) \sin (β) \sin (γ) - \sin (α) \cos (γ) & \cos (α) \sin (β) \cos (γ) + \sin (α) \sin (γ) \\ \sin (α) \cos (β) & \sin (α) \sin (β) \sin (γ) + \cos (α) \cos (γ) & \sin (α) \sin (β) \cos (γ) - \cos (α) \sin (γ) \\ - \sin (β) & \cos (β) \sin (γ) & \cos (β) \cos (γ) \end{array}] \end{array}$ 4

The forward velocity derived from the stereo VO system is then given by the following:

$v_{f_{VIS}}^{l} = \frac{| d_{k}^{l} |}{Δ t_{k}}$ 5

where $Δ t_{k}$ is the k-th time interval between two consecutive frames.

4.3 OBD-II Velocity Integration Framework

The OVIF aims to improve the forward-velocity estimate of the ego-vehicle by fusing measurements from two distinct sensors: a stereo camera and a WSS. The WSS provides reliable speed estimates at higher speeds (typically above 4 km/h), while the stereo camera-enhanced with SS-OR offers better estimates at lower speeds (Ragab et al., 2025).

A constant-velocity model is adopted for state propagation, where the state vector is defined as $x_{k} = [v_{k} {\dot{v}}_{k}]$ and the state transition matrix is given by the following:

$\begin{array}{l} F = [\begin{array}{l} 1 & Δ t_{k} \\ 0 & 1 \end{array}] \end{array}$ 6

Measurement models are selected based on the available sensor data. Specifically, the measurement vector is defined as follows:

$\begin{array}{l} z_{k} = {\begin{cases} {[\begin{array}{l} v_{VIS} & 0 \end{array}]}^{⊤} & if v_{WSS} \leq 4 \\ {[\begin{array}{l} v_{VIS} & v_{WSS} \end{array}]}^{⊤} & if 4 < v_{WSS} \leq 6 \\ {[\begin{array}{l} 0 & v_{WSS} \end{array}]}^{⊤} & if v_{WSS} > 6 \end{cases} \end{array}$ 7

$\begin{array}{l} R_{k} = {\begin{cases} [\begin{array}{cc} σ_{VIS}^{2} & 0 \\ 0 & σ_{\infty}^{2} \end{array}] & if v_{WSS} \leq 4 \\ [\begin{array}{cc} σ_{VIS}^{2} & 0 \\ 0 & σ_{WSS}^{2} \end{array}] & if 4 < v_{WSS} \leq 6 \\ [\begin{array}{cc} σ_{\infty}^{2} & 0 \\ 0 & σ_{WSS}^{2} \end{array}] & if v_{WSS} > 6 \end{cases} \end{array}$ 8

where $σ_{\infty}^{2}$ denotes a large variance assigned to the inactive measurement channel, effectively minimizing its contribution during the state update (typically set to $10^{6} m / s$ or higher). Thus, the measurement noise covariance matrices are dynamically adapted according to the sensor availability. The variance associated with the stereo vision measurement is defined as follows:

$\begin{array}{l} σ_{{VIS}_{k}}^{2} = \frac{c}{n_{3 D_{k}}} \end{array}$ 9

where c is a tuning constant and $n_{3 D_{k}}$ is the number of 3D features used for motion estimation. The WSS noise variance is modeled as follows:

$σ_{{WSS}_{k}}^{2} = {(0.3095 e^{- 0.1241 v_{f_{k}}} + 0.1477 e^{- 0.0009149 v_{f_{k}}})}^{2}$ 10

with $v_{f_{k}}$ denoting the forward velocity at time step k. For further details on the implementation of the Kalman filter, readers are referred to the work by Grewal and Andrews (2008).

4.4 Waypoint Extractor

We demonstrate the benefits of route planning in GNSS-denied environments, such as downtown Toronto, by post-processing the high-end NovAtel trajectory into a set of strategic waypoints. To achieve this, we employ the Ramer-Douglas-Peucker (RDP) algorithm (Saalfeld, 1999), which simplifies the raw path while retaining critical geometry within a tolerance of ϵ = 3m, as illustrated in Figure 7.

FIGURE 7

RDP simplification with a to-scale band of ϵ = 3m and smooth road boundaries

This tolerance was selected empirically through trial and error, as it provided a good representation of the original trajectory while accounting for measurement noise and map inaccuracies. The chosen value also aligns with Ontario’s average lane width of approximately 3.5 m (City of Toronto, 2017), ensuring realistic geometric fidelity in the simplified route.

In contrast to real-time planners that adapt to user preferences and dynamic traffic, our implementation demonstrates that a sparse waypoint list can be leveraged by a local planner to generate navigation-aiding measurements from a purely topological map lacking high-definition-level detail. Using a moderate waypoint density, we further show that these path-based constraints effectively bound inertial drift during prolonged GNSS outages.

On another note, waypoints offer an essential fallback for AV disengagements, where control reverts to a human driver. The operational design domain limits full autonomy in conditions like severe weather or unmapped areas, reinforcing the need for robust path planning. For a broader perspective on route planning in AV systems, see the works by Pek et al. (2020), Sharma et al. (2021), Urmson et al. (2009), and Wei et al. (2014).

4.5 Waypoint-Informed Localization (WiL)

The current implementation of WiL relies on the dynamic window approach (DWA), which is a motion planning algorithm used for vehicle navigation in dynamic environments (Fox et al., 1997). This approach determines the optimal control inputs for a vehicle at each time step by evaluating all possible trajectories within a dynamic window, which is a subset of the vehicle’s control space defined by its maximum achievable velocities and accelerations. At each time step, the search space is reduced by the kinematic constraints of the vehicle to a certain span of velocities around its current velocity vector. This span of velocities, denoted by V_d, is defined by the following equations:

$\begin{array}{l} V_{d} = {(v, ω) | \begin{array}{l} v \in [v_{c} - {\dot{v}}_{b} Δ t, v_{c} + {\dot{v}}_{a} Δ t] \land \\ ω \in [ω_{c} - {\dot{ω}}_{b} Δ t, ω_{c} + {\dot{ω}}_{a} Δ t] \end{array}} \end{array}$ 11

where $v_{c}$ and $ω_{c}$ denote the current forward velocity and turn rate of the vehicle, respectively, and ${\dot{v}}_{a}, {\dot{ω}}_{a}, {\dot{v}}_{b}$ , and ${\dot{ω}}_{b}$ represent the vehicle’s maximum acceleration, turn rate acceleration, maximum deceleration (braking capability), and turn rate brake deceleration, respectively. This model prioritizes computational efficiency and realistic handling of kinematic constraints, ensuring rapid trajectory evaluations and adherence to feasible vehicle movements within a limited control space.

The resulting set of admissible velocities and turn rates, denoted by $V_{a}$ , is further restricted by a safety condition based on the minimum turning radius $ρ_{min}$ of the vehicle. This condition ensures that the selected trajectory can be followed without exceeding the vehicle’s physical limits. Specifically, $V_{a}$ includes only those velocities and turn rates that satisfy two conditions:

$V_{a} = {v, ω \leq \sqrt{2 ρ_{min} (v, ω) {\dot{v}}_{b}} \land \sqrt{2 ρ_{min} (v, ω) {\dot{ω}}_{b}}}$ 12

Here, the first term inside the min function limits the turn rate based on the vehicle’s forward velocity and minimum turning radius, and the second term limits the turn rate based on the maximum turn rate that can be achieved while satisfying the same conditions. The trajectory that minimizes a cost function is selected as the optimal trajectory for the vehicle to follow. The cost function is defined as a combination of the distance to the goal, the speed of the vehicle, and the proximity to obstacles in the environment. The control inputs are sampled in two dimensions, namely, linear velocity and yaw rate. The dynamic window is defined by four parameters, namely, the minimum and maximum linear velocity ( $v_{min}, v_{max}$ ) and the minimum and maximum yaw rate ( $ω_{\min}, ω_{\max}$ ).

Figure 8 illustrates an example of the WiL process at an intersection. The black dots correspond to topological map features extracted from the GIS database (e.g., road edges) that define the geometric constraints of the maneuver. The colored curves represent candidate trajectories generated by WiL, where red paths indicate higher-cost solutions and the green path denotes the optimal feasible trajectory that satisfies both kinematic and safety constraints. The blue circle represents the goal-tolerance region centered at the active waypoint, characterized by a radius $r_{goal}$ . A waypoint is considered to be reached once any part of the vehicle’s body intersects this region:

FIGURE 8

WiL at an intersection, with two waypoints defined to characterize the right turn that should be made by the vehicle in order to reach its destination. (a) Navigation within a global path. Red lines indicates high costs, and the green line indicates the lowest-cost path, (b) WiL-based solution within the en-plane with actual predicted trajectories in downtown Toronto.

$\sqrt{{(x - x_{goal})}^{2} + {(y - y_{goal})}^{2}} \leq r_{goal}$ 13

When this condition is satisfied, WiL transitions to the next waypoint in the topological sequence. Figure 8(b) shows the same process applied to real data from the downtown Toronto data set, demonstrating how the algorithm continuously updates the vehicle trajectory based on local map constraints.

At each planning cycle, the algorithm iterates over all admissible combinations of linear velocity and yaw rate within the defined dynamic window, generating a trajectory for each pair. The cost function is then calculated for each possible trajectory using the following:

$J = w_{g} J_{g} + w_{s} J_{s} + w_{o} J_{o}$ 14

where $J_{g}, J_{s}$ , and $J_{o}$ are the costs associated with the distance to the goal, the speed of the vehicle, and the proximity to obstacles, respectively. The weights $w_{g}, w_{s}$ , and $w_{o}$ are empirically tuned hyperparameters, selected once for the entire data set and held fixed across all scenarios. These weights determine the relative contribution of each component. The individual cost terms are defined below.

4.5.1 Distance and Angle Cost

The cost associated with the distance to the goal is calculated as follows:

$J_{g} = κ_{d} | [\begin{array}{l} x - x_{goal} \\ y - y_{goal} \end{array}] | + κ_{a} | atan 2 (\frac{\sin (θ - θ_{goal})}{\cos (θ - θ_{goal})}) |$ 15

where $(x, y)$ represents the current position of the vehicle, $(x_{goal}, y_{goal})$ represents the goal location, and θ and $θ_{goal}$ represent the current and goal orientations, respectively. The first term in the equation represents the Euclidean distance between the current position and the goal location scaled by a tuning coefficient $κ_{d}$ , while the second term represents the difference in orientation between the current and goal states scaled by a tuning coefficient $κ_{a}$ . The $a \tan 2$ function is used to ensure that the orientation difference is within the range of $[- π, π]$ .

4.5.2 Speed Cost

The cost associated with the speed of the vehicle is calculated as follows:

$J_{s} = v_{max} - v$ 16

where v is the linear velocity of the vehicle.

4.5.3 Collision Avoidance

The main criterion for any navigation system is to have an estimated position within the road or lane boundary in which the vehicle is traveling. As a first step in the filtering process, we define the vehicle dimensions with an added safety boundary as depicted in Figure 9, where $ρ_{w}$ and $ρ_{l}$ are the dimensions of the rectangular bounding box along the x - and y-axes, respectively.

FIGURE 9

Vehicle dimensions and boundaries with respect to a road lane

To ensure that the vehicle does not collide with the obstacles (i.e., road boundaries), we define a function ξ with the following properties:

$ξ (l_{o, i}) = {\begin{cases} 1 & if - \frac{ρ_{w}}{2} \leq l_{o, i, 1} \leq \frac{ρ_{w}}{2} and - \frac{ρ_{l}}{2} \leq l_{o, i, 2} \leq \frac{ρ_{l}}{2} \\ 0 & otherwise \end{cases}$ 17

where $l_{o, i} = (l_{o, i, 1}, l_{o, i, 2})$ represents the position of the i-th obstacle. The function ξ returns 1 if the obstacle is within the bounding box and 0 otherwise. This function can be used in the collision check as follows:

$J_{coll} = {\begin{cases} \infty & if ξ (l_{o, i}) = 1 for any obstacle i \\ 0 & otherwise \end{cases}$ 18

where $J_{coll}$ denotes the collision cost. The collision cost is assigned a value of ∞ if any obstacle lies within the bounding box and 0 otherwise. Trajectories with $J_{coll} = \infty$ are deemed infeasible and are discarded prior to cost evaluation.

4.5.4 Proximity to Obstacles

The cost associated with the proximity to obstacles is calculated as follows:

$J_{o} = \sum_{i = 1}^{n} [\exp (- \frac{d_{i}^{2}}{2 σ^{2}}) - \exp (- \frac{{(d_{i} - r_{safe})}^{2}}{2 σ^{2}})]$ 19

where n is the number of obstacles in the environment, $d_{i}$ is the distance between the vehicle and the i-th obstacle, $r_{safe}$ is the safe distance from obstacles, and σ is a parameter that controls the spread of the cost function.

The control inputs $({\hat{v}}_{k}, {\hat{ω}}_{k})$ that yield the minimum-cost trajectory are selected as the optimal control inputs for the vehicle to follow. These inputs are returned together with the associated trajectory and its corresponding minimum cost.

4.5.5 Generation of WiL Pseudomeasurements

WiL integrates the local planner’s yaw-rate profile with the fused forward velocity ${\hat{v}}_{f_{k}}$ from OVIF. After linear time alignment to the EKF timestamps, the horizontal position (ENU) and azimuth evolve as follows:

$p_{k}^{WiL} = p_{k - 1}^{WiL} + {\hat{v}}_{f, k} Δ t_{k} [\begin{matrix} \cos A_{k}^{WiL} \\ \sin A_{k}^{WiL} \end{matrix}], where A_{k}^{WiL} = A_{k - 1}^{WiL} + {\hat{ω}}_{k} Δ t_{k}$

With $p_{k}^{WiL} = {[E_{k}, N_{k}]}^{⊤}$ and ${\hat{ω}}_{k}$ , the time-aligned planner yaw rate about the local up axis, mapping ( $E_{k}, N_{k}$ ) to geodetic yields the WiL pseudomeasurement $z_{k}^{WiL} = {[λ_{k}^{WiL}, φ_{k}^{WiL}, A_{k}^{WiL}]}^{⊤}$ , used in the EKF measurement update during GNSS outages.

4.6 3D Reduced Inertial Sensor System

The RISS uses a reduced inertial suite and an external forward speed to limit drift during GNSS gaps. In this work, we mechanize RISS with the OVIF forward-velocity estimate ${\hat{v}}_{f_{k}}$ , driving along-track motion; heading and lateral errors are bounded by EKF updates from GNSS when available or WiL pseudomeasurements during outages.

For enhanced attitude estimation, a three-axis accelerometer is used to determine pitch and roll, while a single gyroscope provides heading information. This configuration exploits the cost-effectiveness of accelerometers compared with gyroscopes, as most vehicles already include a yaw-rate sensor for electronic stability control. Early RISS implementations relied on two accelerometers; however, incorporating a third accelerometer removes dependence on a gravity model, thereby improving pitch and roll estimates (Ragab, Abdelaziz, et al., 2020). The IMU data, originally measured in the body frame, are mechanized into the LLF using the ENU convention to yield position and velocity estimates. The 3D-RISS navigation state vector is defined as follows:

$x = {[φ, λ, h, v_{e}, v_{n}, v_{u}, r, p, A]}^{T}$

where φ and λ are latitude and longitude, h is altitude, $v_{e}, v_{n}$ , and $v_{u}$ are the east, north, and upward velocities, and $r, p$ and A denote roll, pitch, and azimuth, respectively. Detailed mechanization equations have been presented by Ragab et al. (2020 and 2023); readers seeking a comprehensive review of RISS variants are referred to the work by Ragab (2024).

4.7 EKF for the WiL/3D-RISS System

The RISS is integrated with WiL using an EKF to bridge GNSS outages. Figure 3 presents the system architecture. Detailed derivations are omitted (see the work by Noureldin et al. (2013) for a comprehensive description).

4.7.1 System Model

The continuous-time error state dynamics are modeled as follows:

$δ x = F δ x + W$ 20

where $δ x$ is the error state vector, F denotes the system dynamic matrix, and w represents the process noise with covariance Q. In the discrete-time domain, the state propagation is given by the following:

$δ x_{k + 1} = ϕ_{k, k + 1} δ x_{k} + G_{k} w_{k} Δ t_{k}$ 21

Here, the state transition matrix is approximated as $ϕ_{k, k + 1} \approx I + F Δ t_{k}$ , and $w_{k}$ is approximated as a zero-mean Gaussian noise vector. The 3D-RISS error state vector is defined as follows:

$δ x_{R} = {[\begin{array}{l} δ φ & δ λ & δ h & δ v_{e} & δ v_{n} & δ v_{u} & δ A & δ b_{z} \end{array}]}^{T}$ 22

where $δ φ, δ λ$ , and $δ h$ represent errors in latitude, longitude, and altitude, $δ v_{e}, δ v_{n}$ and $δ v_{u}$ denote the velocity errors in the east, north, and upward directions, $δ A$ is the azimuth error, and $δ b_{z}$ is the gyroscope bias error.

4.7.2 Measurement Model

The measurement update is expressed as follows:

$δ z_{k} = H_{k} δ x_{k} + η_{k}$ 23

where $η_{k} \sim N (0, R_{k})$ . When GNSS data are available, the measurement vector is given by the following:

$δ z_{k}^{G} = [\begin{matrix} r_{k}^{R} - r_{k}^{G} \\ v_{k}^{R} - v_{k}^{G} \end{matrix}]$ 24

with the design matrix $H_{k}^{G} = [\begin{array}{l} I_{6 \times 6} & 0_{6 \times 2} \end{array}]$ and the measurement noise covariance:

$R_{k}^{G} = diag ([σ_{φ_{G}}^{2}, σ_{λ_{G}}^{2}, σ_{h_{G}}^{2}, σ_{v_{e_{G}}}^{2}, σ_{v_{n_{G}}}^{2}, σ_{v_{u_{G}}}^{2}])$ 25

In the absence of a GNSS, WiL provides horizontal position and azimuth estimates, and the measurement vector is as follows:

$δ z_{k}^{W i L} = [\begin{matrix} φ^{R} - φ^{W i L} \\ λ^{R} - λ^{W i L} \\ A^{R} - A^{W i L} \end{matrix}]$ 26

The corresponding design matrix is as follows:

$H_{k}^{WiL} = [\begin{array}{l} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \end{array}]$ 27

and the noise covariance matrix is defined as follows:

$R_{k}^{W i L} = diag ([σ_{φ_{W L L}}^{2}, σ_{λ_{W L L}}^{2}, σ_{A_{W L L}}^{2}])$ 28

Following the dynamic covariance-tuning concept of Saleh et al. (2022), $R_{k}^{WiL}$ is adapted according to the cost of the optimal WiL trajectory. Each standard deviation term is scaled as follows:

$\begin{array}{l} σ_{i} = σ_{i_{0}} (1 + α_{i} J^{*} / \bar{J}), i \in {λ_{WiL}, φ_{WiL}, A_{WiL}} \end{array}$ 29

Here, $J^{*}$ denotes the cost associated with the selected minimum-cost trajectory, whereas $\bar{J}$ is the mean cost over all admissible trajectories at epoch k. The term $σ_{i_{0}}$ represents the baseline measurement uncertainty obtained from calibration, and $α_{i}$ is a small adaptive gain that is chosen empirically. This formulation ensures that trajectories exhibiting lower costs yield smaller values of $σ_{i}$ , thereby increasing the confidence assigned to WiL pseudomeasurements. Conversely, higher-cost trajectories lead to inflated values of $σ_{i}$ , appropriately reducing their contribution during the EKF update.

4.7.3 Measurement Correlation and Filter Consistency

A theoretical challenge in integrating WiL pseudomeasurements is the potential for statistical correlation between pseudomeasurement errors and the EKF state estimation error. WiL outputs are generated using the propagated pose estimate ${\hat{x}}_{k}$ (prior to the epoch-k measurement update), obtained by mechanizing the vehicle kinematic model with the filtered OVIF forward velocity. Because the OVIF velocity is itself produced by a linear Kalman filter that aggregates historical sensor information, the common EKF assumption that measurement noise is uncorrelated with the state estimation error is only approximately satisfied. In particular, we have the following:

$cov (δ x_{k}, η_{k}^{WiL}) \neq 0$ 30

Consequently, adopting a diagonal measurement noise covariance $R_{k}^{WiL}$ under strict independence assumptions may lead to partial information double-counting and an overconfident posterior covariance. As introduced in the preceding section, this work reduces the impact of potential correlation through adaptive covariance inflation by scaling the WiL standard deviations $σ_{i}$ using the trajectory cost ratio $J^{*} / \bar{J}$ , thereby conservatively moderating pseudomeasurement influence during high-uncertainty epochs. This heuristic ensures robustness during extended outages while formal decorrelation of pseudomeasurement errors remains a subject for future research.

4.8 Computational Considerations

WiL operates on top of the DWA planner while consuming interpolated pseudomeasurement data derived from the vehicle’s onboard sensors. In practice, the DWA module runs in planning time at approximately 10 Hz and does not represent a computational bottleneck. Topological map data are iteratively loaded through a $60 m \times 60 m$ shapefile window with negligible overhead. The primary latency arises from the measurement pipeline: OBD-II speed updates are limited by parameter ID request timing, and the stereo VO and deep neural network inference stages introduce delays resulting in an effective update rate of $\approx 1.75 Hz$ . Despite these constraints, the overall computational demand remains modest and is well within the capabilities of modern embedded platforms suitable for real-time deployment.

5 EXPERIMENTAL WORK AND RESULTS

This section describes the experimental setup, road test details, and results for three real-road trajectory tests conducted in Kingston and Toronto, Ontario, Canada.

5.1 Experimental Setup

The experiments were conducted using an Anti-Lock Braking System (ABS)-equipped Toyota Sienna minivan. Despite its considerable size, which may seem like a drawback in urban driving conditions, the minivan offered advantages by enabling the strategic installation of various sensors. Figure 10(a) illustrates the sensors that were affixed to the vehicle. In addition, a testbed was placed within the vehicle’s trunk and was meticulously aligned with the road’s level by employing a built-in spirit level with a bubble indicator placed on the top left corner, as shown in Figure 10(b). Table 1 displays the equipment used in this work, along with their respective update rates.

FIGURE 10

Vehicle testbed: Sensor integration and hardware setup. (a) 2004 Toyota Sienna van with strategically mounted sensor and systems, (b) Testbed with different grades of IMUs and GNSS receivers rigidly mounted on the surface.

View this table:

TABLE 1 Equipment Details

5.1.1 Data Capture

To minimize data capture latency, the sensor load was distributed across two separate computers. The first computer managed the logging of both the high-end tactical grade GNSS/INS reference and low-cost systems. The high-end reference system comprised the NovAtel ProPak6^TM GNSS receiver and the fiber optic gyroscope (FOG)-based KVH 1750 IMU. The low-cost system included the u-blox LEA-6T Global Positioning System (GPS) receiver with a micro-electromechanical system (MEMS)-based VTI IMU (model SCC1300-D04). GPS timestamping was applied directly to the readings of both inertial systems using the manufacturers' proprietary software. The second computer handled real-time OBD-II vehicle speed data, Garmin GNSS messages, and perception data, such as stereo camera frames, lidar point clouds, and radar targets, all through the robot operating system on an Ubuntu 16.04 Linux environment. Additional details on the OBD-II protocol and associated measurements can be found in our original publication (Ragab et al., 2025).

5.1.2 Time Synchronization

To ensure synchronization between both machines, the Network Time Protocol (NTP) was employed. NTP, using the ntpd program (“Ntpd(8): network time protocol daemon - Linux man page”, n.d.), synchronized the Linux computer to remote NTP time servers. This allowed the computer to correct for time drift, ensuring a negligible drift within tens of milliseconds relative to the atomic clock. This correction enabled accurate clock skew adjustment for all sensor readings on the second machine, effectively synchronizing all of the sensors used in the experiments.

5.1.3 Multi-Sensor Calibration

Cameras and IMUs were calibrated prior to the experiments, and all lever-arm offsets were accurately measured with tape and then cross-referenced with the car’s blueprint in top and side views.

5.2 Road Test Experiments

Experiments were conducted in both Kingston and Toronto, ensuring a diverse range of environments. This varied setting not only enriched the vehicle kinematics and dynamics data but also provided diverse GNSS scenarios. Figure 11 illustrates trajectories with varying numbers of visible satellites, represented by a color scale. Table 2 shows the details of each trajectory, including its respective label, city, date, duration, and average speed of travel.

FIGURE 11

Trajectories assessed in the cities of Toronto and Kingston (left: T19.1, middle: T19.2, right: K19.1) with the number of visible GNSS satellites depicted by color, ranging from less than four satellites (red) to more than 9 satellites (green)

View this table:

TABLE 2 Road Test Trajectories

5.3 Results and Discussion

In this section, we present our findings and a comprehensive discussion. We start by introducing the evaluation metrics employed in this study and provide a rationale for their selection. Subsequently, we delve into the evaluation process, which is divided into two key aspects. The first aspect focuses on assessing the performance of along-track distance (ATD), specifically addressing longitudinal performance. The second aspect pertains to the evaluation of trajectory error (TE).

5.3.1 Evaluation Metrics

To evaluate the navigation performance, we employ the following five metrics:

CDF-ATD and CDF-TE: We produce cumulative distribution functions (CDFs) for ATD and TE. The 1σ, 2σ and 3σ values characterize the error distributions in meters.
RMSE: The root-mean-square error (in meters) provides a concise measure of average error magnitude.
Maximum Error: The largest deviation (in meters) from the ground truth identifies the worst-case performance.
sub-Xm: The fraction of predicted positions lying within X meters of the ground truth is critical for applications requiring strict accuracy bounds.
%E/DT: The percentage of error over distance traveled indicates how errors accumulate relative to trajectory length.

5.3.2 Along-Track Performance Analysis

To comprehensively evaluate the ATD performance, we conducted a rigorous analysis, comparing distance measurements obtained from various solutions: standalone stereo VO, stereo VO with SS-OR, the vehicle’s proprietary passive WSS (PWSS), and the proposed fusion approach combining stereo VO-SS-OR with PWSS. This assessment was conducted over two complete trajectories, namely, K19.1 and T19.1. It is important to note that trajectory T19.2 was intentionally excluded from this comparative analysis. The exclusion of trajectory T19.2 was a deliberate decision due to consistent challenges faced by the stereo VO system when encountering the entrance of an underground parking garage on this trajectory. These challenges primarily stemmed from difficult illumination conditions, which, in turn, negatively impacted the feature tracking process. Table 3 highlights the distance traveled and total missed distance for both the PWSS and the proposed fusion.

View this table:

TABLE 3 ATD Accuracy for Traj. T19.1 and K19.1

Additionally, Figure 13 shows the CDF-ATD of all benchmarked solutions along these two trajectories. The comparison indicates that the proposed fusion (Kalman filter [KF]-PWSS/Stereo VO-SS-OR) consistently outperformed the standalone PWSS, achieving higher accuracy by minimizing the missed distance. Furthermore, Figure 12 illustrates the effectiveness of the SS-OR scheme in mitigating the impact of dynamic objects, thereby improving motion estimation. The speed estimates, derived from positional components, closely align with the reference, demonstrating the robustness of the proposed approach. Notably, the proposed fusion system (depicted in black), which places greater reliance on PWSS at higher speeds and utilizes stereo VO-SS-OR at lower speeds, outperforms both individual solutions and closely tracks the reference speed.

FIGURE 12

Speed estimation with and without SS-OR and with filtered output

FIGURE 13

CDF of the ATD error for two road trajectories. (a) Trajectory K19.1, (b) Trajectory T19.1.

5.3.3 Navigation Performance Analysis

To assess the efficacy of our proposed solutions, trajectory T19.1 was specifically selected, owing to its inherent challenges in GNSS signal reception, as shown in Figure 11, primarily caused by the presence of numerous high-rise buildings encountered by the vehicle as it traversed both urban and densely urban areas. Within this trajectory, three distinct outages were identified, each exhibiting unique characteristics. Figure 14 highlights the locations of these outages. In our analysis, we computed the likelihood of the vehicle being stationary to gain insights into the effect of the different forward-velocity estimation techniques on the overall navigation performance.

FIGURE 14

Trajectory T19.1 with GNSS outages on the Toronto topological map

To classify the vehicle as stationary, a hard threshold of 0.02 m/s was applied to the reference speed, which was subsequently used to calculate the likelihood as a percentage. This parameter is significant in our study, as it provides a basis for evaluating the efficacy of our proposed OVIF in comparison to using the car’s standalone proprietary PWSS.

The detailed characteristics of the GNSS outages are presented in Table 4, including outage duration, average speed during the outage, distance traveled during the outage, and the percentage of time for which the vehicle was stationary during each outage. Table 5 shows the navigation performance of each solution. Solutions designated with the subscript “E” denote enhanced versions, meaning that the solution was scaled using the proposed forward-velocity fusion scheme, while solutions without subscripts incorporate raw PWSS measurements without integration with the stereo VO-SS-OR-derived speed estimates.

View this table:

TABLE 4 GNSS Outage Details

View this table:

TABLE 5 Experimental Results for Trajectory T19.1

The first observation revolves around the use of WiL in enhancing the overall navigation solution. From Table 5, we see that integrating WiL helped to substantially reduce the RISS drift. During Outage 1 (the longest GNSS outage; lasting nearly 11.25 min and covering over 2.8 km), the vehicle frequently halted owing to urban traffic. This behavior is visualized in Figure 15(a), which depicts approximately 13 stops. This significant frequency provides a robust assessment of our proposed forward-velocity fusion in tandem with WiL. The inclusion of our fusion scheme led to a decrease in the RMSE from 9 m to 2.29 m, marking a notable 74.56% improvement. In comparison, RISS_E and RISS yielded RMSEs of 11.685 m and 17.023 m, respectively, without WiL. Intriguingly, this result suggests that merely enhancing the forward velocity does not invariably refine the inertial solution. This inconsistency can be attributed to the bias drift of inertial sensors, particularly the vertical gyroscope, which is exacerbated by accumulating sensor biases. A potential remedy could be a new stationary detection system that more effectively resets IMU biases during GNSS outages. This prospect, however, merits separate research.

FIGURE 15

Speed comparison for trajectory T19.1 across multiple GNSS outages. (a) Outage 1: Speed comparison, (b) Outage 2: Speed comparison, (c) Outage 3: Speed comparison.

The second key observation centers around the speed profiles of all tested solutions in comparison to the reference solution, as shown in Figures 15(a)-15(c). Again, the results demonstrate the effectiveness of both the SS-OR paradigm in mitigating the effect of dynamic objects on the end solution, especially at lower speeds as demonstrated in the zoom-in views, and the proposed fusion algorithm. The proposed integration exhibits notable performance in accurately tracking the reference speed across the entire speed spectrum within all three GNSS outages. From the table illustrating the GNSS outage characteristics (Table 4) and the results presented in Table 5, we notice an interesting correlation between the likelihood of the vehicle coming to a stop within a shorter distance traveled and the accuracy of the navigation solution. This relationship becomes evident in Outage 3, where the vehicle remained stationary 41.296% of the time during the GNSS outage. Notably, RISS_E showcased superior performance, surpassing RISS by a margin of 11.09% in maintaining sub-5-m accuracy. Furthermore, the integration of WiL_E/RISS_E consistently upheld sub-5-m positioning accuracy throughout 100% of the travel time, outperforming the WiL/RISS integration, which maintained sub-5-m accuracy for only 74.18% of the time. This result underscores the significant benefits offered by the proposed forward-velocity fusion scheme, even when operating independently of WiL.

It is important to note that WiL relies on waypoints for navigation, which effectively mitigates error drift, especially when waypoints are sufficiently dense to capture turns and lane changes. This dense waypoint distribution is crucial, particularly in the context of global planning for AVs. To evaluate the performance when waypoints were less dense, we set ϵ to 5 m and compared it against the trajectory obtained with ϵ = 3 m. This analysis highlighted the importance of waypoint density for navigation. Figure 16(b) illustrates the DWA outage marked with squares, with RISS taking over until the WiL state improves. One potential enhancement and error mitigation strategy for WiL could involve using higher-resolution maps with denser road centerlines and edges or incorporating perception data within the WiL framework, or more specifically, the DWA routine. However, this approach would necessitate a real-time operation of WiL without long-term pre-emptive trajectory forecasting.

FIGURE 16

Navigation performance with WiL (SA: standalone; WP: waypoint). (a) Navigation performance under GNSS Outage 3, (b) Impact of waypoint density on vehicle guidance.

Another noteworthy point concerns the error in projection between geodetic and ENU coordinates. When employing a map projected to a single ENU plane for long-term planning over a 2-km straight-line trajectory, a drift occurs in the geodetic frame due to the map not being re-instantiated. Figure 17 illustrates this drift. It is important to note that the test cases presented involve extended durations and travel distances, which may not entirely mimic real-world scenarios. However, such GNSS scenarios could conceivably occur beneath elevated train railways, as seen in cities like New York City or Chicago, USA, where GNSS signals may be unreliable over similar trajectory segments. To address the error in projection, one approach is to re-initialize the map in ENU coordinates whenever a reliable GNSS fix becomes available. Alternatively, the Universal Transverse Mercator coordinate system can be used, or a precise and accurate localization paradigm relative to the geocentric coordinate system can be established to reset this error. In parallel, emerging approaches based on secure cellular vehicle-to-everything (C-V2X) technology, integrated with cloud-based mapping services, offer the capability to dynamically incorporate high-fidelity environmental priors, including road boundaries, lane structure, and obstacles. These avenues offer promising prospects for future research.

FIGURE 17

Navigation drift in Outage 2 due to prolonged use of a single ENU map over distances exceeding 2 km without re-instantiation

6 CONCLUSION

We have presented a lightweight navigation architecture that maintains reliable vehicle localization and navigational accuracy during extended GNSS outages by leveraging vehicular forward velocity and publicly available topological maps. Our first contribution is a forward-velocity fusion engine (OVIF), which adaptively integrates stereo VO-derived speed with OBD-based WSS measurements using a linear Kalman filter informed by data protocols, sensor errors, and empirical analysis. We also introduced a novel SS-OR module that reduces drift in long-distance VO in urban environments. Next, we developed WiL, integrating Toronto’s topological maps (road edges and centerlines) to estimate lateral motion and reduce inertial drift. Experimental results show that coupling OVIF with these map-based constraints yields substantial accuracy gains over baselines that omit either component, particularly in challenging urban environments. Future research will investigate richer map priors (e.g., lane-level geometry), improved uncertainty modeling for pseudomeasurement integration, and secure connected-vehicle inputs (e.g., C-V2X) with cyber-physical integrity monitoring to detect and isolate spoofed or compromised measurements through residual-based consistency checks.

CONFLICT OF INTEREST

The authors declare that there are no conflicts of interest regarding the publication of this paper.

HOW TO CITE THIS ARTICLE:

Ragab, H., Givigi, S., & Noureldin, A. (2026). Waypoint-informed localization (WiL): A novel integration of 2D maps and onboard sensors for robust GNSS-denied navigation. NAVIGATION, 73. https://doi.org/10.33012/navi.767

ACKNOWLEDGMENTS

We thank the City of Toronto for the topological maps (licensed under Open Government Licences). This work was supported by the Natural Sciences and Engineering Research Council of Canada under grants RGPIN-2020-03900 and ALLRP-560898-20.

This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited.

REFERENCES

↵
Aqel, M. O. A., Marhaban, M. H., Saripan, M. I., & Ismail, N. B. (2016). Review of visual odometry: Types, approaches, challenges, and applications. SpringerPlus, 5(1), 1897. https://link.springer.com/article/10.1186/s40064-016-3573-7
Google Scholar
↵
Badrinarayanan, V., Kendall, A., & Cipolla, R. (2017). SegNet: A deep convolutional encoderdecoder architecture for image segmentation.IEEE Transactions on Pattern Analysis and Machine Intelligence, 39(12), 2481-2495. https://doi.ieeecomputersociety.org/10.1109/TPAMI.2016.2644615
Google Scholar OpenURL CrossRef
↵
Bescos, B., Campos, C., Tardos, J. D., & Neira, J. (2021). DynaSLAM II: Tightly-coupled multiobject tracking and SLAM.IEEE Robotics and Automation Letters, 6(3), 5191-5198. https://doi.org/10.1109/LRA.2021.3068640
Google Scholar OpenURL
↵
Bescos, B., Facil, J. M., Civera, J., & Neira, J. (2018). DynaSLAM: Tracking, mapping, and inpainting in dynamic scenes.IEEE Robotics and Automation Letters, 3(4), 4076-4083. https://doi.org/10.1109/LRA.2018.2860039
Google Scholar OpenURL
↵
Chen, D., Cho, K.-T., Han, S., Jin, Z., & Shin, K. G. (2015). Invisible sensing of vehicle steering with smartphones. In Proceedings of the 13th Annual International Conference on Mobile Systems, Applications, and Services (pp. 1-13). https://doi.org/10.1145/2742647.2742659
Google Scholar
↵
Chen, L.-C., Zhu, Y., Papandreou, G., Schroff, F., & Adam, H. (2018). Encoder-decoder with atrous separable convolution for semantic image segmentation. In V.Ferrari, M.Hebert, C.Sminchisescu, & Y.Weiss (Eds.), Computer vision - ECCV 2018 (pp. 833-851). Springer International Publishing. https://link.springer.com/chapter/10.1007/978-3-030-01234-2_49
Google Scholar
↵
Cheng, J., Zhang, L., Chen, Q., Fu, Z., & Du, L. (2024). High precision and robust vehicle localization algorithm with visual-LiDAR-IMU fusion. IEEE Transactions on Vehicular Technology, 73(8), 11029-11043. https://doi.org/10.1109/TVT.2024.3379435
Google Scholar OpenURL
↵
Cho, Y., Kim, G., Lee, S., & Ryu, J.-H. (2022). OpenStreetMap-based LiDAR global localization in urban environment without a prior LiDAR map. IEEE Robotics and Automation Letters, 7(2), 4999-5006. https://doi.org/10.1109/LRA.2022.3152476
Google Scholar OpenURL
↵
City of Toronto. (2017). Road engineering design guidelines 2.0 lane widths. https://www.toronto.ca/wp-content/uploads/2017/11/921b-ecs-specs-roaddg-Lane_Widths_Guideline_Version_2.0_Jun2017.pdf
Google Scholar
↵
City of Toronto. (2017a, August). Open data. Retrieved July 1, 2023, from https://www.toronto.ca/city-government/data-research-maps/open-data/
Google Scholar
↵
Detone, D., Malisiewicz, T., & Rabinovich, A. (2018). SuperPoint: Self-supervised interest point detection and description. In IEEE Computer Society Conference on Computer Vision and Pattern Recognition Workshops. https://doi.org/10.1109/CVPRW.2018.00060
Google Scholar
↵
Forster, C., Zhang, Z., Gassner, M., Werlberger, M., & Scaramuzza, D. (2017). SVO: Semidirect visual odometry for monocular and multicamera systems. IEEE Transactions on Robotics, 33(2), 249-265.
Google Scholar OpenURL
↵
Fox, D., Burgard, W., & Thrun, S. (1997). The dynamic window approach to collision avoidance. IEEE Robotics & Automation Magazine, 4(1), 23-33. https://doi.org/10.1109/100.580977
Google Scholar OpenURL CrossRef Web of Science
↵
Fuentes-Pacheco, J., Ruiz-Ascencio, J., & Rendon-Mancha, J. M. (2015). Visual simultaneous localization and mapping: A survey. Artificial Intelligence Review, 43(1), 55-81. https://link.springer.com/article/10.1007/s10462-012-9365-8
Google Scholar OpenURL
↵
Grewal, M. S., & Andrews, A. P. (2008). Kalman filtering: Theory and practice using MATLAB (3rd ed.). John Wiley & Sons, Inc. https://onlinelibrary.wiley.com/doi/book/10.1002/9780470377819
Google Scholar
↵
Gross, M. (2016). A planet with two billion cars. Current Biology, 26(8), R307-R310. https://doi.org/10.1016/j.cub.2016.04.019
Google Scholar OpenURL
↵
Ma, W.-C., Wang, S., Brubaker, M. A., Fidler, S., & Urtasun, R. (2017). Find your way by observing the sun and other semantic cues. In 2017 IEEE International Conference on Robotics and Automation (ICRA) (pp. 6292-6299). https://doi.org/10.1109/ICRA.2017.7989744
Google Scholar
↵
Mounier, E., Elhabiby, M., Korenberg, M., & Noureldin, A. (2025). LiDAR-based multisensor fusion with 3-D digital maps for high-precision positioning. IEEE Internet of Things Journal, 12(6), 7209-7224. https://doi.org/10.1109/JIOT.2024.3492913
Google Scholar OpenURL
↵
Mur-Artal, R., & Tardos, J. D. (2017). ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras. IEEE Transactions on Robotics, 33(5), 1255-1262. https://doi.org/10.1109/TRO.2017.2705103
Google Scholar OpenURL
↵
Noureldin, A., Karamat, T. B., & Georgy, J. (2013). Fundamentals of inertial navigation, satellitebased positioning and their integration. Springer International Publishing. https://link.springer.com/book/10.1007/978-3-642-30466-8
Google Scholar
↵
Ntpd(8): Network time protocol daemon - Linux man page. (n.d.). Retrieved October 23, 2023, from https://linux.die.net/man/8/ntpd
Google Scholar
↵
Pek, C., Manzinger, S., Koschi, M., & Althoff, M. (2020). Using online verification to prevent autonomous vehicles from causing accidents. Nature Machine Intelligence, 2(9), 518-528. https://www.nature.com/articles/s42256-020-0225-y
Google Scholar OpenURL
↵
Ragab, H. (2024, July). Integration of topological maps with GNSS and onboard sensors for robust land vehicle navigation[Doctoral dissertation, Queen’s University]. https://hdl.handle.net/1974/33163
Google Scholar
↵
Ragab, H., Abdelaziz, S. K., Elhabiby, M., Givigi, S., & Noureldin, A. (2020, September). Machine learning-based visual odometry uncertainty estimation for low-cost integrated land vehicle navigation. In Proceedings of the 33rd International Technical Meeting of the Satellite Division of the Institute of Navigation, (ION GNSS+ 2020) (pp. 2569-2578). https://doi.org/10.33012/2020.17740
Google Scholar
↵
Ragab, H., Elhabiby, M., Givigi, S., & Noureldin, A. (2020, April). The utilization of DNN-based semantic segmentation for improving low-cost integrated stereo visual odometry in challenging urban environments. In Proceedings of the 2020 IEEE/ION Position, Location and Navigation Symposium (PLANS) (pp. 960-966). https://ieeexplore.ieee.org/document/9110144
Google Scholar
↵
Ragab, H., Givigi, S., & Noureldin, A. (2023). Enhancing urban vehicular navigation: Improving classical topological map matching through ray-casting. The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, XLVIII-1-W2-2023, 915-922. https://doi.org/10.5194/isprs-archives-xlviii-1-w2-2023-915-2023
Google Scholar
↵
Ragab, H., Givigi, S., & Noureldin, A. (2025, April-May). Automotive speed estimation: Sensor types and error characteristics from OBD-II to ADAS. In Proceedings of the 2025 IEEE/ION Position, Location and Navigation Symposium (PLANS) (pp. 124-130). https://ieeexplore.ieee.org/document/11028310
Google Scholar
↵
Rashed, M. A., Elghamrawy, H., Elhabiby, M., Korenberg, M. J., & Noureldin, A. (2024). Enhanced land vehicle positioning in challenging GNSS urban environments utilizing automotive RADARs. IEEE Transactions on Vehicular Technology, 73(2), 1637-1651. https://doi.org/10.1109/TVT.2023.3316612
Google Scholar OpenURL
↵
Reid, T. G. R., Houts, S. E., Cammarata, R., Mills, G., Agarwal, S., Vora, A., & Pandey, G. (2019). Localization requirements for autonomous vehicles. SAE International Journal of Connected and Automated Vehicles, 2(3), 173-190. https://doi.org/10.4271/12-02-03-0012
Google Scholar OpenURL
↵
Saalfeld, A. (1999). Topologically consistent line simplification with the Douglas-Peucker algorithm. Cartography and Geographic Information Science, 26(1), 7-18. https://doi.org/10.1559/152304099782424901
Google Scholar OpenURL
↵
Saleh, S., El-Wakeel, A. S., & Noureldin, A. (2022). 5G-enabled vehicle positioning using EKF with dynamic covariance matrix tuning. IEEE Canadian Journal of Electrical and Computer Engineering, 45(3), 192-198. https://doi.org/10.1109/ICJECE.2022.3187348
Google Scholar OpenURL
↵
Sarlin, P.-E., DeTone, D., Malisiewicz, T., & Rabinovich, A. (2020). SuperGlue: Learning feature matching with graph neural networks. In 2020 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR) (pp. 4937-4946). https://doi.ieeecomputersociety.org/10.1109/CVPR42600.2020.00499
Google Scholar
↵
Scaramuzza, D. (2011). 1-point-RANSAC structure from motion for vehicle-mounted cameras by exploiting non-holonomic constraints. International Journal of Computer Vision, 95(1), 74-85. https://link.springer.com/article/10.1007/s11263-011-0441-3
Google Scholar OpenURL
↵
Scaramuzza, D., & Fraundorfer, F. (2011). Visual odometry: Part I: The first 30 years and fundamentals. IEEE Robotics & Automation Magazine. https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=6096039
Google Scholar
↵
Sharma, O., Sahoo, N. C., & Puhan, N. B. (2021). Recent advances in motion and behavior planning techniques for software architecture of autonomous vehicles: A state-of-the-art survey. Engineering Applications of Artificial Intelligence, 101,104211. https://doi.org/10.1016/j.engappai.2021.104211
Google Scholar
↵
Singh, S. P., Mazotti, B., Mayilvahanan, S., Li, G., Rajani, D. M., & Ghaffari, M. (2023, April). Twilight SLAM: A comparative study of low-light visual SLAM pipelines. arXiv. https://doi.org/10.48550/arXiv.2304.11310
Google Scholar
↵
Siuhi, S., & Mwakalonge, J. (2016). Opportunities and challenges of smart mobile applications in transportation. Journal of Traffic and Transportation Engineering (English Edition), 3(6), 582-592. https://doi.org/10.1016/j.jtte.2016.11.001
Google Scholar OpenURL
↵
Stelzer, A., Hirschmüller, H., & Görner, M. (2012). Stereo-vision-based navigation of a six-legged walking robot in unknown rough terrain. The International Journal of Robotics Research, 31(4), 381-402. https://doi.org/10.1177/0278364911435161
Google Scholar OpenURL CrossRef
↵
Subaru EyeSight - Canada. (2025). Retrieved February 5, 2024, from https://www.subaru.ca/WebPage.aspx?WebSiteID=282&WebPageID=18112
Google Scholar
↵
Tang, C., Wang, C., Zhang, L., Zhang, Y., & Song, H. (2024). Vehicle heterogeneous multi-source information fusion positioning method. IEEE Transactions on Vehicular Technology, 73(9), 12597-12613. https://doi.org/10.1109/TVT.2024.3393720
Google Scholar OpenURL
↵
Urmson, C., Anhalt, J., Bagnell, D., Baker, C., Bittner, R., Clark, M. N., Dolan, J., Duggins, D., Galatali, T., Geyer, C., Gittleman, M., Harbaugh, S., Hebert, M., Howard, T. M., Kolski, S., Kelly, A., Likhachev, M., McNaughton, M., Miller, N., . . . Ferguson, D. (2009). Autonomous driving in urban environments: Boss and the urban challenge. In M.Buehler, K.Iagnemma, & S.Singh (Eds.), The DARPA urban challenge: Autonomous vehicles in city traffic (pp. 1-59). Springer. https://link.springer.com/chapter/10.1007/978-3-642-03991-1_1
Google Scholar
↵
Wei, J., Snider, J. M., Gu, T., Dolan, J. M., & Litkouhi, B. (2014). A behavioral planning framework for autonomous driving. 2014 IEEE Intelligent Vehicles Symposium Proceedings, 458-464. https://doi.org/10.1109/IVS.2014.6856582
Google Scholar
↵
Wen, S., Li, X., Liu, X., Li, J., Tao, S., Long, Y., & Qiu, T. (2023). Dynamic SLAM: A visual SLAM in outdoor dynamic scenes. IEEE Transactions on Instrumentation and Measurement, 1-1. https://doi.org/10.1109/TIM.2023.3317378
Google Scholar
↵
Zhao, Y., & Vela, P. A. (2020). Good feature matching: Toward accurate, robust VO/VSLAM with low latency. IEEE Transactions on Robotics, 36(3), 657-675. https://doi.org/10.1109/TRO.2020.2964138
Google Scholar OpenURL
↵
Zhu, C., Meurer, M., & Günther, C. (2022). Integrity of visual navigation—developments, challenges, and prospects. NAVIGATION, 69(2). https://navi.ion.org/content/69/2/navi.518
Google Scholar
↵
Ziegler, J., Bender, P., Schreiber, M., Lategahn, H., Strauss, T., Stiller, C., Dang, T., Franke, U., Appenrodt, N., Keller, C. G., Kaus, E., Herrtwich, R. G., Rabe, C., Pfeiffer, D., Lindner, F., Stein, F., Erbs, F., Enzweiler, M., Knoppel, C., . . . Zeeb, E. (2014). Making bertha drive-an autonomous journey on a historic route. IEEE Intelligent Transportation Systems Magazine. https://doi.org/10.1109/MITS.2014.2306552
Google Scholar

PDF

Previous Next

Waypoint-Informed Localization (WiL): A Novel Integration of 2D Maps and Onboard Sensors for Robust GNSS-Denied Navigation

Abstract

NOMENCLATURE

1 INTRODUCTION

2 RELATED WORK

2.1 Visual and Wheel Odometry

2.2 Addressing the Impact of Dynamic Objects on VBN

2.3 Usage of Maps for Guidance, Navigation, and Control

3 TOPOLOGICAL MAP

4 METHODOLOGY

4.1 System Overview

4.2 Stereo VBN System

4.2.1 Inputs

4.2.2 Feature Detection and Association

4.2.3 Improved Outlier Rejection

Semantic segmentation-based outlier rejection

4.2.4 Motion Estimation

4.3 OBD-II Velocity Integration Framework

4.4 Waypoint Extractor

4.5 Waypoint-Informed Localization (WiL)

4.5.1 Distance and Angle Cost

4.5.2 Speed Cost

4.5.3 Collision Avoidance

4.5.4 Proximity to Obstacles

4.5.5 Generation of WiL Pseudomeasurements

4.6 3D Reduced Inertial Sensor System

4.7 EKF for the WiL/3D-RISS System

4.7.1 System Model

4.7.2 Measurement Model

4.7.3 Measurement Correlation and Filter Consistency

4.8 Computational Considerations

5 EXPERIMENTAL WORK AND RESULTS

5.1 Experimental Setup

5.1.1 Data Capture

5.1.2 Time Synchronization

5.1.3 Multi-Sensor Calibration

5.2 Road Test Experiments

5.3 Results and Discussion

5.3.1 Evaluation Metrics

5.3.2 Along-Track Performance Analysis

5.3.3 Navigation Performance Analysis

6 CONCLUSION

CONFLICT OF INTEREST

HOW TO CITE THIS ARTICLE:

ACKNOWLEDGMENTS

REFERENCES