# Co-Operatively Increasing Smoothing and Mapping Based on Switching Function
## 3 Models and Methods
### Factor Graph  
A factor graph is a two-way graph used to represent the factorization of a function, which includes variable nodes and factor nodes and represents the relationship between variables and factors through edges. Its advantage is that it can utilize the relative distance information between UAVs to improve positioning accuracy and robustness and reduce the dependence on base stations [28]. In probability theory and its applications, the factor graph is a model that has been widely used in Bayesian inference. The factor graph can model the collaborative localization problem as a probabilistic graph model, where the variable nodes represent the positional states of the UAVs, the factor nodes represent the distance observations between the UAVs or the positional observations of the base station, and the edges represent the associations between the variables and the factors. By performing Bayesian inference on the factor graph, the optimal position estimation of the UAVs can be solved.  
The factor graph can flexibly represent multiple types of observations, such as TOA, TDOA, angle, etc., as well as data sources with different accuracies and reliabilities, such as UWB, GPS, vision, etc., so as to improve the accuracy and robustness of the localization. The factor graph can utilize distributed or centralized inference algorithms, such as belief propagation (BP) [12] or maximum a posteriori probability (MAP) [29], to exchange local information among UAVs and achieve globally optimal or suboptimal position estimation. The factor graph can be decomposed or aggregated according to the topology and communication constraints of the UAV swarm, thus reducing the computational complexity and communication overhead and improving the localization efficiency and scalability.  
The basic structure of the factor graph is shown in Figure 3. To describe the factor graph model, we assume that the global function is \(\text{g}(x_{1},\ \ldots x_{n})\), where the set \(\{x_{1},\ldots x_{n}\}\) refers to the variables present in the model. Due to the dichotomous nature of factor graphs, this global function can be expressed as  
\[g(x_{1},\ \ldots x_{n})=\prod\limits_{j\in J}f_{j}\big{(}X_{j}\big{)} \tag{12}\]  
where \(f_{j}\big{(}X_{j}\big{)}\) refers to a single factor or function and consists of a subset of variables in \(\{x_{1},\ \ldots x_{n}\}\). To further illustrate this, we can represent this graphically, as shown in the image above, where the variable node is represented by \(x_{n}\) and the factor node is represented by \(f_{n}\). By turning global functions into maximization problems, \(\operatorname*{argmax}_{X_{j}}\prod_{j}f_{j}\big{(}X_{j}\big{)}\) is obtained.  
The optimal solution for the measurement \(f_{j}(X_{j})\) can be found by looking for the right combination of \(X_{j}\), where \(X_{j}\) represents all known vehicle gestures and landmarks in that factor, maximizing the probability of that particular measurement. Assuming Gaussian noise, linearizing \(f(x)\) yields  
\[\Delta x^{*}=min||A\Delta x+b||^{2} \tag{13}\]  
where, for \(g(x)=\frac{1}{2}||f(x)+J^{T}(x)\Delta x||^{2}\), Taylor series expansion is performed, the first-order linear component is retained, the \(x\) component is derived, and the value after derivation is 0 to obtain the minimum value of \(g(x)\) as \(\Delta x^{*}\).  
\[J(x)J^{T}(x)\Delta x=-J(x)f(x) \tag{14}\]  
For the incremental equation, \(H(x)\Delta x=g(x)\)  
\[J(x)=\frac{\partial f(x(k),l(k),d(k))}{\partial x} \tag{15}\]  
Based on the factor graph theory mentioned above, the factor diagram structure of the proposed system is designed, as shown in the Figure 4. The topology can effectively suppress error divergence.  
Figure 4: The factor graph composition proposed by this algorithm.  
Figure 3: Factor graph for sum-product algorithm.