Cooperative Navigation Framework for UAV Formations Using LSTM and Dynamic Model Fusion

Highlights

What are the main findings?

• The proposed cooperative navigation framework based on LSTM and dynamic model information fusion integrates airflow-angle prediction with the ${\mathrm{O}}_{\mathrm{INS}}$-$\Delta {\mathrm{P}}_{\mathrm{GNSS}}$ scheme to suppress inertial drift and enhance navigation robustness under GNSS-denied.
• A consistency-based node optimization strategy identifies high-precision UAV nodes via altitude and wind-speed consistency evaluations together with geometric constraints.

What are the implications of the main finding?

• This work demonstrates that the framework achieves the navigation objective of short-term stability and long-term drift suppression of the leader layer in GNSS-denied.
• The designed node selection strategies improve the cooperative navigation reliability and robustness, confirming the feasibility and effectiveness of the framework.

Abstract

In GNSS-denied environments, achieving accurate and reliable positioning for unmanned aerial vehicle (UAV) formations remains a major challenge. This paper presents a cooperative navigation framework for UAV formations based on LSTM and dynamic model information fusion to enhance formation navigation performance under GNSS-denial. The framework employs a dual-driven hierarchical architecture that integrates an LSTM-based dynamic state predictor with historical motion features, including velocity, acceleration, airflow angle, or thrust, thereby enhancing the robustness and positioning accuracy of the leader UAV layer. Furthermore, a multi-source optimal selection strategy based on consistency evaluation is developed to dynamically fuse pseudo-GNSS (P-GNSS), barometric altitude (BA), and wind-speed consistency information, optimizing node allocation between the leader and follower layers. In addition, an IMM-based resilient fusion filtering algorithm is introduced for the follower UAV layer, incorporating UWB, wind-speed, and external-force estimations to maintain reliable navigation under UWB outages and leader-node degradation. Experimental results demonstrate that the proposed framework significantly improves positioning accuracy and formation stability, exhibiting strong adaptability in complex GNSS-denied environments.

1. Introduction

Most of the currently employed UAVs are equipped with onboard inertial measurement units (IMUs) as well as position-aware sources such as Global Navigation Satellite System (GNSS) systems [1], motion capture (MoCap) systems [2], radio frequency (RF) signals [3], electromagnetic signals [4], and ultra-wideband (UWB) systems [5]. However, these sensors increase the overall system complexity, leading to higher costs; more importantly, their performance strongly depends on environmental conditions [6]. In particular, interference with and denial of GNSS signals pose a serious threat to flight safety, during large-scale formation flight operations [7].

Compared with external sensors, model information assistance does not require physical sensors, as it originates from the intrinsic model constraints of the platform and can be exploited to update navigation states when external sensors fail or are unavailable. Model-based assistance offers two main advantages over external sensor-based assistance: avoiding additional hardware-related costs and continuous availability. Such information is generally derived from kinematic or dynamic models. Specifically, model-assisted information transforms knowledge of platform motion into pseudo-measurements, which are then fed back into the navigation filter. Its key feature is that prior knowledge of the INS carrier platform or its operating environment can be directly converted into measurement information to support navigation filtering [8]. Typical approaches include zero-velocity updates [9], zero-heading updates [10], and relative position updates, which have been widely applied in mobile robots, pedestrian navigation, and UAVs. On the other hand, dynamic models, containing inherent state information, can be regarded as a unique, embedded virtual sensor suite. Model-assisted INS can be applied in the absence of external sensors, and in terms of aiding navigation, it is fundamentally analogous to actual sensors, exhibiting heterogeneous characteristics. For different UAV platforms, the corresponding dynamic models do not constrain exactly the same states within the navigation system; however, when integrated into the system’s filter fusion framework, their constraint effects are functionally equivalent to those of conventional sensors [11].

In addition to serving as an auxiliary information source, dynamic model information provides powerful physical predictions, primarily manifested in aerodynamic or hydrodynamic effects, which are typically not measurable by conventional sensors [12]. UAV dynamic models have been employed for various purposes, including control, navigation performance enhancement, replacement of other sensors, sensor fault diagnosis, and action selection through learning [13]. Consequently, dynamic models have also been utilized as substitutes for inertial measurement units (IMUs) in GNSS-integrated navigation, enabling attitude determination in the absence of an IMU [14]. In model-assisted approaches, precise INS dynamic models are employed, with Koifman and Bar-Itzhack [15] being notable pioneers in this field. In their work, a comprehensive dynamic model of a fixed-wing UAV was proposed as an alternative to GNSS, wherein errors in the model coefficients and parameters were estimated using an extended Kalman filter (EKF) during flight. In other words, this approach establishes a bidirectional correction mechanism between the inertial navigation system and the dynamic model [16].

A common feature in the current UAV state estimation literature is that dynamics-based estimators can adapt in real-time to rotational and translational drag parameters while providing smooth, lag-free inertial acceleration estimates [17]. Estimators capable of online model parameter estimation, without the need for expensive sensors or additional measurement sources, enable the large-scale deployment of high-performance, low-cost UAVs [18]. Moreover, most mainstream navigation approaches focus on the optimal fusion of onboard proprioceptive sensors and exteroceptive instruments to measure UAV motion and environmental signals.

In practice, a group of flying robots (FRs) exhibits greater robustness than a single flying robot and can accomplish more complex tasks [19]. For cooperative modes of data link systems, numerous studies have been conducted to achieve high-precision kinematic model-assisted navigation for multiple UAVs. Yang et al. proposed a multi-UAV cooperative navigation method based on relative distance and magnetic sensor measurements [20], in which UAV kinematic models were constructed using the outputs of magnetic sensors. For the minimal cooperative unit composed of a navigator UAV with known position and follower UAVs with unknown positions, Zhu et al. [21] proposed a CP algorithm based on the motion vectors of follower UAVs. Zhou et al. [22], within the random finite set (RFS) theoretical framework, presented a multi-agent navigation algorithm based on the probability hypothesis density (PHD) and cardinalized PHD (CPHD) filters. The proposed algorithm redefines virtual potential fields, enabling agents to avoid obstacles according to their relative velocities. Beyond UAV kinematic model assistance, substantial research has also explored model-assisted approaches for underwater vehicles (UVs) [23], marine vehicles (MVs) [24], and land vehicles [25].

However, applications of dynamic model-assisted navigation in the field of cooperative navigation remain limited, primarily because they rely heavily on the platform’s own dynamic information and cannot interact with other UAVs. Relative position updates, in contrast, require sensor redundancy on the UAVs [26]. Integrating additional vehicle dynamic constraints with a state estimation fusion framework can further enhance navigation performance [27], particularly improving robustness in complex environments and operating conditions. According to previous studies, dynamic model-assisted navigation systems can be categorized into two main approaches based on the type of UAV dynamics [28]: thrust-based [17] and aerodynamic-based [29]. While these methods are primarily designed for UAVs, the classification is also applicable to underwater vehicles and land vehicles.

In addition, most studies on dynamic model-assisted navigation focus solely on the data fusion level, unlike visual navigation, which emphasizes feature-level fusion. In fact, some dynamic models cannot be directly set as measurement information; their variation trends often better represent the current state of the UAV. Data-level fusion is regarded as the fundamental fusion strategy, directly fusing raw data from sensors, such as radar and camera measurements [30]. For navigation systems, data-level navigation primarily addresses issues related to information uncertainty [31], data heterogeneity [32], and data correlation [33]. While data-level fused navigation can effectively preserve raw measurements, offering high accuracy, reliability, and low noise, it suffers from large data volumes and poor real-time performance, and is limited to processing homogeneous data. Due to the relatively weak robustness of data-level navigation, appropriate fusion algorithms must be designed.

Therefore, feature-level fusion has been proposed as an approach to extract relevant data features, producing more refined features or higher-level outputs, such as decisions [34]. Feature extraction typically involves information such as position and velocity from radar data, and edge or texture features from images. By combining the advantages of heterogeneous sensors at the feature level, the accuracy and richness of feature extraction can be improved [35]. Additionally, integrating carrier dynamic features in navigation methods to accurately characterize the motion properties of the platform under specific environments, tasks, and scenarios represents an effective multi-source cooperative navigation approach. Yang et al. [36] proposed an adaptive Kalman filtering framework that determines the contributions of dynamic model information and observation model information in the PNT fusion results based on their deviations, thereby achieving multi-source fusion of dynamic features and navigation information [37]. At a finer scale, the sensors embedded within multi-source navigation systems and the dynamics of the system itself exhibit distinct information features during their evolution. Precisely characterizing these sensor and dynamic information features is beneficial for enhancing the perception accuracy of the navigation system. By extracting a more detailed and comprehensive set of features, both similar and heterogeneous information can be effectively processed. Although this approach is suitable for large-scale data scenarios, information loss is inevitably encountered.

Research has been conducted on the self-learning capabilities of carrier multi-source information, focusing on the reliability and completeness of navigation system decision-making [38]. Different scoring metrics are constructed based on the weights and importance of various tasks and are subsequently fused in a weighted manner, ultimately enabling intelligent, reliable decision-making and dynamic iterative optimization in multi-source autonomous navigation systems [39]. In the face of continuously changing scenarios and environments, unpredictable faults and disturbances, and dynamically evolving navigation tasks, deep learning leverages neural networks to perform regression and data fitting based on sensor predictions, overcoming the limitations of traditional methods and inconsistencies in data [40]. To this end, imitation learning can be employed to integrate cooperative navigation information features into a learning framework, whereby navigation databases are collected and neural networks are trained to generalize across other scenarios, thus achieving intelligent decision-making and dynamic iterative optimization for navigation behaviors in complex environments.

To explore the application of dynamic model information in cooperative UAV clusters, this paper proposes a cooperative navigation framework based on LSTM and multi-information fusion. In GNSS-denied scenarios, the historical motion states of UAV clusters, including acceleration and velocity, exhibit consistency. Due to the similar characteristics of their dynamic constraints, these historical dynamic data can be effectively exploited [41]. The specific contributions of this work are as follows:

• This paper proposes an LSTM-based complementary cooperative prediction framework integrating airflow-angle prediction with the ${\mathrm{O}}_{\mathrm{INS}}$-$\Delta {\mathrm{P}}_{\mathrm{GNSS}}$ scheme under GNSS-denied. The predicted airflow-angle correction facilitates mitigating drift accumulation and improving the stability of state estimation, while the concurrent use of P-GNSS observations imposes closed-loop constraints on inertial position errors. This dual mechanism effectively suppresses inertial drift, enhances overall navigation accuracy and robustness, and provides a basis for subsequent hierarchical fusion and node selection optimization.
• A consistency-based node optimization strategy is developed for hierarchical architecture and node selection. This strategy introduces a high-level consistency evaluation by constructing statistics of altitude differences of BA/P-GNSS measurement, enabling real-time detection of severe P-GNSS drifts and selecting out high-precision layers. Moreover, through joint wind-speed consistency evaluation and geometric optimization, high-precision nodes with superior prediction reliability are identified as leader nodes for follower-layer navigation.
• Building on the consistency-evaluated hierarchical layers, a modified geometric configuration-based IMM filtering algorithm is proposed to flexibly fuse multi-source node information, integrating leader-node multi-sensor data and dynamic model-assisted information such as UWB, wind speed $v_{\mathrm{wind}}$, or external force estimates $f_{\mathrm{ex}}^b$. This algorithm enhances system robustness under GNSS-denied conditions when facing various contingencies, including UWB outages and leader-node accuracy degradation, and significantly improves navigation accuracy at the follower node layer.

The remainder of this article is organized as follows. Section 2 presents the proposed framework. Section 3 describes the design and training of the LSTM network using historical dynamic features. Section 4 details the Leader’s filter design with AHRS integration based on the RIEKF. Section 5 introduces the hierarchical cooperative navigation subfilter. Section 6 presents experimental validation and performance evaluation. Finally, Section 7 concludes the article and outlines future research directions.

2. Proposed Framework

As shown in Figure 1, the core of this framework lies in utilizing the dynamic models embedded in historical data as auxiliary information sources and integrating them into the information fusion process to address challenges in GNSS-denied environments. The overall cooperative framework is primarily divided into leader-node and follower-node layers. Among them, the leader-layer nodes achieve higher prediction accuracy, while the follower-layer nodes rely on the leader-layer information as a reference for assisted navigation correction. The leader-node layer employs an AHRS-based RIEKF full-source navigation algorithm, fully integrating predicted dynamic information under GNSS-denied conditions. The follower-node layer leverages UWB measurements and wind field estimates from the selected leader nodes to perform filtering and fusion of the leader layer’s dynamic information. Unlike the sensor-based fusion methods, this approach emphasizes leveraging the prior knowledge contained in historical dynamic models. When a homogeneous UAV cluster passes through a given region in formation navigation, its dynamic states exhibit consistency. This implies that the dynamic behavior patterns of the UAVs are both predictable and reproducible.

Figure 1. Schematic diagram of cooperative navigation.

In addition to GNSS position increment prediction, this work employs an LSTM network to learn dynamic models from historical trajectories, leveraging dynamic information to reduce the difficulty of directly predicting coordinates. The original historical dynamic data are utilized either directly or indirectly in current navigation estimation, avoiding complex model identification or environmental modeling processes, and representing an efficient data reuse strategy. To fully exploit multi-source information features, this work distinguishes leader and follower node layers based on P-GNSS/BA consistency evaluations, while proposing an optimization strategy that combines wind speed consistency with geometric configuration characteristics for node selection within the follower layer. To address the elastic fusion of multiple leader-node information, a consistency-based cooperative factor is introduced at the follower-node layer, establishing a modified IMM-based multi-source elastic fusion filtering model.

3. LSTM-Aided Navigation Method

3.1. Aerodynamic Estimation Model

In low-altitude flight scenarios, the monitoring center can collect extensive historical trajectory data from UAVs, providing a reference for flight states. This study exploits deep learning of historical trajectory features from a fleet of fixed-wing UAVs to extract key information for the aerodynamic model. The aerodynamic forces along the longitudinal and lateral axes, as well as the aerodynamic moments, are computed as follows:

$$D={\displaystyle \frac{1}{2}}\rho V_a^{2}S{C}_D,L={\displaystyle \frac{1}{2}}\rho V_a^{2}S{C}_L,Y={\displaystyle \frac{1}{2}}\rho V_a^{2}S{C}_Y$$

(1)

where D denotes the aerodynamic drag of the aircraft, L denotes the aerodynamic lift, $\rho$ is the air density, $V_a$ is the resultant airspeed of the aircraft, S is the wing area, c is the mean aerodynamic chord, $C_L$ is the lift coefficient, and $C_D$ is the drag coefficient. Y represents the lateral aerodynamic force, b is the wingspan, and $C_y$ is the side-force coefficient.

According to (1), for the same type of aircraft, the aerodynamic states should theoretically satisfy geometric consistency, kinematic consistency, environmental consistency, and control input consistency. Although certain error factors may lead to slight discrepancies, the aerodynamic states are expected to remain consistent in theory. Since accurate aerodynamic parameters are typically obtained through wind tunnel experiments, which are often impractical for low-cost UAVs, simplified models with key parameter identification are commonly employed [42]. However, due to the inherent nonlinearity of aerodynamic dynamics, purely linear identification approaches may induce substantial output noise. Noting that lift variations during UAV flight are primarily affected by time-varying parameters such as the lift coefficient and air density, this work introduces an LSTM network, built on a recurrent neural network (RNN) architecture, to replace conventional linear identification models and thereby improve airflow angle prediction accuracy under strongly nonlinear conditions.

In conventional UAV geometric configurations, the longitudinal and lateral planes are often analyzed separately. The typical simplified forms of the vertical and lateral aerodynamic coefficient equations are expressed as follows:

$$\left.\begin{array}{c}{C}_L=C_{L0}+C_{L\alpha }\alpha +C_{Lq}{\displaystyle \frac{c}{2V_a}}q+C_{L{\delta }_e}{\delta }_e\\ C_D=C_{D0}+C_{D\alpha }\alpha +C_{Dq}{\displaystyle \frac{c}{2V_a}}q+C_{D{\delta }_e}{\delta }_e\\ C_Y=C_{Y0}+C_{Y\beta }\beta +C_{Yp}{\displaystyle \frac{b}{2V_a}}p\\ +C_{Yr}{\displaystyle \frac{b}{2V_a}}r+C_{Y{\delta }_a}{\delta }_a+C_{Y{\delta }_r}{\delta }_r\end{array}\right\}$$

(2)

where $C_{L_0}$ denotes the lift coefficient at zero AOA, while $C_{L_{\alpha }}$, $C_{L_q}$, and $C_{L_{{\delta }_e}}$ represent the lift coefficient contributions due to $\alpha$, pitch rate, and elevator deflection angle ${\delta }_e$, $\alpha$ is the angle of attack, $\beta$ is the sideslip angle, respectively. Based on Equation (2), by the accelerometer, the total lift L, side force Y, and drag D acting on the UAV can be observed through the following relations:

$$\hat{L}\approx -m{a}_z^a=\overline{q}S{C}_L,\hat{Y}\approx -m{a}_y^a=\overline{q}S{C}_Y,\hat{D}\approx -m{a}_x^a=\overline{q}S{C}_D$$

(3)

where,

$$a_x^a=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]{\mathit{R}}_b^a{\mathit{a}}^b=a_x^{b}cos\alpha cos\beta +a_y^{b}sin\beta +a_z^{b}sin\alpha cos\beta$$

$$a_z^a=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]{\mathit{R}}_b^a{\mathit{a}}^b=-a_x^{b}sin\alpha +a_z^{b}cos\alpha$$

$$a_y^a=\left[\begin{array}{ccc}0& 1& 0\end{array}\right]{\mathit{R}}_b^a{\mathit{a}}^b=-a_x^{b}cos\alpha sin\beta +a_y^{b}cos\beta -a_z^{b}sin\alpha sin\beta$$

where $a_{(.)}^a$ is the acceleration of the UAV body frame, ${\mathit{R}}_b^a$ is the rotation matrix from the UAV body frame to the airflow-angle frame. Furthermore, the equivalent lift, side-force, and drag coefficients, $\overline{C_L}$, $\overline{C_Y}$, and $\overline{C_D}$, can be determined, where $\overline{q}$ denotes the equivalent dynamic pressure, S is the equivalent wing area, and $V_{TAS}$ is the true airspeed.

$$\overline{C_L}={\displaystyle \frac{-m{a}_z^a}{\overline{q}S}},\overline{C_Y}={\displaystyle \frac{m{a}_y^a}{\overline{q}S}},\overline{C_D}={\displaystyle \frac{m{a}_x^a}{\overline{q}S}},$$

(4)

where m is the mass of UAV, $\overline{q}$ is the equivalent dynamic pressure, $\overline{q}={\displaystyle \frac{1}{2}}\rho V_{TAS}^2$, and $\rho$ is the atmospheric density, which is related to altitude H.

The equivalent coefficients depend not only on dynamic pressure, equivalent aerodynamic area, aerodynamic coefficients, and airspeed, but also on flight states such as AOA and SA. To this end, this work (5) leverages readily available information from historical states to establish a mapping relationship, reducing reliance on conventional UAV parameters and improving observation consistency.

$$\left(H,V_{TAS},q,{\delta }_e\right)\sim \alpha ,\left(H,V_{TAS},\delta r,p,r\right)\sim \beta$$

(5)

There exists a deep coupling between aerodynamic forces and sensor information. The variations in airflow angles are influenced not only by the current instantaneous sensor readings but also significantly depend on the historical states of the formation. However, low-dimensional simplified models struggle to fully represent these implicit temporal dependencies. Therefore, the long short-term memory (LSTM) network is adopted due to its strong capability in modeling time-series data and capturing long-term temporal dependencies. Through its gated architecture, including the forget, input, and output gates, LSTM can selectively preserve and update historical flight-state information, enabling the effective extraction of key temporal features from flight-state sequences. As a result, accurate prediction of the airflow angles can be achieved under complex and time-varying flight conditions.

3.2. Airflow-Angle and Position Prediction Model

For single-UAV navigation, the ${\mathrm{O}}_{\mathrm{INS}}$-$\Delta {\mathrm{P}}_{\mathrm{GNSS}}$ approach [43] is trained on the error patterns of a specific IMU. Replacing the IMU may invalidate these patterns, leading to distorted outputs. In navigation systems, even IMUs of the same model, batch, or operating conditions exhibit discrepancies in biases, scale factors, noise densities, and temperature drifts. Consequently, the PGNSS estimates may be accurate in the short term but diverge over longer durations. Considering the formation consistency, the velocity geometry between the leader and follower UAVs can be assumed approximately equivalent. The cooperative navigation correction based on the predicted airflow angles effectively compensates for the deficiencies inherent in conventional ${\mathrm{O}}_{\mathrm{INS}}$-$\Delta {\mathrm{P}}_{\mathrm{GNSS}}$.

Therefore, in this work, airflow-angle prediction correction and the ${\mathrm{O}}_{\mathrm{INS}}$-$\Delta {\mathrm{P}}_{\mathrm{GNSS}}$ integrated navigation operate in a complementary manner, serving as upstream calibration and downstream closed-loop auxiliary navigation, respectively. In tightly coupled configurations, the ${\mathrm{O}}_{\mathrm{INS}}$-$\Delta {\mathrm{P}}_{\mathrm{GNSS}}$ method directly constrains position and velocity errors using pseudorange and carrier-phase measurements, thereby providing external closed-loop correction for inertial drift and a reliable environment for evaluating wind-speed consistency during early stages. The airflow angle correction addresses aerodynamic measurement errors, including biases and scale errors in $\alpha$ and $\beta$, directly improving the airspeed–attitude–trajectory coupling chain and enhancing the input accuracy for inertial navigation mechanization.

When GNSS signals are available, the SINS/GNSS performs standard integrated navigation, while the GNSS-derived position increments $\Delta {\mathrm{P}}_{\mathrm{GNSS}}$ and formation-based dynamic information ${\mathit{\chi }}_{\mathrm{Dynamic}}$ serve as training targets for the neural network. Raw IMU data and additional sensor measurements $z_{\mathrm{sensors}}$ are used as inputs to the LSTM model. During GNSS outages, the system switches to prediction mode, using the trained model to estimate GNSS position increments and dynamic states. Integrating the predicted increments yields pseudo-GNSS position references to replace unavailable measurements. Meanwhile, the predicted dynamics enable aerodynamic force estimation, ensuring continuous and stable navigation performance under GNSS-denied conditions.

The system adopts a dual-mode operational strategy, consisting of a training mode and a prediction mode. These two modes work collaboratively to significantly enhance navigation accuracy in complex environments, particularly under GNSS-limited conditions. As shown in Figure 2, the LSTM neural network receives the following data for joint training and prediction:

Figure 2. P-GNSS and Airflow angle estimation diagram.

• Input: Specific forces $\mathit{f}$ and angular rates $\mathit{\omega }$ from inertial sensors, together with measurements and states from auxiliary sensors (e.g., airspeed, servos, ESCs);
• Output: GNSS position increments $\Delta {\mathrm{P}}_{\mathrm{GNSS}}$ and aerodynamic states, including angle of attack $\alpha$, sideslip angle $\beta$, and external forces ${\mathit{F}}_{ex}^{b_k}$.

Accordingly, the LSTM network was designed to capture the nonlinear inertial–aerodynamic coupling of flight mechanics, consisting of an input layer, two stacked LSTM layers (200 units each), and an output layer. Standard gated units were employed to mitigate gradient vanishing, while no activation was applied to the output layer to allow direct regression of airflow angles and GNSS position increments $\Delta {\mathrm{P}}_{\mathrm{GNSS}}$. During the training process, flight data from 20 groups of 10 UAVs were used, with 5 aircraft per group forming the historical trajectory set and the remaining 5 serving as the test set. 20 sets of dual-UAV flight data were utilized. The loss function was defined as Mean Squared Error (MSE), and the optimizer was Adam with weight decay, with an initial learning rate of $5\times {10}^{-3}$ and a decay coefficient of $1\times {10}^{-5}$. Since flight experiment data from the leader UAV are used as input, and the RIEKF algorithm is employed to fuse GNSS signals with airspeed measurements, generating $\alpha$ and $\beta$ as the target values for training. The training process is iterated 100 times to ensure convergence.

4. RIEKF-Based Leader Layer’s Positioning with AHRS

4.1. System State

When exploring the auxiliary correction of the dynamic model, it is inseparable from the prediction and estimation of the airflow angle. Due to the estimation iteration of the attitude not meeting the traditional additive rules, it is reflected in the update of the state quantity and covariance. Therefore, it is more appropriate to establish $\alpha$ and $\beta$ on the manifold ${\mathit{R}}_a^b\in \mathbb{SO}\left(3\right)$. The process of updating and compensation is more suitable for the use of mature Lie group theory to improve the accuracy and consistency of estimation [29].

By integrating the attitude estimates provided by the AHRS (Attitude and Heading Reference System) based on the magnetometer, IMU accelerometer, and gyroscope with the predictions from LSTM-assisted navigation, a collaborative navigation RIEKF main filter centered on the AHRS is established. The 15-dimensional full-system nominal state variables employed in the design are defined as follows:

$${\mathit{\chi }}_{IMU}=\left({\mathit{R}}_b^n,{\mathit{v}}^n,{\mathit{p}}^n,{\mathit{b}}_g^n,{\mathit{b}}_a^n,{\mathit{R}}_a^b,{\mathit{v}}_{wind}^n\right)$$

(6)

where $\left({\mathit{R}}_b^n,{\mathbf{v}}^n,{\mathbf{p}}^n\right)\in {\mathbb{SE}}_2\left(3\right)$ indicates the pose and velocity of the vehicle. $\left({\mathbf{b}}_g^n,{\mathbf{b}}_a^n\right)\in {\left({\mathbb{R}}^3\right)}^2$ represents the tri-axial bias error for the IMU’s gyroscope and accelerometer. ${\mathit{R}}_a^b\in \mathbb{SO}\left(3\right)$ is the attitude rotation matrix of the air flow coordinate system relative to the body frame, and ${\mathbf{v}}_{\mathrm{wind}}^n\in {\mathbb{R}}^3$ indicates the wind speed magnitude.

The sub-model and prediction model adopt the advanced manifold-based Error-State Right-Invariant Extended Kalman Filter algorithm, which is designed to enhance convergence and estimation accuracy in the presence of disturbances. The system state is represented in the error-state form, and the right- (or left-) invariant error is defined as follows:

$$\begin{array}{c}\hfill \delta \mathit{\chi }=(\delta {\chi }_I,\delta {\mathit{\chi }}_b,\delta {\chi }_A,\delta {\mathbf{v}}_{wind}^n)={\left[\begin{array}{cccccccccc}\delta {\mathit{\theta }}_I^{n\top }& \delta {\mathbf{v}}^{n\top }& \delta {\mathbf{p}}^{n\top }& |& \delta {\mathit{b}}_g^{b\top }& \delta {\mathit{b}}_a^{b\top }& |& \delta {\mathit{\theta }}_a^{b\top }& |& \delta {\mathbf{v}}_{wind}^{n\top }\end{array}\right]}^{\top }\end{array}$$

(7)

where $\delta {\mathit{\theta }}_I^n$, $\delta {\mathit{v}}^n$, and $\delta {\mathit{p}}^n$ represent the invariant errors of attitude, velocity, and position, respectively, while $\delta {\mathit{b}}_g^b$ and $\delta {\mathit{b}}_a^b$ denote the bias errors of the gyroscope and accelerometer in the body frame $\left\{b\right\}$, respectively.

Therefore, the error formulation of RIEKF satisfies the following equation:

$$\begin{array}{cc}\hfill \mathit{\chi }& =\widehat{\mathit{\chi }}⊞\delta \mathbf{x}=\left(exp\left(\delta {\mathit{\chi }}_I\right){\widehat{\mathbf{x}}}_{I_R},{\widehat{\mathbf{x}}}_b+\delta {\mathbf{x}}_b,exp\left(\delta {\mathit{\chi }}_A\right){\widehat{\mathbf{x}}}_A,{\widehat{\mathbf{x}}}_{vw}+\delta {\mathbf{x}}_{vw}\right)\hfill \\ \hfill & =\left[\begin{array}{c}exp\left(\delta {\mathit{\theta }}_I\right){\mathit{R}}_b^n\\ exp\left(\delta {\mathit{\theta }}_I\right){\widehat{\mathbf{p}}}^n+{\mathit{J}}_l\left(\delta {\mathit{\theta }}_I\right)\delta \mathbf{p}\\ exp\left(\delta {\mathit{\theta }}_I\right){\widehat{\mathbf{v}}}^n+{\mathit{J}}_l\left(\delta {\mathit{\theta }}_I\right)\delta \mathbf{v}\\ {\widehat{\mathbf{b}}}_g+\delta {\mathbf{b}}_g\\ {\widehat{\mathbf{b}}}_a+\delta {\mathbf{b}}_a\\ exp\left(\delta {\mathit{\theta }}_a\right){\mathbf{R}}_a^b\\ {\widehat{\mathbf{v}}}_w^n+\delta {\mathbf{v}}_w^n\end{array}\right]\hfill \end{array}$$

(8)

where

$${\mathit{\chi }}_{I_R}=\left[\begin{array}{cc}{\mathit{R}}_b^n& {\mathit{v}}^n{\mathit{p}}^n\\ {\mathbf{0}}_{2,3}& {\mathit{I}}_2\end{array}\right]\in {\mathbb{SE}}_2\left(3\right)$$

(9)

where ${\mathit{\chi }}_{I_R}$ denotes the right-invariant state of the system.

The continuous-time state propagation model of the system can be expressed by the following equation:

$$\begin{gathered} {\dot{\mathit{R}}}_b^n={\mathit{R}}_b^n{\left[\dot{\widehat{\mathit{\theta }}}\right]}_{\times },{\dot{\mathit{v}}}^n={\mathit{R}}_b^n{\mathit{a}}^b+{\mathit{g}}^n,{\dot{\mathit{p}}}^n={\mathit{v}}^n \\ {\dot{\mathit{b}}}_g={\mathit{n}}_{b_g},{\dot{\mathit{b}}}_a={\mathit{n}}_{b_a},{\dot{\mathit{R}}}_a^b={\mathit{n}}_{b_a} \end{gathered}$$

(10)

where ${\mathit{g}}^n={\left[\begin{array}{ccc}0& 0& -9.79\end{array}\right]}^{\top }$ represents the gravitational acceleration projected in the n-frame, and ${[·]}_{\times }$ denotes the skew-symmetric matrix operator.

Although the AHRS provides high-accuracy attitude measurements, slow drift errors still exist. To balance estimation accuracy and computational efficiency, the attitude angle errors from the AHRS are modeled as a first-order Markov process:

$$\dot{\mathit{\theta }}=-1/\tau \delta \mathit{\theta }+n_{\mathit{\theta }}$$

(11)

where $\tau$ denotes the correlation time of the first-order Markov process for the attitude angle errors, and $n_{\mathit{\theta }}$ represents the Gaussian white noise associated with the AHRS attitude errors.

The continuous-time right-invariant dynamics model of the system is given by:

$$\delta {\dot{\mathit{\chi }}}_R={\mathit{F}}_t^R\delta {\mathit{\chi }}_R+{\mathit{G}}_t^R\mathit{n}$$

(12)

where ${\mathit{F}}_t^R$ represents the right-invariant state transition matrix and ${\mathit{G}}_t^R$ denotes the right-invariant noise excitation matrix.

Multiplying both sides of (9) by ${\widehat{\mathit{\chi }}}_{I_R}^{\top }$ results in the invariant state error component ${\mathit{\chi }}_{I_R}$, consistent with the group affine property. This can be expressed as follows, where the bias states that don’t satisfy the group affine property are represented additively:

$$\begin{array}{c}\hfill \delta {\mathit{\chi }}_{I_R}={\mathit{\chi }}_{I_R}{\widehat{\mathit{\chi }}}_{I_R}^{\top }=\left[\begin{array}{cc}{\mathit{R}}_b^n{\widehat{\mathit{R}}}_b^{n\top }& {\mathit{p}}^n-{\mathit{R}}_b^n{\widehat{\mathit{R}}}_b^{n\top }{\widehat{\mathit{p}}}^n{\mathit{v}}^n-{\mathit{R}}_b^n{\widehat{\mathit{R}}}_b^{n\top }{\widehat{\mathit{v}}}^n\\ {\mathbf{0}}_{2\times 3}& {\mathit{I}}_2\end{array}\right]\end{array}$$

(13)

The system dynamics error model is derived based on the right-invariant error definition (Equation (13)) and the system state update equation (Equation (10)). In previous work [44], detailed derivations of the dynamics and measurement equations for IEKF are presented. The continuous-time transition matrix of the system, ${\mathit{F}}_t$, can be expressed as:

$${\mathbf{F}}_t^R={\left[\begin{array}{ccc}{\mathbf{F}}_{davp}& {\mathbf{F}}_{davp2b}& {\mathbf{0}}_{9\times 6}\\ {\mathbf{0}}_{9\times 9}& {\mathbf{I}}_{9\times 9}& {\mathbf{0}}_{9\times 6}\\ {\mathbf{0}}_{6\times 9}& {\mathbf{0}}_{6\times 9}& {\mathbf{F}}_{\alpha \mathbf{v}}\end{array}\right]}_{24\times 24}{\mathbf{G}}_t^R={\left[\begin{array}{ccc}{\mathbf{G}}_{davp}& {\mathbf{0}}_{9\times 9}& {\mathbf{0}}_{6\times 6}\\ {\mathbf{0}}_{9\times 6}& {\mathbf{I}}_{9\times 9}& {\mathbf{0}}_{9\times 6}\\ {\mathbf{0}}_{6\times 6}& {\mathbf{0}}_{6\times 9}& {\mathbf{G}}_{\alpha \mathbf{v}}\end{array}\right]}_{24\times 21}$$

(14)

where the error state transition matrix ${\mathbf{F}}_{davp}$ is associated with the attitude, velocity, and position, and represents the transition matrix of the $\mathit{avp}$ error states, where $\mathbf{0}$ denotes the zero matrix and $\mathit{I}$ denotes the identity matrix. Its expression is given as follows:

$${\mathbf{F}}_{davp}={\left[\begin{array}{ccc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\left[{\mathbf{g}}^n\right]}_{\times }& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ 1/2{\left[{\mathbf{g}}^n\right]}_{\times }& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]}_{9\times 9}{\mathbf{G}}_{avp}={\left[\begin{array}{cc}-{\widehat{\mathbf{R}}}_b^n& {\mathbf{0}}_{3\times 3}\\ -{\left[{\mathbf{v}}^n\right]}_{\times }{\widehat{\mathbf{R}}}_b^n& -{\widehat{\mathbf{R}}}_b^n\\ -{\left[{\mathbf{p}}^n\right]}_{\times }{\widehat{\mathbf{R}}}_b^n& -{\widehat{\mathbf{R}}}_b^n\end{array}\right]}_{9\times 6}$$

(15)

The matrix ${\mathbf{F}}_{davp2b}$ representing the relationship between the attitude velocity position (avp) error state and the bias error states $\delta {\mathbf{b}}_g,\delta {\mathbf{b}}_a$ is shown in (16).

$${\mathbf{F}}_{davp2b}={\left[\begin{array}{cc}-{\widehat{\mathbf{R}}}_b^n& {\mathbf{0}}_{3\times 3}\\ -{\left[{\mathbf{v}}^n\right]}_{\times }{\widehat{\mathbf{R}}}_b^n& -{\widehat{\mathbf{R}}}_b^n\\ -{\left[{\mathbf{p}}^n\right]}_{\times }{\widehat{\mathbf{R}}}_b^n& -{\widehat{\mathbf{R}}}_b^n\end{array}\right]}_{9\times 6}$$

(16)

The following mainly establishes the error dynamics equation of the airframe-body rotation matrix. However, since the iterative estimation of attitude does not meet the traditional additive rules, which is reflected in the update of the state variables and covariance, it is preferable to establish $\alpha$ and $\beta$ on the manifold ${\mathbf{R}}_a^b\in \mathbb{SO}\left(3\right)$. The process of updating and compensating them is more suitable for using mature Lie group-related theories to improve the accuracy and consistency of estimating $\alpha$ and $\beta$. The error state update matrix for ${\mathbf{R}}_a^b$ and ${\mathbf{v}}_{wind}^n$, denoted as ${\mathbf{F}}_{\alpha \mathbf{v}}$, is expressed as follows:

$${\mathbf{F}}_{\alpha \mathbf{v}}=\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\right]{\mathbf{G}}_{\alpha \mathbf{v}}=\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\right]$$

(17)

4.2. Measurement Submodel

4.2.1. Relative Measurement Update

When the unmanned aerial vehicle (UAV) has access to GNSS signals, the correction of positioning errors can be achieved solely through the INS/GNSS sub-filter. However, in GNSS-denied environments, the UAV relies on communication links established with surrounding nodes to obtain relative ranging information and employs cooperative navigation algorithms to determine its own position. The relative distance and angle between the follower UAV and the leader node can be expressed as:

$$\left\{\begin{array}{c}{\mathit{f}}_d=\sqrt{{\left(x_m-x_s\right)}^2+{\left(y_m-y_s\right)}^2+{\left(z_m-z_s\right)}^2}\hfill \\ {\mathit{f}}_{\theta }=arcsin\left({\displaystyle \frac{z_m-z_s}{d}}\right)\hfill \\ {\mathit{f}}_{\phi }=arctan\left({\displaystyle \frac{y_m-y_s}{x_m-x_s}}\right)\hfill \end{array}\right.$$

(18)

The error equation of the relative measurements is described as follows:

$$\begin{array}{c}{\widehat{\mathit{r}}}_r^n={\mathit{r}}_r^n+{\mathit{n}}_r,{\mathit{n}}_r\sim \mathcal{N}\left(0,{\sigma }_r^2\right)\end{array}$$

(19)

The linearization process can be obtained by

$$\begin{array}{c}{\widehat{\mathit{Y}}}_r^n={\mathit{H}}_r^n{\mathit{p}}^n+{\mathit{n}}_r,{\mathit{n}}_r\sim \mathcal{N}\left(0,{\sigma }_r^2\right)\end{array}$$

(20)

where

$${\mathit{H}}_r^{k+1}={\left.{\displaystyle \frac{\partial h}{\partial {\mathit{p}}^n}}\right|}_{{\widehat{p}}_{k,k+1}}=\left[\begin{array}{ccc}{\displaystyle \frac{\partial f_d}{\partial x}}\hfill & {\displaystyle \frac{\partial f_d}{\partial y}}\hfill & {\displaystyle \frac{\partial f_d}{\partial z}}\hfill \\ {\displaystyle \frac{\partial f_{\theta }}{\partial x}}\hfill & {\displaystyle \frac{\partial f_{\theta }}{\partial y}}\hfill & {\displaystyle \frac{\partial f_{\theta }}{\partial z}}\hfill \\ {\displaystyle \frac{\partial f_{\phi }}{\partial x}}\hfill & {\displaystyle \frac{\partial f_{\phi }}{\partial y}}\hfill & {\displaystyle \frac{\partial f_{\phi }}{\partial z}}\hfill \end{array}\right]$$

(21)

By employing the left-invariant form, it can be directly expressed as:

$$\begin{gathered} {\mathit{z}}_p=\mathit{\chi }b+w_t \\ \left[\begin{array}{c}{\widehat{\mathit{p}}}^n\\ {\mathit{I}}_2\end{array}\right]=\left[\begin{array}{cc}{\mathit{R}}_b^n& {\mathit{v}}^n{\mathit{p}}^n\\ {\mathbf{0}}_{2,3}& {\mathit{I}}_2\end{array}\right]\left[\begin{array}{c}{\mathbf{0}}_{3\times 3}\\ 0\\ {\mathit{H}}_{\mathit{r}}\end{array}\right]+w_t \end{gathered}$$

(22)

Considering that the state variable is a group element, the measurement innovation ${\mathbf{\gamma }}_p^L$ can be defined as follows:

$${\mathbf{\gamma }}_p^L={\mathit{\chi }}^{-1}{\mathit{z}}_p^L-b$$

(23)

According to the derivation in [29], the GNSS measurement innovation can be further transformed into the following linear equation:

$$\begin{array}{cc}\hfill {\mathbf{\gamma }}^L& \approx {\mathit{\xi }}^{L\wedge }b+{\widehat{\mathit{\chi }}}^{-1}w={\mathit{H}}_r^L{\mathit{\xi }}^L+{\widehat{\mathit{\chi }}}^{-1}w\hfill \\ \hfill {\widehat{\mathit{R}}}_n^b\left[\begin{array}{c}\left({\widehat{r}}^n-r^n\right)\end{array}\right]& \approx \left[{\mathbf{0}}_{3\times 3}\hspace{1em}{\mathbf{0}}_{3\times 3}\hspace{1em}{\mathit{H}}_r | {\mathbf{0}}_{3\times 12}\right]\delta {\mathit{\chi }}^L+{\widehat{\mathit{R}}}_n^b{\mathit{n}}_r\hfill \end{array}$$

(24)

where ${\mathit{H}}_r^L$ is trajectory-independent. Since the right-invariant error representation is adopted, ${\mathit{H}}_r^L$ needs to be transformed into ${\mathit{H}}_r^R$ through the adjoint matrix. According to the derivation of the invariant Kalman filter, one has ${\mathit{\xi }}^R={Ad}_{\mathit{\chi }}{\mathit{\xi }}^L$. Substituting this relation into (24) yields:

Therefore, the following transformation holds:

$${\mathit{H}}_r^R={\mathit{H}}_r^L{Ad}_{\widehat{\mathit{\chi }}}^{-1}$$

(25)

where

$${Ad}_{\widehat{\mathit{\chi }}}^{-1}=\left[\begin{array}{cccc}{{\mathit{R}}_b^n}^{\top }& {\mathbf{0}}_{3\times 3}& \dots & {\mathbf{0}}_{3\times 3}\\ -{{\mathit{R}}_b^n}^{\top }{\left[{\mathit{v}}^n\right]}_{\times }& {{\mathit{R}}_b^n}^{\top }& \dots & {\mathbf{0}}_{3\times 3}\\ ⋮& ⋮& \ddots & ⋮\\ -{{\mathit{R}}_b^n}^{\top }{\left[{\mathit{p}}^n\right]}_{\times }& {\mathbf{0}}_{3\times 3}& \dots & {{\mathit{R}}_b^n}^{\top }\end{array}\right]$$

(26)

By substituting into (25), we obtain:

$${\mathit{H}}_r^R={{\mathit{R}}_b^n}^{\top }{\mathit{H}}_r\left[\begin{array}{cccc}-{\left[{\mathit{p}}^n\right]}_{\times }& {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]$$

(27)

Based on (24) and (27), the right-invariant measurement error dynamics for UWB/LiDAR can be expressed as:

$$\begin{array}{cc}\hfill {\mathit{\gamma }}^L& ={\mathit{H}}_r^{R^{\prime }}{\mathit{\xi }}^R+{\widehat{\mathit{\chi }}}^{-1}w\hfill \\ \hfill \left[\begin{array}{c}{\widehat{\mathit{r}}}^n-{\mathit{r}}^n\end{array}\right]& ={\mathit{H}}_r[-{[{\mathit{p}}^n]}_{\times }\hspace{1em}{\mathbf{0}}_{3\times 3}\hspace{1em}{\mathit{I}}_{3\times 3}\hspace{1em}|\hspace{1em}{\mathbf{0}}_{3\times 12}\hspace{1em}]{\mathit{\xi }}^R+{\widehat{\mathit{\chi }}}^{-1}w\hfill \end{array}$$

(28)

4.2.2. Barometer Update

Without loss of generality, the error model of the barometer after temperature compensation is established as Gaussian white noise, as shown in (29).

$${\widehat{\mathit{H}}}_b^n={\mathit{H}}_b^n+{\mathit{n}}_b,{\mathit{n}}_b\sim \mathcal{N}\left(0,{\sigma }_{H_b}^2\right)$$

(29)

where ${\mathit{H}}_b$ represents the absolute altitude.

The barometer measures the altitude information in the $\left\{n\right\}$ frame. Since the barometer and GNSS share the same coordinate system, the measurement is one-dimensional, as expressed by the following equation:

$$\begin{array}{cc}\hfill {\mathit{\gamma }}^L& ={\mathit{H}}_{H_b}^{R^{\prime }}{\mathit{\xi }}^R+{\widehat{\mathit{\chi }}}^{-1}w\hfill \\ \hfill {\widehat{H}}_b^n-H_b^n& ={\mathit{A}}_b\left[\begin{array}{cccc}-{\left[{\mathit{p}}^n\right]}_{\times }& {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 12}\end{array}\right]{\mathit{\xi }}^R+{\mathit{A}}_b{\widehat{\mathit{\chi }}}^{-1}w\hfill \end{array}$$

(30)

where ${\mathit{A}}_b=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]$ is the dimensionality reduction matrix.

4.2.3. Airspeed Tube

The airspeed sensor measures the magnitude of airspeed in a-frame, and its error model is established as a Gaussian white noise model.

$${\widehat{v}}_{TAS}^a=v_{TAS}^a+{\mathit{n}}_{TAS},{\mathit{n}}_{TAS}^a\sim \mathcal{N}\left(0,{\sigma }_{TAS}^2\right)$$

(31)

where v denotes the error of the airspeed sensor measurement, and $\mathcal{N}(0,{\sigma }_{TAS}^2)$ represents zero-mean Gaussian white noise with variance ${\sigma }_{TAS}^2$.

The relationship between true airspeed ${\mathbf{v}}_{TAS}^a$ and ground speed ${\mathbf{v}}^n$ is shown in (32).

$${\mathbf{v}}_{TAS}^a={\mathbf{R}}_a^{b\top }{\mathbf{R}}_b^{n\top }\left({\mathbf{v}}^n-{\mathbf{v}}_{wind}^n\right)$$

(32)

Therefore, ${\mathbf{v}}^n$ can be solved for using the altitude, ${\mathbf{v}}^n$ and ${\mathbf{v}}_{wind}^n$.

$${\mathbf{v}}^n={\mathbf{R}}_n^b{\mathbf{R}}_b^a{\mathbf{v}}_{TAS}^a+{\mathbf{v}}_{wind}^n$$

(33)

In this study, a dynamics-aided navigation approach based on the leader UAV is adopted, incorporating the predicted ${\mathbf{R}}_b^a$ and the estimated wind velocity ${\mathbf{v}}_{\mathrm{wind}}^n$. Because the ${\mathbf{v}}^n$ derived from airspeed is independent of both ${\mathbf{R}}_b^a$ and ${\mathbf{v}}_{\mathrm{wind}}^n$, the Jacobian for (33) need not be computed explicitly.

Based on (24), similar results can be obtained. Then, the airspeed update equation can be written as follows:

$$\left[\begin{array}{c}{\widehat{v}}^n-v^n\end{array}\right]={\mathbf{H}}_{v_A}^R{\mathit{\xi }}^R+{\widehat{\mathit{\chi }}}^{-1}w,{\mathbf{H}}_{v_A}^R=\left[\begin{array}{ccc}-{\left[{\mathbf{v}}^n\right]}_{\times }& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 15}\end{array}\right]$$

(34)

The matrix ${\mathbf{H}}_{v_A}^R$ is the observation matrix derived from the airspeed update equation.

5. Improved IMM-RIEKF-Based Positioning Method for the Follower Layer

5.1. Dynamic Feature and Consistency Assessment

5.1.1. Maximum Likelihood Estimation Theory

Due to the limited temporal span and numerical range of historical dynamic data, the prediction phase is prone to error accumulation. To improve prediction reliability, this work introduces a dynamic information consistency evaluation method based on maximum likelihood estimation (MLE), which assesses the consistency between two types of observations and quantifies the reliability of model predictions. MLE identifies the model parameters most likely to have generated the observed data. First, barometric altitude (BA) and predicted P-GNSS data are evaluated using MLE to assess their consistency weights $h{W}_k^{\left(i\right)}$ with predicted values, filtering out the low-accuracy layer. Next, the consistency of wind speed estimates $v_{wind}$ within the high-accuracy layer is assessed to provide weights $v{W}_k^{\left(i\right)}$ for the elastic fusion of multi-source information in the low-accuracy layer.

The core assumption of this framework is that if two information sources represent the same physical quantity and their error distributions are known and reasonable, the resulting maximum likelihood solution should be statistically consistent and pass hypothesis testing. The proposed method relies on three key assumptions: (1) the two observations are mutually independent; (2) each observation’s error relative to the true value follows a zero-mean Gaussian distribution; and (3) both observations correspond to the same unknown true value $y_{\mathrm{true}}$. The following illustrates the maximum likelihood estimation process using barometric altitude (BA) as an example.

$$\left\{\begin{array}{cc}{y}_{\mathrm{LSTM}}=y_{\mathrm{true}}+{\epsilon }_{\mathrm{LSTM}},\hfill & {\epsilon }_{\mathrm{LSTM}}\sim \mathcal{N}(0,{\sigma }_{\mathrm{LSTM}}^2)\hfill \\ y_{\mathrm{BA}}=y_{\mathrm{true}}+{\epsilon }_{\mathrm{BA}},\hfill & {\epsilon }_{\mathrm{BA}}\sim \mathcal{N}(0,{\sigma }_{\mathrm{BA}}^2)\hfill \end{array}\right.$$

(35)

where $\mathcal{N}\left(y_{\mathrm{true}},{\sigma }_{\mathrm{BA}}^2\right)$ and $\mathcal{N}\left(y_{\mathrm{true}},{\sigma }_{y_{\mathrm{LSTM}}}^2\right)$ indicate that the barometric altitude measurement $y_{\mathrm{BA}}$ and the prediction based on historical data $y_{\mathrm{LSTM}}$ follow normal distributions with the true altitude $y_{\mathrm{true}}$ as the mean and variances ${\sigma }_{\mathrm{BA}}^2$ and ${\sigma }_{y_{\mathrm{LSTM}}}^2$, respectively. To jointly fuse the two types of altitude information, $y_{\mathrm{LSTM}}$ and $y_{\mathrm{BA}}$, a joint likelihood function parameterized by the true altitude $y_{\mathrm{true}}$ can be constructed:

$$\begin{array}{c}\hfill L(y_{\mathrm{true}}\mid y_{\mathrm{LSTM}},y_{\mathrm{BA}},{\sigma }_{\mathrm{LSTM}}^2,{\sigma }_{\mathrm{BA}}^2)=p(y_{\mathrm{LSTM}}\mid y_{\mathrm{true}},{\sigma }_{\mathrm{LSTM}}^2)·p(y_{\mathrm{BA}}\mid y_{\mathrm{true}},{\sigma }_{\mathrm{BA}}^2)\end{array}$$

(36)

where the probability density functions of individual observations are given by:

$$p(y_{\mathrm{BA}}\mid y_{\mathrm{true}},{\sigma }_{\mathrm{BA}}^2)={\displaystyle \frac{1}{\sqrt{2\pi }{\sigma }_{\mathrm{BA}}}}exp\left(-{\displaystyle \frac{{(y_{\mathrm{BA}}-y_{\mathrm{true}})}^2}{2{\sigma }_{\mathrm{BA}}^2}}\right)$$

(37)

According to (37), the joint likelihood function can be rewritten as:

$$\begin{array}{c}\hfill L(y_{\mathrm{true}}\mid y_{\mathrm{LSTM}},y_{\mathrm{BA}})={\displaystyle \frac{1}{\sqrt{2\pi {\sigma }_{\mathrm{LSTM}}^2}}}exp\left(-{\displaystyle \frac{{(y_{\mathrm{LSTM}}-y_{\mathrm{true}})}^2}{2{\sigma }_{\mathrm{LSTM}}^2}}\right)\times {\displaystyle \frac{1}{\sqrt{2\pi {\sigma }_{\mathrm{BA}}^2}}}exp\left(-{\displaystyle \frac{{(y_{\mathrm{BA}}-y_{\mathrm{true}})}^2}{2{\sigma }_{\mathrm{BA}}^2}}\right)\end{array}$$

(38)

Solving the above equation gives the maximum likelihood estimate (MLE):

$${\widehat{y}}_{\mathrm{ml}}={\displaystyle \frac{w_{\mathrm{LSTM}}{y}_{\mathrm{LSTM}}+w_{\mathrm{BA}}{y}_{\mathrm{BA}}}{w_{\mathrm{LSTM}}+w_{\mathrm{BA}}}}$$

(39)

where $w_{\mathrm{LSTM}}={\displaystyle \frac{1}{{\sigma }_{\mathrm{LSTM}}^2}},w_{\mathrm{BA}}={\displaystyle \frac{1}{{\sigma }_{\mathrm{BA}}^2}}$.

5.1.2. Consistency Assessment

For two uncorrelated observations, the inverse-variance weighted residual represents the overall deviation after fusing the two measurements, defined as

$$\delta y_w={\displaystyle \frac{\delta y_{\mathrm{LSTM}}{w}_{\mathrm{LSTM}}+\delta y_{\mathrm{BA}}{w}_{\mathrm{BA}}}{w_{\mathrm{LSTM}}+w_{\mathrm{BA}}}}$$

(40)

where ${\sigma }_{\delta y}^2$ denotes the overall uncertainty of the height difference between the LSTM estimate and the barometer (BA), reflecting the combined noise effect of the two measurement sources, and is given by

$${\sigma }_{\delta y}^2={\left({\displaystyle \frac{1}{{\sigma }_{\mathrm{LSTM}}^2}}+{\displaystyle \frac{1}{{\sigma }_{\mathrm{BA}}^2}}\right)}^{-1}.$$

To evaluate the consistency level between the two altitude observations, a normalized height-difference statistic $\delta y_w^{\mathrm{norm}}$ is introduced after the weighted residual (40) computing, defined as:

$$\delta y_w^{\mathrm{norm}}={\displaystyle \frac{\delta y_w}{\sqrt{{\sigma }_{\delta y}^2}}}$$

(41)

This statistic normalizes the residual according to the uncertainty weights of the different observation sources, facilitating a comparison of altitude estimate consistency on a unified scale. $\delta y_{\mathrm{LSTM}}$ and $\delta y_{\mathrm{BA}}$ are the residuals of the GNSS-LSTM and barometric measurements with respect to the maximum likelihood estimate, defined as

$$\delta y_{\mathrm{LSTM}}=y_{\mathrm{LSTM}}-{\widehat{y}}_{\mathrm{ml}},\delta y_{\mathrm{BA}}=y_{\mathrm{BA}}-{\widehat{y}}_{\mathrm{ml}}.$$

To improve measurement utilization and robustness against sudden noise, an enhanced IGG robust strategy is proposed. A consistency factor $W_k^{\left(i\right)}$ is defined to weight the agreement between subfilter predictions and observations, and a hierarchical fusion approach is employed to optimize measurement integration. Separate consistency factors are constructed for the altitude and airspeed channels, denoted $W_{h_k^{\left(i\right)}}$ (altitude) and $W_{v_k^{\left(i\right)}}$ (wind speed). The weighting function is then formulated following the IGG segmented strategy as:

$$W_k^{\left(i\right)}=\left\{\begin{array}{cc}1\hfill & \left|q_k^{\left(i\right)}\right|<k_0\hfill \\ {\displaystyle \frac{k_0}{\left|q_k^{\left(i\right)}\right|}}{\left[{\displaystyle \frac{\left(k_1-\left|q_k^{\left(i\right)}\right|\right)}{\left(k_1-k_0\right)}}\right]}^2\hfill & k_0\le \left|q_k^{\left(i\right)}\right|<k_1\hfill \\ 0\hfill & k_1\le \left|q_k^{\left(i\right)}\right|\hfill \end{array}\right.$$

(42)

where $k_0$ and $k_1$ are the segmented thresholds of the observation weighting function, linked to the current innovation magnitude $\left|q_k^{\left(i\right)}\right|$. The adaptive weight $W_k^{\left(i\right)}$ is dynamically adjusted according to this magnitude, serving both as a measure of observation consistency and as a reference to regulate the influence of associated subnodes on the leader node.

5.2. Improved Geometric Configuration Optimization Strategy

Multi-node cooperative localization can be viewed as a state prediction process, with performance evaluated via the prediction error covariance matrix. The Cramér–Rao inequality provides a lower bound on the covariance of any unbiased estimator, whose inverse—the Fisher information—is commonly used to quantify the state information contained in the measurements.

In general, a larger Fisher Information indicates that the system model or measurement process provides richer information, thereby reducing the uncertainty in state prediction and enhancing overall estimation accuracy and robustness. For the state error correction of the assisted UAV, the observation equation can be expressed as

$${\mathit{Z}}_c\left(t\right)=\mathit{H}\mathit{X}\left(t\right)+\mathit{V}$$

(43)

where ${\mathit{Z}}_c\left(t\right)={\left[{\rho }_{12},{\rho }_{13},{\rho }_{23}\right]}^{\mathrm{T}}$ denotes the measurement vector from cooperative nodes, including relative distance measurements between nodes; $\mathit{X}\left(t\right)$ represents the UAV state vector; $\mathit{H}$ is the measurement matrix; and $\mathit{V}$ is the observation noise vector associated with ranging and angular measurements, given by $\mathit{V}={\left[v_{{\rho }_{12}},v_{{\rho }_{13}},v_{{\rho }_{23}}\right]}^{\mathrm{T}}$.

To further analyze the system performance, the posterior information matrix is simplified in the horizontal $\mathit{xy}$ plane as an example. Assuming the measurement noise covariance matrix $\mathit{R}$ is constant and negligible in most practical applications, the posterior information matrix ${\mathit{J}}_{t+1}$ can be defined as:

$${\mathit{J}}_{t+1}\triangleq {\mathit{H}}_{t+1}^{\top }{\mathit{H}}_{t+1}={\mathit{J}}_{t+1}^{\mathrm{H}}\triangleq \left[\begin{array}{cc}\sum _{i=1}^m{cos}^{2}i{\theta }_k^{t+1}& \sum _{i=1}^{m}cosi{\theta }_k^{t+1}sini{\theta }_k^{t+1}\\ \sum _{i=1}^{m}cosi{\theta }_j^{t+1}sini{\theta }_j^{t+1}& \sum _{i=1}^m{sin}^{2}i{\theta }_j^{t+1}\end{array}\right]$$

where the observation matrix ${\mathit{H}}_{t+1}$ is defined as:

$${\mathit{H}}_{t+1}=\left[\begin{array}{cc}cos{\theta }_k^{(i,t+1)}& sin{\theta }_k^{(i,t+1)}\\ cos{\theta }_j^{(i,t+1)}& sin{\theta }_j^{(i,t+1)}\end{array}\right]$$

(44)

Then the traditional performance metric $\Gamma$ for multi-UAV cooperative localization is constructed, defined as the inverse of the trace of the information matrix [45]:

$${\mathit{J}}^{-1}\triangleq 4{sin}^2{\vartheta }^{(i+1)}$$

(45)

where ${\vartheta }^{(i+1)}={\theta }_k^{(i+1,t+1)}-{\theta }_j^{(i+1,t+1)}$ represents the angular difference between two cooperative UAV nodes at time $t+1$. A smaller value of $\Gamma$ indicates lower uncertainty in position estimation and better cooperative observation performance.

Considering measurement errors and potential deviations in leader node performance, for a cooperative system with N nodes—where each subnode communicates with 2 to $N-1$ other nodes—the Fisher information matrix for each subnode can be constructed to evaluate system performance. Specifically, for node n, the Fisher information matrix ${\mathit{J}}_n\left(x\right)$ is defined as:

$$\begin{array}{c}\hfill {\mathit{J}}_n^{\prime }\left(x\right)=\mathit{J}·{\delta }_{kn}^{\left(i\right)}·W_{v_k^{\left(i\right)}}\end{array}$$

(46)

where ${\sigma }_{nk}$ denotes the standard deviation of ranging errors between node n and node k, and ${\delta }_{kn}$ is a Dirac function that equals 1 if node n is connected to node k, and 0 otherwise. $W_{v_k^{\left(i\right)}}$ represents the predictive consistency factor of the leader node k from (42), reflecting its reliability, with values ranging from 0 (completely unreliable) to 1 (fully trustworthy).

The node information matrix ${\mathit{J}}_n\left(x\right)$ incorporates geometric relative positions between nodes (via angles $\theta$), measurement error levels (via ${\sigma }_{nk}$), and leader node reliability (via $W_{v_k^{\left(i\right)}}$), thereby enhancing robustness and performance evaluation in cooperative localization. According to the Cramér–Rao inequality, the lower bound of the position estimation variance for node n is given by the inverse of its information matrix:

$${\mathit{e}}_{{\mathrm{error}}_n}={\left({\mathit{J}}_n^{\prime }\left(x\right)\right)}^{-1}$$

(47)

This bound represents the theoretical limit of estimation error and serves as a system performance metric, validating the effectiveness of the multi-node cooperative configuration optimization strategy incorporating the leader node consistency factor $W_{v_k^{\left(i\right)}}$.

5.3. IMM Framework for Multi-Source Fusion

To enable effective fusion, a unified framework is developed to integrate heterogeneous sensor measurements from different subsystems and to dynamically allocate submodel fusion weights based on prior sensor performance in previous studies [46]. Building on this framework, a dynamic ATPM scheduling strategy incorporating multi-source credibility information is developed.

5.3.1. Resilient Interactive

Since all submodels are processed synchronously and in parallel, an effective fusion of the common state $\left(i\right){\mathit{\chi }}^c$ from their estimation results is required. The IMM algorithm, grounded in Bayesian theory, provides the theoretical basis for this process [47]. According to Bayesian theory, the predicted prior probability of the i-th submodel is expressed as:

$$\left(i\right){\mu }_k={\displaystyle \frac{\left(i\right){\mu }_{k/k-1}\left(i\right)L_k}{{\sum }_{j=1}^M\left(j\right){\mu }_{k/k-1}\left(j\right)L_k}}$$

(48)

where $\left(i\right){\mu }_{k-1}$ is the probability of the j-th sub-model at the previous step $k-1$. The likelihood value $\left(i\right)L_k$ denotes the probability of observing the current measurement given the model and its predictions. The interactive probability sum for the sub-models is:

$$\left(j\right){\widehat{\mu }}_{k\mid k-1}=\sum _{j=1}^n{\mathit{\pi }}_{ji}\left(j\right){\mu }_{k-1},i=1,2$$

(49)

Finally, after each sub-model completes its measurement update, the overall system state and covariance matrix are obtained by fusing the interactive weights from all sub-models:

$$\begin{array}{c}\hfill {\widehat{\mathit{\chi }}}_{k/k}=\sum _{i=1}^M{\widehat{\mathit{\chi }}}_{k/k}^{\left(i\right)}{\mu }_k^{\left(i\right)}{\widehat{\mathit{P}}}_{k/k}=\sum _{i=1}^M\left[{\widehat{\mathit{P}}}_{k/k}^{\left(i\right)}+\left({\widehat{\mathit{\chi }}}_{k/k}-{\widehat{\mathit{\chi }}}_{k/k}^{\left(i\right)}\right){\left({\widehat{\mathit{\chi }}}_{k/k}-{\widehat{\mathit{\chi }}}_{k/k}^{\left(i\right)}\right)}^{\mathrm{T}}\right]{\mu }_k^{\left(i\right)}\end{array}$$

(50)

5.3.2. Model Transition Probability

This subsection presents a dynamic adjustment mechanism for the model transition probability matrix in the IMM algorithm, driven by a multi-source information Credibility Performance Index. The proposed method enables flexible switching among multiple models while preserving strong scenario adaptability and ensuring compatibility with the IEKF algorithm [48].

The strategy leverages the robustness of the IMM multi-model framework and the geometric consistency of the IEKF, thereby accelerating convergence and mitigating the effects of accuracy discrepancies or partial sensor failures in heterogeneous multi-source data. The model transition probabilities are calculated as follows:

(1)

Computation of Submodel Weights: The cooperative navigation system consists of n leader nodes, each forming a submodel with m sensors. Since the study focuses on optimizing relative-information/dynamics cooperative subfilter configurations, sensor setups are assumed identical across UAV platforms. Specifically, the fusion credibility weight of each submodel can be represented by the leader node consistency. The credibility weight of the j-th leader node in the i-th submodel is obtained by averaging as follows:

$$\left(i\right){\omega }_H={\displaystyle \frac{\left(i\right)H_1{+}^{\left(i\right)}{H}_2+\dots {+}^{\left(i\right)}{H}_j}{m}}{W}_{v_k^{\left(i\right)}}\approx W_{v_k^{\left(i\right)}}$$

(51)

where $\left(i\right)H_j$ denotes the credibility index of the sensor combination in the j-th leader node layer of the i-th submodel. $W_v^{\left(i\right)}$ denotes the predictive consistency weight of the leader node layer corresponding to the i-th submodel.

This weight not only reflects the performance stability of the leader node but also provides critical support for subsequent IMM model probability updates and state fusion.

After the weights of all submodels are obtained, normalization is carried out:

$$\left(i\right)\overline{\omega }={\displaystyle \frac{\left(i\right)\omega }{{\sum }_{i=1}^n\left(i\right)\omega }}$$

(52)

(2)

Calculate the self-transition probability ${\pi }_{ii}$:

$${\pi }_{ii}=b·\left(i\right)\overline{\omega }$$

(53)

where b represents the baseline probability, which encapsulates the inherent tendency of each model to maintain its current state without external observational influences.

(3)

The transition probability between submodels, ${\mathit{\pi }}_{ij}$$(j\ne i)$, is computed as ${\mathit{\pi }}_{ij}={\displaystyle \frac{1-{\pi }_{ii}}{n-1}}$, with the remaining probability uniformly distributed among the other $n-1$ models. This uniform allocation ensures a balanced “competitive” environment among the submodels, especially when precise evaluation of each transition probability is difficult.

$$\mathit{\pi }=\left[\begin{array}{cccc}b·\left(1\right)\overline{\omega }& {\displaystyle \frac{1-b·\left(1\right)\overline{\omega }}{n-1}}& \dots & {\displaystyle \frac{1-b·\left(1\right)\overline{\omega }}{n-1}}\\ {\displaystyle \frac{1-b·\left(2\right)\overline{\omega }}{n-1}}& b·\left(2\right)\overline{\omega }& \dots & {\displaystyle \frac{1-b·\left(2\right)\overline{\omega }}{n-1}}\\ ⋮& ⋮& \ddots & ⋮\\ {\displaystyle \frac{1-b·\left(n\right)\overline{\omega }}{n-1}}& {\displaystyle \frac{1-b·\left(n\right)\overline{\omega }}{n-1}}& \dots & b·\left(n\right)\overline{\omega }\end{array}\right]$$

(54)

6. Experimental Analysis

6.1. Experimental Setup

To assess the practical effectiveness of the proposed fusion algorithm, offline validation was performed using flight data collected from fixed-wing UAVs. To further demonstrate the applicability and robustness of the proposed method under realistic operating conditions, a series of cooperative flight experiments was carried out with a fleet of ten UAVs flying at different altitudes and formation trajectories in GNSS-denied environments. The speed characteristics of the UAVs involved in the experiments are consistent. The flight paths and platform configurations are depicted in Figure 3.

Figure 3. Flight paths of leader and follower UAVs.

Each UAV was equipped with a comprehensive sensor suite. The flight control system incorporated an ADIS16488 Inertial Measurement Unit (IMU), operating at a sampling rate of 100 Hz, which integrates a three-axis gyroscope, three-axis accelerometer, three-axis magnetometer, and UWB ranging modules, and a barometer. The GNSS subsystem utilized a NovAtel receiver with a sampling rate of 5 Hz. A wireless communication network links the UAVs, enabling the follower to access the leader data. The detailed specifications and sampling rates of the onboard sensors are summarized in Table 1.

Table 1. Specifications of Multisensors on the fix-wing UAVs.

Source	Frequency	Parameters	Typical Value
IMU	100 Hz	In-Run Bias Stability of Gyroscope Angular Random Walk	$6.{25}^{\circ }/\mathrm{h}$
		Angular Random Walk	$0.3^{\circ }/\sqrt{\mathrm{h}}$
		In Run Bias Stability of Accelerometer	$0.1\mathrm{m}/{\mathrm{s}}^2$
		Velocity Random Walk	$0.029\mathrm{m}/\mathrm{s}/\sqrt{\mathrm{h}}$
UWB	10 Hz	Position Noise	2 m
GNSS	5 Hz	Position Noise	$0.1$ m
		Velocity Noise	$0.1\mathrm{m}/\mathrm{s}$
RTK	5 Hz	Position Noise	$0.02$ m
		Velocity Noise	$0.03\mathrm{m}/\mathrm{s}$
Baro	100 Hz	Output Noise	$0.025\mathrm{mbar}$
Pitot Tube	100 Hz	Output Noise	$1\mathrm{m}/\mathrm{s}$

6.2. Navigation Performance Analysis of the Leader Node Layer

6.2.1. Prediction Results of the LSTM Network

To evaluate the proposed formation-consistency-aided navigation algorithm, commonly observed state information from historical trajectory data was first used to train UAV swarms under GNSS-denied conditions. The trained model was then employed to predict AOA and estimate P-GNSS increments, thereby facilitating the assessment of both prediction accuracy and the feasibility of the resulting estimates.

As shown in Figure 4, the integration of P-GNSS incremental information produces the reconstructed P-GNSS results and the corresponding error comparison with RTK measurements. To evaluate generalizability, multiple UAV nodes experienced GNSS denial 30 s after takeoff, at which point predictions were initiated. The results indicate that the overall prediction trends closely follow the RTK reference. In principle, navigation schemes incorporating P-GNSS increments are sufficient to maintain accurate positioning and stable attitude convergence.

Figure 4. Multi-Node OINS-PGNSS Prediction and Error Curves.

However, as indicated by the error curves on the right-hand side of the figure, the P-GNSS estimates gradually diverge, with divergence rates varying across UAV nodes. This observation confirms the previously noted characteristic of being “effective in the short term but unreliable in the long term.” For different UAV platforms, relying solely on P-GNSS incremental predictions may not satisfy the stringent requirements of practical engineering applications. Therefore, in addition to identifying nodes with short-term reliable P-GNSS predictions, the proposed framework incorporates predicted AOA information to improve long-term navigation reliability.

This study leverages readily available historical states to construct a dynamical mapping relationship, thereby avoiding reliance on conventional UAV parameters and enabling online enhancement of airflow angle estimation in terms of both consistency and reliability. As shown in Figure 5, to evaluate the effectiveness of the LSTM network trained on historical flight trajectories, the estimated AOA and SA were compared across LSTM, BP, and LS models. The results indicate that although the BP neural network exhibits a trend similar to that of the proposed LSTM, the latter achieves substantially higher prediction accuracy. Furthermore, compared with the semi-aerodynamic LSTM [44] that relies on ground parameters, the full-aerodynamic LSTM developed in this study demonstrates superior performance in airflow angle prediction. Therefore, integrating the LSTM network with the state estimation system can effectively improve the accuracy of airflow angle estimation.

Figure 5. Estimated AOA and SA under Different Network Models (LSTM, BP, and LS).

It should be noted that, during certain phases (e.g., the initialization stage), substantial discrepancies occur between the estimated values and the ground-truth measurements. This phenomenon is primarily attributed to the limited representativeness of historical trajectory data, which inherently constrains the model’s fitting capability. Moreover, the magnitude and occurrence of such errors vary across UAV platforms and flight conditions. To mitigate this limitation, a consistency evaluation mechanism is subsequently introduced, and adaptive weights are assigned based on the evaluation results to serve as reliability criteria for estimation.

6.2.2. Analysis of RIEKF Filtering Results Based on AHRS

Furthermore, the effectiveness of the proposed LSTM-based historical trajectory training model in supporting navigation under GNSS-denied conditions is evaluated, where complementary fusion is implemented, and navigation accuracy across different states is compared. To ensure consistency, the first 220 s of flight data were used for simulation, with GNSS denial imposed at $t=30$ s. The algorithms compared in this experiment include: (i) RIEKF corrected by the full aerodynamic model (proposed method), (ii) RIEKF corrected by P-GNSS, and (iii) RIEKF corrected by the semi-aerodynamic model. Specifically, the P-GNSS-corrected RIEKF reduces to a P-GNSS/INS-based maneuver-aided correction of pure inertial navigation during the GNSS-denied period, serving as the baseline for comparison. In contrast, the semi-aerodynamic corrected RIEKF relies on barometer and airspeed updates to substitute for GNSS measurements, with aerodynamic angles estimated using the semi-aerodynamic prediction model.

Figure 6b,c present the velocity and attitude estimation results obtained from the full data sequence for the different algorithms, where the black curve represents the RTK-based reference solution, the green curve corresponds to the semi-aerodynamic RIEKF, and the red curve denotes the proposed method. Since the AHRS ensures attitude stability, the differences in attitude estimation accuracy across the compared algorithms are negligible. Regarding velocity accuracy, the proposed method (PROP) outperforms the semi-aerodynamic RIEKF, whereas the Semi-Aero method slightly surpasses the P-GNSS-corrected RIEKF due to its capability for velocity correction.

Figure 6. Comparison of Leader UAVs’ result analysis of different algorithms.

Figure 6a presents the position trajectories and error comparisons of the three approaches 30 s after GNSS failure. Compared with the P-GNSS baseline, the proposed algorithm demonstrates the dual advantages of “short-term stability and long-term drift suppression”: the position error not only increases more slowly but also remains consistently constrained without significant oscillations throughout the flight.

Specifically, the system conducts an online evaluation of the BA/P-GNSS altitude consistency at approximately 90 s. When the vertical error exceeds a predefined threshold, the corresponding channel is immediately frozen, and the system seamlessly switches to the full aerodynamic model architecture, effectively preventing altitude drift from propagating into the horizontal channels. As summarized in Table 2, under a 190 s GNSS-denied scenario, this strategy reduces the maximum position drift from 70 m to 55 m, lowers the RMSE from 18.24 m to 7.37 m, and decreases the equivalent drift rate by approximately 20%. Additionally, the navigation survival time is extended from 1 min to nearly 2 min, providing a practical transition window for the reactivation of absolute observation sources such as GNSS.

Table 2. Comparison of precision indexes of three-axis position estimation.

Channel	Pos MAE (m)			Pos RMSE (m)
	P-GNSS	Semi-Aero	PROP	P-GNSS	Semi-Aero	PROP
North	15.354	7.905	5.402	18.242	11.271	7.367
East	18.051	17.298	12.057	21.993	20.864	16.258
Down	1.825	1.800	1.806	1.965	1.945	1.948

Compared with the semi-aerodynamic (Semi-Aero) scheme, the proposed method (PROP) exhibits only a 1.5 m difference in northward drift and approximately 7.5 m in eastward error. The most notable improvement is observed in the RMSE: PROP demonstrates substantially lower position fluctuations than Semi-Aero, confirming that the P-GNSS plus full aerodynamic fusion framework effectively mitigates the robustness limitations of the semi-aerodynamic model. In addition, the vertical channel remains constrained by the barometer throughout the entire flight, maintaining a steady-state error within 3 m.

Figure 6d presents the comparison of AOA and SA estimations under GNSS-denied conditions. The direct estimation method (Direct AOA/SA) exhibits large errors in both AOA and SA, with a pronounced divergence, particularly in SA estimation. This degradation is primarily attributed to the method’s strong dependence on velocity measurements, which become unreliable in the absence of GNSS, resulting in poor estimation performance.

Specifically, SA is highly sensitive to the lateral velocity component v. Under low-speed or high-maneuvering conditions, velocity errors are amplified through trigonometric relationships, leading to severe fluctuations or even divergence in angle estimates.

In contrast, the proposed LSTM-based full aerodynamic model leverages historical flight data to capture the underlying flight dynamics. Even under GNSS signal loss, it can predict aerodynamic angles with high accuracy. By implicitly modeling the nonlinear relationships among aerodynamic forces, vehicle states, and sensor measurements, while exploiting the consistency in historical trajectories, the proposed method achieves substantially improved AOA and SA estimation performance, demonstrating its effectiveness in enhancing navigation robustness under GNSS-denied conditions.

6.3. Follower Node Layer Navigation Performance Analysis

6.3.1. Consistency Evaluation

To evaluate the effectiveness of the constructed normalized statistics in assessing observation consistency, the consistency evaluation results for BA/P-GNSS and wind speed estimates $v_{\mathrm{wind}}$ from multiple UAVs are compared. Taking wind speed as an example, the consistency evaluation among UAV nodes considers pairwise estimation deviations. For multiple UAV nodes, the absolute deviation $|v_{\mathrm{wind}}^i-v_{\mathrm{wind}}^j|$ and the normalized statistic $\delta v_w^{\mathrm{norm}}$ are computed. Based on the ICG segmentation strategy, a weight $v{W}_k^{\left(i\right)}$ representing the consistency of wind speed estimation is derived.

To further analyze the effectiveness of the normalized statistic in reflecting the consistency of P-GNSS predictions, Figure 7 presents the temporal evolution of the normalized statistic (denoted as ${\delta }_{\mathrm{norm}}$) for wind speed and altitude estimation tasks, along with the corresponding consistency weights (${\mathit{W}}_k$) derived using the ICG strategy. The right subplots provide a comparison between altitude and wind speed estimation results. As observed, the normalized statistics exhibit substantial temporal fluctuations, with magnitudes closely related to estimation deviations among UAV nodes. When the statistic is highly correlated with deviations (weight ${\mathit{W}}_k$ approaching 1), it indicates that the statistic reliably reflects consistency; conversely, when the correlation is weak (weight approaching 0), the statistic may fail to capture consistency information and should be reconstructed.

Figure 7. Consistency Evaluation for Leader Layer BA/P-GNSS and Wind Speed $v_{wind}^{\left(i\right)}$.

Specifically, for altitude estimation, the normalized statistic remains relatively smooth during the initial phase (30–80 s), and the consistency weight $h{W}_k^{\left(i\right)}$ largely stays at a high level (near 1), with only minor reductions in certain intervals. This behavior is attributed to the altitude channel providing more reliable observations. The vertical positioning accuracy, influenced by P-GNSS predictions, ensures that altitude estimation deviations remain within a controllable range, offering a reference for preliminary selection of high- and low-precision layers under GNSS-denied conditions. Notably, around 125 s, the consistency weights of two BA/P-GNSS channels simultaneously decrease, reflecting the degradation of altitude consistency after prolonged divergence ($h{W}_k^{\left(i\right)}<0.5$).

In contrast, for the wind speed estimation scenario, around 105 s the system experiences an external disturbance, resulting in significant differences in wind speed magnitudes $|{\mathit{v}}_{\mathrm{wind}}|$ among UAV nodes. The normalized statistic $\delta {\mathit{v}}_w^{\mathrm{norm}}$ exhibits a distinct peak, indicating increased estimation discrepancies between nodes 1 and 2. The ICG mechanism adjusts the weight $v{W}_k^{\left(i\right)}$ to reflect the low consistency of these UAVs’ wind speed estimates. This procedure not only selects a set of high-consistency, high-precision UAVs but also serves as a cooperative factor for elastic fusion at the follower layer.

6.3.2. Elastic Analysis of IMM Subfilter with Geometry Configuration Optimization

To evaluate the effectiveness of the proposed improved geometry configuration optimization-based cooperative algorithm, this study focuses on the elastic fusion of leader nodes, analyzing the combined influence of configuration and reference node performance, and assessing its applicability in a real-world system. As described previously, based on the screening of BA/P-GNSS and wind speed estimation consistency, the formation system is partitioned into two precision layers: a high-accuracy leader layer and a low-accuracy follower layer. The follower layer relies on relative information from the leader layer and auxiliary dynamic information. Navigation and positioning under GNSS-denied conditions are achieved using the elastic fusion algorithm built upon the improved IMM framework.

In the experiments, all nodes are initialized with consistent sensor measurements, initial covariance matrices, and measurement noise covariance matrices. During the cruise phase, different levels of perturbations are applied to the leader node signals to simulate dynamic variations in reference node performance and configuration. For the follower layer, Figure 8 and Figure 9a present the RMSE error curves and position/attitude variations under various scenarios, including GNSS degradation (20–30 s) and denial (from 30 s), leader node accuracy degradation (50–80 s and 100–135 s), UWB signal degradation (50–80 s), and UWB outage (135–150 s). As shown in Figure 9d, it can be observed that the wind estimates between the leader and follower nodes reveal a strong consistency, demonstrating the effectiveness of using wind information from the leader UAV as an auxiliary input for cooperative navigation.

Figure 8. Follower UAVs’ position error and ${\mathit{e}}_{{\mathrm{error}}_n}^{-1}$ comparsion of the various CL algorithm.

Figure 9. Follower UAV’s velocity avp and $v^{wind}$ comparison by using different methods.

The proposed elastic IMM fusion algorithm with improved configuration optimization was evaluated by comparison with the EKF/TAS algorithm, the HCF algorithm (random signal source selection), and the MCF algorithm (traditional configuration optimization [45]) to assess its robustness and effectiveness. As shown in Figure 8, the position error curve of UAV Node 1 indicates that, although the traditional HCF algorithm suppresses the follower UAV’s position error to some extent, the MCF method—due to its multi-signal-source selection—achieves superior suppression. However, both methods still underperform compared to the proposed algorithm. Under conditions of the leader node and UWB accuracy degradation, the horizontal channel error is maintained within 1 m during the first 20 s of flight. When only the leader node accuracy degrades after 110 s, the maximum error remains below 2 m. This improvement is attributed to the interaction between the submodel transition probability matrix $\pi$ and the cooperative factor $v{W}_k^{\left(i\right)}$, which enables the submodels to exploit adaptive elastic advantages during interaction.Moreover, although the MCF and the proposed configuration-based method share similar geometric alignment, the inverse error matrix ${\mathit{e}}_{\mathrm{error}}^{-1}$ differs. Specifically, during the 50–70 s interval, under perturbations and UWB performance degradation, the weight of the reference leader node ${\mathit{W}}_k$ approaches zero, causing the information matrix ${\mathit{J}}^{\prime }$ to also approach zero. Similarly, during the 135–150 s UWB outage of Submodel 1, although ${\mathit{W}}_k$ remains non-zero, ${\mathit{J}}^{\prime }$ is directly set to zero. Previous studies (HCF, MCF) did not consider the impact of reference node performance, often resulting in overestimation of ${\mathit{e}}_{\mathrm{error}}^{-1}$. In the proposed multi-UAV cooperative navigation framework, the follower layer’s multi-source selection mechanism accounts for both the information matrix ${\mathit{J}}^{\prime }$ (related to the relative configuration $\vartheta$ between nodes) and the intrinsic accuracy of the leader nodes.

Figure 10 presents the posterior probabilities $\left(i\right){\mu }_k$ of the IMM submodels under perturbation injection. It can be observed that, during the 20–30 s interval, the weights of Submodel 1 and Submodel 2 remain near the midline, indicating a complementary state. During 50–80 s, due to perturbations applied to Leader 1 and UWB performance degradation, $\left(1\right){\mu }_k$ rapidly approaches zero while $\left(2\right){\mu }_k$ becomes dominant. This indicates that Submodel 1 becomes unreliable and its weight decreases, whereas the more reliable Submodel 2 seamlessly takes over, maintaining system stability. Similarly, during the 100–135 s phase, a small perturbation is applied to the leader node, and the weights of Submodel 1 and Submodel 2 differ only slightly. During the 135–150 s UWB outage, Submodel 1 is rendered completely untrustworthy, and the IMM algorithm switches to a new Submodel 1 at 135 s. Figure 9b,c show the attitude and velocity error curves of the UAV nodes. Comparative analysis indicates that attitude and velocity fluctuations are significantly smaller than those of the HCF and MCF algorithms, with no noticeable jumps during model-switching phases. The proposed algorithm consistently exhibits superior robustness and high accuracy in position, attitude, and velocity estimation across all fault-injection scenarios, as summarized in Table 3.

Figure 10. Switching results of IMM posterior probability $\left(i\right){\mu }_k$.

Table 3. Comparison of precision indexes of position estimation.

Channel	Pos MAE (m)		Pos MAE (m)		Att MAE (deg)		Att MAE (deg)		Vel MAE (m/s)		Vel MAE (m/s)
	MCF	PROP	MCF	PROP	MCF	PROP	MCF	PROP	MCF	PROP	MCF	PROP
North/$\varphi$	0.742	0.246	1.293	0.314	2.271	0.858	3.273	1.554	0.762	0.348	1.097	0.491
East/$\theta$	0.455	0.331	0.612	0.432	2.065	6.686	2.962	0.884	0.594	0.357	0.788	0.509
Down/$\psi$	0.082	0.057	0.105	0.076	4.424	2.348	13.36	7.539	0.153	0.188	0.210	0.282

Moreover, its accuracy and stability under GNSS-denied conditions further validate the feasibility and effectiveness of the proposed system for real-world applications. As shown in Table 3, under injected disturbances, the proposed algorithm is compared with RTK observations and existing methods, including the MCF algorithm and conventional configuration optimization approaches. The results indicate that, while the MCF algorithm can partially suppress the position errors of follower UAVs, the inclusion of the cooperative weight in the proposed method further enhances positioning accuracy. Specifically, the improved IMM algorithm effectively selects appropriate cooperative information to improve follower UAV localization.

Additionally, the experiments were conducted on a computer equipped with a 13th-generation Intel Core i7-13700H processor, and the algorithmic models were implemented in MATLAB 2023a. The total algorithm execution duration was set to 150 s, with a time step of 0.01 s. The proposed algorithm required a total runtime of 68.85 s, corresponding to an average computation time of 4.588 ms per time step, which satisfies the real-time operational requirements of systems.

7. Conclusions

This paper proposes a hierarchical cooperative navigation framework based on historical dynamic model information and multi-source sensor data fusion to address the challenges of cluster navigation in GNSS-denied environments. By combining P-GNSS and AOA/SA estimations, the framework achieves the navigation objective of short-term stability and long-term drift suppression, thereby maintaining node-layer navigation accuracy in the absence of GNSS. Node selection strategies constructed from altitude and wind-speed consistency evaluations, together with geometric constraints, improve the collaborative reliability of leader and follower node layers, while a modified IMM filter enables elastic fusion of multi-source information, effectively enhancing the robustness of follower systems in complex environments. Simulations and experiments separately evaluate the navigation performance of the leader and follower node layers under GNSS-denied conditions, fully considering contingencies such as UWB outages and leader-node accuracy degradation. Results demonstrate that the proposed method significantly improves formation cooperative navigation accuracy and stability in GNSS-denied scenarios, confirming the feasibility and effectiveness of the framework in practical applications. Future work will include quadrotor cluster formation simulations to further validate the framework’s applicability under rotorcraft dynamic consistency, as well as exploring dynamic model-assisted navigation for heterogeneous UAV clusters.