# Fusion-Based Localization System Integrating UWB, IMU, and Vision
## 3 Tightly Coupled UWB/IMU Fusion Localization Based on Sliding-Window Factor Graph
### 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(X|Z)\) given the observations \(Z\). This leads to a MAP estimation problem:  
\[X^{*}=\underset{x}{argmax}P(X_{k:1}|Z_{k:2})=\underset{x}{argmax}\frac{P(Z_{ k:2}|X_{k:1})P(X_{k:1})}{P(Z_{k:2})} \tag{3}\]  
Here, \(k\) denotes the discrete time step. The term \(P(Z_{k:2})\) does not affect the estimation of the state variables. According to [PERSON] theorem, this expression is expanded as the product of a likelihood function and a prior distribution. Here, \(P(Z_{k:2}|X_{k:1})\) denotes the likelihood of the observations given the state, reflecting the sensor measurement model. \(P(X_{k:1})\) 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  
Figure 1: Distance-based measurement method. as factor nodes connected to the corresponding variables. This structure results in a sparse graphical representation. \[X^{*}=\underset{x}{argmax}P(Z_{k:2}|X_{k:1})P(X_{k:1}) \tag{4}\]  
Based on the first-order Markov assumption and the observation independence assumption. \[P(Z_{k:2}|X_{k:1})=\prod_{i=2}^{k}P(z_{i}|x_{i}) \tag{5}\]  
\[P(X_{k:1})=\prod_{i=2}^{k}P(x_{i}|x_{i-1})P(x_{1}) \tag{6}\]  
Assuming that both process noise and measurement noise follow a Gaussian distribution, we obtain:  
\[P(x_{i}|x_{i-1})\propto exp\left(-\frac{1}{2}||x_{i}-f(x_{i-1})||_{R_{i}}^{2}\right) \tag{7}\]  
\[P(z_{i}|x_{i})\propto exp\left(-\frac{1}{2}||z_{i}-h(x_{i})||_{Q_{i}}^{2}\right) \tag{8}\]  
\(f(\ )\) denotes the transition function that models the relationship between state variables \(x_{i-1}\), and \(x_{i}\). \(h(\ )\) 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. \(||\ ||_{R_{i}}^{2}\) denotes the marginal distance with respect to the covariance matrix \(R_{i}\), and is computed as:  
\[||\alpha_{i}||_{R_{i}}^{2}=\alpha_{i}\cdot R_{i}\cdot(\alpha_{i})^{T} \tag{9}\]  
Therefore, the above equation can be reformulated as:  
\[X^{*}=\underset{i=2}{argmax}\prod_{i=2}^{k}exp\left(-\frac{1}{2}||x_{i}-f(x_{ i-1})||_{R_{i}}^{2}\right)\times\prod_{i=2}^{k}exp\left(-\frac{1}{2}||Z_{i}-h(x_{ i})||_{Q_{i}}^{2}\right) \tag{10}\]  
By simplifying the above expression, we obtain:  
\[X^{*}=\underset{x}{argmax}\left(exp\left(-\frac{1}{2}\left(\sum_{i=1}^{k}||x_ {i}-f(x_{i-1})||_{R_{i}}^{2}+\sum_{i=1}^{k}||Z_{i}-h(x_{i})||_{Q_{i}}^{2} \right)\right)\right) \tag{11}\]  
To achieve the maximum a posteriori probability, the cost function must be minimized; that is:  
Figure 1:0  
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.