# 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
 \tag{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 \(\hat{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} \tag{20}\]

where F = \( \begin{bmatrix}1&0&t&0\\ 0&1&0&t\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix} \) is the system state transition matrix; \(W_{k-1}= \begin{bmatrix}\frac{t^{2}}{2}a_{k-1}^{x}\\ \frac{t^{2}}{2}a_{k-1}^{y}\\ a_{k-1}^{x}t\\ a_{k-1}^{y}t\end{bmatrix} \) represents the system noise vector. ### 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} \tag{21}\]where the observation vector is defined as \(Y_{k}= \begin{bmatrix}x_{k}^{III}-x_{k}^{VIO}\\ y_{k}^{III}-y_{k}^{VIO}\end{bmatrix} \), \(V_{k}\) represents the observation noise, and the observation matrix is given by:

\[H= \begin{bmatrix}\frac{x_{k}^{III}-x_{k,i}^{B}}{\Delta_{k}^{VI}}&\frac{y_{k}^{III }-y_{k,i}^{B}}{\Delta_{k}^{VI}}&0&0\\ \frac{x_{k}^{VIO}-x_{k-1}^{VIO}}{x_{k-1}^{VIO}}&\frac{y_{k}^{VIO}-y_{k-1}^{VIO} }{\Delta_{k-1}^{VIO}}&0&0\end{bmatrix}  \tag{22}\]

\[\Delta_{k,i}^{UI}=\sqrt{\left(x_{k}^{III}-x_{k,i}^{B}\right)^{2}-\left(y_{k}^{ III}-y_{k,i}^{B}\right)^{2}} \tag{23}\]

\[\Delta_{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}} \tag{24}\]

where \(\left(x_{k}^{III},y_{k}^{III}\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\). ### 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. \[ \begin{cases}\delta X_{k|k-1}=F\delta X_{k-1}+W_{k-1}\\ P_{k|k-1}=FP_{k-1}F^{T}+Q_{k-1}\end{cases}  \tag{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}\). \[ \begin{cases}K_{k}=P_{k|k-1}H^{\mathsf{T}}\big{(}HP_{k|k-1}H^{\mathsf{T}}+R_{ k}\big{)}^{-1}\\ \delta X_{k|k}=\delta X_{k|k-1}+K_{k}\big{(}Y_{k}-H\delta X_{k|k-1}\big{)}\\ P_{k|k}=(I-K_{k}H)P_{k|k-1}\end{cases}  \tag{26}\]

Here, \(R_{k}\) denotes the observation noise covariance at time step \(k\). ## 5 Experiments and Results Analysis

### 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 \(\times\) 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 vast 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.