Visual-Inertial-Wheel Odometry with Slip Compensation and Dynamic Feature Elimination

Abstract

Inertial navigation systems augmented with visual and wheel odometry measurements have emerged as a robust solution to address uncertainties in robot localization and odometry. This paper introduces a novel data-driven approach to compensate for wheel slippage in visual-inertial-wheel odometry (VIWO). The proposed method leverages Gaussian process regression (GPR) with deep kernel design and long short-term memory (LSTM) layers to model and mitigate slippage-induced errors effectively. Furthermore, a feature confidence estimator is incorporated to address the impact of dynamic feature points on visual measurements, ensuring reliable data integration. By refining these measurements, the system utilizes a multi-state constraint Kalman filter (MSCKF) to achieve accurate state estimation and enhanced navigation performance. The effectiveness of the proposed approach is demonstrated through extensive simulations and experimental validations using real-world datasets. The results highlight the ability of the method to handle challenging terrains and dynamic environments by compensating for wheel slippage and mitigating the influence of dynamic objects. Compared to conventional VIWO systems, the integration of GPR and LSTM layers significantly improves localization accuracy and robustness. This work paves the way for deploying VIWO systems in diverse and unpredictable environments, contributing to advancements in autonomous navigation and multi-sensor fusion technologies.

1. Introduction

In wheeled mobile robot navigation, visual inertial navigation system (VINS) algorithms are considered state of the art for localization in GPS-denied environments [1,2,3]. The initialization procedure of VINS algorithms significantly influence the overall accuracy and robustness of the algorithm [4,5]. The initialization methods can be categorized as joint versus disjoint ones [6]. Joint methods estimate both the camera trajectory and inertial parameters simultaneously within a tightly coupled framework. This approach ensures global consistency in the estimates, reducing error propagation. However, joint methods often require higher computational resources and are more complex to implement, making them less practical for real-time applications. On the other hand, the disjoint methods assume that an accurate camera trajectory can be derived through monocular vision, and the inertial parameters are then estimated by using this trajectory.

Many of these algorithms rely on the use of monocular vision due to its affordability. However, they present inherent challenges, particularly with degenerate motions that introduce unobservable directions in the estimation process [7]. These motions, often uniform linear or circular, make scale and orientation unobservable, compromising the system’s robustness and accuracy [8]. To address these issues, researchers have integrated wheel odometry (WO) measurements into VINS, resulting in robust initialization and improved performance [9,10,11]. The existing VIWO schemes mainly focus on fusing WO information in the backend while ignoring it in the front end [12] and often assume little to no wheel slippage, which does not hold in practice [13]. Furthermore, side-slip effects pose challenges for tracking cornering trajectories, which require additional adaptive control techniques, observer design, and trajectory planning schemes [14,15]. Hence, when augmenting a VINS with a WO one, the longitudinal and lateral errors due to wheel slippage need to be taken into account and compensated.

Many studies in the literature on slip compensation rely on accurate slip ratio models [16], which may not hold under varying surface conditions. As an alternative, Gaussian process (GP) modeling can encode prior distributions over functions, allowing updates to derive posterior distributions based on new test data. Leveraging this feature of GP modeling, regression tasks can be carried out where estimates of a function model are obtained given observed input–output data [17]. Notably, in the recent past, GP regression (GPR) has been used for learning the inverse dynamics of systems [18,19]. Unlike these model-based approaches, Liu et al. address slip effects by detecting and omitting updates during slip events using a threshold-based method, ensuring real-time performance but leaving the underlying slip dynamics unmodeled [20]. In contrast, our work utilizes GPR alongside other tools to estimate wheel slippage continuously, providing a more robust and adaptive compensation strategy.

In visual odometry (VO) estimations using sparse features, random sample consensus (RANSAC) is used as the preferred outlier handling mechanism [21]. However, RANSAC’s performance is inadequate when a significant portion of feature points are in motion or when dynamic objects obstruct the camera’s field of view (FOV) [22]. To address this issue, additional sensory data, such as those obtained from IMUs, are utilized to evaluate the reliability of matched features and eliminate dynamic feature points [23], which represent a preliminary version of the results of this article.

Recent state-of-the-art visual-inertial-wheel odometry (VIWO) algorithms have focused on addressing key challenges such as long-term drift, sensor calibration, and multi-modal data integration. The robust neural gyroscope calibration VIWO (RNGC-VIWO) system by Zhi et al. reduces gyroscope drift and improves localization accuracy through neural gyroscope calibration, while the approach by Lee et al. performs online calibration of wheel encoders’ intrinsic and extrinsic parameters to maintain consistent performance across various terrains [24]. Additionally, the partial invariant extended Kalman filter VIWO (PIEKF-VIWO) algorithm by Hua et al. enhances positioning accuracy by leveraging a partial invariant extended Kalman filter to optimally fuse wheel measurements with kinematic constraints [25]. Similarly, Zhou et al. propose the real-time appearance-based mapping VIWO (RTABMAP-VIWO) method, which uniquely incorporates wheel odometry and IMU data into RTABMAP using an extended Kalman filter (EKF) to reduce local cumulative errors in simultaneous localization and mapping (SLAM) systems [26]. However, despite these advancements, a common area for improvement remains in accurately compensating for wheel slippage, which can introduce significant errors in localization, particularly in challenging terrains or during dynamic maneuvers. Most existing methods do not fully account for the longitudinal and lateral slip effects, leading to reduced accuracy in real-world applications.

Two other effective technical tools used in the literature for improving VINS and VIWO results via modeling and mitigating slippage-induced errors are recurrent neural network (RNN) and long short-term memory (LSTM) [27]. These tools are particularly useful in learning sequential correlations of a sensor’s error propagation with time to more accurately position a vehicle. LSTM networks have also been employed in global navigation satellite system (GNSS)-free navigation systems to correct drift in velocity estimates derived from optical flow measurements [28]. In such drift correction, optical odometry and radar height estimates are fused with LSTM-based velocity corrections to mitigate the effects of GNSS signal loss. Similarly, deep-learning-based visual odometry (VO) methods, such as recurrent convolutional neural networks (RCNNs) and bi-directional LSTM (Bi-LSTM) architectures, have been used to improve pose estimation accuracy by capturing both spatial and temporal dependencies [29,30]. These methods, including frameworks like MagicVO, leverage CNNs for feature extraction and Bi-LSTM for sequential modeling, outperforming traditional VO systems in pose estimation accuracy and generalization ability.

A preliminary version of the results of this article has appeared in [23]. In contrast to [23], this work introduces a data-driven methodology employing GPR and LSTM networks to address wheel slippage, which is not previously addressed in [23]. This integration of GPR and LSTM enhances the prior approach substantially via effectively mitigating errors induced by slippage; thus, it significantly improves the precision of wheel odometry measurements. Furthermore, this work advances our earlier methodology in [23] by integrating a feature confidence estimator that utilizes additional sensory data. This ensures that only reliable sensor data influences the system’s state estimation. The advancements presented in this work are rigorously validated through extensive experimental trials using real-world datasets, demonstrating the methodologies’ capability to effectively manage challenging terrains and dynamic environments. In addition, a GPR [31] scheme is designed using a deep kernel approach with LSTM layers that are known to capture spatio-temporal variations [32]. The WO error residuals are learned between the physical prediction and the predicted ground truth data obtained using a differential drive kinematic model and the history of control input values. The refined WO measurements are then fused in a multi-state constraint Kalman filter (MSCKF) [33] framework for state estimation. Comparative simulations based on real-world data and indoor experiments reveal the performance improvement using the proposed approach.

In summary, this paper introduces a novel data-driven methodology to enhance visual-inertial-wheel odometry (VIWO) by addressing wheel slippage and improving localization accuracy. The key contributions of this work are

(i)

The use of a data-driven methodology employing GPR and LSTM networks to address wheel slippage. This integration of GPR and LSTM presents a substantial enhancement over prior approach by effectively mitigating errors induced by slippage; thus, it significantly improves the precision of wheel odometry measurements.

(ii)

The incorporation of a feature confidence estimator that utilizes additional sensory data. This ensures that only reliable sensor data influence the system’s state estimation.

A comprehensive comparative analysis of the key features of our work with respect to some related state-of-art works in the literature on VIWO/VINS and some recently developed deep-learning-based VO algorithms are presented in

Table A1. Comparative analysis of the proposed approach’s key features with respect to some related VIWO/VINS designs in the literature.

Category	Proposed Approach	VINS with WO [1,6,24]	Filter-Based VIWO [5,25,35]	VIWO with Slip Detection [20]
Objective	Compensate for wheel slippage and improve VIWO accuracy.	Refine inertial measurements and improve the accuracy of VIWO.	Fuse WO and VINS for improved localization.	Detect and mitigate slip in real time.
Methodology	GPR and LSTM to model and correct slip errors; integrates a feature confidence estimator.	Neural gyroscope calibration to reduce drift in localization.	Partial invariant EKF (PIEKF) to optimally fuse WO and VINS.	Threshold-based slip detection and rejection.
Key Innovation and Limitations	Integrates adaptive slip correction with dynamic feature elimination for robust VIWO.	Uses deep learning for enhanced gyroscope drift compensation. Affected by wheel slip and inherent IMU drifts, leading to localization errors. Ref. [24] has focused on reducing the gyroscope drift.	Leverages kinematic constraints to refine odometry accuracy but assumes minimal slip, causing errors in dynamic conditions.	Ensures real-time slip detection but does not model slip dynamics, leading to accumulated drift over time.

and

Table A2. Comparative feature summary of the proposed approach with respect to some recently developed deep-learning-based VO algorithms in the literature.

Feature	Proposed	GNSS-Free UAV Navigation [28]	VO Using RCNNs [29]	VO Using CNN and Bi-LSTM [30]
Objective	Uses GPR and LSTM to compensate for wheel slippage, enhancing VO accuracy.	LSTM-based GNSS-free UAV navigation using optical odometry and radar.	Monocular VO with deep RCNNs estimating poses from raw RGB images.	Monocular VO with CNN and Bi-LSTM for 6-DoF pose estimation.
Methodology	GPR-LSTM fusion to correct slippage-induced errors in VO.	LSTM for drift correction and radar for scale estimation.	CNN for feature extraction, RNN for sequential motion modeling.	Hybrid CNN for features, Bi-LSTM for sequence learning.
Key Innovation and Limitations	Combines GPR and LSTM for adaptive slippage compensation, improving localization.	Uses LSTM for velocity correction and radar for scale resolution.	End-to-end learning without explicit VO components.	Bi-LSTM utilizes past and future frames for improved estimation.
Main Contributions	Enhances wheel odometry with ML and sensor fusion.	Reliable UAV navigation in GNSS-denied areas.	VO without feature engineering or scale priors.	End-to-end VO without geometric constraints.
Technological Focus	Ground navigation with: (1) Slippage compensation, (2) Robust localization, (3) Improved odometry.	UAV navigation, GNSS signal loss mitigation.	Learning-based feature extraction and motion estimation.	Enhancing feature extraction and motion estimation.
Validation	Tested on KAIST dataset, showing robust performance in dynamic terrains.	Flight-tested, reducing velocity errors in GNSS outages.	Validated on KITTI dataset, competitive with traditional VO.	Tested on KITTI, ETH-asl-cla, suited for structured driving.
Advantages	Accurate slippage compensation, improving localization in uneven terrains.	Reliable UAV navigation under GNSS loss.	Adaptable VO without manual tuning.	No manual feature extraction, robust across environments.

in the Appendix A. The remainder of this article is structured as follows: Section 2 outlines the system description and preliminary details. Section 3 introduces the proposed method for compensating wheel slippage and other inherent errors associated with WO. The experimental results are presented and analyzed in Section 6. Finally, Section 7 concludes the paper and discusses future research directions.

2. The System Description and Preliminaries

In this section, we briefly describe the key components of the tightly coupled visual inertial wheel odometry (VIWO) system. To describe the system, we first define the frame notations. The notations ${}^b(·)$, ${}^w(·)$, ${}^c(·)$, ${}^o(·)$ denote the body-fixed IMU {b}, world {w}, camera {c}, and WO {o} frames, respectively. The origin of frame {o} is at the center of the axle connecting the two wheels of the robot. Furthermore, $\mathit{R}$ represents a rotation matrix, and ${}^{o_k}\mathit{R}$ represents the rotation in {o} frame when the $k\mathrm{th}$ image is taken. The following sections describe the key measurements of the system. The system receives measurements from camera, IMU, and wheel encoders. Figure 1 represents the key overall system components of our proposed method.

2.1. IMU Kinematics Model

An IMU provides angular velocity ${\mathsf{\omega }}_m$ and linear acceleration ${\mathit{a}}_m$, which can be measured as [34]

$${\mathsf{\omega }}_m=\mathsf{\omega }+{\mathit{b}}_g+{\mathit{n}}_g,$$

(1)

$$\mathit{S}{\mathit{a}}_m=\mathit{a}+{}_G{}^I\mathit{R}\mathit{g}+{\mathit{b}}_a+\mathit{S}{\mathit{n}}_a,$$

(2)

where $\mathsf{\omega }\in {\mathcal{R}}^3$ and $\mathit{a}\in {\mathcal{R}}^3$ are the true angular velocity and acceleration, $\mathit{g}$ is a vector of gravitational acceleration, $\mathit{S}$ is a symmetric matrix representing the scale factors, and ${\mathit{n}}_g$ and ${\mathit{n}}_a$ are the considered zero-mean Gaussian noises with $\mathbb{E}\left[n_g\right]=0,\mathbb{E}\left[n_a\right]=0$.

Both scale factors and biases can be determined and integrated into the IMU signal processing prior to their application in the multi state constraint Kalman (MSCKF) measurement update. The scale factor of the IMU is determined based on the methodology outlined in reference [5,35]. The IMU biases ${\mathit{b}}_a$ and ${\mathit{b}}_g$ are computed through a least squares optimization procedure. To determine the gyroscope bias, the optimization aims to minimize the difference between the rotation calculated from the pre-integrated IMU data and the one derived from WO [36]. Similarly, the accelerometer bias is estimated using the WO translation.

Once the biases are estimated or known, one can compensate for the bias and determine the translation of the robot with respect to the world frame ${}_b{}^w\mathit{p}$ and the rotation $\mathit{q}$ of the robot ${}_w{}^{b_k}\mathit{q}$, which can be determined by solving the following differential kinematics equations [37]:

$${\displaystyle \frac{\mathrm{d}}{\mathrm{d}t}}\left({}_w{}^b\mathit{q}\right)={\displaystyle \frac{1}{2}}\Omega ({\mathsf{\omega }}_m-{\mathit{b}}_g){}_w{}^b\mathit{q}$$

(3)

$${\displaystyle \frac{\mathrm{d}}{\mathrm{d}t}}\left({}_b{}^w\mathit{p}\right)={}_b{}^w\mathit{v}$$

(4)

$${\displaystyle \frac{\mathrm{d}}{\mathrm{d}t}}\left({}_b{}^w\mathit{v}\right)=\mathit{S}{\mathit{a}}_m-{}_G{}^I\mathit{R}\mathit{g}-{\mathit{b}}_a$$

(5)

where $\Omega (·)$ is the skew-symmetric operator on a 3-dimensional vector. These three equations serve as references for the discrete-time process model of the proposed MSCKF.

2.2. Camera Measurement Model

The visual measurements corresponding to feature i measured by the camera projection function $\mathsf{\pi }\left({[x,y,z]}^{\top }\right)={\left[{\displaystyle \frac{x}{z}},{\displaystyle \frac{y}{z}}\right]}^{\top }$, are given as

$${\mathit{z}}_{m\left(i\right)}=\mathsf{\pi }\left({}_{f\left(i\right)}{}^c\mathit{p}\right)+{\mathit{n}}_i,$$

(6)

$${}_{f\left(i\right)}{}^c\mathit{p}={}_b{}^c\mathit{R}{}_w{}^b\mathit{R}({}_{f\left(i\right)}{}^w\mathit{p}-{}_b{}^w\mathit{p})+{}_b{}^c\mathit{p},$$

(7)

where ${}_{f\left(i\right)}{}^w\mathit{p}$ is the 3D feature position, ${\mathit{n}}_i$ is the measurement noise, ${}_b{}^c\mathit{R}$ and ${}_b{}^c\mathit{p}$ are the camera to IMU extrinsic information. Equation (6) is primarily used as a reference for a measurement model on refined feature points after static feature point selection. As stated in [23], nullspace marginalization can be applied to (6) to obtain a new measurement equation that is independent of feature positions, resulting in a substantial reduction in computation and the need for storing feature positions in the state vector of MSCKF.

Filtered measurement estimates ${\widehat{\mathit{z}}}_{m\left(i\right)}$ and the state estimate $\widehat{\mathit{x}}$ are obtained by Kalman filtering based on the residuals ${\tilde{\mathit{z}}}_{m\left(i\right)}={\mathit{z}}_{m\left(i\right)}-{\widehat{\mathit{z}}}_{m\left(i\right)}$, $\tilde{\mathit{x}}=\mathit{x}-\widehat{\mathit{x}}$ and the measurement model

$${\tilde{\mathit{z}}}_{m\left(i\right)}={\mathit{H}}_{x,i}\tilde{\mathit{x}}+{\mathit{H}}_{f,i}{}_{f\left(i\right)}{}^w\tilde{\mathit{p}}+{\mathit{n}}_i$$

(8)

where ${\mathit{H}}_{x,i}$ is the Jacobian related to the robot state, and ${\mathit{H}}_{f,i}$ is the Jacobian related to feature position. Stacking the residuals ${\tilde{\mathit{z}}}_{m\left(i\right),k}$ for all feature points i and computing the nullspace marginalization as in (8), a camera measurement residual process model independent of the feature positions is obtained as

$${\tilde{\mathit{z}}}_{m\left(i\right)}={\mathit{H}}_x\tilde{\mathit{x}}+{\mathit{n}}_i,$$

(9)

which is directly used in the multi state constraint Kalman filter (MSCKF) design to estimate the robot states, as in [11]. Since the feature positions are instantly marginalized from the state vector, this improves the computational efficiency of the filter, making it suitable for resource constrained platforms and applications.

3. Overall Methodology

The overall structure of the proposed multi-state constraint Kalman filter (MSCKF) is discussed in this section. Figure 1 shows the block diagram with key components of the proposed system. The MSCKF takes three sets of measurements: camera images, 6-axis IMU measurements, and wheel encoder measurements. Each set of raw data measurements is processed in an interconnected manner before being fed through the measurement update of the filter.

3.1. Robot State Vector

For any $k\mathrm{th}$ iteration of sampling, the computation or sampling is performed at a sampled time $t_k$. At each sampled time, the state vector ${\mathit{x}}_k={\left[\begin{array}{ccc}{\mathit{x}}_{I_k}^{\top }& {\mathit{x}}_{C_k}^{\top }& {\mathit{x}}_{O_k}^{\top }\end{array}\right]}^{\top }$ of the robot consists of the current inertial state,

$${\mathit{x}}_{I_k}={\left[\begin{array}{ccccc}{{}_w{}^{b_k}\overline{\mathit{q}}}^{\top }& {{}_{b_k}{}^w\mathit{p}}^{\top }& {{}_{b_k}{}^w\mathit{v}}^{\top }& {\mathit{b}}_g^{\top }& {\mathit{b}}_a^{\top }\end{array}\right]}^{\top },$$

(10)

and the lumped vector of the past n world to inertial frame transformations

$${\mathit{x}}_{C_k}={\left[\begin{array}{ccccc}{{}_w{}^{b_{k-1}}\overline{\mathit{q}}}^{\top }& {{}_{b_{k-1}}{}^w\mathit{p}}^{\top }& \cdots & {{}_w{}^{b_{k-n}}\overline{\mathit{q}}}^{\top }& {{}_{b_{k-n}}{}^w\mathit{p}}^{\top }\end{array}\right]}^{\top },$$

(11)

where ${}_w{}^{b_j}\overline{\mathit{q}}$ denotes the quaternion corresponding to the rotation ${}_w{}^{b_j}\mathit{R}$ and a translation ${}_{b_j}{}^w\mathit{p}$ at time step j, for $j=k-n,\cdots ,k$. ${}_{b_k}{}^w\mathit{p}$ and ${}_{b_k}{}^w\mathit{v}$ are the position and velocity vectors, and ${\mathit{b}}_g$ and ${\mathit{b}}_a$ are the gyroscope and accelerometer biases, respectively. Furthermore, ${\mathit{x}}_{O_k}={\left[\begin{array}{cc}{{}^{o_k}\overline{\mathit{q}}}_w^{\top }& {{}_{o_k}{}^w\mathit{p}}^{\top }\end{array}\right]}^{\top }$ consists of the rotation and position information obtained using wheel encoder data.

3.2. Process Model

The proposed MSCKF uses the generic IMU nonlinear kinematics process model to propagate the state $\mathit{x}$ by numerically integrating Equations (3)–(5) [38]. The discretized form of the integration can be described as

$${\mathit{x}}_{k+1}=f({\mathit{x}}_k,{\mathit{a}}_{\mathit{m}},{\mathsf{\omega }}_m,{\mathit{n}}_I),$$

(12)

where ${\mathit{n}}_I$ denotes zero-mean white Gaussian IMU measurement noise. The predicted state, $\mathit{x}$, in the prediction step is generated as

$${\check{\mathit{x}}}_k=f({\widehat{\mathit{x}}}_k,{\mathit{a}}_{\mathit{m}},{\mathsf{\omega }}_m,\mathbf{0}).$$

(13)

The associated predicted state covariance matrix, $\check{\mathit{P}}$, is propagated via

$${\check{\mathit{P}}}_k={\mathsf{\Phi }}_k{\widehat{\mathit{P}}}_k{\mathsf{\Phi }}_k^{\top }+{\mathit{Q}}_k,$$

(14)

where ${\widehat{\mathit{P}}}_k$ is the estimated state covariance matrix from the previous iteration, and ${\mathsf{\Phi }}_k$ and ${\mathit{Q}}_k$ are the system Jacobian and noise covariance matrices [39].

3.3. Dynamic Feature Elimination

The computed translation and rotation from the process model can also be used for calculating the corresponding fundamental matrix ${\mathit{F}}^{\prime }$ using the camera-IMU extrinsic parameters, which then be used in the dynamic point elimination method [23]. A fundamental matrix $\mathit{F}$ can also be calculated using the image data. With both fundamental matrices, a distance measure between the epipolar line and the corresponding features using both matrices can be computed and denoted as $d_{k,i}^1$ and $d_{k,i}^2$, respectively, via

$${[x,y,z]}^{\top }=\mathit{F}{{}_{f\left(i\right)}{}^c\mathit{p}}_k,{[x^{\prime },y^{\prime },z^{\prime }]}^{\top }={\mathit{F}}^{\prime }{{}_{f\left(i\right)}{}^c\mathit{p}}_{k+1}$$

(15)

$$d_{k,i}^1={\displaystyle \frac{\left|{{}_{f\left(i\right)}{}^c\mathit{p}}_{k+1}^{\top }\mathit{F}{{}_{f\left(i\right)}{}^c\mathit{p}}_k\right|}{\sqrt{{\left|\left|x\right|\right|}^2+{\left|\left|y\right|\right|}^2}}}$$

(16)

$$d_{k,i}^2={\displaystyle \frac{\left|{{}_{f\left(i\right)}{}^c\mathit{p}}_k^{\top }{\mathit{F}}^{\prime }{{}_{f\left(i\right)}{}^c\mathit{p}}_{k+1}\right|}{\sqrt{{\left|\left|x^{\prime }\right|\right|}^2+{\left|\left|y^{\prime }\right|\right|}^2}}}$$

(17)

A threshold $\rho$ is used to determine whether each matched feature point is static or dynamic via

$${\varphi }_{k,i}=\left\{\begin{array}{c}1,|d_{k,i}^1-d_{k,i}^2|\ge \rho \hfill \\ 0,|d_{k,i}^1-d_{k,i}^2|<\rho \hfill \end{array}\right.,$$

(18)

where ${\varphi }_{k,i}$ indicates the consistency between the $i\mathrm{th}$ feature point of the $k\mathrm{th}$ frame and the fundamental matrix. ${\varphi }_{k,i}$ is set to 0 if the feature point is consistent and 1 if otherwise.

Additionally, there can be a case when IMU data is invalid for the elimination method. To combat this issue, we use an additional threshold $\mu$ to keep track of match count.

$${\psi }_k=\left\{\begin{array}{c}0,\sum _{i=1}^m{\varphi }_{k,i}<\mu \hfill \\ 1,\sum _{i=1}^m{\varphi }_{k,i}\ge \mu \hfill \end{array}\right..$$

(19)

The dynamic point elimination will only use the IMU data if and only if the data are valid (i.e., ${\psi }_k=0$) and trustworthy. Alternatively, if the confidence in the IMU-based dynamic point elimination is insufficient, the system switches to a RANSAC-based outlier rejection scheme.

3.4. Measurement Update

After deriving the camera measurement functions from the provided measurement model in Equation (9) and wheel odometry from the wheel-slip compensation, the estimation of the robot’s pose $\mathit{x}$ is executed through an extended Kalman filter (EKF) process. The bearing measurement model in Equation (6) is reformulated in the following manner.

$${\mathit{z}}_{m,k}=\mathit{h}\left({\check{\mathit{x}}}_k\right)+{\mathit{n}}_{m,k}$$

(20)

where the measurement noise is assumed to be ${\mathit{n}}_{m,k}\sim \mathcal{N}(0,{\mathit{R}}_{m,k})$. (20) is then linearized as

$${\tilde{\mathit{z}}}_{m,k}={\mathit{H}}_k{\tilde{\mathit{x}}}_k$$

(21)

where ${\mathit{H}}_k$ is the measurement Jacobian. Using this linearized measurement model, the EKF update equations are set as

$${\widehat{\mathit{x}}}_{k+1}={\check{\mathit{x}}}_k+{\mathit{K}}_k{\tilde{\mathit{z}}}_{m,k},$$

(22)

$${\widehat{\mathit{P}}}_{k+1}={\check{\mathit{P}}}_k-{\mathit{K}}_k{\mathit{H}}_k{\check{\mathit{P}}}_k,$$

(23)

$${\mathit{K}}_k={\check{\mathit{P}}}_k{\mathit{H}}_k^{\top }{\left({\mathit{H}}_k{\check{\mathit{P}}}_k{\mathit{H}}_k^{\top }+{\mathit{R}}_{m,k}\right)}^{-1},$$

(24)

where ${\mathit{K}}_k$ is the Kalman gain.

In WO pre-integration, one common approach is to integrate the velocity kinematics directly. However, in this research, the proposed scheme uses the odometry increments estimated from the wheel slip compensation algorithm, which is described in details in the following section.

4. Slip Compensation

Instead of directly integrating the twist of a robot for the fusion with MSCKF state estimator, the proposed algorithm utilizes slip-compensated estimation to obtain a more accurate robot pose instead. In this section, we describe the process to estimate the wheel slippage using GPR as well as the methodology used to construct the kernel to encapsulate a deep architecture. Our slip compensation design, as seen in Figure 2, involves a learning-based estimation of the (error) residuals between ground truth and physical WO model considering a planar motion differential drive robot. The goal of this scheme is to obtain more accurate robot poses based on a sequence of given velocity inputs from the encoders and the fiber optic gyroscope (FOG).

Instead of a full 6-DOF robot pose, a simplified pose (see Figure 3) is represented by the state vector

$${\mathsf{\xi }}_k=\left[\begin{array}{c}{\xi }_{1k}\\ {\xi }_{2k}\\ {\xi }_{3k}\end{array}\right]={\left[\begin{array}{ccc}{x}_k& y_k& {\psi }_k\end{array}\right]}^{\top },$$

(25)

where $(x_k,y_k)$ denotes the position of the robot, and ${\psi }_k$ denotes yaw angle of the robot at training sample k for the residual estimator design in the sequel, respectively.

The velocity of both left and right wheels, $v_l$ and $v_r$, and the yaw angle that is converted from the pre-integrated IMU data, $\psi$, are used to obtain the robot’s differential odometry, $\mathit{u}$, based on the forward kinematics of the differential drive mobile robot,

$${\mathit{u}}_k=\left[\begin{array}{c}{v}_k\\ \Delta {\psi }_k\end{array}\right]=\left[\begin{array}{c}{\displaystyle \frac{1}{2}}(v_{l_k}+v_{r_k})\\ {\psi }_{k+1}-{\psi }_k\end{array}\right],$$

(26)

where $v_k$ and $\Delta {\psi }_k$ are the predicted linear and angular velocity of the robot based on the encoders and the FOG. The obtained differential odometry can be transformed and integrated into the corresponding positional state increment $\Delta \mathsf{\xi }$ using the discrete-time dynamic system model [40]

$$\begin{array}{ccc}\hfill \Delta {\mathsf{\xi }}_k& =& {\mathsf{\xi }}_{k+1}-{\mathsf{\xi }}_k={\overline{\mathit{u}}}_k+{\mathit{w}}_k,\hfill \\ \hfill {\overline{\mathit{u}}}_k& =& \left[\begin{array}{c}{\overline{u}}_{1k}\\ {\overline{u}}_{2k}\\ {\overline{u}}_{3k}\end{array}\right]={\mathit{f}}_o\left({\mathit{u}}_k\right)=\left[\begin{array}{c}{v}_{k}cos(\Delta {\psi }_k){\Delta }_k\\ v_{k}sin(\Delta {\psi }_k){\Delta }_k\\ \Delta {\psi }_k\end{array}\right],{\mathit{w}}_k=\left[\begin{array}{c}{w}_{1k}\\ w_{2k}\\ w_{3k}\end{array}\right],\hfill \end{array}$$

(27)

where ${\mathit{w}}_k$ denotes the Gaussian process noise such that $w_{ik}\sim \mathcal{N}(0,{\sigma }_{w_{ik}}^2)$ and $\Delta k$ denotes the sampling time.

In training of this learning-based estimator design utilizing GPR, which is formulated for predictive correction of the odometry increments $\Delta {\mathsf{\xi }}_k$ governed by (27), consider a sequence of inputs (from training data sequences) collected in the individual input vector ${\overline{\mathit{U}}}_i={[u_{i_k},\cdots ,u_{i_{kn}}]}^{\top }\in {\mathcal{R}}^n$ and the corresponding sequence of state increments collected in the vector $\mathsf{\Delta }{\mathit{X}}_i={[\Delta {\xi }_{i_k},\cdots ,\Delta {\xi }_{i_{k_n}}]}^{\top }\in {\mathcal{R}}^n$.

For predictions $\mathsf{\Delta }{\mathit{X}}_i^{*}={[\Delta {\xi }_{i_k}^{*},\cdots ,\Delta {\xi }_{i_{km}}^{*}]}^{\top }$ on new data ${\overline{\mathit{U}}}_i^{*}\in {\mathcal{R}}^m$ (test sequences), the predictive inference is given by

$$p\left(\mathsf{\Delta }{\mathit{X}}_i^{*}\right|{\overline{\mathit{U}}}_i,\mathsf{\Delta }{\mathit{X}}_i,{\overline{\mathit{U}}}_i^{*})\sim \mathcal{N}(\mathsf{\Delta }{\overline{\mathit{X}}}_i^{*},\mathit{cov}\left(\mathsf{\Delta }{\mathit{X}}_i^{*}\right)),$$

(28)

where

$$\mathsf{\Delta }{\overline{\mathit{X}}}_i^{*}=\mathit{K}({\overline{\mathit{U}}}_i^{*},{\overline{\mathit{U}}}_i){[\mathit{K}({\overline{\mathit{U}}}_i,{\overline{\mathit{U}}}_i)+{\sigma }_{w_i}^2\mathit{I}]}^{-1}\mathsf{\Delta }{\mathit{X}}_i$$

(29)

is the mean prediction, and

$$\begin{array}{cc}\hfill \mathit{cov}\left(\mathsf{\Delta }{\overline{\mathit{X}}}_i^{*}\right)& =\mathit{K}({\overline{\mathit{U}}}^{*},{\overline{\mathit{U}}}^{*})-\hfill \\ & \mathit{K}({\overline{\mathit{U}}}^{*},\overline{\mathit{U}}){[\mathit{K}(\overline{\mathit{U}},\overline{\mathit{U}})+{\sigma }_{w_i}^2\mathit{I}]}^{-1}\mathit{K}(\overline{\mathit{U}},{\overline{\mathit{U}}}^{*})\hfill \end{array}$$

(30)

is the prediction covariance; $\mathit{K}({\overline{\mathit{U}}}^{*},{\overline{\mathit{U}}}^{*})\in {\mathcal{R}}^{m\times m}$, $\mathit{K}({\overline{\mathit{U}}}^{*},\overline{\mathit{U}})\in {\mathcal{R}}^{m\times n}$, $\mathit{K}(\overline{\mathit{U}},{\overline{\mathit{U}}}^{*})\in {\mathcal{R}}^{n\times m}$, and $\mathit{K}(\overline{\mathit{U}},\overline{\mathit{U}})\in {\mathcal{R}}^{n\times n}$ are the kernel matrices where the element (i,j) of ${\mathit{k}}_{ij}=\mathit{k}(u_i,u_j)$. $\mathit{k}(·,·)$ is the kernel function, which consists of the Matern kernel [41] as the base kernel $\overline{\mathit{k}}$ combined with the non-linear mapping given by the neural network (NN) architecture $g_f(·)$. As shown in Figure 4 the kernel function can be transformed into a deep kernel architecture. Hence, the inputs are transformed as [42]

$$k({\overline{\mathit{u}}}_i,{\overline{\mathit{u}}}_j|\Theta )=\overline{k}({\mathit{g}}_f({\overline{\mathit{u}}}_i,\mathit{w}),{\mathit{g}}_f({\overline{\mathit{u}}}_j,\mathit{w})),$$

(31)

with the Matern kernel function $\overline{k}(·,·)$ given as

$$\overline{k}({\overline{\mathit{u}}}_i,{\overline{\mathit{u}}}_j|\Theta )={\sigma }_k^2\left(1+{\displaystyle \frac{\sqrt{5}|{\overline{\mathit{u}}}_i-{\overline{\mathit{u}}}_j|}{l}}+{\displaystyle \frac{\sqrt{5}{|{\overline{\mathit{u}}}_i-{\overline{\mathit{u}}}_j|}^2}{3l^2}}\right)Exp(-{\displaystyle \frac{\sqrt{5}|{\overline{\mathit{u}}}_i-{\overline{\mathit{u}}}_j|}{l}})$$

(32)

where ${\sigma }_k$ is the magnitude parameter and $l>0$ is the scale parameter, which are hyperparameters of the kernel. The input to the kernel function is warped by the non-linear mapping ${\mathit{g}}_f(\overline{\mathit{u}},\mathit{w})$, which includes ReLU activation functions where the NN architecture is parameterized by the weights $\mathit{w}$. We construct two deep kernel architectures utilizing convolutional neural networks (CNNs) and recurrent neural networks (RNNs). The CNN-based architecture follows two layers of 1D CNN layers followed by a fully connected layer. The LSTM-based architecture follows two layers of LSTM followed by a fully connected layer.

In the training process, the hyperparameters $\mathsf{\gamma }=\{\mathsf{\Theta }$ and $\mathit{w}\}$, which include the weights $\mathit{w}$ of the NN network and $\Theta =\{{\sigma }_k,l\}$ the parameters of the base kernel $\overline{k}(·,·)$, are learned and optimized. The magnitude parameter ${\sigma }_k$ dictates the vertical span of the kernel function in (32), and the scale parameter l dictates the rate at which the correlation between two points decreases with increasing distance.

Table 1. Parameters for training GP.

Training Parameter	Value
Optimizer	Adam
Learning rate	0.01
No. of epochs	100
sample rate	100 Hz
Kernel length parameter	1

provides the pre-defined learning parameters used. These parameters are initially set by trial and error and this library [43] was used to develop the training function that optimized the hyperparameters. As shown in Figure 4, we use the ReLU activation function for CNN-based deep kernel architecture. In addition, sigmoid and tanh activation functions are used for RNN-based deep kernel architecture. The computational bottleneck of the system for inference is the computation of ${[\mathit{K}(\overline{\mathit{U}},\overline{\mathit{U}})+{\sigma }_{w_i}^2\mathit{I}]}^{-1}$. Due to the large number of inputs in the data-set, we use a sparse GPR with approximate kernel learning based on induced points. Hence, instead of having to invert an ${\mathcal{R}}^{n\times n}$ matrix, we make a low rank approximation and invert an ${\mathcal{R}}^{m\times m}$ matrix instead, where m is the number of inducing points. Using the optimized parameters, the corrected state vector increment $\Delta {\xi }_{i_q}$ for $i=1,2,3$, and $q=1,\cdots ,m$ is obtained. Once the refined compensated set of odometry increments $\mathsf{\Delta }{\mathsf{\xi }}_q$ are obtained for a test sequence, they can be used by the MSCKF state estimator for updating the state.

5. Real-World Data Simulation

In this section, we present experimental findings utilizing the publicly available KAIST dataset [44]. Gathered in an urban environment populated by dynamic objects such as vehicles, pedestrians, and bicycles, the dataset was selected due to its incorporation of wheel encoder data, which are a feature missing in many existing visual inertial datasets. The data were collected using a robotic platform equipped with a camera, an IMU, and wheel encoders.

Table 2. Sensor specifications for KAIST dataset.

Sensor	Manufacturer	Model	Description	Freq. (Hz)
Camera	Pointgray	Flea3	1600 × 1200 color, 59 FPS	10
IMU	Xsens	MTi-300	Enhanced AHRS Gyro	100
wheel encoder	RLS	LM13	Magnetic rotary encoder	100

details the sensor specifications.

For training the GPR for slip compensation, the parameters employed were as follows: the optimizer was ‘Adam’ [45], which is an inbuilt optimizer in pytorch for stochastic optimization; the learning rate was set to 0.01; the process was trained over 100 epochs; and the sample rate was 100 Hz.

The total lengths of the two test sequences were 11.19 km and 7.6 km, respectively. The paths for both test sequences are shown in Figure 5 and Figure 6, respectively. To quantify the enhancement provided by our proposed method, we measured the mean absolute trajectory error (MATE) and mean absolute rotation error (MRTE) against the ground truth trajectory. We examined three variations of multi-state constraint Kalman filters (MSCKFs): a standard MSCKF VIWO, a MSCKF VIWO with dynamic feature point elimination, and a MSCKF VIWO incorporating both dynamic feature point elimination and wheel slip compensation.

Table 3. MATE/MRTE KAIST dataset.

Method	MATE [m]	MRTE [Degree]
VIWO uncorrected	11.45	2.526
VIWO with DPE	9.13	1.912
VIWO with DPE and CNN GPR	6.04	1.279
VIWO with DPE and RNN GPR	5.45	1.079

demonstrates the superior alignment of the ground truth with the third approach, which combined slip compensation and dynamic point removal.

In the case of VIWO without the incorporation of slip compensation and dynamic feature point elimination, the MATE/MRTE recorded was 10.825 m. However, this error reduced to 6.121 m with the integration of these features, indicating a substantial decrease in both MATE and MRTE, which are shown in Figure 7.

6. Indoor Experimentation

We validate our proposed methodology by conducting experiments in an indoor setup. We implement and test the scheme using Clearpath Robotics’ Jackal unmanned ground vehicle (UGV) (Clearpath Robotics, Kitchener, ON, Canada), as shown in Figure 8, equipped with readily available sensors such as wheel encoders and an inertial measurement unit (IMU). We also attach an Intel RealSense T265 tracking camera (Intel Corporation, Santa Clara, CA, USA) to obtain visual data. The Jackal UGV is a skid-steer differential-drive robot that consists of four wheels, where each pair of wheels on the left and right sides is actuated by a single motor. Hence, for modeling, it can be represented as a standard two-wheel differential-drive system. The sensor specifications and associated measurements can be found in

Table 4. Jackal Sensor specifications.

Sensor	Description	Frequency
Camera	Intel realsense T265	10 fps
IMU		400 Hz
Wheel encoders	78,000 pulses/m quadrature	100 Hz

. The data from the Jackal UGV are obtained via Robot Operating System (ROS2 Humble) [46]. To validate the experimental results, we compare them with data obtained from the OptiTrack motion capture system (NaturalPoint, Inc., Corvallis, OR, USA), which serves as the ground truth reference.

Figure 9 illustrates the difference in wheel speeds when turning and traversing in an arc and the associated slippage.

With the indoor set-up, we tested the proposed algorithm under the same metrics as in Section 5. Figure 10 and Figure 11 depict the robot trajectories for the cases, which include slip compensation plotted against the ground truth trajectory obtained with the optitrack motion capture system. As seen in Figure 10 and Figure 11, which displays the barplots for RMSE(m), the trajectory obtained via augmented slip compensation and dynamic feature point elimination had the lowest RMSE, as shown in Figure 12. In summary, our methodology was able to increase the accuracy of localization for the outdoor real-world data, as well as the indoor experiments conducted using a UGV.

To further test our methodology, we used gravel and sand on the test floor to determine the effectiveness of the proposed system. Figure 13 shows the robot trajectories for when the navigating surface is layered with gravel and sand. Our proposed scheme was able to improve the localization accuracy through aiding in slippage compensation.

7. Conclusions

In this study, we introduced a data-driven Gaussian process regression (GPR) approach for wheel slip compensation, paired with a dynamic feature elimination mechanism to enhance visual-inertial-wheel odometry (VIWO)-aided navigation systems. The proposed slip compensation scheme incorporates advanced deep kernel designs and long short-term memory (LSTM) network layers, which are adept at capturing spatiotemporal variations in odometry errors. A core component of the scheme is the feature point confidence estimator, which plays a pivotal role in mitigating the adverse effects of dynamic feature points on visual measurements. By refining these measurements, the system achieves improved pose estimation accuracy. Furthermore, inertial measurements are corrected and fused with wheel encoder data through the multi-state constraint Kalman filter (MSCKF), ensuring robust state estimation. The effectiveness of the approach was validated through extensive real-world data simulations and controlled indoor experiments, demonstrating its superiority in reducing errors and enhancing localization accuracy under various operational conditions.

Extensive real-world outdoor experiments using the KAIST dataset and controlled indoor tests with a Clearpath Jackal UGV validated the effectiveness of our approach. In outdoor experiments, our method reduced the mean absolute trajectory error (MATE) from 11.45 m to 5.45 m and the mean absolute rotation error (MRTE) from 2.526° to 1.079°. Similarly, in indoor trials using OptiTrack as ground truth, the proposed scheme significantly reduced RMSE, particularly on challenging terrains such as gravel and sand, demonstrating its robustness in varying conditions.

Looking ahead, the proposed framework sets the foundation for several promising research directions. One key area of improvement is enhancing the neural network architecture used for slip compensation. Future work will explore more advanced deep learning models, such as transformer-based architectures and convolutional-recurrent hybrid networks, to improve the system’s ability to capture complex temporal dependencies in slip dynamics. Additionally, integrating unsupervised and self-supervised learning techniques into the slip compensation process could enable the system to adapt autonomously to new environments and conditions, further improving its robustness without the need for extensive labeled data. Furthermore, refining the system architecture to incorporate terrain-aware modeling using deep learning techniques could allow real-time adjustments based on surface characteristics such as friction, inclination, and deformability. This would improve adaptability in dynamic and challenging environments, making the system less sensitive to shifts in slip dynamics. Such advancements would broaden the applicability of the system to diverse scenarios, including off-road navigation, disaster response, and industrial automation. The insights gained from this research highlight the potential of combining advanced neural-network-based slip compensation with multi-sensor fusion, paving the way for more reliable and efficient autonomous navigation systems.