Double-Stage Filtering Fusion Navigation Algorithm Based on Multisource Optimal Selection

To address the navigation accuracy issues of UAV formations arising from leader failures, environmental factors, and other elements, the paper presents the double-stage filtering fusion (DF) navigation algorithm based on multi-source optimal selection. Traditional cooperative navigation algorithms often overlook the influence of geometric relationships inherent in cooperative information, which can significantly impact estimation accuracy. In response to this oversight, we propose a multi-source optimal selection. Traditional cooperative navigation algorithms often overlook the influence of geometric relationships inherent in cooperative information, which can significantly impact estimation accuracy. In response to this oversight, we propose a multi-source optimal selection strategy founded on the posterior error matrix. This strategy identifies cooperative nodes characterized by optimal performance evaluation coefficients to aid in rectifying the estimated positions of malfunctioning UAVs. Furthermore, to address challenges related to external environmental disturbances and the relative states within formations, we have developed a double-stage filtering that incorporates both local and relative state estimators. The local state filter employs a hybrid time difference of arrival (TDOA) and direction of arrival (DOA) to propose a two-step weighted least squares (TSWLS) method that considers errors in observation station location. Subsequently, the relative state filter integrates differential values derived from absolute positioning estimates with relative state information to attain high-accuracy relative navigation within formations. Under the simulation and experimental conditions established in this study, the proposed multi-source optimal selection strategy effectively filters cooperative information, demonstrating substantial estimation accuracy and robustness for cooperative localization (CL). By deriving the cramér-rao lower bound (CRLB) applicable to TDOADOA methods, we elucidate the impact of location errors on system positioning performance and highlight both the effectiveness and superiority of the TSWLS methodology. Ultimately, when compared to traditional centralized filtering approaches, the DF navigation technique significantly enhances relative state estimation and reduces error divergence, providing the significance and reference value of research in relative estimation for formation navigation.