# Co-Operatively Increasing Smoothing and Mapping Based on Switching Function
## 1 Introduction  
The co-operative positioning of UAV swarms involves multiple UAVs achieving the high-precision positioning of themselves and targets through mutual communication and perception. UAV swarm co-operative positioning technology has a wide range of application prospects in military, civil, and scientific research [1, 2]. However, UAV swarm co-operative positioning technology is restricted by [3], for example, limited accuracy, limited range, sensitivity to environmental conditions, cost, and so on. In particular, we are more focused on satellite denial scenarios, as GNSS signals may encounter issues with fading, multipath effects, interference, and spoofing, so GNSS is not reliable [4]. In addition, the electromagnetic environment in GNSS-denied scenarios is often complex and noisy [5]. Therefore, we design the algorithm in this paper to reduce the error and accuracy divergence of relative positioning in the environments mentioned above and improve the robustness of the system without increasing the amount of computation.  
To address the issue of location divergence in a swarm, we propose that adding ranging information is the most effective approach. Bluetooth and Wi-Fi are not very accurate, while radio-frequency identification (RFID) has a very limited working distance [6]. Therefore,we suggest using UWB due to its high accuracy and ease of deployment, which is well-suited for the scenario we discussed. However, the ranging accuracy of UWB is limited to the decimeter level. To enhance accuracy, we will apply the visual-inertial odometry (VIO) scheme to the algorithm, which can bring the accuracy level close to the centimeter range [7; 8; 9]. We have conducted simulations to maintain an overall high accuracy level. Moreover, the advantage of UWB is still in the case of anchors so far, and few people discuss the advantages that UWB can provide in collaborative positioning in the case of anchor-free scenarios. This paper clearly explains the advantages of UWB fusion positioning under anchor-free circumstances.  
At the algorithmic level, in the latest research based on ranging methods, there are still scholars who are improving the traditional distributed MDS algorithm [10], and semi-definite programming algorithm [11], but they are not suitable for the scenario in this paper. This paper also involves the problem of multi-sensor fusion. In the field of fusion algorithms, filter-based methods have been gradually replaced by graph optimization methods. In this paper, we mainly focus on graph optimization algorithms.  
In summary, the main challenge that this paper focuses on of co-operative positioning at this stage is how to further reduce the divergence of the positioning error and suppress the higher error term in time. The new approach proposed in this paper can effectively solve the current challenges. Therefore, the main contributions of this paper are as follows:  
* A novel co-operative incremental smoothing and mapping (CI-SAM) algorithm is constructed and the approach of this paper is innovative.
* This algorithm can accurately realize cluster positioning without additional algorithms such as target detection, reduce swarm location divergence, and avoid single-UAV positioning failures, and can be applied to the cluster formation flight of small UAVs to achieve an accurate collaborative localization capability.
* The new method is easily scalable and can be applied to all mainstream VIO systems; as long as the UWB module is accessed, a collaborative localization system with superior performance can be obtained.  
The structure of this paper is organized as follows: Section 1 introduces the current problems and challenges from the perspective of application scenarios, and leads to the purpose and innovation of this paper. Section 2 presents the current development of methods. Section 3 first models the problem, and then introduces the architecture and principles of the algorithms. Section 4 is a simulation comparison of the paper, and Sections 5 and 6 include our discussions and conclusions.