# Co-Operatively Increasing Smoothing and Mapping Based on Switching Function
## 3 Models and Methods
### Switching Functions
This paper uses a switching function to deal with IMU-camera data, which do not participate in the IMU pre-integration process, but only participate in the joint solution process of data fusion. In the joint solution process, this paper uses the following formula to calculate the loss function of IMU-camera data:  
\[L_{IC}=w_{v}\cdot w_{IC}\cdot(z_{IC}-h(x)) \tag{19}\]  
where \(L_{IC}\) is the loss function of the VIO system, \(w_{IC}\) is the output value of the switching function, \(z_{IC}\) is the observation value of the VIO system, and \(h(x)\) is the prediction value calculated based on the state variable \(x\). In this way, this paper effectively deals with the abnormality and wild values of IMU-camera data. UWB signals have advantages of high accuracy, low power consumption, and anti-interference, which can provide absolute position information between drones. The advantage of UWB data is that it can perform positioning using distance information without visual information, and it can provide low-frequency and high-accuracy output. The disadvantage of UWB data is that, due to various factors in the signal propagation process, such as obstacles, occlusions, reflections, dynamic objects, etc., it causes phenomena such as signal attenuation, reflection, refraction, multipath, etc., which affect the accuracy and reliability of the data. To deal with these problems, this paper describes a switching function to adjust the weight of the loss function of UWB data. Moreover, we describe a switching function based on the consistency of UWB data, which judges the reliability of UWB data according to its differences. If there is a large difference in the UWB data, this indicates that there is a problem with the data, which may have noise or outliers, and the weight of the UWB data should be reduced; if UWB data are consistent, this indicates that the UWB data are reliable and the weight should be increased. The following equation is used to compute the switching function based on the movement of the UAV:  
\[w_{u}=e^{-k_{u}(d_{u}-d_{e})^{2}} \tag{20}\]  
where \(w_{u}\) is the output value of the switching function, \(d_{u}\) is the sum of distances between UWB data, \(d_{e}\) is the sum of distances calculated based on position estimation, and \(k_{u}\) is the set parameter. It can be seen that, when \(d_{u}-d_{e}\) is very large, \(w_{u}\) is close to zero, indicating that the UWB data are inconsistent and the weight of the loss function is very small; when \(d_{u}-d_{e}\) is very small, \(w_{u}\) is close to one, indicating that UWB data are consistent and the weight of the loss function is large. This way, the weight of UWB data can be adjusted according to consistency. In the joint solution process, we use the following formula to calculate the loss function of UWB data:  
\[L_{UWB}=w_{u}\cdot(z_{UWB}-h(x)) \tag{21}\]  
where \(L_{UWB}\) is the loss function of UWB data, \(Z_{UWB}\) is the observation value of UWB data, and \(h(x)\) is the prediction value calculated based on the state variable \(x\). In this way, this paper can effectively deal with the abnormality and wild values of UWB data. The switching function proposed in this paper can effectively deal with the outliers and wild values of UWB data. The following represents a simulation of the switching function using Python. Assuming that the simulation steps numbered 1000, the switching function had little effect on the simulation results without introducing large errors. Then, random large errors were introduced into the simulation, and the simulation results are shown in Figure 5.  
It can be seen from Figure 5a,b that, after adding the switching function, the accuracy was significantly improved when facing random large errors. After multiple tests, the accuracy was improved by about 50% in Figure 5c,d; of course, this was only limited to the introduction of random large errors. If there was always a peaceful and uniform error, the effect improvement was not significant.