## 4 Enhanced Collaborative Localization Based on Belief Propagation  
### Edge Probability Model for Air-Ground Swarm Cooperative Navigation  
An applicable scenario of the algorithm proposed in this article is shown in Figure 9. An air-ground swarm consists of members such as unmanned aerial vehicles and unmanned vehicles. Due to electromagnetic interference, i.e., swarm members carrying different sensors, etc., there is a relative measurement of heterogeneous ranging and ranging/angle information among members. The air-ground swarm belief propagation cooperative navigation graph is first modeled to approximate the air-ground swarm member locations using a normal distribution. According to the coordinates of the \(i\)th member \((x_{i},y_{i},z_{i})\), the position covariance of the \(i\)th member is estimated to be \(\mathbf{P}_{i}\). The prior probability distribution function of the position of the \(i\)th member is as follows:  
\[p(\mathbf{X}_{i})=N(\mathbf{X}_{i},\mathbf{P}_{i}) \tag{3}\]  
where \(\mathbf{X}_{i}=(x_{i},y_{i},z_{i})\) is the mean value of the position of the \(i\)th member and \(N\) is the normal distribution in probability statistics. In the belief propagation model, variable nodes are used to represent the belief \(q(\mathbf{X}_{i})\sim N(\mathbf{X}_{i},\mathbf{P}_{i})\) of the position of the \(i\)th member, as shown in Figure 9. Swarm members transmit their own position beliefs through hybrid observation factors \(\mu\) and correct the positions of adjacent members. \(\mathbf{z}_{i,j}\) is used to denote the relative heterogeneous measurements of the \(i\)th and \(j\)th members, which can be either ranging or ranging angular measurements. It can be thought of as consisting of a function \(h_{i,j}(\mathbf{X}_{i},\mathbf{X}_{j})\) associated with the position of the \(i\)th, \(j\)th member and Gaussian noise, i.e.,  
\[z_{i,j}=h_{i,j}(\mathbf{X}_{i},\mathbf{X}_{j})+\mathbf{\eta}_{i,j} \tag{4}\]  
where \(\mathbf{\eta}_{i,j}\) is Gaussian-distributed noise with mean 0 and variance matrix \(\mathbf{R}_{i,j}\). To estimate the mean and variance of the members' position, the relative heterogeneous measurement data are approximated as the affine function associated with \(\mathbf{X}_{i}\), \(\mathbf{X}_{j}\) with Gaussian noise, as shown in the following equation:  
\[h_{i,j}(\mathbf{X}_{i},\mathbf{X}_{j})\approx A_{i,j}^{1}\mathbf{X}_{i}+A_{i,j}^{2}\mathbf{X} _{i}+\mathbf{b}_{i,j}+\mathbf{e}_{i,j} \tag{5}\]  
Figure 9: Air–ground swarm with heterogeneous observation information. where \(\left[ \begin{array}{c}A_{i,j}^{1}\\ A_{i,j}^{2}\end{array} \right]=\mathbf{A}_{i,j}\), \(\mathbf{b}_{i,j}\in\mathbf{R}^{n_{z}}\) and \(\mathbf{e}_{i,j}\in\mathbf{R}^{n_{z}}\) are variables subject to a Gaussian distribution whose mean is zero and covariance matrix is \(\mathbf{\Omega}_{i,j}\). To establish a hybrid observation factor \(\mu\), it is necessary to linearize the relative observation information \(z\) between swarm members. Taking the relative observation data of the \(i\)th and \(j\)th members as an example, since \(\mathbf{z}_{i,j}\) usually contains nonlinear errors, the linear regression is decomposed by the sigma-point method [30]. Select \(m\) sigma sampling points \(\mathbf{\chi}_{1},\ldots,\mathbf{\chi}_{m}\) and weights \(\omega_{1},\ldots,\omega_{m}\) by unscented transformation, where the sampling points \(\mathbf{\chi}_{t}=[\mathbf{X}_{t}^{t},\mathbf{X}_{t}^{t}]\quad t=1,2,\cdots,m\). \(m\) sigma sampling points are calculated according to the type of relative observation data \(\mathbf{z}_{i,j}\)  
\[\mathbf{Z}_{t}=\left\{ \begin{array}{ll}d_{t}&\mathbf{z}_{i,j}\text{ contains ranging information}\\ \left[d_{t},\mathbf{\alpha}_{t},\beta_{t}\right]&\mathbf{z}_{i,j}\text{ contains ranging/angle information}\end{array} \right.t=1,2,\cdots,m \tag{6}\]  
where \(d_{t}=\left\|\mathbf{X}_{t}^{t}-\mathbf{X}_{t}^{t}\right\|\), \(\mathbf{\alpha}\) and \(\beta\) are the azimuth and elevation angle, respectively, calculated by sampling position \([\mathbf{X}_{i}^{t},\mathbf{X}_{i}^{t}]\).