A Consensus-Driven Distributed Moving Horizon Estimation Approach for Target Detection Within Unmanned Aerial Vehicle Formations in Rescue Operations

Abstract

In the last decades, the increasing employment of unmanned aerial vehicles (UAVs) in civil applications has highlighted the potential of coordinated multi-aircraft missions. Such an approach offers advantages in terms of cost-effectiveness, operational flexibility, and mission success rates, particularly in complex scenarios such as search and rescue operations, environmental monitoring, and surveillance. However, achieving global situational awareness, although essential, represents a significant challenge, due to computational and communication constraints. This paper proposes a Distributed Moving Horizon Estimation (DMHE) technique that integrates consensus theory and Moving Horizon Estimation to optimize computational efficiency, minimize communication requirements, and enhance system robustness. The proposed DMHE framework is applied to a formation of UAVs performing target detection and tracking in challenging environments. It provides a fully distributed architecture that enables UAVs to estimate the position and velocity of other fleet members while simultaneously detecting static and dynamic targets. The effectiveness of the technique is proved by several numerical simulation, including an in-depth sensitivity analysis of key algorithm parameters, such as fleet network topology and consensus iterations and the evaluation of the robustness against node faults and information losses.

1. Introduction

Unmanned aerial vehicles (UAVs) are widely recognized as cost-effective and versatile mobile sensing platforms capable of autonomously performing tasks that are challenging, hazardous, or monotonous for human operators. Applications span a wide range of domains such as agricultural monitoring, exploration and mapping, and search and rescue missions, as well as surveillance and tracking [1]. The increasing complexity of missions has highlighted the use of coordinated UAV fleets as a promising approach to achieve greater operational efficiency and resilience [2].

A wide range of applications that involve UAV fleets has been documented in the literature, in fields such as surveillance, mapping, and distributed sensing [3,4,5,6]. However, situational awareness is a substantial challenge that must be addressed to allow effective coordination of the UAV fleet [7]. UAV swarms can be conceptualized as large-scale multi-sensor systems, similar to grids used for data fusion in sensor networks [8]. In this field, an interesting task is the accurate localization of targets, which relies heavily on the effectiveness of the data fusion process and the relative positioning of the UAVs with respect to the target [9,10].

The problem of localizing moving targets has been studied in signal processing and control literature [11,12,13,14], driven by applications across civilian, military, and transportation sectors. The primary goal of target tracking is to estimate the trajectory of a mobile object, which can be modeled using various dynamic frameworks.

In this field, several studies focus on a multi-lateration approach using distance measurements from ToF (time of flight) or ToA (time of arrival) sensors, where UAVs, acting as mobile agents, share their positions and ToA data to estimate the target location. The literature also addresses multi-target tracking using static sensors [15,16] or mobile agents [17] under various filtering approaches. Recent advancements in UAV technology and multi-drone coordination [18,19,20] have encouraged industrial adoption of mobile tracking methodologies.

However, traditional estimation techniques are predominantly centralized [21,22,23,24], relying on a fusion center to aggregate data from all sensors and apply algorithms such as Kalman filtering (KF), extended Kalman filtering (EKF), or maximum likelihood estimation (MLE) [23,25]. In these techniques, as the fleet size increases, communication and computational bottlenecks, as well as increased software complexity, can be expected. Furthermore, these limitations are compounded by the risk of single-point failure in the leader node, which can compromise the entire system operation.

The development of cloud-based and distributed computing frameworks has involved interest in decentralized estimation approaches [26,27]. Decentralized architectures offer a promising alternative by distributing computational and communication burdens across all nodes in the fleet. In these systems, each node iteratively refines its local estimation, incorporating data from neighboring nodes, in order to collaboratively track the target state [28,29]. This approach relies on determining efficient protocols for data exchange to achieve agreement on the target measurements, a challenge known as the consensus or synchronization problem [30,31,32,33].

One critical challenge in decentralized estimation lies in ensuring observability. To achieve stable estimation with bounded error, specific observability conditions must be satisfied. Semi-centralized approaches [34,35,36,37] address it by ensuring local observability within each agent neighborhood, and requiring densely connected networks where each UAV communicates with at least three neighbors [38]. However, several studies have proposed distributed estimation protocols [39,40] that relax these requirements. These protocols eliminate the need for local observability, reducing the communication burden and enabling operation under less restrictive network connectivity.

Key considerations for decentralized methods include scalability and robustness to dynamic changes in network topology [41]. Scalability ensures that computational complexity grows modestly with the size of the network, while robustness enables the system to adapt to changes in relative positions of UAVs or environmental factors, preserving the integrity of the fleet even as formation evolves [42].

Decentralized Kalman filters (DKFs) represent a key innovation in distributed estimation, with numerous implementations documented in the literature [43,44,45,46].

When constraints on state variables are present or the noise deviates from being white and Gaussian, Kalman filtering techniques may become suboptimal and, in some cases, unstable. Moving Horizon Estimation (MHE) has been studied as a promising solution, presenting some conceptual similarities to model predictive control (MPC), as both methods rely on optimization problems over a moving time horizon [47,48]. Compared to traditional estimation methods, MHE is capable of handling constraints and providing more accurate estimates in complex scenarios [49]. Several MHE schemes have been developed in recent years to address estimation problems in linear, nonlinear, and hybrid systems [50,51,52,53].

Consequently, a distributed MHE (DMHE) framework can offer several advantages: it allows constraints to be directly incorporated into the optimization problem that is solved at each time step, it ensures optimal estimation, and it guarantees convergence within a deterministic framework under weak local observability conditions [54].

Several distributed implementations of MHE have been proposed, based on fully connected communication graphs. In [55], a DMHE scheme was proposed for non-linear constrained systems, using consensus and ensuring estimation error stability under suitable conditions. The authors in [56] presented a scalable distributed state estimation method for linear systems over peer-to-peer sensor networks, using MHE to handle constraints and ensure stable error dynamics under minimal connectivity requirements. Ref. [57] introduced a DMHE with event-triggered communication, optimizing data transmission in wireless sensor networks while ensuring bounded estimation error under strong connectivity and collective observability conditions. The authors in [58] considered a DMHE for linear systems in wireless sensor networks, employing event-triggered communication to reduce transmissions while ensuring stability and bounded estimation error under connectivity and observability conditions. In [59], the MHE-based estimation in a distributed power system was described, leveraging operator splitting to handle constraints, enhance robustness, and enable parallel computation for improved estimation accuracy. Ref. [60] addressed MHE for networked systems over relay channels, designing an estimator that ensures mean-square bounded error dynamics under packet loss conditions.

In this paper, a consensus-based MHE is proposed, for the situational awareness of a UAV formation operating in complex and uncertain environments. The fully distributed architecture allows each UAV to estimate the position and velocity of all other UAVs in the formation and supports the detection of static and dynamic targets. The use of local models with a reduced dimensionality decreases the computational burden while maintaining the accuracy and robustness of the estimate.

To prove the effectiveness of the proposed strategy, this paper includes a comprehensive sensitivity analysis of key parameters affecting algorithm performance, such as network topology and the number of consensus iterations. Numerical simulations are presented to evaluate the robustness of the algorithm against node faults and information losses.

The paper is organized as follows:

• In Section 2, the problem statement is presented, detailing the mathematical model used to describe the dynamics of both the UAVs and the target. This section also provides a comprehensive description of the sensor configurations employed in the system, including their capabilities and limitations.
• Section 3 introduces the proposed DMHE algorithm. The theoretical foundations of the method are explained, highlighting its integration with consensus-based techniques to enable a distributed and scalable estimation framework.
• In Section 4, the numerical results are presented to evaluate the performance of the proposed approach. This includes a sensitivity analysis of key parameters, such as the network topology and the number of consensus iterations, along with a reliability assessment under conditions of node faults and information losses.

2. Problem Statement

We consider a formation of N autonomous UAVs involved in a mission of aerial surveillance within a prescribed area of interest. The primary objective is to detect the presence of $N_t$ non-collaborative targets, while ensuring global situational awareness, estimating the positions of UAVs within the formation.

To address these tasks, each UAV employs an MHE algorithm to process local sensor data and information shared by neighboring vehicles through dedicated communication channels.

The following assumptions have been considered:

• Each UAV is equipped with its own flight control system to ensure stability and maneuverability.
• For each UAV, the on-board Attitude and Heading Reference System (AHRS) provides reliable measurements of roll, pitch, and yaw with respect to the NED frame.
• The frequency of the AHRS $f_{AHRS}$ is higher than the frequency of the DMHE $f_{DMHE}$, with $f_{AHRS}=10f_{DMHE}$.

At a time instant k, the i-th UAV dynamics is defined by the evolution of its position in a inertial frame, e.g., the north–east–down (NED) frame, ${\mathit{S}}_i\left(k\right)={[X_i\left(k\right),Y_i\left(k\right),Z_i\left(k\right)]}^T$ and its velocity ${\mathit{V}}_i\left(k\right)={[V_{x_i},V_{y_i},V_{z_i}]}^T$, expressed in state-space form as follows:

$${\mathit{\xi }}_i(k+1)={\mathit{A}}_i{\mathit{\xi }}_{\mathit{i}}\left(k\right)+{\mathit{B}}_i{\mathit{u}}_i\left(k\right)+{\mathit{\upsilon }}_i\left(k\right)$$

(1)

where

-

${\mathit{A}}_i$ and ${\mathit{B}}_i$ model the temporal evolution of UAV state. The explicit forms of ${\mathit{A}}_i$ and ${\mathit{B}}_i$ are defined as follows:

$${\mathit{A}}_{\mathit{i}}=\left[\begin{array}{cccccc}1& 0& 0& \Delta t& 0& 0\\ 0& 1& 0& 0& \Delta t& 0\\ 0& 0& 1& 0& 0& \Delta t\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right];{\mathit{B}}_{\mathit{i}}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ \Delta t& 0& 0\\ 0& \Delta t& 0\\ 0& 0& \Delta t\end{array}\right]$$

(2)

-

${\mathit{\xi }}_i\left(k\right)={[{\mathit{S}}_i{\left(k\right)}^T,{\mathit{V}}_i{\left(k\right)}^T]}^T$ is the state vector.

-

${\mathit{u}}_i\left(k\right)={[a_{x_i}\left(k\right),a_{y_i}\left(k\right),a_{z_i}\left(k\right)]}^T$ is the input vector composed of the linear accelerations in the NED frame, obtained by applying a suitable rotation to the raw data provided by the accelerometers and subtracting the gravitational acceleration.

-

${\mathit{\upsilon }}_i\left(k\right)$ represents the process noise.

-

$\Delta t$ is an appropriate sampling time.

To navigate and accurately localize targets, each UAV is equipped with a suite of sensors, including a GPS receiver to determine the UAV position and velocity, and multiple transponders, ToF cameras, or millimeter-wave radars [61], which are used to measure relative distances between UAVs in the formation as well as the distances between UAVs and any target in the scenario.

It is worth noting that, in this framework, both UAVs and targets are assumed to be equipped with devices able to emit and detect signals for relative distance estimation. They can implement technologies such as time of flight (ToF), time of arrival (ToA), or difference ToA (DToA), in order to compute distances between vehicles. Active cooperative systems, like transponders [62] or avalanche transceivers (ARTVA) [63], require that vehicles are equipped with compatible transceivers to exchange signals and compute relative distances. On the other hand, non-cooperative devices, such as millimeter-wave radars [64] or ToF cameras [65], do not require targets to be equipped with dedicated transmitters. In this case, we assume that the identification of vehicles within the sensor range, together with data association, is given and reliable, as these aspects are beyond the scope of the present work. In this paper, we refer to a generic transponder definition.

Consequently, at any time instant k, each UAV acquires the following data array:

$${\tilde{\mathit{z}}}_i\left(k\right)=[{\tilde{\mathit{z}}}_i^{GPS}{\left(k\right)}^T,{\tilde{\mathit{z}}}_i^D{\left(k\right)}^T]$$

(3)

where ${\tilde{\mathit{z}}}_{\mathit{i}}^{\mathit{GPS}}\left(k\right)$ represents the measurement provided by the GPS, and ${\tilde{\mathit{z}}}_i^D\left(k\right)$ is the vector of distances from other vehicles and targets measured by transponders.

We suppose that GPS supplies data about position ${\mathit{S}}_i^{GPS}\left(k\right)$ and velocity vectors $V_i^{GPS}\left(k\right)$,

$${\tilde{\mathit{z}}}_i^{GPS}\left(k\right)={[{\mathit{S}}_i^{GPS}{\left(k\right)}^T,{\mathit{V}}_i^{GPS}{\left(k\right)}^T]}^T+{\tilde{\mathit{\nu }}}_i^{GPS}\left(k\right)$$

(4)

where ${\tilde{\mathit{\nu }}}_i^{GPS}\left(k\right)$ denote the GPS noise.

On the other hand, we considered transponders capable of measuring the mutual distance $d_{i,j}\left(k\right)={\parallel {\mathit{S}}_{\mathit{j}}\left(k\right)-{\mathit{S}}_{\mathit{i}}\left(k\right)\parallel }_2$ between the i-th UAV and any other vehicle j in its sensor range ${\mathcal{S}}_i$.

By denoting with ${\overline{d}}_i$ the radius of ${\mathcal{S}}_i$, at time instant k, it is possible to define the set of visible UAVs, ${\mathcal{N}}_i\left(k\right)$, and the set of visible targets, ${\mathcal{N}}_i^t\left(k\right)$, as follows:

$${\mathcal{N}}_i\left(k\right)=\{1,2,\dots ,N\}\cap \{j:d_{i,j}\left(k\right)\le {\overline{d}}_i\}$$

$${\mathcal{N}}_i^t\left(k\right)=\{N+1,N+2,\dots ,N+N_t\}\cap \{j:d_{i,j}\left(k\right)\le {\overline{d}}_i\}$$

Consequently, the vector ${\tilde{\mathit{z}}}_{\mathit{i}}^{\mathit{D}}\left(k\right)$ has a length equal to the cardinality of the set ${\mathcal{N}}_i\left(k\right)\cup {\mathcal{N}}_i^t\left(k\right)$, including the overall distance measurements:

$${\tilde{\mathit{z}}}_i^D\left(k\right)=\bigcup _{j\in {\mathcal{N}}_i\left(k\right)\cup {\mathcal{N}}_i^t\left(k\right)}{d}_{i,j}\left(k\right)+{\tilde{\mathit{\nu }}}_i^D\left(k\right)$$

(5)

with ${\tilde{\mathit{\nu }}}_i^D\left(k\right)$ as the noise vector affecting any measurement.

At any time instant k, complete situational awareness is achieved if it is possible to estimate the global state vector $\mathbf{\Xi }\left(k\right)\in \mathbb{X}\subset {\mathbb{R}}^{6(N+N_t)}$:

$$\mathbf{\Xi }\left(k\right)={\left[{\mathit{\xi }}_1{\left(k\right)}^T,\dots ,{\mathit{\xi }}_N{\left(k\right)}^T,{\mathit{\xi }}_{N+1}{\left(k\right)}^T,\dots ,{\mathit{\xi }}_{N+N_t}{\left(k\right)}^T\right]}^T$$

(6)

where ${\mathit{\xi }}_i\left(k\right)={\left[{\mathit{S}}_i{\left(k\right)}^T,{\mathit{V}}_i{\left(k\right)}^T\right]}^T$, with $i=\{1,\dots ,N\}$ is the state of the i-th aircraft, defined by (1) and ${\mathit{\xi }}_{N+j}\left(k\right)={\left[{\mathit{S}}_{N+j}{\left(k\right)}^T,{\mathit{V}}_{N+j}{\left(k\right)}^T\right]}^T$, with $j\in \{1,\dots ,N_t\}$ is the state of the j-th target, composed of the position vector ${\mathit{S}}_{N+j}\left(k\right)={\left[X_{N+j}\left(k\right),Y_{N+j}\left(k\right),Z_{N+j}\left(k\right)\right]}^T$ and the velocity vector ${\mathit{V}}_{N+j}\left(k\right)={\left[V_{x_{N+j}}\left(k\right),V_{y_{N+j}}\left(k\right),V_{z_{N+j}}\left(k\right)\right]}^T$, defined in an NED inertial reference frame as follows:

$${\mathit{\xi }}_{\mathit{N}+\mathit{j}}(k+1)={\mathit{A}}_{\mathit{N}+\mathit{j}}{\mathit{\xi }}_{\mathit{N}+\mathit{j}}\left(k\right)+{\mathit{\upsilon }}_{N+j}\left(k\right)$$

(7)

with

$$\begin{array}{cc}{\mathit{A}}_{\mathit{N}+\mathit{j}}={\mathit{A}}_{\mathit{i}}& \mathrm{with}i=1\dots N\cup j=1\dots N_t\end{array}$$

(8)

The overall system dynamics $\mathbf{\Xi }\left(k\right)$ can be defined as follows:

$$\mathbf{\Xi }(k+1)=\mathit{A}\mathbf{\Xi }\left(k\right)+\mathit{B}\mathit{U}\left(k\right)+\mathit{{\rm Y}}\left(k\right)$$

(9)

where $\mathit{U}\left(k\right)={\left[{\mathit{u}}_1{\left(k\right)}^T,{\mathit{u}}_2{\left(k\right)}^T,...,{\mathit{u}}_N{\left(k\right)}^T\right]}^T$ represents the global input vector and the global process noise is indicated with $\mathit{{\rm Y}}\left(k\right)={\left[{\mathit{\upsilon }}_1{\left(k\right)}^T,...{\mathit{\upsilon }}_N{\left(k\right)}^T,...,{\mathit{\upsilon }}_{N+1}{\left(k\right)}^T,{\mathit{\upsilon }}_{N+N_t}{\left(k\right)}^T\right]}^T$.

The model dynamics matrices $\mathit{A}$ and $\mathit{B}$ are block-diagonal matrices defined as follows:

$$\mathit{A}=\left[\begin{array}{ccccc}{\mathit{A}}_{\mathbf{1}}& \dots & {\mathbf{0}}_{6\times 6}& {\mathbf{0}}_{6\times 6}\dots & {\mathbf{0}}_{6\times 6}\\ \dots & \dots & \dots & \dots \\ {\mathbf{0}}_{6\times 6}& \dots & {\mathit{A}}_{\mathit{N}}& {\mathbf{0}}_{6\times 6}\dots & {\mathbf{0}}_{n\times n}\\ {\mathbf{0}}_{6\times 6}& \dots & {\mathbf{0}}_{6\times 6}& {\mathit{A}}_{N+1}\dots & {\mathbf{0}}_{6\times 6}\\ \dots & \dots & \dots & \dots \\ {\mathbf{0}}_{6\times 6}& \dots & {\mathbf{0}}_{6\times 6}& {\mathbf{0}}_{6\times 6}\dots & {\mathit{A}}_{\mathit{N}+{\mathit{N}}_{\mathit{t}}}\end{array}\right]$$

(10)

$$\mathit{B}=\left[\begin{array}{ccccc}{\mathit{B}}_{\mathbf{1}}& \dots & {\mathbf{0}}_{6\times 3}& {\mathbf{0}}_{6\times 3}\dots & {\mathbf{0}}_{6\times 3}\\ \dots & \dots & \dots & \dots \\ {\mathbf{0}}_{6\times 3}& \dots & {\mathit{B}}_{\mathit{N}}& {\mathbf{0}}_{6\times 3}\dots & {\mathbf{0}}_{n\times n}\\ {\mathbf{0}}_{6\times 3}& \dots & {\mathbf{0}}_{6\times 3}& {\mathbf{0}}_{6\times 3}\dots & {\mathbf{0}}_{6\times 3}\\ \dots & \dots & \dots & \dots \\ {\mathbf{0}}_{6\times 3}& \dots & {\mathbf{0}}_{6\times 3}& {\mathbf{0}}_{6\times 3}\dots & {\mathbf{0}}_{6\times 3}\end{array}\right]$$

(11)

where ${\mathbf{0}}_{n\times p}$ denotes a null matrix with n rows and p columns, ${\mathit{A}}_{\mathit{i}}$ and ${\mathit{B}}_{\mathit{i}}$ are reported in (2), and ${\mathit{A}}_{\mathit{N}+\mathit{j}}$ is in (8).

For each vehicle i, the measurements vector ${\tilde{\mathit{z}}}_i\left(k\right)$, defined by (3), (4), and (5), can be expressed as a function of the global state vector $\mathbf{\Xi }$:

$${\tilde{\mathit{z}}}_i\left(k\right)=\mathit{h}\left(\mathbf{\Xi }\left(k\right)\right)+{\tilde{\mathit{\nu }}}_i\left(k\right)$$

(12)

where $\mathit{h}(·)$ is the measurement function, mapping the global state vector $\mathbf{\Xi }\left(k\right)$ to the expected measurements for any vehicle i, and ${\tilde{\mathit{\nu }}}_i\left(k\right)={[{\tilde{\mathit{\nu }}}_i^{{GPS}^T}\left(k\right),{\tilde{\mathit{\nu }}}_i^{D^T}\left(k\right)]}^T$ represents the measurement noise.

At each time step k, Equation (12) can be linearized as follows:

$${\mathit{z}}_i\left(k\right)={\mathit{H}}_i\mathbf{\Xi }\left(k\right)+{\mathit{\nu }}_i\left(k\right)$$

(13)

where

$$\begin{array}{cc}\hfill {\mathit{z}}_i\left(k\right)=& {\tilde{\mathit{z}}}_i\left(k\right)-{\tilde{\mathit{z}}}_i(k-1)+{\mathit{H}}_i\mathbf{\Xi }\left(k\right)\end{array}$$

(14)

The matrix ${\mathit{H}}_i$ is given by

$${\mathit{H}}_i=\left[\begin{array}{cccccc}{\mathit{C}}_{i,1}& \dots & {\mathit{C}}_{i,N}& {\mathit{C}}_{i,N+1}& \dots & {\mathit{C}}_{i,N+N_t}\end{array}\right]$$

(15)

where the elements of ${\mathit{H}}_i$ are defined as

$${\mathit{C}}_{i,i}\left(k\right)={\left.{\displaystyle \frac{\partial \mathit{h}}{\partial {\mathit{\xi }}_{\mathit{i}}}}\right|}_{\mathbf{\Xi }(k-1)}$$

(16)

and

$${\mathit{C}}_{i,j}\left(k\right)=\left\{\begin{array}{cc}{\left.{\displaystyle \frac{\partial \mathit{h}}{\partial {\mathit{\xi }}_{\mathit{j}}}}\right|}_{\mathbf{\Xi }(k-1)}& \forall j\in {\mathcal{N}}_i\left(k\right)\cup {\mathcal{N}}_i^t\left(k\right)\\ {\mathbf{0}}_{p\times 6}& otherwise\end{array}\right.$$

(17)

3. Consensus on Moving Horizon Estimation

To improve the robustness of the fleet navigation system, a DMHE was introduced, combining the concepts of MHE and consensus theory.

The MHE is an optimization-based strategy that estimates unknown variables or parameters using a series of past measurements [66]. Typically, the MHE is employed as a state observer to estimate, at each time step k, the state dynamics in the time horizon $\{k-n_t,k-n_t+1,\dots ,k\}$ by solving a constrained optimization problem. The objective is the minimization of the discrepancy between the model prediction and the measurements collected in the time window.

Adopting a centralized MHE algorithm, managed by a leader within the fleet or a ground station that gathers measurements from all vehicles, is impractical due to the rapid growth in design variables in the optimization process. Furthermore, if the central agent fails, the fleet would lose critical state information, with the consequent risk of failures in navigation-dependent functions, including guidance systems.

To address these issues, each vehicle is equipped with embedded systems capable of running a local version of MHE, dividing the estimation problem into N smaller optimization tasks. The solutions from each vehicle are then combined using consensus theory.

Consensus theory operates on the premise that a group of agents can reach an agreement on a global function value through local information exchanges. In a typical consensus estimation scenario, each agent estimates the global state and communicates partial measurements or state estimates with its neighbors. Therefore, inter-vehicle communication is crucial for achieving consensus.

The communication network among the collaborative agents can be represented as a sensor node network. At any given time instant k, the communication topology can be modeled as a directed graph $\mathbb{G}\left(k\right)=(\mathcal{A},\mathcal{E}(k\left)\right)$, where $\mathcal{A}=1,2,\dots ,N$ is the set of nodes corresponding to the aircraft and $\mathcal{E}\left(k\right)$ is the set of edges. Each edge $(i,j)\in \mathcal{E}\left(k\right)$ represents a communication link from UAV j to i, determined by their relative positions and communication range; consequently, at time instant k, UAV i can receive information from j only if $(i,j)\in \mathcal{E}\left(k\right)$.

Define ${\mathcal{M}}_i\left(k\right)=\left\{j:(i,j)\in \mathcal{E}\left(k\right)\right\}$ as the set of neighboring vehicles for the i-th agent, and let ${\eta }_i\left(k\right)$ denote the local information available to the i-th aircraft at time k. Convergence is achieved in a finite number of consensus steps L when all agents agree on the same value of ${\eta }_i\left(k\right)$ [67]. At each iteration l, with $l\in \{1,2,\dots ,L\}$, the variables ${\eta }_i^l\left(k\right)$ are computed as follows:

$${\eta }_i^l\left(k\right)=\sum _{j\in \mathcal{A}}{W}_{i,j}\left(k\right){\eta }_j^{l-1}\left(k\right)$$

(18)

where the coefficients $W_{i,j}\left(k\right)$ are computed using the Metropolis formula [68]:

$$W_{i,j}=\left\{\begin{array}{cc}{\displaystyle \frac{1}{1+max\{{\delta }_i,{\delta }_j\}}}\hfill & \mathrm{if}(i,j)\in {\mathcal{M}}_i\left(k\right)\hfill \\ 1-\sum _{p\in {\mathcal{M}}_i\left(k\right)}{W}_{i,p}\hfill & \mathrm{if}i=j\hfill \\ 0\hfill & \mathrm{otherwise}\hfill \end{array}\right.$$

(19)

Here, ${\delta }_i$ and ${\delta }_j$ denote the degrees of nodes i and j, respectively.

For convergence, the communication graph $\mathbb{G}\left(k\right)$ must remain connected at all times k, and at least $L>1$ consensus iterations must be performed.

3.1. Distributed Moving Horizon Estimation

Denote with ${\widehat{\mathbf{\Xi }}}^i\left(k\right|k)$ the estimate of the global state vector performed by i-th UAV at time k using all the information acquired until time k.

The goal of the proposed DMHE is to ensure that each aircraft has a complete situational awareness and, thus, an estimate of the global state vector ${\widehat{\mathbf{\Xi }}}^i\left(p\right|k)$, with $p=k-n_t,\dots ,k$.

In [56], each agent solves an MHE problem at every consensus step and then exchanges information about the estimates with its neighbors according to the consensus protocol.

To reduce the computational burden, each aircraft i does not solve an optimization problem involving all the design variables, but it estimates only the local state vector which includes its own state as well as the position and velocity of visible targets:

$${\mathbf{\Xi }}_{\mathit{i}}\left(k\right)={\left[{\mathit{\xi }}_i{\left(k\right)}^T,\bigcup _{j\in {\mathcal{N}}_i^t\left(k\right)}{\mathit{\xi }}_j{\left(K\right)}^T\right]}^T$$

(20)

The local state vector is derived from the global state vector as follows:

$${\mathbf{\Xi }}_i\left(k\right)={\mathit{T}}_i\left(k\right)\mathbf{\Xi }\left(k\right)$$

(21)

where ${\mathit{T}}_i\left(k\right)$ is used to pick specific states or their linear combinations from the global state vector. The number of columns of ${\mathit{T}}_i\left(k\right)$ depends on the targets visible to the i-th aircraft at time k. This matrix consists of elements equal to 1 or 0, depending on the states to be selected, and varies over time according to ${\mathcal{N}}_i^t\left(k\right)$.

Figure 1 illustrates the communication between two generic nodes of the formation, highlighting the local estimation flow during consensus.

3.2. Local MHE

Consider a sliding window spanning $n_t+1$ time steps into the past. The measurements acquired by the i-th UAV within this sliding window are given by

$${\mathit{Z}}_i\left(k\right)={\left[{\mathit{z}}_i{(k-n_t)}^T,{\mathit{z}}_i{(k-n_t+1)}^T,\dots ,{\mathit{z}}_i{\left(k\right)}^T\right]}^T$$

(22)

At each time step k, for each consensus iteration l, with $l\in \{1,\dots ,L\}$, a local constrained optimization problem is solved to compute an estimate of the state vector ${\mathbf{\Xi }}_i\left(k\right)$, denoted as ${\mathbf{\Xi }}_i^{i,l}\left(k\right)$.

Given a local function $J_i^{k,l}$, defined as

$$\begin{array}{c}\hfill J_i^{k,l}=\sum _{p=k-n_t}^k\left|\right|{{\mathbf{\Xi }}_i}^{i,l}\left(p\right|k)-{\overline{\mathbf{\Xi }}}_i\left(p\right){\left|\right|}_{{\mathit{Q}}_i}^2\\ \hfill +\sum _{p=k-n_t}^k\left|\right|{\mathit{H}}_{\mathit{i}}{{\mathit{T}}_{\mathit{i}}}^{+}\left(p\right){{\mathbf{\Xi }}_{\mathit{i}}}^{i,l}\left(p\right|k)+-{\mathit{Y}}_i\left(m\right){\left|\right|}_{{\mathit{R}}_i}^2\\ \hfill +\parallel {\mathbf{\Xi }}_{\mathit{i}}^{i,l}(k-n_t|k)-{\overline{\mathbf{\Xi }}}_i(k-n_t){\parallel }_{{\mathit{P}}_i}^2\end{array}$$

(23)

with three main components:

• State dynamics: the first term addresses the state dynamics, weighted by a positive semi-definite matrix ${\mathit{Q}}_i$.
• Measurement-model consistency: the second term accounts for the discrepancy between the measured data and the model output, weighted by a positive definite matrix ${\mathit{R}}_i$, which corresponds to the inverse of the measurement noise covariance.
• Arrival cost: the third term penalizes the error between the estimated state ${\mathbf{\Xi }}_i^{i,k}(k-n_t|k)$ and the prediction ${\overline{\mathbf{\Xi }}}_i(k-n_t)$, weighted by a suitable positive definite matrix ${\mathit{P}}_i$, contributing to the arrival cost [50,52,69,70].

The optimization problem is defined as follows:

$$\underset{{\mathbf{\Xi }}_i^{i,l}(k-n_t)}{min}{J}_i^{k,l}$$

(24)

subject to the following constraints:

$${\overline{\mathbf{\Xi }}}_i(p+1)={\mathcal{A}}_i{\overline{\mathbf{\Xi }}}_i\left(p\right)+{\mathcal{B}}_i{\mathit{u}}_i\left(p\right),\forall p=k-n_t,\dots ,k$$

(25)

$${\overline{\mathbf{\Xi }}}_i(k-n_t-1)={\widehat{\mathbf{\Xi }}}_i^i(k-n_t-1|k-1)$$

(26)

$${\mathbf{\Xi }}_i^{i,l}\left(p\right|k)\in {\mathbb{X}}_j,\forall p=k-n_t,k-n_t+1,k,\forall j\in {\mathcal{N}}_i^t\left(k\right)$$

(27)

where the matrix ${\mathcal{A}}_i={\mathit{T}}_{\mathit{i}}\left(p\right)\mathit{A}{\mathit{T}}_{\mathit{i}}{\left(p\right)}^{+}$ is obtained by extracting the rows and columns corresponding to the local state from the global state matrix $\mathit{A}$, whereas ${\mathcal{B}}_i={\mathit{T}}_{\mathit{i}}\left(p\right)\mathit{B}$ is the block matrix of $\mathit{B}$ relative to the state vector ${\mathbf{\Xi }}_{\mathit{i}}$ and the input vector ${\mathit{u}}_i$. ${\mathit{T}}_{\mathit{i}}{\left(p\right)}^{+}$ indicates the pseudoinverse matrix of ${\mathit{T}}_{\mathit{i}}\left(p\right)$.

Dynamics constraints (25) define the dynamics of a priori predicted state ${\overline{\mathbf{\Xi }}}_i\left(p\right)$ in the time window $\{k-n_t-1,\dots ,k\}$.

The initial condition (26) ${\widehat{\mathbf{\Xi }}}_i(k-n_t-1|k-1)$ is obtained by selecting the suitable components of the solution at time step $k-1$, after completing the consensus iterations, as ${\widehat{\mathbf{\Xi }}}_i(k-n_t-1|k-1)={\mathbf{\Xi }}_i^{i,L}(k-n_t-1|k-1)$.

Inequality constraints (27) restrict the possible locations of the targets to be within the sensor range ${\mathcal{S}}_i$.

The optimization problem represents a quadratic programming (QP) problem with linear constraints, which can be solved efficiently using well-established numerical solvers [71].

3.3. Consensus Iteration

After solving the problem defined in Section 3.2, each vehicle computes an updated estimate of the global state vector as follows:

$${\mathbf{\Xi }}^{i,l}(k-n_t|k)={\mathbf{\Xi }}^{i,l-1}(k-n_t|k)-{{\mathit{T}}_{\mathit{i}}}^{+}(k-n_t){\mathbf{\Xi }}_i^{i,l-1}(k-n_t|k)+{{\mathit{T}}_{\mathit{i}}}^{+}(k-n_t){\mathbf{\Xi }}_i^{i,l}(k-n_t|k)$$

(28)

Vehicle i exchanges its vector ${\mathbf{\Xi }}^{i,l}(k-n_t|k)$ with the neighbors $j\in {\mathcal{M}}_i\left(k\right)$ according to the consensus paradigm [72], to achieve the agreement on the global state estimation across all UAVs:

$${\mathbf{\Xi }}^{i,l+1}(k-n_t|k)=\sum _{j=1}^N{W}_{i,j}{\mathbf{\Xi }}^{j,l}(k-n_t|k)$$

(29)

The final estimate of the global state is obtained at the end of the consensus iterations as follows:

$${\widehat{\mathbf{\Xi }}}^i(k-n_t|k)={\mathbf{\Xi }}^{i,L}(k-n_t|k)$$

(30)

The proposed algorithm is detailed in pseudocode in Algorithm 1 and depicted in Figure 2.

Algorithm 1: Decentralized Moving Horizon Estimator (DMHE)

4. Results

In this section, four distinct test cases are illustrated, to evaluate the performance of the proposed algorithm. They involve a swarm of $N=9$ UAVs operating over a designated area of interest.

The first test case was designed to carry out a sensitivity analysis, aiming to investigate how variations in key parameters, such as the number of consensus iterations (L), the number of receding iterations ($n_t$), and different communication link schemes ($\mathcal{E}$), affect the algorithm behavior.

The objective of the second test case was to assess the performance of the algorithm in the presence of communication faults, by analyzing the effects of two distinct communication interruptions among the collaborative agents.

Test cases #3 and #4 were designed to evaluate the performance of the algorithm in estimating the position and the speed of a non-collaborative target.

The numerical simulations were carried out in a Matlab environment, considering a simulation period of $t=250$ s. In every test, at the beginning of the simulation, each aircraft initializes its global state estimate with random values. This assumption was useful for assessing the convergence of the algorithm toward an accurate estimate and its ability to achieve consensus.

Each simulation considers white Gaussian noise affecting GPS and the transponder, as summarized in

Table 1. Sensor biases and measurement noise covariance matrices considered in the simulations.

Description	Value
GPS bias (m)	${[2,2,4]}^T$
Transponder bias (m)	4 $\times {10}^{-2}$
GPS noise covariance (m2)	$\mathrm{diag}({[4\times {10}^{-1},4\times {10}^{-1},1]}^T)$
Transponder noise covariance (m2)	${\mathit{I}}_9\times {10}^{-4}$

.

To assess the performance of the proposed algorithm, two indicators were defined: the standard deviation (SD) and the average error [73]. Assuming that ${\widehat{\mathit{S}}}_j^i\left(k\right)$ indicates the position of the j-th agent estimated by the aircraft i, the estimation error at time instant k is given by

$$e_i^{{\mathit{S}}_j}\left(k\right)=∥{\widehat{\mathit{S}}}_j^i\left(k\right)-{\mathit{S}}_j\left(k\right)∥$$

(31)

The average error ${\mu }_i^{{\mathit{S}}_j}$ in a time window composed by $n_k$ time steps is

$${\mu }_i^{{\mathit{S}}_j}={\displaystyle \frac{1}{n_k}}\sum _{k=1}^{n_k}{e}_i^{{\mathit{S}}_j}\left(k\right)$$

(32)

Similarly, it is possible to define the standard deviation ${\sigma }_i^{{\mathit{S}}_j}$ with respect to the average error as follows:

$${\sigma }_i^{{\mathit{S}}_j}=\sqrt{{\displaystyle \frac{1}{(n_k-1)}}\sum _{k=1}^{n_k}{\left(e_i^{{\mathit{S}}_j}\left(k\right)-{\mu }_i^{{\mathit{S}}_j}\right)}^2}$$

(33)

4.1. Test Case #1

This subsection shows the results of a sensitivity analysis conducted considering a scenario involving a formation of nine quadrotor UAVs, with eight agents flying in the same direction at a constant speed of 2 m/s, while UAV #1 serves as a communication base for the rest of the swarm, remaining in hovering at a fixed point in the operational scenario. During the flight, drones perform several maneuvers, both in the horizontal and the vertical plane, changing the altitude of the drones of the swarm, to assess the quality of the estimates even during dynamic phases.

Table 2. UAVs starting positions.

Agent	$\mathit{X}\left(0\right)$ (m)	$\mathit{Y}\left(0\right)$ (m)	$\mathit{Z}\left(0\right)$ (m)
UAV #1	0	0	20
UAV #2	−20	−20	22
UAV #3	−20	20	22
UAV #4	−40	−40	24
UAV #5	−40	40	24
UAV #6	−60	−60	18
UAV #7	−60	60	18
UAV #8	−80	−80	16
UAV #9	−80	80	16

summarizes the initial position of each drone of the swarm.

This first test case is useful for evaluating how the operational parameters, such as the communication link schemes, the number of consensus steps, and the number of horizon steps, influence the performance of the proposed algorithm.

The sensitivity analysis considers four configurations for the number of consensus steps ($L\in \left(2,3,5,10,20\right)$), three configurations for the number of receding steps ($n_t\in \left(2,3,4\right)$), and two communication schemes (${\mathcal{E}}_2$, ${\mathcal{E}}_4$), characterized by an increasing number of communication links, as illustrated in Figure 3.

The results of the numerical simulations, in terms of the estimation error $e_1^{S_2}$ made by UAV #1 in estimating the trajectory of UAV #2, are presented in Figure 4. Each figure corresponds to a different moving time horizon ($n_t=2$, $n_t=3$, and $n_t=4$) and shows error bars that represent the average error and the standard deviation for each communication scheme $\mathcal{E}$ and number of consensus steps L.

It is worth noting that increasing the size of the moving time horizon does not significantly impact performance, which is already satisfactory for $n_t=2$, given the same controller tuning parameters. At the same time, as expected, the communication scheme ${\mathcal{E}}_2$ requires a higher number of communication steps. However, beyond $L=5$, further improvements in the estimates become negligible.

To choose the best configuration trade-off, an evaluation of the computational load on the CPU is needed. Figure 5 shows the computation time per estimation step for all the previously discussed configurations, varying the estimation window size ($n_t$) and the number of consensus steps (L). From the analysis of the figures, the configuration with $n_t=2$ and $L=5$ emerges as the optimal trade-off. This choice ensures a low estimation error while maintaining a reasonable computational cost, with the CPU load peaking at $85\%$ under a sampling time of 1 s. The results presented in the following sections were obtained using such optimal configuration.

As a representative example, Figure 6 illustrates the estimated trajectories of UAV #1, UAV #2, and UAV #9 as observed from UAV #1, alongside their actual trajectories. It is worth noting that, under the ${\mathcal{E}}_2$ communication scheme, only UAV #2 and UAV #3 communicate directly with UAV #1, as depicted in Figure 3. Nevertheless, UAV #1 successfully estimates the trajectories of both the visible UAV (UAV #2) and the non-visible UAV (UAV #9), achieving a limited estimation error, even in the presence of maneuvers. The scenario includes left and right turning maneuvers in the horizontal plane at $t=40$ s and $t=80$ s, followed by pull-up and pull-down maneuvers in the vertical plane at $t=120$ s and $t=140$ s, each lasting 20 s.

4.2. Test Case #2

The following test case considers the same initial formation configuration used in test case #1 (see Table 2). Here, all UAVs fly along the same direction at a constant speed of 2 m/s, performing a series of maneuvers in the horizontal plane before reaching the destination point, as highlighted in Figure 7.

The primary objective of this test case was to evaluate the ability of the proposed algorithm to recover the estimation of the UAVs trajectories following interruptions in the inter-agents communication. In particular, to emphasize this aspect, this scenario introduces communication link failures between UAVs during flight due to obstacle avoidance at two specific time intervals: the first at $t_1=50$ s and the second at $t_2=100$ s, both with a duration of $\Delta t=10$ s. Figure 8 depicts the communication link scheme during the communication interruption. In particular, when data transmission between UAV #4 and UAV #6, and between UAV #5 and UAV #7 is interrupted, the two subformations are reorganized into two distinct cycle graphs.

Figure 9 shows the estimated coordinates of UAV #8 as computed by UAV #1, compared with the actual trajectory. The estimation error increases during the two designated communication interruptions and effectively decreases once the communication link is restored.

4.3. Test Case #3

Test case #3 was designed to assess the ability of the proposed algorithm to estimate the position of a fixed target placed at $T=(500,-50,0)$ m in the operational scenario. Here, a formation of nine UAVs is considered, with eight UAVs that fly along the same direction at a constant speed of 5 m/s, with different altitudes above the terrain (see Table 2), while UAV #1 is in hovering at a fixed point, serving as a communication base. Every agent can communicate with its neighbors to form a cycle graph, as depicted in Figure 10, and it is equipped with a further transponder to detect the presence of the fixed target.

Table 3. Test case #3: Bias and noise covariance of the additional transponder.

Description	Value
Target transponder bias (m)	4 $\times {10}^{-4}$
Target transponder noise covariance (m2)	${\mathit{I}}_9\times {10}^{-4}$

includes information about the bias and noise covariance of the additional transponder [63].

Figure 11 shows the estimation error of the coordinates of the target, comparing the actual value and the estimates made by UAV #1. Starting from random initial values, the estimation converges to the actual value in less than 10 s. The presence of a limited estimation error is further confirmed by the analysis of

Table 4. Test case #3: Estimation error of the target position. The mean value, $\mu$, and the standard deviation, $\sigma$, are evaluated in the time window 30 s $\le t\le$ 180 s.

${\mathit{\sigma }}_1^{{\mathit{X}}_{\mathit{T}}}\left[\mathbf{m}\right]$	${\mathit{\sigma }}_1^{{\mathit{Y}}_{\mathit{T}}}\left[\mathbf{m}\right]$	${\mathit{\sigma }}_1^{{\mathit{Z}}_{\mathit{T}}}\left[\mathbf{m}\right]$	${\mathit{\mu }}_1^{{\mathit{X}}_{\mathit{T}}}\left[\mathbf{m}\right]$	${\mathit{\mu }}_1^{{\mathit{Y}}_{\mathit{T}}}\left[\mathbf{m}\right]$	${\mathit{\mu }}_1^{{\mathit{Z}}_{\mathit{T}}}\left[\mathbf{m}\right]$
0.2629	0.5985	0.0389	0.0565	0.0897	0.0045

, which shows the average error and the standard deviation of the estimated position.

4.4. Test Case #4

The main aim of test case #4 was to assess the ability of the algorithm to estimate the position of a moving target, initially placed at $T=(500,-50,0)$ m and moving with a velocity vector $V_T=(0.5,0.5,0)$ m/s. The formation is composed of nine UAVs and the initial positions of the agents of the swarm are resumed in Table 2.

Figure 12 depicts the estimation error of the target coordinates as computed by UAV #1. As shown, the estimation converges to the actual value in less than 10 s. However, as shown in Figure 12b, the estimation error grows during the first 100 s and then drops below one meter once the formation moves directly above the target. This behavior is also evident in

Table 5. Test case #4: Estimation error of the target position. The standard deviation, $\mu$, and the standard deviation, $\sigma$, are evaluated in the time window 150 s $\le t\le$ 200 s.

${\mathit{\sigma }}_1^{{\mathit{X}}_{\mathit{T}}}\left[\mathbf{m}\right]$	${\mathit{\sigma }}_1^{{\mathit{Y}}_{\mathit{T}}}\left[\mathbf{m}\right]$	${\mathit{\sigma }}_1^{{\mathit{Z}}_{\mathit{T}}}\left[\mathbf{m}\right]$	${\mathit{\mu }}_1^{{\mathit{X}}_{\mathit{T}}}\left[\mathbf{m}\right]$	${\mathit{\mu }}_1^{{\mathit{Y}}_{\mathit{T}}}\left[\mathbf{m}\right]$	${\mathit{\mu }}_1^{{\mathit{Z}}_{\mathit{T}}}\left[\mathbf{m}\right]$
0.2171	0.5238	0.0355	0.1005	1.3336	0.0367

, which reports the average error and the standard deviation of the error made by UAV #1 in the estimation of the target trajectory.

5. Conclusions

This study presented the results of the development of a DMHE for a formation of UAVs, combining consensus theory and receding horizon estimation techniques. The primary objective was to distribute computational workload across a network of UAVs while minimizing network connectivity requirements. The DMHE was designed to overcome the limitations of traditional Kalman filtering. Additionally, the algorithm was tailored to facilitate the identification of a target position, providing robust situational awareness within a distributed UAV swarm.

The results proved the algorithm’s ability to achieve consensus on the positions of both the UAVs and the target, even under challenging conditions such as communication interruptions. Several tests were carried out to evaluate the performance of the algorithm. A sensitivity analysis of the main configuration parameters showed the ability of the DMHE to successfully estimate the trajectories of UAVs, despite limited communication links, using several configurations in terms of consensus steps and receding horizon size. The results highlight the configuration $n_t=2$ and $L=5$ as an optimal trade-off, balancing estimation accuracy and computational cost.

The DMHE represents an interesting solution for distributed estimation in UAV swarms, offering reliable performance and robustness to communication faults. Its integration into real-world applications could enhance situational awareness and coordination in autonomous systems.