# Co-Operatively Increasing Smoothing and Mapping Based on Switching Function
## 3 Models and Methods
### Switching Functions  
A switching function is a function that adjusts the weights of the loss function according to variations and anomalies in the data and suppresses or rejects the effects of anomalies and wild values, thus improving the localization robustness. In this paper, two different types of switching functions are used: one for IMU+ camera data and the other for UWB data. The two types of switching functions have different design and judgment criteria, and the following describes these two types of switching functions, respectively. IMU-camera data refer to the data provided by the inertial measurement unit (IMU) carried by the UAV itself and the camera, which can form a visual-inertial odometry (VIO) system for estimating the UAV's position and motion status. The advantage of the VIO system is that it can utilize the visual information from the camera and the inertial information from the IMU for autonomous localization without an external reference, and it can provide high-frequency and low-latency outputs. The disadvantage of the VIO system is that it can cause data anomalies and wild values due to noise, failure, and interference of the IMU and the camera itself, thus affecting the positioning accuracy and robustness. To deal with these problems, this paper describes a switching function to adjust the weights of the loss function of the VIO system. Two different judgment criteria are used to design the switching function of the VIO system: one is based on the moving speed of the UAV, and the other is based on the consistency of the Mahalanobis distance (MD). The judgment criterion based on the UAV's moving speed refers to the reliability of the VIO system based on the UAV's movement state. If the UAV's moving speed is too fast and exceeds the set threshold, this means that there is a problem with the VIO system and there may be a drift or cumulative error, in which case its weight should be reduced, and vice versa:  
\[w_{v}=\frac{2}{1+e^{k_{v}(v-v_{0})}} \tag{16}\]  
where \(w_{v}\) is the output value of the switching function, \(v\) is the drone's moving speed, \(v_{0}\) is threshold, and \(k_{v}\) is coefficient. It can be seen that, when \(v\gg v_{0}\), the switching function \(w_{v}\) is close to zero, which indicates that the VIO system data are unreliable and the weight of the loss function is very small; when \(v\) is close to \(v_{0}\), \(w_{v}\) is close to one, indicating that the VIO system data are reliable and the weight of the loss function is large. In this way, the weight of the VIO system data can be adjusted according to the drone's moving speed. The switching function based on MD can judge the reliability of data according to the difference between the data and the prediction value. MD is a distance measure that considers the data distribution and covariance matrix, and it can reflect the correlation and anomaly degree of the data. The larger the MD, the less reliable the data; the smaller the MD, the more reliable the data. This paper uses the following formula to calculate the MD:  
\[d_{M}=\sqrt{\left(z-\hat{z}\right)^{T}S^{-1}(z-\hat{z})} \tag{17}\]  
where \(z\) is the observation value, \(\hat{z}\) is the prediction value, and \(S\) is the covariance matrix. This paper uses the following formula to calculate the switching function:  
\[w=\frac{1}{1+d_{M}^{2}} \tag{18}\]  
where \(w\) is the output value of the switching function, which represents the weight of the loss function. It can be seen that, when the MD is very large, the switching function is close to zero, indicating that the observation value is unreliable and the weight of the loss function is very small. When the MD is very small, the switching function is close to one; this indicates that the observation value is reliable and the weight of the loss function isvery large. In this way, it can effectively suppress or eliminate the influence of outliers and wild values.