# 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
\[\rho=\frac{F(a_{i})-F\Big{(}a_{i}+a_{i}^{d}\Big{)}}{L(0)-L\big{(}a_{i}^{d}\big{)}} \tag{17}\]  
Here, \(F()\) denotes the actual cost function, while \(L()\) 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. ```
1:begin
2:initialize \(k=0\)\(r=r_{0}\)
3:While \(k<k_{\max}\)
4:compute \(g_{i}=J(a_{i-1})^{T}F(a_{i-1})\)
5:if\(\parallel g_{i}\parallel_{2}\)\(\leq\)\(th_{1}\) or \(r_{i-1}\)\(\leq\)\(th_{2}(\parallel a_{i-1}\parallel_{2}+th_{2})\) or \(\parallel F(a_{i})\parallel_{2}\)\(\leq\)\(th_{3}\)
6:break
7:endif
8:\(\Delta_{i}^{GN}=-\frac{J(a_{i-1})^{T}F(a_{i-1})}{J(a_{i-1})^{T}J(a_{i-1})}\)
9:\(\Delta_{i}^{SD}=-J(a_{i-1})^{T}F(a_{i-1})\)
10:\(\alpha=\frac{\parallel g_{i}\parallel_{2}^{2}}{\parallel(I(a_{i-1})g_{i} \parallel_{2}^{2}}\)
11:if\(\parallel\Delta_{i}^{GN}\parallel_{2}\)\(\leq\)\(r_{i-1}\)
12:\(\Delta_{i}^{DL}=\Delta_{i}^{SD}\)
13:elseif\(\parallel\Delta_{i}^{SD}\parallel_{2}\)\(\geq\)\(r_{i-1}\)
14:\(\Delta_{i}^{DL}=\frac{r_{i-1}\Delta_{i}^{SD}}{\parallel\Delta_{i}^{SD} \parallel_{2}}\)
15:else
16:\(u=\Delta 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{\frac{p^{2}-4 ac}{2t}}\right)\)
21:\(\Delta a_{i}^{DL}=\Delta a_{i}^{SD}+tau*u\)
22:endif
23:if\(\parallel\Delta a_{i}^{DL}\parallel_{2}\)\(\leq\)\(th_{2}(\parallel a_{i-1}\parallel_{2}+th_{2})\)



### 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}= \begin{bmatrix}x_{k}&y_{k}&v_{x,k}&v_{y,k}\end{bmatrix} ^{T} \tag{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 \(\hat{X}_{k}\) and the associated error is \(\delta X_{k}\), then the true state is given by \(X_{k}=\hat{X}_{k}+\delta X_{k}\). The error-state model of the stereo VIO/UWB fusion system is defined as:

\[ \begin{cases}\delta x_{k}=\delta x_{k-1}+\delta v_{k-1}^{x}t^{+} \frac{t^{2}}{2}a_{k-1}^{x}\\ \delta y_{k}=\delta y_{k-1}+\delta v_{k-1}^{y}t^{+}\frac{t^{2}}{2}a_{k-1}^{x}\\ \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{cases}