Fusion-Based Localization System Integrating UWB, IMU, and Vision

Abstract

Accurate indoor positioning services have become increasingly important in modern applications. Various new indoor positioning methods have been developed. Among them, visual–inertial odometry (VIO)-based techniques are notably limited by lighting conditions, while ultrawideband (UWB)-based algorithms are highly susceptible to environmental interference. To address these limitations, this study proposes a hybrid indoor positioning algorithm that combines UWB and VIO. The method first utilizes a tightly coupled UWB/inertial measurement unit (IMU) fusion algorithm based on a sliding-window factor graph to obtain initial position estimates. These estimates are then combined with VIO outputs to formulate the system’s motion and observation models. Finally, an extended Kalman filter (EKF) is applied for data fusion to achieve optimal state estimation. The proposed hybrid positioning algorithm is validated on a self-developed mobile platform in an indoor environment. Experimental results show that, in indoor environments, the proposed method reduces the root mean square error (RMSE) by 67.6% and the maximum error by approximately 67.9% compared with the standalone UWB method. Compared with the stereo VIO model, the RMSE and maximum error are reduced by 55.4% and 60.4%, respectively. Furthermore, compared with the UWB/IMU fusion model, the proposed method achieves a 50.0% reduction in RMSE and a 59.1% reduction in maximum error.

1. Introduction

In modern society, the demand for high-precision positioning services has increased dramatically. Accurate and reliable localization plays a crucial role not only in national defense and emergency responses, but also in the rapid development of industrial automation, smart logistics, and autonomous navigation [1]. Although outdoor positioning technologies have become relatively mature, especially with the widespread use of global navigation satellite systems (GNSS), their applicability significantly declines in complex or obstructed environments [2].

In urban canyons, industrial sites, and indoor environments, GNSS signals are frequently blocked or reflected by surrounding structures. These disruptions lead to signal attenuation, multipath interference, and reduced satellite visibility, which in turn degrade positioning accuracy and reliability. As studies show that approximately 80–90% of human activities occur indoors, solving the challenge of accurate indoor positioning in GNSS-denied environments has become a key research direction [3].

Visual simultaneous localization and mapping (VSLAM) has emerged as a widely explored alternative [4]. VSLAM methods utilize monocular, stereo, or RGB-D cameras to estimate ego-motion and map unknown environments. However, in dynamic or texture-poor scenarios, visual methods alone often suffer from motion blur, occlusions, or lighting variations, leading to drift or even tracking failure. To address these issues, many recent studies incorporate inertial measurements, such as those from an inertial measurement unit (IMU), to complement visual data [5]. The combination results in visual–inertial odometry (VIO), which achieves high-frequency pose estimation and drift correction [6].

In parallel, wireless positioning technologies based on signal strength indicators—such as wireless local area network (WLAN), radio frequency identification (RFID), Bluetooth, and ZigBee—have seen rapid development in recent years [7]. These systems estimate position by analyzing the received signal strength (RSS) or signal patterns within known environments. However, due to their high sensitivity to environmental interference, such as multipath propagation and signal attenuation caused by walls or obstacles, their performance often suffers from instability and low accuracy. As a result, they are generally unsuitable for applications that demand reliable and robust localization, such as autonomous indoor robotics. On the other hand, ultra-wideband (UWB) has gained significant attention for indoor wireless positioning due to its ability to achieve centimeter-level accuracy, high time resolution, and strong resistance to multipath interference [8]. As an active ranging technology, UWB can provide global positioning constraints that help mitigate the drift of visual-inertial systems.

Motivated by the complementary characteristics of VIO and UWB, this study aims to propose a loosely coupled multi-sensor fusion framework that combines UWB/IMU positioning with VIO to provide accurate and robust indoor localization. The system is designed to address the limitations of GNSS in indoor environments by leveraging the complementary strengths of global and relative positioning. Our approach offers a practical solution for reliable localization under challenging conditions.

The main contributions of this paper are summarized as follows:

(1)

A tightly coupled UWB/IMU localization model based on a sliding-window factor graph is proposed. This model constructs a cost function for state estimation by leveraging the localization principles of both UWB and IMU.

(2)

An indoor UWB and stereo VIO fusion localization method based on the EKF is proposed to enable long-term, accurate, and reliable real-time localization.

The remainder of this paper is organized as follows:

Section 3 presents the theoretical background of factor graphs and introduces a tightly coupled UWB/IMU fusion method based on factor graph optimization. Section 4 provides an overview of the EKF and details the proposed UWB–stereo VIO fusion localization method. Section 5 presents experimental validation and result analysis. Section 6 concludes the paper and outlines future research directions.

2. Related Work

2.1. Visual–Inertial Odometry

In GNSS-denied environments, VIO has become one of the most prominent solutions for motion estimation and mapping. By fusing the complementary information of visual and inertial sensors, VIO provides robust and accurate short-term localization [6]. Leutenegger et al. [9] introduced OKVIS, a keyframe-based VIO system that utilizes nonlinear optimization. Although it achieves high-quality localization and mapping, it lacks loop closure mechanisms, which results in the accumulation of drift over time. Similarly, Mourikis et al. [10] developed the Multi-State Constraint Kalman Filter (MSCKF), a filtering-based approach that reduces computational complexity by limiting state dimensionality, demonstrating good performance in large-scale real-world environments. Mur-Artal et al. [11] proposed a tightly coupled optimization-based VI-SLAM system that integrates loop closure to address the significant drift present in earlier VI systems. This system is capable of achieving unbiased pose estimation in looped trajectories and has been widely adopted in the research community. Shen et al. [12] from the Hong Kong University of Science and Technology developed the Visual–Inertial Navigation System-Mono (VINS-Mono), a robust monocular VIO system. It employs KLT optical flow tracking for front-end feature management and a sliding-window nonlinear optimization backend, while also integrating loop closure detection. This design offers excellent accuracy in diverse indoor and outdoor scenarios. Bloesch et al. [13] presented a VIO method based on an iterated extended Kalman filter (EKF), which minimizes photometric error from image patches. This approach achieves lightweight, real-time fusion of inertial and visual measurements and provides reliable localization performance even in the absence of loop closure. Campos et al. [14] introduced ORB-SLAM3, a feature-based SLAM framework that supports monocular, stereo, and inertial configurations. However, it is sensitive to environmental variations such as lighting changes. VINS-Mono [13] combines visual and inertial data using a sliding-window filter and requires careful initialization. Another notable contribution is OpenVINS [15], an open-source, real-time visual–inertial odometry system developed by Geneva et al. It supports monocular, stereo, and multi-camera configurations and is designed for easy integration and deployment in resource-constrained embedded systems. OpenVINS features a tightly coupled EKF backend and emphasizes repeatability, robustness, and portability, making it an attractive baseline system for real-time robotics applications.

Despite these significant advancements, current VIO systems still suffer from limitations such as accumulated drift, sensitivity to visual degradation (e.g., poor lighting, motion blur), and the lack of absolute positioning references.

2.2. UWB Positioning

UWB has become a prominent candidate for high-accuracy indoor localization due to its excellent time resolution, wide bandwidth, and robustness against multipath interference [8]. In indoor localization systems based on UWB, nodes are typically classified into two types: anchor nodes, which are fixed and known in position, and unknown nodes, which are mobile and require positioning. The unknown node’s location is calculated using its spatial relationship with multiple anchors [16].

Prorok et al. [17] proposed a Time Difference of Arrival (TDOA) measurement model for UWB, revealing the multimodal statistical distribution of UWB positioning error in space. Hanssens et al. [18] further characterized the geometric properties of UWB signal propagation, including arrival time delays, angles of arrival and departure, and the variances of these parameters. Common UWB localization techniques rely on Time of Arrival (TOA) [19], TDOA [20], and Two-Way Ranging (TWR) [21] measurement principles, with solution algorithms such as least squares, CHAN, and Taylor series methods [22]. In addition to these traditional approaches, intelligent optimization algorithms have gained popularity in recent years to improve estimation accuracy under non-ideal conditions [23]. For challenging environments with multipath and non-line-of-sight (NLOS) effects, various strategies have been proposed to mitigate signal distortion. For instance, B. Silva [24] introduced a high-accuracy localization system based on Symmetric Double-Sided Two-Way Ranging (SDS-TWR) combined with trilateration, which reduces errors introduced by timing offsets.

Deep learning has also been explored for direct signal processing. Reference [25] proposed a method to extract accurate ToA from raw Channel Impulse Response (CIR) data using a neural network, thereby enabling accurate positioning in multipath scenarios. Additionally, Ma et al. [26] proposed a hybrid solution that combines the C-Taylor weighting approach with an Adaptive Robust Kalman Filter (ARKF) to optimize the weight assignment of TOA measurements, effectively reducing the positioning error caused by NLOS conditions.

Overall, UWB provides a strong foundation for high-precision indoor localization. However, signal degradation in complex environments remains a challenge. To improve long-term accuracy and robustness, recent trends are increasingly exploring multi-sensor fusion frameworks, combining UWB with vision, IMU, or other modalities.

2.3. Multi-Sensor Fusion Strategies

Given the complementary strengths of VIO and UWB positioning, their integration has become a promising direction for achieving accurate and robust indoor localization. UWB, as an active positioning technique, provides absolute global position measurements, while VIO, as a passive method, ensures high-frequency relative pose estimation. Combining these modalities enables both global accuracy and local continuity.

In recent years, various fusion frameworks have been proposed. Reference [27] introduced a tightly coupled VIO-UWB fusion scheme, where VIO and UWB raw measurements are jointly optimized to estimate the robot’s pose. This approach achieves high accuracy but often requires complex, computation-intensive optimization. Reference [28] presented a loosely coupled monocular VIO system assisted by UWB, using an EKF [29] to integrate relative and absolute positioning information. EKF-based fusion is computationally efficient and suitable for real-time applications, though it may be sensitive to sensor noise models and linearization errors.

To address scalability and modularity, an optimization-based loosely coupled framework was proposed in [30], demonstrating that UWB data can effectively reduce drift in visual localization and enhance system robustness. In addition, graph-based optimization methods have gained significant attention. Notably, the Georgia Tech Smoothing and Mapping (GTSAM) [31] framework provides a flexible factor graph structure for fusing heterogeneous sensor inputs. GTSAM enables efficient batch or incremental optimization and is increasingly used in VIO-UWB fusion systems for its ability to model measurement constraints explicitly and maintain system consistency across time.

3. Tightly Coupled UWB/IMU Fusion Localization Based on Sliding-Window Factor Graph

3.1. UWB Ranging Model

In fusion-based localization systems, UWB technology has been widely adopted for indoor positioning due to its high accuracy and strong resistance to multipath interference. Our system employs a TOA-based positioning method, which determines the signal propagation time by directly measuring the interval between when the UWB pulse is transmitted by the mobile station and when it is received by the reference station [32]. The distance is then calculated by multiplying the propagation time by the speed of light in free space. This distance is subsequently used to determine the position of the mobile station. Specifically, the UWB mobile station transmits a pulse signal at time $t_1$ and the reference station receives it at time $t_2$. The propagation time is $t_2-t_1$ and the distance $d_i$ between the two stations is given by:

$$d_i=c·\left(t_2-t_1\right)+n_i$$

(1)

Here, $c$ denotes the speed of light, $n_i$ represents the measurement noise, and $d_i$ is the measured distance between the $i$-th reference station and the mobile station. In a two-dimensional localization scenario, let the unknown position of the UWB mobile station be denoted as $(x,y)$, and the known position of the $i$-th reference station be $\left(x_i,y_i\right)$. The corresponding TOA ranging equation can be expressed as:

$$d_i=\sqrt{{\left(x-x_i\right)}^2+{\left(y-y_i\right)}^2}+n_i$$

(2)

As illustrated in Figure 1, by introducing multiple reference stations (at least three), the nonlinear equations can be jointly solved to accurately estimate the two-dimensional position of the UWB mobile station. This method achieves high ranging accuracy and stability, provided that strict time synchronization is maintained among all base stations. Compared with other positioning models such as TDOA and Angle of Arrival (AOA), the TOA-based method demonstrates superior absolute positioning performance in systems where hardware-level time synchronization is supported. It is particularly suitable for indoor navigation and mobile robotics applications that require high positioning accuracy.

3.2. Factor Graph-Based Fusion Localization

The core idea of factor graph-based navigation lies in constructing a global optimization model by establishing probabilistic constraints between system state variables and navigation observations. The optimal system state is then estimated under the maximum a posteriori (MAP) criterion. The system state $X$ typically includes 3D position, velocity, attitude, and sensor biases. The observation data, denoted as $Z$, may originate from various sensors, including IMU, UWB, and cameras [33]. Within the probabilistic inference framework, the navigation and localization problem can be formulated as estimating the system state $X$ that maximizes the posterior probability $P\left(X\right|Z)$ given the observations $Z$. This leads to a MAP estimation problem:

$$X^{*}=\underset{x}{\mathit{argmax}}P\left(X_{k:1}|Z_{k:2}\right)=\underset{x}{\mathit{argmax}}{\displaystyle \frac{P\left(Z_{k:2}|X_{k:1}\right)P\left(X_{k:1}\right)}{P\left(Z_{k:2}\right)}}$$

(3)

Here, $k$ denotes the discrete time step. The term $P\left(Z_{k:2}\right)$ does not affect the estimation of the state variables. According to Bayes’ theorem, this expression is expanded as the product of a likelihood function and a prior distribution. Here, $P\left(Z_{k:2}\right|X_{k:1})$ denotes the likelihood of the observations given the state, reflecting the sensor measurement model. $P\left(X_{k:1}\right)$ represents the prior knowledge about the system states. In the factor graph model, state variables are represented as variable nodes, while sensor observations are formulated as factor nodes connected to the corresponding variables. This structure results in a sparse graphical representation.

$$X^{*}=\underset{x}{\mathit{argmax}}P\left(Z_{k:2}|X_{k:1}\right)P\left(X_{k:1}\right)$$

(4)

Based on the first-order Markov assumption and the observation independence assumption.

$$P\left(Z_{k:2}|X_{k:1}\right)=\prod _{i=2}^{k}P\left(z_i|x_i\right)$$

(5)

$$P\left(X_{k:1}\right)=\prod _{i=2}^{k}P\left(x_i|x_{i-1}\right)P\left(x_1\right)$$

(6)

Assuming that both process noise and measurement noise follow a Gaussian distribution, we obtain:

$$P\left(x_i|x_{i-1}\right)\propto exp\left(-{\displaystyle \frac{1}{2}}{\left|\left|x_i-f\left(x_{i-1}\right)\right|\right|}_{R_i}^2\right)$$

(7)

$$P\left(z_i|x_i\right)\propto exp\left(-{\displaystyle \frac{1}{2}}{\left|\left|z_i-h\left(x_i\right)\right|\right|}_{Q_i}^2\right)$$

(8)

$f\left(\right)$ denotes the transition function that models the relationship between state variables $x_{i-1}$, and $x_i$. $h\left(\right)$ denotes the observation function relating the state variable $x_i$ to the measurement $z_i$. $R_i$ denotes the measurement noise covariance matrix, and $Q_i$ denotes the process noise covariance matrix. Here, ${\alpha }_i$ is an arbitrary vector. ${\left|\left| \right|\right|}_{R_i}^2$ denotes the marginal distance with respect to the covariance matrix $R_i$, and is computed as:

$${\left|\left|{\alpha }_i\right|\right|}_{R_i}^2={\alpha }_i·R_i·{\left({\alpha }_i\right)}^T$$

(9)

Therefore, the above equation can be reformulated as:

$$X^{*}=\mathit{argmax}\prod _{i=2}^{k}exp\left(-{\displaystyle \frac{1}{2}}{\left|\left|x_i-f\left(x_{i-1}\right)\right|\right|}_{R_i}^2\right)\times \prod _{i=2}^{k}exp\left(-{\displaystyle \frac{1}{2}}{\left|\left|Z_i-h\left(x_i\right)\right|\right|}_{Q_i}^2\right)$$

(10)

By simplifying the above expression, we obtain:

$$X^{*}=\mathit{argmax}\left(exp\left(-{\displaystyle \frac{1}{2}}\left(\sum _{i=1}^k{\left|\left|x_i-f\left(x_{i-1}\right)\right|\right|}_{R_i}^2+\sum _{i=1}^k{\left|\left|Z_i-h\left(x_i\right)\right|\right|}_{Q_i}^2\right)\right)\right)$$

(11)

To achieve the maximum a posteriori probability, the cost function must be minimized; that is:

$$X^{*}=\underset{x}{\mathit{argmin}}\left(\sum _{i=1}^k{\left|\left|x_i-f\left(x_{i-1}\right)\right|\right|}_{R_i}^2+\sum _{i=1}^k{\left|\left|Z_i-h\left(x_i\right)\right|\right|}_{Q_i}^2\right)$$

(12)

Once the global cost function based on the MAP criterion is established, the state estimation problem in the navigation system can be transformed into a nonlinear least-squares optimization problem. The objective is to obtain the optimal state estimate by minimizing the cost function. Common methods for solving this optimization problem include the Gauss–Newton algorithm and the Levenberg–Marquardt algorithm. These algorithms iteratively approximate the local minimum of the objective function at each step.

3.3. UWB/IMU Fusion Localization

The sliding-window factor graph structure combining the UWB and IMU localization mechanisms is illustrated in Figure 2.

Let $a_i$ denote the system state at time $i$, which includes the object’s position along the x- and y-axes, i.e.,

$$a_i=\left[x_i,y_i\right]$$

(13)

The IMU factor models the transition between the states $a_{i-1}$ and $a_i$. The UWB factor establishes the constraint between the state $a_i$ and the UWB range measurement. Based on the IMU measurement model, the corresponding cost function can be formulated as:

$${\left|\left|a_i-f_{IMU}\left(a_{i-1}\right)\right|\right|}_{R_i}^2={‖a_i-\left(a_{i-1}+\left[\begin{array}{c}{s}_k\times \mathit{cos}{\theta }_k\\ s_k\times \mathit{sin}{\theta }_k\end{array}\right]\right)‖}_{R_i}^2$$

(14)

Here, ${\theta }_k$ represents the yaw angle of the platform at time step $k$. $f_{IMU}\left(\right)$ is the IMU-based motion prediction function, and $h_{UWB}$ is the UWB measurement model. The cost function based on the UWB range measurement is given by:

$${\left|\left|z_i-h_{UWB}\left(a_i\right)\right|\right|}_{Q_i}^2={‖r_j-\sqrt{‖a_i-{anc}_j‖}‖}_{R_i}^2$$

(15)

The global cost function constructed from IMU and UWB factors can thus be expressed as:

$$X^{*}=\underset{a}{\mathit{argmin}}\left(\sum _{i=k}^{k+window}{\left|\left|a_i-f_{IMU}\left(a_{i-1}\right)\right|\right|}_{R_i}^2+\sum _{i=k}^{k+window}{\left|\left|z_i-h_{UWB}\left(a_i\right)\right|\right|}_{Q_i}^2\right)$$

(16)

In the global cost function, the sliding window is used to control the number of historical states involved in the optimization. When the number of states has not yet reached the predefined maximum window size, the actual length of the sliding window equals the current number of available states. Once the number of states reaches the threshold, the window maintains a fixed length by discarding the oldest state and incorporating the newest one in a first-in, first-out (FIFO) manner, enabling dynamic updates of the state sequence. Upon receiving new sensor data, the system constructs the current global cost function within the sliding window based on the included state nodes and their corresponding observations. This cost function incorporates residual terms from all sensor constraints and seeks the optimal solution in the state space. This optimization problem is typically solved using nonlinear optimization algorithms, such as the Gauss–Newton or Levenberg–Marquardt methods. By iteratively minimizing the sum of squared residuals, the optimal estimate of the state variables within the current window is obtained. The sliding window strategy significantly reduces computational complexity while preserving temporal continuity and observation redundancy, thereby enabling efficient and accurate real-time navigation. In this paper, the DogLeg algorithm is employed to solve the optimization problem.

The DogLeg algorithm is a widely used iterative method for solving nonlinear least-squares problems and is extensively applied in navigation and localization tasks such as factor graph optimization. Its core idea is to operate within a trust-region framework and combine the strengths of the Gauss–Newton and steepest descent methods by constructing a piecewise linear search path to efficiently determine the step direction. The DogLeg algorithm first computes the steepest descent step $a_i^{SD}$ and the Gauss–Newton step $a_i^{GN}$. If $a_i^{GN}$ lies within the trust region radius $∆$, it is directly accepted as the update step. If it exceeds the radius, a DogLeg path is constructed between $a_i^{SD}$ and $a_i^{GN}$, and the point within the trust region along this path is selected as the update step $a_i^d$. At each iteration, the algorithm evaluates the step quality using a performance metric $\rho$.

$$\rho ={\displaystyle \frac{F\left(a_i\right)-F\left(a_i+a_i^d\right)}{L\left(0\right)-L\left(a_i^d\right)}}$$

(17)

Here, $F\left(\right)$ denotes the actual cost function, while $L\left(\right)$ represents the second-order approximation of the model-predicted cost. If $\rho$ is close to 1, the model prediction is considered accurate, and the trust-region radius is increased. If $\rho$ is small or negative, it indicates that the current step is ineffective, and the trust-region radius should be reduced to improve convergence. This iterative process continues until the termination condition is met. The detailed steps are presented in Algorithm 1.

Algorithm 1 DogLeg Method

1: begin 2: intialize $k=0r=r_0$ 3: While $k<k_{\max }$ 4:   compute $g_i=J{\left(a_{i-1}\right)}^{T}F\left(a_{i-1}\right)$ 5:   if ${\parallel g_i\parallel }_2\le t{h}_1$ or $r_{i-1}\le t{h}_2({\parallel a_{i-1}\parallel }_2+t{h}_2)$ or ${\parallel F\left(a_i\right)\parallel }_2\le t{h}_3$ 6:    break 7:   endif 8:   ${∆a}_i^{GN}=-{\displaystyle \frac{J{\left(a_{i-1}\right)}^{T}F\left(a_{i-1}\right)}{J{\left(a_{i-1}\right)}^{T}J\left(a_{i-1}\right)}}$ 9:   $\Delta a_i^{SD}=-J{\left(a_{i-1}\right)}^{T}F\left(a_{i-1}\right)$ 10:  $\alpha ={\displaystyle \frac{{\parallel g_i\parallel }_2^2}{{\parallel J\left(a_{i-1}\right)g_i\parallel }_2^2}}$ 11:  if ${\parallel \Delta a_i^{GN}\parallel }_2\le r_{i-1}$ 12:   $\Delta a_i^{DL}=\Delta a_i^{SD}$ 13:  elseif ${\parallel \Delta a_i^{SD}\parallel }_2\ge r_{i-1}$ 14:   $\Delta a_i^{DL}={\displaystyle \frac{r_{i-1}\Delta a_i^{SD}}{{\parallel \Delta a_i^{SD}\parallel }_2}}$ 15:   else 16:    $u={∆a}_i^{GN}-\Delta a_i^{SD}$ 17:    $a=u^{T}u$ 18:    $b=2\Delta a_i^{SD}u$ 19:    $c={\left(\Delta a_i^{SD}\right)}^T\Delta a_i^{SD}-{\left(r_{i-1}\right)}^2$ 20:    $tau=\left(-b+\sqrt{{\displaystyle \frac{b^2-4ac}{2a}}}\right)$ 21:    $\Delta a_i^{DL}=\Delta a_i^{SD}+tau\ast u$ 22:   endif 23:  if ${\parallel \Delta a_i^{DL}\parallel }_2\le t{h}_2\left({\parallel a_{i-1}\parallel }_2+t{h}_2\right)$ 24:    break 25:   endif 26:  $a_i=a_{i-1}+\Delta a_i^{DL}$ 27:  $L\left(0\right)=0.5\times f{\left(a_i\right)}^{T}f\left(a_i\right);$ 28:  $L\left(\Delta a_i^{DL}\right)=f{\left(a_i\right)}^{T}f\left(a_i\right)+{\left(\Delta a^{DL}\right)}^{T}J{\left(a_i\right)}^{T}F\left(a_i+\Delta a^{DL}\right)+{\displaystyle \frac{1}{2}}{\left({\Delta a}^{DL}\right)}^{T}J{\left(a_i\right)}^{T}J(a_i)\Delta a_i^{DL}$; 29:  $\rho ={\displaystyle \frac{F\left(a_{i-1}\right)f{\left(a_{i-1}\right)}^T-F\left(a_i\right)F{\left(a_i\right)}^T}{L\left(0\right)-L\left(\Delta a_i^{DL}\right)}}$ 30:  if $\rho >0.75$ 31:   $r_i=\{max{r}_{i-1},3\times {\parallel \Delta a_i^{DL}\parallel }_2\}$ 32:  elseif $\rho <0.25$ 33:   $r_i=0.5r_{i-1}$ 34:   endif 35:  $k=k+1$ 36: end

4. Stereo VIO/UWB Fusion Localization Algorithm

The two representative mainstream VIO algorithms are (1) the filter-based MSCKF, and (2) the optimization-based VINS. The MSCKF algorithm maintains the current IMU state and a limited number of past camera poses to construct constraints on feature observations without explicitly recovering the 3D positions of the features. As the MSCKF only retains a fixed number of camera states within a sliding window, its update and estimation process is strictly recursive. However, it cannot incorporate global observation information for consistent global pose optimization, which limits its ability to suppress long-term drift. In contrast, VINS introduces a sliding-window optimization framework that jointly incorporates IMU pre-integration and visual feature residuals into a nonlinear optimization problem, enabling batch optimization within the window and significantly improving localization accuracy. This method fully exploits the high-order correlations among all states within the window, making more effective use of observation data. It demonstrates robust and accurate trajectory reconstruction, especially in dynamic or low-texture environments.

Therefore, this paper adopts VINS for stereo VIO localization and integrates it with the aforementioned tightly coupled UWB/IMU fusion method based on a sliding-window factor graph. A filtering algorithm is then employed to fuse the localization results from both systems, in order to obtain the optimal position estimates of the mobile robot at each time step. The workflow of the stereo VIO/UWB fusion localization system is illustrated in Figure 3.

4.1. State Model Construction

The commonly used fusion method is the Kalman filter, which requires defining a state equation and a measurement equation based on the integrated positioning sources. It performs predictions using the state equation and updates using the observation equation to estimate the state vector. The standard Kalman filter assumes a linear state and observation models, whereas in practical scenarios, nonlinearities are often present. To address this issue, the EKF is employed for fusion-based state estimation. The nonlinear functions are linearized via first-order Taylor expansion, enabling the application of Kalman filtering to estimate the system’s position updates. In the UWB-VIO fusion system, the independent coordinate frame used by UWB is typically selected as the global reference frame for the entire system. The local VIO coordinate system is then transformed into this global frame to achieve spatial alignment of the sensor observations. The system state is modeled using the 2D position and velocity of the mobile platform. Assume that at time step k, the UWB and VIO measurements are approximately time-aligned, with the algorithm retrieving the most recent VIO data that precedes or closely matches the corresponding UWB timestamp. This allows both observations to jointly constrain the state estimation at this time step. The system state vector is defined as:

$$X_k={\left[\begin{array}{cccc}{x}_k& y_k& v_{x,k}& v_{y,k}\end{array}\right]}^T$$

(18)

Here, $x_k$ and $y_k$ denote the robot’s position in the 2D plane, while $v_{x,k}$ and $v_{y,k}$ represent the velocity components along the x- and y-axes, respectively. Assuming that the nominal state vector at time $k$ is ${\widehat{X}}_k$ and the associated error is ${\delta X}_k$, then the true state is given by $X_k={\widehat{X}}_k+{\delta X}_k$. The error-state model of the stereo VIO/UWB fusion system is defined as:

$$\left\{\begin{array}{c}{\delta x}_k={\delta x}_{k-1}+{\delta v}_{k-1}^{x}t+{\displaystyle \frac{t^2}{2}}{a}_{k-1}^x\\ {\delta y}_k={\delta y}_{k-1}+{\delta v}_{k-1}^{y}t+{\displaystyle \frac{t^2}{2}}{a}_{k-1}^y\\ {\delta v}_k^x={\delta v}_{k-1}^x+a_{k-1}^{x}t\\ {\delta v}_k^y={\delta v}_{k-1}^y+a_{k-1}^{y}t\end{array}\right.$$

(19)

Here, $t$ denotes the sampling interval, and $a_{k-1}^x$ and $a_{k-1}^y$ represent the accelerations in the $x$ and y directions at time $k-1$. The nominal state ${\widehat{X}}_k$ is initialized using the received VIO data, followed by an error-state update. By reorganizing Equation (19), the predicted error state and covariance at time $k$ can be obtained, as shown in Equation (20).

$${\delta X}_{k|k-1}={F\delta X}_{k-1}+W_{k-1}$$

(20)

where $\mathrm{F}=\left[\begin{array}{cccc}1& 0& t& 0\\ 0& 1& 0& t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ is the system state transition matrix; $W_{k-1}=\left[\begin{array}{c}{\displaystyle \frac{t^2}{2}}{a}_{k-1}^x\\ {\displaystyle \frac{t^2}{2}}{a}_{k-1}^y\\ a_{k-1}^{x}t\\ a_{k-1}^{y}t\end{array}\right]$ represents the system noise vector.

4.2. Observation Model Construction

Upon obtaining the UWB/IMU localization data, a measurement equation can be constructed, where the difference between the UWB/IMU and VIO localization results is used as the observation vector $Y_k$. The observation model for the stereo VIO/UWB fusion system is thus given by:

$$Y_k={H\delta X}_k+V_k$$

(21)

where the observation vector is defined as $Y_k=\left[\begin{array}{c}{x}_k^{UI}-x_k^{VIO}\\ y_k^{UI}-y_k^{VIO}\end{array}\right]$, $V_k$ represents the observation noise, and the observation matrix is given by:

$$H=\left[\begin{array}{cccc}{\displaystyle \frac{x_k^{UI}-x_{k,i}^B}{{∆}_{k,i}^{UI}}}& {\displaystyle \frac{y_k^{UI}-y_{k,i}^B}{{∆}_{k,i}^{UI}}}& 0& 0\\ {\displaystyle \frac{x_k^{VIO}-x_{k-1}^{VIO}}{{∆}_{k,k-1}^{VIO}}}& {\displaystyle \frac{y_k^{VIO}-y_{k-1}^{VIO}}{{∆}_{k,k-1}^{VIO}}}& 0& 0\end{array}\right]$$

(22)

$${∆}_{k,i}^{UI}=\sqrt{{\left(x_k^{UI}-x_{k,i}^B\right)}^2-{\left(y_k^{UI}-y_{k,i}^B\right)}^2}$$

(23)

$${∆}_{k,k-1}^{VIO}=\sqrt{{\left(x_k^{VIO}-x_{k-1}^{VIO}\right)}^2-{\left(y_k^{VIO}-y_{k-1}^{VIO}\right)}^2}$$

(24)

where $\left(x_k^{UI},y_k^{UI}\right)$ denote the UWB/IMU coordinates at time $k$; $\left(x_{k,i}^B,y_{k,i}^B\right)$ represent the coordinates of the $i$-th anchor at time $k$; $\left(x_k^{VIO},y_k^{VIO}\right)$ and $\left(x_{k-1}^{VIO},y_{k-1}^{VIO}\right)$ are the stereo VIO coordinates at time steps $k$ and $k-1$.

4.3. Extended Kalman Filter

As shown in Equations (20) and (21), the proposed stereo VIO/UWB fusion system can be approximated as a linear system. Once the state and observation equations are established, state estimation can be performed using the standard Kalman filtering equations. The standard Kalman filter begins with a prediction step based on the state transition equation.

$$\left\{\begin{array}{c}{\delta X}_{k|k-1}={F\delta X}_{k-1}+W_{k-1}\\ P_{k|k-1}=F{P}_{k-1}{F}^T+Q_{k-1}\end{array}\right.$$

(25)

The subsequent update step involves computing the Kalman gain $K_k$, updating the error state ${\delta X}_{k|k}$, and revising the covariance matrix $P_{k|k}$.

$$\left\{\begin{array}{c}{K}_k=P_{k|k-1}{H}^T{\left({HP}_{k|k-1}{H}^T+R_k\right)}^{-1}\\ {\delta X}_{k|k}={\delta X}_{k|k-1}+K_k\left(Y_k-H{\delta X}_{k|k-1}\right)\\ P_{k|k}=\left(I-K_{k}H\right)P_{k|k-1}\end{array}\right.$$

(26)

Here, $R_k$ denotes the observation noise covariance at time step $k$.

5. Experiments and Results Analysis

5.1. Experimental Platform Setup

5.1.1. Hardware Platform

The stereo localization platform employs the Intel RealSense D435i camera, which captures single-frame RGB images at a resolution of 640 × 480 pixels and a frame rate of 30 Hz. The UWB module is based on the Qorvo DW1000 chip, operating at a sampling frequency of 1 Hz. The IMU employs the ICM-42688 chip with a sampling frequency of 1000 Hz. As shown in Figure 4, all devices are integrated into a wearable testing that is vest worn by the test subject during data collection.

5.1.2. Software Platform

In this experiment, data acquisition and processing were integrated into a development board centered on the Orange Pi 5B for sensor fusion. All computing platforms ran on the Ubuntu 20.04 Linux operating system. The system was developed under the Robot Operating System (ROS) framework using Visual Studio Code v. 1.100.2 as the primary development tool. The ROS is an open-source meta-operating system designed specifically for robotics software development, featuring strong modularity and cross-platform compatibility. It provides hardware abstraction and device driver support for low-level robotic components, and integrates an efficient message-passing mechanism with a comprehensive toolchain and extensive open-source libraries for software retrieval, building, development, and execution. For multi-sensor fusion localization tasks, ROS offers unified data encapsulation and high-precision timestamp management for heterogeneous sensor inputs, enabling effective data alignment and temporal synchronization across different sensors. Based on this, the ROS built-in timestamping mechanism was employed to achieve time synchronization between monocular VIO and UWB measurements during fusion. In addition, the ROS integrates a powerful 3D visualization toolkit, RViz, which enables real-time 3D visualization of localization results by subscribing to trajectory topics published by the VIO module. This provides intuitive and effective support for system debugging and performance evaluation.

5.2. Experiment and Analysis

5.2.1. Experimental Scenario

To evaluate the positioning accuracy of the stereo VIO/UWB fusion system, the experiment was conducted in an underground parking garage, which is a common indoor localization environment, as shown in Figure 5. The environment is relatively open with good line-of-sight conditions, making it suitable for validating the system’s localization performance. As shown in Figure 6, the test subject walked along a rectangular path measuring 30 m in length and 4 m in width.

Four UWB modules were used in total, with three serving as fixed anchors and one mounted on the test subject as a mobile tag. The test subject moved at an approximately constant speed in a clockwise direction while collecting data.

5.2.2. Localization Accuracy Evaluation Criteria

Localization accuracy is typically evaluated using the root mean square error (RMSE), which quantifies the average deviation between the estimated trajectory and the ground truth. It reflects the overall error magnitude and provides a clear indication of the average trend in localization deviations. It is defined as follows:

$$RMSE=\sqrt{\sum _{i=1}^n{\left(x_i-{\widehat{x}}_i\right)}^2+{\left(y_i-{\widehat{y}}_i\right)}^2}$$

(27)

Here, ${(x}_i,y_i)$ denotes the ground-truth position at time step $i$, and $\left({\widehat{x}}_i,{\widehat{y}}_i\right)$ represents the corresponding estimated position.

$$CDF\left(y\right)=P\left(e\le Y\right)$$

(28)

5.2.3. Results and Analysis

To evaluate the effectiveness of the proposed method, four comparative experiments were designed: (1) UWB-only localization, (2) UWB/IMU fusion localization, (3) stereo VIO-based localization, and (4) stereo VIO/UWB fusion localization.

The UWB data collected during the experiment were processed and compared against the ground-truth trajectory, as illustrated in Figure 7a. It can be observed that the UWB system provides higher localization accuracy when the user is close to the anchor nodes, but as the distance from the anchors increases, the trajectory begins to drift, which is particularly evident along the longer sides of the walking path. The maximum localization error in this scenario reaches 0.8446 m.

Using the stereo VIO model, estimated positions were obtained and compared with the actual walking trajectory, as shown in Figure 7b. It is evident that the system maintains high localization accuracy in the early phase of motion, but the trajectory gradually drifts over time due to cumulative error. The maximum localization error reaches 0.4997 m, especially when returning to the initial position.

The UWB/IMU fusion model was used for localization estimation and compared with the ground-truth trajectory, as shown in Figure 8a. Compared to the UWB-only system, the fusion model demonstrates higher localization accuracy, with reduced drift in regions far from the anchor nodes. The overall trajectory aligns more closely with the actual walking path. Although the factor graph-based UWB/IMU fusion model incorporates more historical states and observations, some drift still occurs in the latter part of the trajectory, resulting in increased error. The maximum localization error in this case is 0.3947 m.

The proposed model in this study was also used for localization estimation and was compared against the actual walking trajectory, as shown in Figure 8b. The EKF-based stereo VIO and UWB fusion system achieved superior localization accuracy, effectively addressing the drift commonly observed in VIO-only systems, while also mitigating the impact of UWB NLOS errors on the fusion system. As a result, the estimated trajectory is more consistent with the reference path. The maximum localization error of the fusion system is 0.2465 m.

Figure 9 presents the statistical localization errors for UWB, stereo VIO, UWB/IMU fusion models, and stereo VIO/UWB fusion models. As shown in

Table 1. Comparison of localization error across algorithms.

Localization Algorithm	Error Metrics	Error Magnitude (m)
UWB	RMSE	0.4212
	MAX	0.9297
VIO	RMSE	0.3052
	MAX	0.7778
UWB/IMU	RMSE	0.2722
	MAX	0.7515
Proposed Algorithm	RMSE	0.1361
	MAX	0.3073

, compared with the UWB-only method, the proposed algorithm improves localization accuracy by 67.6% and reduces the maximum error by approximately 67.9%. Compared with the stereo VIO model, the accuracy improves by 55.4%, and the maximum error decreases by about 60.4%. Compared with the UWB/IMU fusion model, it improves accuracy by 50.0% and reduces the maximum error by approximately 59.1%.

6. Conclusions

To address the limitations of stereo VIO—such as drift and significant localization errors in indoor environments—and the susceptibility of UWB to large errors under NLOS conditions, this study recognizes that single-sensor systems still struggle to meet the stability requirements of high-precision navigation. To overcome these challenges, this paper adopts the EKF to fuse UWB/IMU localization data with stereo VIO observations, and develops a UWB-VIO hybrid localization system based on the ROS platform for mobile robot applications. Comparative experiments demonstrate that the proposed method significantly improves localization accuracy and robustness in complex indoor environments. It effectively mitigates the drift problem in VIO and enhances the system’s resistance to NLOS interference.

Although the proposed filter-based fusion method provides good real-time performance and meets the continuous localization needs of indoor mobile robots, it still lags behind state-of-the-art graph-based optimization methods in terms of localization accuracy. Therefore, future work may explore the integration of filtering and optimization strategies to balance real-time performance with accuracy. In addition, the current method does not explicitly address the influence of environmental interference, including RF noise, signal multipath, or visual occlusions. These factors may degrade localization performance in dynamic or cluttered environments. In future research, we plan to incorporate interference-aware strategies in future research, such as robust outlier detection, adaptive covariance tuning, and sensor confidence weighting, to improve reliability.

Since the current system primarily performs localization in a two-dimensional plane, it does not fully account for dynamic variations in altitude. With the advancement of spatial operations, three-dimensional mobile platforms, such as unmanned aerial vehicles (UAVs), are expected to play a key role in indoor intelligent systems. Therefore, future research may extend to the development of 3D localization systems based on UAV platforms in order to meet the demands of higher-dimensional and more accurate localization applications. Moreover, although the current work mainly focuses on indoor scenarios due to the limitations of GNSS signals in such environments, we believe that the proposed method has the potential to be extended to outdoor or mixed environments. Future research may consider incorporating GNSS information to enhance localization robustness, especially in cases where UWB or visual features are limited.

Although the present work targets indoor positioning, we recognize that the core fusion strategies may be applicable in other domains facing similar challenges. For example, high-frequency surface wave radar (HFSWR) systems also require robust multi-source data integration under interference. Inspired by recent advances in high-resolution signal processing for HFSWR [34,35], future work may explore the cross-domain adaptation of our framework.