# Co-Operatively Increasing Smoothing and Mapping Based on Switching Function
## 3 Models and Methods  
### System Model  
The mathematical model and optimization algorithm for collaborative positioning in this paper can be fused using both IMU-camera and UWB sensor data to improve positioning accuracy and robustness. In this paper, we use two types of sensor data for collaborative localization: IMU-camera, which can provide VIO information on UAVs, and UWB, which can provide information on the distance measurement between UAVs. The relative positional relationships between the UAVs are taken into account, which increases the observability and constraints. This paper considers a co-operative positioning system composed of \(M\) drones, where the drones can communicate and exchange information with each other. We assume that each drone is equipped with an IMU and a camera, and that the calibration and synchronization between the IMU and the camera have been completed. This paper also assumes that each drone is equipped with a UWB module, and that the calibration and synchronization between the UWB modules have been completed. We use an optimization algorithm based on graph optimization (GOP) to solve the above mathematical model of collaborative localization. Figure 1 is a block diagram of the algorithm flow in this paper. Graph optimization can represent the optimization variables as nodes in the graph, the constraints as edges in the graph, and the loss function as weights or cost functions on the edges [27]. Graph optimization can utilize techniques such as sparsity, localization, and linearization to reduce computational complexity and memory consumption, and to improve the efficiency and accuracy of a solution. The loss function for IMU+ camera data represents the consistency constraints between the VIO information of the UAV itself and its position and attitude; i.e., the relative displacements and rotations between two neighboring moments should be equal to the differences in the positions and attitudes under the global co-ordinate system [28]. The loss function for UWB data represents the constraints between the distance measurements between the UAVs and their positions; i.e., the distance measurements between the UAVs should be equal to the modulus of the difference between the positions in the global co-ordinate system. A simple, basic model is shown above in Figure 2, and, in the scenario of this paper, we use the following qualification: Assuming that the scenario occurs in a two-dimensional co-ordinate system, the vehicle is able to obtain its own a priori position information through GPS or anchor nodes and knows the other nodes' a priori positions. Each member is initialized with itself as the zero point of the environment co-ordinate system. The ranging information of UWB can be shared among all members. The updating frequency of ranging information is synchronized with the key frame rate of the VIO system. There is no time delay or data error in the communication. The environment has zero clutter, so there is no false detection or omission of the vehicle. Moreover, we do not consider the potential effects of restricted communication between robots and assume that robots have full high-bandwidth connectivity when encountering each other. Figure 1: System diagram of the proposed collaborative positioning algorithm. The model IMU/UWB/camera problems are as follows:  
\[models\left\{ \begin{array}{l}x_{i,k}=f_{k}(x_{i,k-1},u_{i,k})+w_{i,k}\\ z_{i,k}=h_{k}\big{(}x_{i_{k},l_{i_{k}}}\big{)}+v_{i,k}\\ d_{i,k}=g_{k}\big{(}x_{i_{k},x_{i_{k}}}\big{)}+q_{k}\end{array} \right. \tag{1}\]  
where \(x_{i,k}\) is the state transition equation of IMU itself at time \(k\), and \(X=\big{\{}x_{i,k}\big{\}}\) with \(i\in 0\)\(\dots M\); \(l_{i,k}\) is the measurement of the observed landmark point, and \(L=\big{\{}l_{i,k}\big{\}}\) with \(j\in 0\)\(\dots N\); \(u_{i,k}\) is the controller input, and \(U=\big{\{}u_{i,k}\big{\}}\) with \(i\in 0\)\(\dots M\); and \(d_{i,k}\) is the UWB measurement from node \(i\) to node \(j\), and \(D=\big{\{}d_{i,k}\big{\}}\) with \(i,j\in 0\)\(\dots M\).