# Co-Operatively Increasing Smoothing and Mapping Based on Switching Function
## 3 Models and Methods
### Collaborative Positioning  
The first three sections of Section 3 describe the structure of the algorithm and the proposed methodology. The algorithm flow and pseudocode are given in this section. In addition, we will extend the topology constraints to the verification of the positioning results and the relocation of the failed positioning of single UAV. When the new switching functions proposed in the previous chapter are added  
the VIO factor of node \(i\) at time \(k\) based on the drone's moving speed switching function and on the MD switching function, respectively. \(w_{u}^{k}{}_{ij}\) represents the switching function based on the consistency of UWB data between node \(i\) and node \(j\) at time \(k\). For the VIO factor, the constraint method is used to calculate the following:  
\[dis_{k,vio}^{2}=\left[\sum_{i=1}^{N-1}\sum_{j=i+1}^{N}\left(x_{x,k}^{i}-x_{x,k}^ {j}\right)^{2}+\sum_{i=1}^{N-1}\sum_{j=i+1}^{N}\left(x_{y,k}^{i}-x_{y,k}^{j} \right)^{2}\right] \tag{23}\]  
where \(x_{x,k}^{i}\) and \(x_{y,k}^{i}\) represent the co-ordinates in position obtained by the VIO system \(i\) at time k. To obtain the value of the switching function, further calculation of the square of the sum of distances obtained by UWB is required, and, in the same way, \(dis_{k,uwb}^{2}\) is obtained. In Equation (20), \((d_{u}-d_{e})^{2}\) is replaced by the following formula: \(\left(dis_{k,vio}^{2}-dis_{k,uwb}^{2}\right).\)  
It is common for VIO to crash when faced with low feature points and low textures or high-mobility scenes. And, in the experiment, it was found that, when the data of UWB and the data of VIO became unreliable, the positioning results were easy to become confused. In order to further improve the robustness of the algorithm, we will further extend the topological constraints. When the UWB data are unreliable at a certain time, the least two-hop distance between the two nodes or the distance from the previous moment is considered, and the experimental results show that the distance at the previous time has higher accuracy. In addition, when the output result of a single VIO is NAN, we consider the topology of the previous moment and the topology between the drones except the position breakdown one at this moment. According to the improved MDS algorithm, a directionless, centralized co-ordinate after dimension reduction is obtained. \[B_{i}=-\frac{1}{2}J{D_{i}}^{2}J \tag{24}\]  
We suppose \(n=3\), where \(D_{i}\) refers to UWB measurements matrix at times \(i\), and  
\[J=\mathbf{E}-n^{-1}\mathbf{I}= \begin{bmatrix}1-\frac{1}{n}&-\frac{1}{n}&-\frac{1}{n} \\ -\frac{1}{n}&1-\frac{1}{n}&-\frac{1}{n}\\ -\frac{1}{n}&-\frac{1}{n}&1-\frac{1}{n}\end{bmatrix}  \tag{25}\]  
is the Eigenvalue decomposition of matrix \(B_{i}\). Then, \(B_{i}\) becomes  
\[B_{i}=-JX_{i}^{T}X_{i}I=VUV^{T}=V\sqrt{U}\left(V\sqrt{U}\right)^{T} \tag{26}\]  
Then, we have  
\[JX_{i}^{T}=V\sqrt{U} \tag{27}\]  
where \(U=diag(\lambda_{1},\lambda_{2})\) and \(V=[q_{1},q_{2}]\). Retain the two largest eigenvalues \(\lambda_{1}\), \(\lambda_{2}\) and corresponding eigenvectors \(q_{1}\), \(q_{2}\) to calculate the 2D co-ordinates of the node. According to our previous thought proposed in paper [10], it is possible to convert a problem with \(2M\) degrees of freedom into a problem with 1 degree of freedom, and this only requires the rotation angle to be solved. Then, the rotation angle is found by the least-squares method, and the location of position breakdown UAV is recovered through the topology. After the position is correct, the position and posture of the drone can be corrected through iteration, so as to improve success rate of positioning. This is the improvement for topological constraints. Algorithm 1 summarizes the algorithm to estimate locations of nodes with noisy measurements.