*Correction* ∀ *i*

$$\mathbf{Q}\_{k|k}^{i} = \mathbf{Q}\_{k|k-1}^{i} + N^{i} I\_{k}^{\text{jl}^{\text{i}}\text{ (L)}} \tag{64}$$

$$\mathfrak{q}\_{k|k}^{i} = \mathfrak{q}\_{k|k-1}^{i} + \mathcal{N}^{i} \mathfrak{i}\_{k}^{i|l\text{ (L)}} \tag{65}$$

$$\mathfrak{X}\_{k\vert k}^{i} = \left(\mathbb{Q}\_{k\vert k}^{i}\right)^{-1} \mathfrak{q}\_{k\vert k}^{i} \tag{66}$$

The logical building process of the Scalable DSE Algorithm is shown in Figure 3, together with the relationships with the algorithms from which it is derived.

The algorithm applies to a set of cooperating nodes (vehicle), in the sense that each node actively communicates information to neighboring nodes. The clustering property of the algorithm causes each node to estimate an automatically selected subset of the global state, based on the communication topology.

The mode selection update step can be approached in various ways. A simple way is based on a consensus procedure carried out on a local defined adjacency matrix initialized, in each node, as if the node were an isolated node.

$$a\_{h\mathbf{h}}^{i\_{\parallel}(0)} = 1 \quad \text{if } h = i \text{ ; otherwise } a\_{h\mathbf{k}}^{i\_{\parallel}(0)} = \mathbf{0} \tag{67}$$

$$\text{for } \ell = 1, \dots \text{L } \quad \mathbf{A}^{i(\ell+1)} = \sum\_{j \in \mathcal{R}\_i} \mu\_{ij} \mathbf{A}^{i(\ell)} \tag{68}$$

where *A i* is a locally known adjacence matrix of elements *a i hk*. It is easy to verify that, for a proper number of consensus steps, the diagonal of each local adjacency matrix contains information on the nodes belonging to the same strongly connected sub graph (cluster).

*Sensors* **2020**, *20*, x FOR PEER REVIEW 12 of 21

**Figure 3.** Scalable Distributed State Estimation (DSE) algorithm logical building process.

**Figure 3.** Scalable Distributed State Estimation (DSE) algorithm logical building process. The equations in which the nodal transformation matrices explicitly appear, Equations (51), (52), (56), (57), (62), (63), perform the mode distribution operations. From an implementation perspective these equations, apparently onerous from a computational point of view, in the case of our interest, in which the matrices *T j <sup>k</sup>* make a simple selection of the states, are therefore reduced to rows and columns reordering and deletion operators. The topology of the system, and then the matrices *T j k* varies with time. When node is added to a cluster, Equations (51) and (52) include the initialization to null information pairs for the corresponding states. When a node is excluded from a cluster, Equations (51) and (52) correspond to the elimination of the corresponding rows and columns.

As far as the stability of the solution is concerned, the algorithm is equivalent to the algorithm described by Equations (30)–(35) for *L* → ∞. On the other hand, the results achieved in the field of internodal transformations guarantee the equivalence of the local estimation and centralized state estimation limited to local retained states.

Stability can also be interpreted in a second way. During its evolution over time the system can be divided in clusters each strongly connected. Such clusters, as mentioned, varies over time through the disaggregation of a cluster or the fusion of two clusters. In each time interval in which a strongly connected cluster exists, the algorithm it is locally equivalent to an EFK algorithm, based on HCMCI consensus. Therefore, it is possible to consider locally the stability conditions applicable to that algorithm. These stability condition can be demonstrated in the linear time invariant form [36].

In the particular case that the following assumption are satisfied:


then, it is guaranteed that the estimation error is asymptotical bounded in mean square i.e., lim k→∞ supE n k *x*ˆ *i k*|*k* − *x i k* k 2 o < ∞. The HCMCI formulation ensures convergence to the centralized EKF in each strongly connected cluster.

#### *2.6. sUAV Positon Estimation*

Let us consider a set of N co-operative flying vehicles V = {V1, . . . VN} free to move in a defined airspace of volume V. A vehicle V<sup>I</sup> can be located by means of its position *r* <sup>i</sup> = x I , y<sup>I</sup> , zI in an inertial reference system *I*. Its attitude is defined in terms of Euler's angles **Θ***<sup>i</sup>* of a body reference frame B, with respect to inertial reference frame *I.*

In the proposed filter architecture, the chosen system dynamic model is a purely kinematic model not affected by any kind of model uncertainties. The state model equations for V<sup>I</sup> assumes the following form:

$$\mathbf{r}^{i} = \mathbf{v}^{i} \tag{69}$$

$$\boldsymbol{w}^{i} = \mathbf{M}\_{BI}^{i} \{ \boldsymbol{\Theta}^{i} \}\_{B}^{i} + \mathbf{G}\_{I}^{i} \tag{70}$$

where vector *v i* is the velocity vector in inertial reference system *I*; vector *f i B* is the specific force (force per unit mass) vector expressed in body frame *B*; *G i I* is the specific gravity force vector expressed in the inertial reference frame *I* and *M<sup>i</sup> BI* represents the rotation matrix from *B* to *I*.

We assume each that vehicle is equipped with a sensor suite including three type of sensors:


Using these three types of sensors, each vehicle can locally generate the set of measurement included in one or more of the following equations:

$$y\_{acc}^{i} = \dot{f}\_{B} \tag{71}$$

$$y\_{\text{alrs}}^i = \Theta^i \tag{72}$$

$$\mathbf{y}\_{\mathcal{g}\text{nss}}^{i} = \left[\mathbf{r}^{i}, \mathbf{v}^{i}\right] \tag{73}$$

$$\mathcal{Y}\_{rel}^{i} = \left| \left( \mathbb{R}^{ij}, \psi^{ij}, \Phi^{ij} \right), \ \mathcal{V}\_{\dot{j}} \in \mathcal{F}^{i} \right| \tag{74}$$

More specifically, Equation (71) represent the specific force vector measured by accelerometers. Furthermore, it can be assumed that each vehicle has a local AHRS (Attitude and Heading reference

system) capable of estimating attitude angles, expressed in Equation (72), by properly filtering the measurements coming from an Inertial Measurement Unit (IMU) sensors.

Equation (73) represents GNSS measures of inertial position and velocity. Finally, Equation (74) is representative of the relative position measurement. It is constituted by a triple including relative distance *R*, elevation Φ and azimuth ψ of all vehicles included into to Field of View F *<sup>i</sup>* of the vehicle Vi . In this formulation, asynchronous measurements can occur both due to the different sampling times of the sensors, and for in and out Field of View transitions.

In order to complete the kinematic model, the following equations are added to the system: (69) and (70), to have an autonomous system capable to filter inertial sensors biases [39]:

$$\dot{f}\_B = 0\tag{75}$$

$$
\boldsymbol{\Theta}^{\dot{i}} = \boldsymbol{0} \tag{76}
$$

Equations (75) and (76) are not restricting, due to the high update rates typical of the inertial measurements.

The state ξ *<sup>i</sup>* of vehicle <sup>V</sup><sup>i</sup> is described by:

$$\boldsymbol{\xi}^{i} = \begin{bmatrix} \boldsymbol{r}^{i\top}, \boldsymbol{v}^{i\top}, \boldsymbol{f}\_{\boldsymbol{B}}^{\cdot \top}, \boldsymbol{\Theta}^{\cdot \top} \end{bmatrix}^{\boldsymbol{T}} \tag{77}$$

The overall state ξ of system is

$$\boldsymbol{\xi} = \begin{bmatrix} \boldsymbol{\xi}^{1T}, \boldsymbol{\xi}^{2T}, \dots, \boldsymbol{\xi}^{NT} \end{bmatrix}^{T} \tag{78}$$

Finally, it can be assumed that each vehicle is equipped with a bi-directional communication device whose range defines the connection graph, according to the rule that a vehicle V*<sup>j</sup>* belong to neighbor <sup>A</sup>*<sup>i</sup>* of vehicle <sup>V</sup>*<sup>i</sup>* if its relative distance is less than the communication range.

#### **3. Results**

In order to highlight the most peculiar aspects of the proposed algorithm, which are those related to the decentralization, scalability and self-clustering of the local states in the nodes, the results are reported for a specific simplified, though sufficiently representative, scenario. The fundamental parameters, characteristic of the scenario, are summarized in Table 1. In order to facilitate the readability of the results, Equations (69), (70) are reduced to the two-dimensional case, i.e., two translational degrees of freedom and one angular degree of freedom. The measurements are asynchronous and obtained at different frequencies. The parameter Communication frequency in this context takes on the meaning of frequency of the consensus operation. The execution time of an entire consensus operation, consisting three Consensus steps (*L* = *3*), is neglected.

**Table 1.** Scenario description.


Figure 4 shows the position of the fleet, consisting of five vehicles, in four different time instants. The blue circles around each vehicle indicate the range of the relative position sensors; the red lines connecting two vehicles highlight the presence of an active communication link. The points in red indicate the absence of the GNSS signal, the position covered by the signal is reported in green. *Sensors* **2020**, *20*, x FOR PEER REVIEW 16 of 21

**Figure 4.** Fleet true position (**a**) t = 5 s (**b**) t = 10 s (**c**) t = 25 s (**d**) t = 30 s. **Figure 4.** Fleet true position (**a**) t = 5 s (**b**) t = 10 s (**c**) t = 25 s (**d**) t = 30 s.

The coverage of the GNSS signal is distributed randomly with the only constraint of guaranteeing a ratio between GNSS denied areas and areas covered by the service equal to 50%. Even if it represents a simplified model, this distribution is representative of an urban canyon scenario. The coverage of the GNSS signal is distributed randomly with the only constraint of guaranteeing a ratio between GNSS denied areas and areas covered by the service equal to 50%. Even if it represents a simplified model, this distribution is representative of an urban canyon scenario.

The simulation shows a single cluster at time instant t = 5 s.; two clusters at time t = 10 s (Vehicle 4 remains separate from the others); three clusters at t = 25 s (Vehicles 4 and 2 separated from the others); once again, a unique strongly connected system at the time instant t = 30 s. The simulation shows a single cluster at time instant t = 5 s.; two clusters at time t = 10 s (Vehicle 4 remains separate from the others); three clusters at t = 25 s (Vehicles 4 and 2 separated from the others); once again, a unique strongly connected system at the time instant t = 30 s.

Table 2 summarizes, at the time instants shown in Figure 4, the states propagated from each vehicle. Within each cluster, collective observability is guaranteed by the presence of at least one vehicle with GNSS available [40]. Table 2 summarizes, at the time instants shown in Figure 4, the states propagated from each vehicle. Within each cluster, collective observability is guaranteed by the presence of at least one vehicle with GNSS available [40].


**Table 2.** Model Distribution (estimated vehicle) for each vehicle. **Table 2.** Model Distribution (estimated vehicle) for each vehicle.

1 in bold active GNSS.

Figures 5–7 show the estimates made locally by Vehicle 1. The EKF reference (black line) is obviously reported only in the case of self-estimation. It corresponds to the estimate made by the

Figures 5–7 show the estimates made locally by Vehicle 1. The EKF reference (black line) is obviously reported only in the case of self-estimation. It corresponds to the estimate made by the vehicle using only its own on-board sensors. In cross-estimation, whenever a vehicle is not in the same cluster of the estimating vehicle, the curve representing the estimate is interrupted and then resumed as soon as that vehicle returns to the cluster. which these vehicles cannot communicate with the node making the estimate. In this case, the vehicles cannot share their data to improve their estimates This condition is obviously not a limitation of the algorithm. Similar conclusions can be drawn for the estimates made by the other vehicle. For illustrative purposes only, estimates for Vehicle 4 are reported in Figures 8–10. which these vehicles cannot communicate with the node making the estimate. In this case, the vehicles cannot share their data to improve their estimates This condition is obviously not a limitation of the algorithm. Similar conclusions can be drawn for the estimates made by the other vehicle. For illustrative purposes only, estimates for Vehicle 4 are reported in Figures 8–10.

some vehicles do not share the same cluster. However, this situation represents the case in

some vehicles do not share the same cluster. However, this situation represents the case in

*Sensors* **2020**, *20*, x FOR PEER REVIEW 17 of 21

*Sensors* **2020**, *20*, x FOR PEER REVIEW 17 of 21

vehicle using only its own on-board sensors. In cross-estimation, whenever a vehicle is not in the same cluster of the estimating vehicle, the curve representing the estimate is interrupted and then

vehicle using only its own on-board sensors. In cross-estimation, whenever a vehicle is not in the same cluster of the estimating vehicle, the curve representing the estimate is interrupted and then

• The self-clustering mechanism is evident. Vehicle 2 is disconnected from the estimate around t = 20 s (Figure 6). Vehicle 4 shows two estimation black out windows (Figure 7) around t = 6 sec and around t = 21 s. In both cases, the estimate is correctly recovered when the vehicle rejoins the cluster of the estimating vehicle. When this happens, the transient is adequately managed in the average process of Equations (56) and (57), in

• The self-clustering mechanism is evident. Vehicle 2 is disconnected from the estimate around t = 20 s (Figure 6). Vehicle 4 shows two estimation black out windows (Figure 7) around t = 6 sec and around t = 21 s. In both cases, the estimate is correctly recovered when the vehicle rejoins the cluster of the estimating vehicle. When this happens, the transient is adequately managed in the average process of Equations (56) and (57), in

which the contribution of each vehicle is weighed with its own covariance matrix. • The self-estimation of Vehicle 1 is more accurate than the traditional centralized EKF estimate. Vehicle 1 periodically loses the GNSS signal. This leads to a widespread degradation of the estimate. It is particularly evident around t = 20 s, in which the GNSS denied persists for a longer interval. In the decentralized solution, the observability of the

which the contribution of each vehicle is weighed with its own covariance matrix. • The self-estimation of Vehicle 1 is more accurate than the traditional centralized EKF estimate. Vehicle 1 periodically loses the GNSS signal. This leads to a widespread degradation of the estimate. It is particularly evident around t = 20 s, in which the GNSS denied persists for a longer interval. In the decentralized solution, the observability of the

cluster 1-3-5 is guaranteed by the GNSS signal received by Vehicle 3 (see Figure 4c).

resumed as soon as that vehicle returns to the cluster. Three fundamental behaviors can be observed:

resumed as soon as that vehicle returns to the cluster. Three fundamental behaviors can be observed:

**Figure 5.** Position estimation performed by Vehicle 1 on its own position (**a**) x axis (**b**) y axis. **Figure 5.** Position estimation performed by Vehicle 1 on its own position (**a**) x axis (**b**) y axis. **Figure 5.** Position estimation performed by Vehicle 1 on its own position (**a**) x axis (**b**) y axis.

(**a**) **Figure 6. Figure 6.**  (**a**) Position estimation performed by Vehicle 1 on Vehicle 2 position ( (**a**) Position estimation performed by Vehicle 1 on Vehicle 2 position ( **a**) x axis ( **a**) x axis ( **b**) y axis. **b**) y axis. *Sensors* **2020**, *20*, x FOR PEER REVIEW 18 of 21

**Figure 7.** Position estimation performed by Vehicle 1 on Vehicle 4 position (**a**) x axis (**b**) y axis. **Figure 7.** Position estimation performed by Vehicle 1 on Vehicle 4 position (**a**) x axis (**b**) y axis.

**Figure 8.** Position estimation performed by Vehicle 4 on its own position (**a**) x axis (**b**) y axis.

**Figure 9.** Position estimation performed by Vehicle 4 on Vehicle 1 position (**a**) x axis (**b**) y axis.

(**b**)

(**b**)

(**a**)

(**a**)

Three fundamental behaviors can be observed:


Similar conclusions can be drawn for the estimates made by the other vehicle. For illustrative purposes only, estimates for Vehicle 4 are reported in Figures 8–10. (**a**) **Figure 7.** Position estimation performed by Vehicle 1 on Vehicle 4 position (**a**) x axis (**b**) y axis. **Figure 7.** Position estimation performed by Vehicle 1 on Vehicle 4 position (**a**) x axis (**b**) y axis.

**Figure 8.** Position estimation performed by Vehicle 4 on its own position (**a**) x axis (**b**) y axis. **Figure 8.** Position estimation performed by Vehicle 4 on its own position (**a**) x axis (**b**) y axis. **Figure 8.** Position estimation performed by Vehicle 4 on its own position (**a**) x axis (**b**) y axis.

(**a**) **Figure 9. Figure 9.**  Position estimation performed by Vehicle 4 on Vehicle 1 position ( Position estimation performed by Vehicle 4 on Vehicle 1 position ( **a**) x axis ( **a**) x axis ( **b**) y axis. **b**) y axis.

**Figure 9.** Position estimation performed by Vehicle 4 on Vehicle 1 position (**a**) x axis (**b**) y axis.

**Figure 10.** Position estimation performed by Vehicle 4 on Vehicle 2 position (**a**) x axis (**b**) y axis. **Figure 10.** Position estimation performed by Vehicle 4 on Vehicle 2 position (**a**) x axis (**b**) y axis.

#### **4. Conclusions 4. Conclusions**

The article discussed a solution to the problem of Distributed State Estimation (DSE) in the framework of Kalman Filter-based algorithms. The proposed solution draws from the theoretical results derived from two different methodologies both related to the Kalman Filter theory: Internodal Transformation Theory and Consensus Theory. The article discussed a solution to the problem of Distributed State Estimation (DSE) in the framework of Kalman Filter-based algorithms. The proposed solution draws from the theoretical results derived from two different methodologies both related to the Kalman Filter theory: Internodal Transformation Theory and Consensus Theory.

From the former, the algorithm inherits the scalability property, that is the ability to decompose the global problem into different reduced order problems on a local level. From the latter, it inherits the ability to efficiently distribute information among local sensing and computational nodes. From the former, the algorithm inherits the scalability property, that is the ability to decompose the global problem into different reduced order problems on a local level. From the latter, it inherits the ability to efficiently distribute information among local sensing and computational nodes.

A novel property, deriving from the fusion of the two methodologies, is the self-clustering property of the nodes which aggregate themselves in local estimation sub problems in response to the variation of the communication topology. The aggregation process is performed by each node through the information dynamically transmitted by its neighboring nodes, and is achieved by reaching an agreement resulting from a Consensus-based process. A novel property, deriving from the fusion of the two methodologies, is the self-clustering property of the nodes which aggregate themselves in local estimation sub problems in response to the variation of the communication topology. The aggregation process is performed by each node through the information dynamically transmitted by its neighboring nodes, and is achieved by reaching an agreement resulting from a Consensus-based process.

The proposed algorithm makes it possible to obtain more accurate estimates than those obtainable individually from each node that uses only local measurements. Furthermore, the scalability property reduces the computational burden in each node by means of reducing the size of the local problems, while decentralization improves the communication efficiency, allowing each vehicle to exchange information only to the nearest vehicle. The proposed algorithm makes it possible to obtain more accurate estimates than those obtainable individually from each node that uses only local measurements. Furthermore, the scalability property reduces the computational burden in each node by means of reducing the size of the local problems, while decentralization improves the communication efficiency, allowing each vehicle to exchange information only to the nearest vehicle.

The algorithm has a general validity, but assumes a specific meaning if applied to set of co-operating sUAVs equipped with heterogeneous on-board sensors and limited range communication devices. In this perspective, the algorithm is proposed as a formalization of an intuitive concept in which vehicles flying nearby other co-operative vehicles share its own on-board measurement to enable a better estimate of each vehicle. The algorithm has a general validity, but assumes a specific meaning if applied to set of co-operating sUAVs equipped with heterogeneous on-board sensors and limited range communication devices. In this perspective, the algorithm is proposed as a formalization of an intuitive concept in which vehicles flying nearby other co-operative vehicles share its own on-board measurement to enable a better estimate of each vehicle.

A particularly significant condition is, for example, that in which a vehicle flying in a GNSS-denied zone can use the measurements of its position transmitted from another vehicle (i.e., from a micro radar or from a vision-based system) to correct the estimate regarding its own position. This scenario represents, among others, typical operational conditions of a fleet of sUAVs in a UTM context, in which registered vehicles can perform free flight operations in a potentially highly density airspace in urban environment. The application of the algorithm to a problem that involves the presence of non-collaborating elements, such as intruders, may be subject A particularly significant condition is, for example, that in which a vehicle flying in a GNSS-denied zone can use the measurements of its position transmitted from another vehicle (i.e., from a micro radar or from a vision-based system) to correct the estimate regarding its own position. This scenario represents, among others, typical operational conditions of a fleet of sUAVs in a UTM context, in which registered vehicles can perform free flight operations in a potentially highly density airspace in urban environment. The application of the algorithm to a problem that involves the presence of non-collaborating elements, such as intruders, may be subject to future works.

to future works. The numerical examples shown have no ambitions of a complete numerical validation, but The numerical examples shown have no ambitions of a complete numerical validation, but want to represent a clear example of the benefits that the proposed algorithm can guarantee.

want to represent a clear example of the benefits that the proposed algorithm can guarantee.

read and agreed to the published version of the manuscript.

**Author Contributions**: Conceptualization, methodology, investigation, formal analysis, creation of models, data curation and validation M.C., E.D., I.N. and M.M. Software development, writing, M.C. All authors have **Author Contributions:** Conceptualization, methodology, investigation, formal analysis, creation of models, data curation and validation M.C., E.D., I.N. and M.M. Software development, writing, M.C. All authors have read and agreed to the published version of the manuscript.

**Funding:** This research was funded by REGIONE CAMPANIA, SCAVIR Project "Advanced Configurations Studies for an Innovative Regional Aircraft" CUP B43D18000210007.

**Conflicts of Interest:** The authors declare no conflict of interest.
