# Co-Operatively Increasing Smoothing and Mapping Based on Switching Function
## 3 Models and Methods
### System Model
\(f\), \(h\), and \(g\) correspond to their measurement functions. The probability of each node for all parameters and measurements is as follows:  
\[P(X,L,U,Z,D)\propto P(x_{0})\prod_{\alpha=1}^{M}P(x_{\alpha}|x_{\alpha-1},u_{ \alpha})\prod_{\beta=1}^{N}P\Big{(}z_{\beta}\Big{|}x_{i_{\beta}},l_{i_{\beta}} \Big{)}\ \prod_{\gamma=1}^{N}P\Big{(}d_{i,j_{\gamma}}\Big{|}x_{i_{\gamma}},x_{j_{ \gamma}}\Big{)} \tag{2}\]  
The MAP estimation of \(X^{*}\), \(L^{*}\) is obtained by minimizing the negative logarithm of the joint probability, which is the linear problem we want to solve:  
\[X^{*},L^{*}=argmax_{X,L}P(X,L,U,Z,D)=argmin_{X,L}-logP(X,L,U,Z,D) \tag{3}\]  
The nonlinear least-squares problem is as follows:  
\[ \begin{array}{l}X^{*},L^{*}=argmin\bigg{\{}\sum\limits_{\alpha=1}^{M}||f_{a }(x_{\alpha-1},u_{\alpha})-x_{\alpha}||_{\Lambda_{\alpha}}^{2}+\sum\limits_{ \beta=1}^{N}||h_{\beta}\Big{(}x_{i_{\beta}},l_{i_{\beta}}\Big{)}-z_{\beta}||_ {T_{\beta}}^{2}\\ \hskip 142.26378 pt+\sum\limits_{\gamma=1}^{K}||g_{\gamma}\Big{(}x_{i_{\gamma }},x_{j_{\gamma}}\Big{)}-d_{i,j_{\gamma}}||_{\Phi_{\gamma}}^{2}\bigg{\}}\end{array}  \tag{4}\]  
Linearizing the Equations (5)-(7) yields the following:  
\[f_{i}(x_{i-1},u_{i})-x_{i}\approx\Big{\{}f_{i}\Big{(}x_{i-1}^{0},u_{i}\Big{)}+ F_{i}^{i-1}\delta x_{i-1}\Big{\}}-\Big{\{}x_{i}^{0}+\delta x_{i}\Big{\}}= \Big{\{}F_{i}^{i-1}\delta x_{i-1}-\delta x_{i}\Big{\}}-a_{i} \tag{5}\]  
where \(F_{i}^{i-1}\) is the \(f_{i}(. )\) Jacobian matrix, the expression of which is as follows:  
\[F_{i}^{i-1}:=\frac{\partial f_{i}(x_{i-1},u_{i})}{\partial x_{i-1}}\Big{|}_{x_ {i-1}^{0}} \tag{6}\]  
\[a_{i}:=x_{i}^{0}-f_{i}\Big{(}x_{i-1}^{0},u_{i}\Big{)} \tag{7}\]  
Figure 2: Five-member UAV application scenarios. The topological constraints for UWB are as follows:  
\[ \begin{split} g_{\gamma}\Big{(}x_{i_{\gamma}},x_{j_{\gamma}}\Big{)}-d _{i,j_{\gamma}}&=\Big{\{}g_{\gamma}\Big{(}x_{i_{\gamma}}^{0},x_{j_{ \gamma}}^{0}\Big{)}+Q_{\gamma}^{j_{\gamma}}\delta x_{i_{\gamma}}+P_{\gamma}^{j_ {\gamma}}\delta x_{i_{\gamma}}\Big{\}}-d_{n}\\ &=\Big{\{}Q_{\gamma}^{j_{\gamma}}\delta x_{i_{\gamma}}+P_{\gamma}^ {j_{\gamma}}\delta x_{i_{\gamma}}\Big{\}}-b_{n}\end{split}  \tag{8}\]  
\[ \begin{split} Q_{\gamma}^{i_{\gamma}}&=\frac{ \partial g_{n}\Big{(}x_{i_{\gamma}},x_{j_{\gamma}}\Big{)}}{\partial x_{i_{ \gamma}}}\Bigg{|}_{(x_{i_{\gamma}}^{0},x_{j_{\gamma}}^{0})}\\ P_{\gamma}^{j_{\gamma}}&=\frac{\partial g_{n} \Big{(}x_{i_{\gamma}},x_{j_{\gamma}}\Big{)}}{\partial x_{j_{\gamma}}}\Bigg{|}_ {(x_{i_{\gamma}}^{0},x_{j_{\gamma}}^{0})}\end{split}  \tag{9}\]  
Similarly, for all parameters \(\theta\), the linearization result becomes the following:  
\[ \begin{split}\delta\theta^{*}=argmin\bigg{\{}\sum\limits_{\alpha= 1}^{M}||F_{\alpha}^{a-1}\delta x_{\alpha-1}+G_{\alpha}^{a}\delta x_{\alpha}-a _{\alpha}||_{\Lambda_{\alpha}}^{2}+\sum\limits_{\beta=1}^{N}||H_{\beta}^{i_{ \beta}}\delta x_{i_{\beta}}+\hat{J}_{\beta}^{j_{\beta}}\delta l_{j_{\beta}}-c_ {\beta}||_{T_{\beta}}^{2}\\ +\sum\limits_{\gamma=1}^{K}||Q_{\gamma}^{j_{\gamma}}\delta x_{i_{ \gamma}}+P_{\gamma}^{j_{\gamma}}\delta x_{i_{\gamma}}-b_{n}||_{\Phi_{\gamma}}^ {2}\bigg{\}}\end{split}  \tag{11}\]