## 4 Enhanced Collaborative Localization Based on Belief Propagation
### Edge Probability Model for Air-Ground Swarm Cooperative Navigation
After the calculation of \(m\) sigma sampling points is complete, the mean and covariance of the relative observation information \(\mathbf{z}_{i,j}\) can be obtained. \[\mathbf{\overline{z}}=\sum\limits_{t=1}^{m}\omega_{i}\mathbf{Z}_{t} \tag{7}\]  
\[\mathbf{\overline{\
u}}=\sum\limits_{t=1}^{m}\omega_{t}(\mathbf{\chi}_{t}-\mathbf{ \overline{\chi}}_{joint})(\mathbf{Z}_{t}-\mathbf{\overline{z}})^{T} \tag{8}\]  
\[\mathbf{\Phi}=\sum\limits_{t=1}^{m}\omega_{t}(\mathbf{Z}_{t}-\mathbf{\overline{z}})(\mathbf{Z }_{t}-\mathbf{\overline{z}})^{T} \tag{9}\]  
Thus, the linearization parameters of the relative observation information are calculated by:  
\[\mathbf{A}_{i,j}=\mathbf{\overline{\
u}}^{T}\mathbf{P}_{joint}^{-1} \tag{10}\]  
\[\mathbf{b}_{i,j}=\mathbf{\overline{z}}-\mathbf{A}_{i,j}\mathbf{\overline{X}}_{joint} \tag{11}\]  
\[\mathbf{\Omega}_{i,j}=\mathbf{\Phi}-\mathbf{A}_{i,j}\mathbf{P}_{joint}(\mathbf{A}_{i,j})^{T} \tag{12}\]  
Then, the relative observation information \(\mathbf{z}_{i,j}\) can be decomposed into:  
\[\mathbf{z}_{i,j}\approx\mathbf{A}_{i,j}^{1}\mathbf{X}_{i}+\mathbf{A}_{i,j}^{2}\mathbf{X}_{j}+\mathbf{ b}_{i,j}+\mathbf{e}_{i,j} \tag{13}\]  
After repeating the above steps until all relative observations \(\mathbf{z}\) are linearized, the hybrid observation factor \(\mu\) is computed in the belief propagation model. If the \(i\)th member has not received any hybrid observation factor \(\mu_{p\to i}\) for a member other than \(j\), directly establish the hybrid observation factor \(\mu_{i\to i}\) from the \(i\)th to \(j\)th members in the belief propagation model  
\[\mu_{i\to j}(\mathbf{X}_{i})\propto N(\mathbf{\xi}_{i\to j},\mathbf{\Gamma}_{i\to j}) \tag{14}\]  
where \(\mathbf{\xi}_{i\to j}=\mathbf{z}_{i,j}-\mathbf{A}_{i,j}^{1}\mathbf{X}_{i}-\mathbf{b}_{i,j}\approx \mathbf{H}_{i\to j}\mathbf{X}_{j}\), \(\mathbf{H}_{i\to j}=\mathbf{A}_{i,j}^{2}\), \(\mathbf{\Gamma}_{i\to j}=\mathbf{R}_{i,j}+\mathbf{\Omega}_{i,j}+\mathbf{A}_{i,j}^{1}\mathbf{P}_{i }(\mathbf{A}_{i,j}^{1})^{T}\), and \(\mathbf{R}_{i,j}\) is the relative observation noise covariance. If the \(i\)th member has received \(n(i)_{7}\) hybrid observation factors \(\mu_{p\to i}\) of other members except \(j\), first calculate the optimized position estimate \(N(\mathbf{\overline{X}}_{i\to j},\mathbf{P}_{i\to j})\) of the \(i\)th member based on the hybrid observation factor \(\mu_{p\to i}\)  
\[ \begin{split} N(\mathbf{\overline{X}}_{i\to j},\mathbf{P}_{i\to j})& \propto\prod\limits_{p\in n(j)_{7}}\mu_{p\to i}(\mathbf{X}_{i})\times N(\mathbf{X}_{i },\mathbf{P}_{i})\\ &\propto\prod\limits_{p\in n(j)_{7}}N(\mathbf{\xi}_{p\to i},\mathbf{ \Gamma}_{p\to i})\times N(\mathbf{X}_{i},\mathbf{P}_{i})\end{split}  \tag{15}\]The calculation of the above formula can be obtained by \(n(i)_{j}\) Kalman filtering updates. Let \(\overline{\mathbf{X}}_{i\to j}=\mathbf{X}_{i}\), \(P_{i\to j}=\mathbf{P}_{i}\), and the formula of filtering update is:  
* Let \(\overline{\mathbf{z}}=\mathbf{H}_{p\to i}\overline{\mathbf{X}}_{i\to j}\);
* Filter gain \(\mathbf{K}=\frac{\mathbf{P}_{i\to j}\mathbf{H}_{p\to i}^{T}}{\mathbf{H}_{p\to i}\mathbf{P}_{i\to j }\mathbf{H}_{p\to i}^{T}+\mathbf{P}_{p\to i}}\);
* State estimation equation \(\overline{\mathbf{X}}_{i\to j}=\overline{\mathbf{X}}_{i\to j}+\mathbf{K}(\mathbf{\xi}_{p\to i}- \overline{\mathbf{z}})\);
* Estimating the mean square error equation \(\mathbf{P}_{i\to j}=\big{(}\mathbf{I}-\mathbf{K}\mathbf{H}_{p\to i}\big{)}\mathbf{P}_{i\to j}\);