# Fusion-Based Localization System Integrating UWB, IMU, and Vision
## 3 Tightly Coupled UWB/IMU Fusion Localization Based on Sliding-Window Factor Graph
### UIWB/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] \tag{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:  
\[||a_{i}-f_{IMU}(a_{i-1})||_{R_{i}}^{2}=\left\|a_{i}-\left(a_{i-1}+ \begin{bmatrix} s_{k}\times cos\theta_{k}\\ s_{k}\times sin\theta_{k}\end{bmatrix} \right)\right\|_{R_{i}}^{2} \tag{14}\]  
Here, \(\theta_{k}\) represents the yaw angle of the platform at time step \(k\). \(f_{IMU}()\) is the IMU-based motion prediction function, and \(h_{UIWB}\) is the UWB measurement model. The cost function based on the UWB range measurement is given by:  
\[||z_{i}-h_{UIWB}(a_{i})||_{Q_{i}}^{2}=\left\|r_{j}-\sqrt{\left\|a_{i}-anc_{j} \right\|}\right\|_{R_{i}}^{2} \tag{15}\]  
The global cost function constructed from IMU and UWB factors can thus be expressed as:  
\[X^{*}=\underset{a}{argmin}\left(\sum_{i=k}^{k+window}||a_{i}-f_{IMU}(a_{i-1} )||_{R_{i}}^{2}+\sum_{i=k}^{k+window}||z_{i}-h_{UIWB}(a_{i})||_{Q_{i}}^{2}\right) \tag{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. Figure 2: UWB/IMU tightly coupled fusion localization based on sliding-window factor graphs. 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 \(\Delta\), 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}^{t}\). At each iteration, the algorithm evaluates the step quality using a performance metric \(\rho\).