A Vision /GNSS/ INS Multi-source Relative Navigation Algorithm Based on Federated UPF

In addressing the problem of multi-source information fusion of Vision /GNSS/INS relative navigation in the process of formation flight of UAVS, a Vision/GNSS/INS relative integrated navigation algorithm based on federated unscented particle filter is proposed. In this paper, the relative centroid motion equation, attitude motion equation, and measurement model for the integrated navigation system of the UAV are established. The importance density function is generated by the unscented Kalman filter to guide the particle distribution, and the square root and correction factor are introduced to the problem of filter interruption caused by the loss of positive definite error covariance matrix. In the information fusion stage, a new particle set is obtained by fusing the sub-filter particle set with federated filter, and weight distribution among the fusing particle set is evaluated to obtain the final fusion information. Finally, the proposed federated unscented particle filter relative navigation algorithm is simulated. The outcomes of the simulation reveal that the improved method can attain more accurate relative attitude, position and velocity information than the federal particle filter algorithm.