<figure><img src="image_1.png" /></figure>

HAL
open science

# Persistent Monitoring of Multiple Moving Targets Using High Order Control Barrier Functions

Lorenzo Balandi, Nicola de Carli, Paolo Robuffo Giordano

► To cite this version:

Lorenzo Balandi, Nicola de Carli, Paolo Robuffo Giordano. Persistent Monitoring of Multiple Moving Targets Using High Order Control Barrier Functions. IEEE Robotics and Automation Letters, 2023, 8 (8), pp.5236-5243. 10.1109/LRA.2023.3293312. hal-04175775

HAL Id: hal-04175775

https://hal.science/hal-04175775

Submitted on 2 Aug 2023

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_2.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Persistent Monitoring of Multiple Moving Targets Using High Order Control Barrier Functions

Lorenzo Balandi, Nicola De Carli, Paolo Robuffo Giordano

*Abstract*—This paper considers the problem of persistently monitoring a set of moving targets using a team of aerial vehicles. Each agent in the network is assumed equipped with a camera with limited range and Field of View (FoV) providing bearing measurements and it implements an Information Consensus Filter (ICF) to estimate the state of the target(s). The ICF can be proven to be uniformly globally exponentially stable under a *Persistency of Excitation* (PE) condition. We then propose a distributed control scheme that allows maintaining a prescribed minimum PE level so as to ensure filter convergence. At the same time, the agents in the group are also allowed to perform additional tasks of interest while maintaining a collective observability of the target(s). In order to enforce satisfaction of the observability constraint, we leverage two main tools: (i) the weighted Observability Gramian with a forgetting factor as a measure of the cumulative acquired information, and (ii) the use of High Order Control Barrier Functions (HOCBF) as a mean to maintain a minimum level of observability for the targets. Simulation results are reported to prove the effectiveness of this approach.

*Index Terms*—Multi-Robot Systems, Localization, Perception-Action Coupling

## I. INTRODUCTION

TARGET tracking is a classical topic in the multi-robot community in which a group of (possibly mobile) sensors needs to cooperatively track the position of a moving target. Each sensor can obtain a measurement of the target and fuse it with the measurements from the other sensors in order to obtain a better estimate [1]–[3]. Mobile sensors can also optimize their position/motion so as to maximize the information collected about the target state [4], [5], thus improving the localization accuracy. This field is often denoted as *active sensing*: the optimization of the sensor motion/placement usually relies on a suitable information metric, which is typically a function of the eigenvalues of an information matrix, e.g., the well-known *Fisher Information Matrix* [6], the *Observability Gramian* (OG) [7], or, alternatively, the covariance matrix [4], [5].

In this work, differently from many previous works on this subject (e.g., [4], [5]), we consider a situation in which the target localization is not the only task for the robot group. This motivates to only enforce maintenance of a *minimum level of persistency of excitation* for localizing the moving target(s) so that the group can exploit its redundancy for realizing additional tasks of interest. Our main contribution is therefore a new formulation of the active sensing problem which, in our

opinion, is more appropriate w.r.t. the typical active sensing approaches, especially when the active sensing task needs to be combined with other tasks of interest. Indeed, most previous works on this subject define the active sensing task as an optimization problem aimed at *maximizing* the collected information. We instead only require maintenance of a *minimum level of information* so as to ensure a proper convergence of the filter used for estimating the target state.

In this work, we leverage *Control Barrier Functions* (CBFs) to devise a distributed algorithm for tackling the active sensing problem. CBFs are by now an established and powerful tool for ensuring constraint satisfaction (e.g., enforcing the state to remain inside a prescribed set) while optimizing performance in nonlinear control problems [8]. CBFs have been employed for multi-agent systems, e.g., for collision avoidance [9], connectivity maintenance [10], and temporal logic tasks [11]. When the constraint is imposed on a function of the state whose first derivative does not explicitly depend on the inputs (as in our case), imposing invariance of the prescribed set becomes more involved. Some solutions have been proposed [12], [13], among which, in particular, High Order Control Barrier Functions (HOCBFs) [13], upon which the following developments are also based.

The setting considered in this work consists of a group of drone UAVs that need to localize (multiple) moving target(s) from camera measurements with range and FoV limits, while also possibly accomplishing other tasks. The ICF [2] is used as estimation algorithm for estimating the target states. We also adapt the ideas in [14], which studied *single robot localization* from bearing measurements w.r.t. beacons with known position. In this work we show that, under mild assumptions and a suitable PE condition, the ICF stability proof in [3] for a linear system is also valid in our case (which involves instead a nonlinear measurement equation). Our main contribution is then the design of a distributed control for the multi-UAV system based on HOCBFs able to guarantee a minimum level of PE (which is necessary for the ICF convergence) while also allowing the execution of other tasks of interest. The rest of the paper is structured as follows: in Section II, the considered system dynamics, multi-robot interaction and sensor model are introduced. The employed ICF is presented in Section III. In Section IV, we introduce the two main tools used, i.e. the HOCBFs and the OG, which are then combined in Section V in order to maintain the system observability. The results presented in Section VI validate the approach, and conclusions are drawn in Section VII.

This work was supported by the ANR-20-CHIA-0017 project "MULTI-SHARED".

N. De Carli and P. Robuffo Giordano are with CNRS, Univ Rennes, Inria, IRISA, Rennes, France. {nicola.de-carli, prg}@irisa.fr

L. Balandi is with University of Bologna, Bologna, Italy. lorenzo.balandi@studio.unibo.it

Digital Object Identifier (DOI): 10.1109/LRA.2023.3293312
## II. MODELING

We consider a group of $N$ drones that needs to localize $M$ possibly moving target robots. The drones are assumed localized among themselves in a common frame, while the targets need to be localized by the drones using relative measurements from onboard sensors. For example, the targets could be a set of ground robots unable to autonomously localize themselves because of limited sensing, cluttered environment, and so forth. Each drone can only communicate with its neighbors according to a fixed, undirected and connected communication graph $G = (V, E)$, where $V$ is the set of nodes and $E \subseteq V \times V$ is the edge set. The set of neighbors of the $i$-th robot is denoted as usual with $N_i \triangleq \{j \in V : (i, j) \in E\}$. Since the communication is bidirectional $(i, j) \in E \iff (j, i) \in E$. The $i$-th drone is modeled as a single integrator with position $\mathbf{p}_i \in \mathbb{R}^3$ and velocity input $\mathbf{u}_i \in \mathbb{R}^3$ such that

$$ \dot{\mathbf{p}}_i = \mathbf{u}_i \quad i = 1, \dots, N. \qquad (1) $$

In the following, we will also refer to the aggregate drones positions as $\mathbf{p} = [\mathbf{p}_1^T \dots \mathbf{p}_N^T]^T$ and analogously for the inputs $\mathbf{u} = [\mathbf{u}_1^T \dots \mathbf{u}_N^T]^T$. The $r$-th target position and velocity are indicated respectively as $\mathbf{p}_r^T$ and $\mathbf{v}_r^T$, and their motion model need not to be known by the drones.

Each drone is assumed equipped with a down-looking onboard camera that can acquire a relative bearing measurement

$$ \beta_{ir} = \frac{\mathbf{p}_{ir}}{d_{ir}} \in \mathbb{S}^2, \qquad (2) $$

with respect to the target(s) in visibility, where $\mathbf{p}_{ir} = \mathbf{p}_r^T - \mathbf{p}_i$ and $d_{ir} = \|\mathbf{p}_r^T - \mathbf{p}_i\|$. The bearing is obtained by projecting the noisy image plane point $\bar{\mathbf{p}}_{ir} = [\bar{x}_{ir} \ \bar{y}_{ir} \ 1]^T$ onto the unit sphere. A bearing measurement $\beta_{ir}$ with respect to the $r$-th target is considered available if the $r$-th target is within a certain range w.r.t. the $i$-th drone, i.e. $D_m < d_{ir} < D_M$, and inside the FoV of the $i$-th drone, i.e. $-\bar{x}_M \le \bar{x}_{ir} \le \bar{x}_M$ and $-\bar{y}_M \le \bar{y}_{ir} \le \bar{y}_M$, where $\bar{x}_M$ and $\bar{y}_M$ the FoV limits (see Fig. 1).

<figure><img src="image_3.png" /><figcaption>Figure 1: The bearing $\beta_{ir}$ is the unit vector from the drone to the target. In the figure a camera with limited FoV is pointing in the negative $z$ direction, represented by the unit vector $\mathbf{e}_3$.</figcaption></figure>

## III. INFORMATION CONSENSUS FILTER

We base our estimation strategy for the target state on the ICF originally presented in [2], for which only collective observability of the target(s) is required [3] (i.e., each robot is not required to individually measure the target). We will show that by linearizing the output equation by output injection it

is possible to obtain a globally uniformly exponentially stable observer under suitable PE conditions.

Since we do not assume knowledge of the target motion model, a simple constant velocity model is used for estimation purposes:

$$ \mathbf{x}_r^T(k+1) = \mathbf{A}_d \mathbf{x}_r^T(k) + \gamma(k) \qquad (3) $$

where $\mathbf{x}_r^T = [\mathbf{p}_r^T \ \mathbf{v}_r^T]^T$ and $\mathbf{A}_d = \begin{bmatrix} \mathbf{I} & \Delta T \mathbf{I} \\ 0 & \mathbf{I} \end{bmatrix}$, with $\Delta T$ being the discretization step, while $\gamma(k) \in \mathcal{N}(0, \mathbf{Q})$ is gaussian process noise with a positive definite covariance matrix $\mathbf{Q}$. The observer is not directly built on the expression (2) as output function but, instead, as done also in other works [14], we consider an output equation linearized via output injection:

$$ \mathbf{z}_{ir}(k) = \mathbf{\Pi}_{\beta_{ir}}(k)\mathbf{p}_r^T(k). \qquad (4) $$

Note that $\mathbf{\Pi}_{\beta_{ir}} := \mathbf{I}_3 - \beta_{ir}\beta_{ir}^T$ is an orthogonal projector, i.e. $\mathbf{\Pi}_{\beta_{ir}}\mathbf{\Pi}_{\beta_{ir}} = \mathbf{\Pi}_{\beta_{ir}}$ and $\mathbf{\Pi}_{\beta_{ir}} = \mathbf{\Pi}_{\beta_{ir}}^T$ (these properties will be used later). Also, although (4) is nonlinear, it only depends on a measured (nonlinear) function of the state (the bearing $\beta_{ir}$). As in [14], the confidence gain $\mathbf{B}_{ir}$ associated to each measurement is simply taken as $\mathbf{B}_{ir} = b\mathbf{I}$, with $b > 0$ being a tunable gain. This is possible because, from the perspective of the filter stability, this matrix is simply required to be positive semi-definite.

For the sake of space, we do not report here the full ICF algorithm but only mention the main steps, for more information the reader is referred to [2]. The algorithm is implemented as follows: 1) a new measurement is obtained; 2) the local information matrix $\mathbf{V}_{ir}^0$ and the information vector $\mathbf{v}_{ir}^0$ are obtained from the prior information matrix $\mathbf{W}_{ir}^-$ and the local measurement information; 3) $K_c$ rounds of average consensus on the information matrix and the information vector are performed; 4) the updated state estimate $\hat{\mathbf{x}}_{ir}^{\tau+} = (\mathbf{V}_{ir}^{K_c})^{\dagger}\mathbf{v}_{ir}^{K_c}$, with $\dagger$ indicating the Moore-Penrose pseudo-inverse, and information matrix $\mathbf{W}_{ir}^+ = N\mathbf{V}_{ir}^{K_c}$ are obtained and, finally; 5) the usual prediction step is performed.

The use of the linear time-varying expression (4) in the ICF requires some care. First, thanks to the orthogonal projector properties, one has

$$ \mathbf{\Pi}_{\beta_{ir}}(k) (\mathbf{p}_r^T(k) - \mathbf{p}_i(k)) = 0 \qquad (5) \\ \Rightarrow \quad \mathbf{z}_{ir}(k) = \mathbf{\Pi}_{\beta_{ir}}(k)\mathbf{p}_r^T(k) = \mathbf{\Pi}_{\beta_{ir}}(k)\mathbf{p}_i(k). $$

The update step of the original ICF [2], by using (5), can then be modified for our case as

$$ \begin{aligned} \mathbf{v}_{ir}^0 &= \frac{1}{N} \mathbf{W}_{ir}^{-}(k) \hat{\mathbf{x}}_{ir}^{-}(k) + b \begin{bmatrix} \mathbf{\Pi}_{\beta_{ir}}(k) \\ \mathbf{0} \end{bmatrix} \mathbf{z}_{ir}(k) \\ &= \frac{1}{N} \mathbf{W}_{ir}^{-}(k) \hat{\mathbf{x}}_{ir}^{-}(k) + b \begin{bmatrix} \mathbf{\Pi}_{\beta_{ir}}(k) \\ \mathbf{0} \end{bmatrix} \mathbf{\Pi}_{\beta_{ir}}(k) \mathbf{p}_i(k) \qquad (6) \\ &= \frac{1}{N} \mathbf{W}_{ir}^{-}(k) \hat{\mathbf{x}}_{ir}^{-}(k) + b \begin{bmatrix} \mathbf{\Pi}_{\beta_{ir}}(k) \\ \mathbf{0} \end{bmatrix} \mathbf{p}_i(k) \end{aligned} $$

where $\hat{\mathbf{x}}_{ir}^{-}(k)$ is the prior state estimate. Note that the final expression in (6) only depends on known quantities, while $\mathbf{z}_{ir}(k)$ in (4) does not (thus showing the advantage of formulation (6)). For completeness, we also add the expression for $\mathbf{V}_{ir}^0$:

$$ \mathbf{V}_{ir}^0 = \frac{1}{N} \mathbf{W}_{ir}^{-}(k) + b \begin{bmatrix} \mathbf{\Pi}_{\beta_{ir}}(k) & \mathbf{0} \\ \mathbf{0} & \mathbf{0} \end{bmatrix}. \qquad (7) $$

The stability of the employed filter can then be shown under the following assumptions (that will be discussed hereafter):
**Definition 1.** A matrix function $\mathbf{P}(k) \succeq 0 \in \mathbb{R}^{n \times n}$ is called persistently exciting (PE) if there exists $K, \mu > 0$ such that for all $k_1 \ge 0$

$$ \frac{1}{K} \sum_{k=k_1}^{k_1+K} \mathbf{P}(k) \succeq \mu \mathbf{I} \qquad (8) $$

where $C \succeq D$ means that $C - D$ is positive semi-definite. The definition of PE in continuous-time is analogous.

**Assumption 1.** No collision drone-target occurs, so that the bearing measurements are always well-defined.

**Assumption 2.** The target state is collectively observable, i.e., the discrete-time Observability Gramian is full-rank $\frac{1}{K} \sum_{k=0}^{K} \sum_{i=1}^{N} (\mathbf{A}_d^k)^T [\mathbf{\Pi}_{\beta_{ir}}(k) \quad \mathbf{0}] \mathbf{A}_d^k \succeq \mu_1 \mathbf{I}$ for some $\mu_1 > 0$, which reduces to $\frac{1}{K} \sum_{k=0}^{K} \sum_{i=1}^{N} \mathbf{\Pi}_{\beta_{ir}}(k) \succeq \mu_2 \mathbf{I}$ for some $\mu_2 > 0$.

**Remark 1.** Assumption 2 is satisfied if there exists either a single persistently exciting direction $\beta_{ir}$ or at least two non-collinear directions $\beta_{ir}$ and $\beta_{jr}$ [14].

**Assumption 3.** The information matrix is initialized so that $\mathbf{W}_{ir}(0) \succeq \mu_3 \mathbf{I}$ for some $\mu_3 > 0$, $i = 1, \dots, N$.

Under these assumptions, the proof provided in [3] still holds, stating that the weighted squared error vector, whose i-th component is $(\mathbf{L}_r(k))_i = \mathbf{e}_{ir}(k)^T \mathbf{W}_{ir}^{-}(k) \mathbf{e}_{ir}(k)$, with $\mathbf{e}_{ir}(k) = \mathbf{x}_r^T(k) - \dot{\mathbf{x}}_{ir}^{-}$, converges to zero exponentially fast for the nominal (i.e. noise free) system. Then since, owing to Assumptions 2 and 3, the information matrices $\mathbf{W}_{ir}^{-}$ are bounded from below and from above, it follows that also the estimation error converges to zero exponentially fast, and thus the observer is globally uniformly exponentially stable. We note that, in general, the real velocity of the target will not be constant but, since the observer dynamics are Lipschitz continuous w.r.t. the perturbation and the observer is globally uniformly exponentially stable, the observer is input-to-state stable with respect to perturbations in the velocity dynamics, hence bounded accelerations will only cause bounded estimation errors (as expected and desired).

**Remark 2.** We point out that, for a generic double integrator system with unknown acceleration, the state is unobservable from a single bearing measurement. Hence, for the filter to have acceptable performance, one of the two following assumptions needs to be verified: 1) the target has a small acceleration so that the ultimate bound of the error system is small, or 2) at least two drones are measuring non collinear bearings w.r.t. the target, so that the position of the target is effectively measured and the target state is observable.

We finally comment about the three Assumptions 1–3: Assumption 1 can be trivially met by adding a constraint on the UAV/target minimum distance (as done in the case study of this work); Assumption 2 is a Persistency of Excitation (PE) condition that will be actually enforced at runtime by the algorithm proposed in the following sections; Assumption 3 is only an initialization condition.

IV. INFORMATION MEASURES AND HOCBFS

In this section, we review the notions needed to develop the main contribution of this paper, that is, a distributed algorithm that guarantees satisfaction of the PE condition necessary for the filter convergence (i.e., Assumption 2). This is obtained by considering 1) the minimum eigenvalue of the Observability Gramian (OG) [7] with a forgetting factor as information measure (E-criterion) [15], and 2) High Order Control Barrier Functions (HOCBFs) [13] for ensuring that the minimum eigenvalue of the OG always remains over a certain threshold. To make the drones aware about their sensing limits, the information acquired is weighted by suitable weights that decrease approaching the sensing limits.

A. The Information Measure

The OG is a tool used to study the observability of linear time-varying systems as well as the local observability of nonlinear systems. This matrix also found applications in the field of active sensing, where it is used in order to quantify the acquired information [7], [16]. The OG representing the information acquired about the target position until time $t$, indicated as $\mathbf{G}_r(t) \in \mathbb{R}^{3 \times 3}$, can be expressed as:

$$ \mathbf{G}_r(t) = \mathbf{G}_r(t_0) + \int_{t_0}^{t} \sum_{i=1}^{N} \mathbf{\Pi}_{\beta_{ir}}^{T} \mathbf{\Pi}_{\beta_{ir}} \, d\tau = \mathbf{G}_r(t_0) + \int_{t_0}^{t} \sum_{i=1}^{N} \mathbf{\Pi}_{\beta_{ir}} \, d\tau \qquad (9) $$

where we used the orthogonal projector properties. Notice that this matrix is the continuous time analogous of the matrix appearing in Assumption 2. The OG is a positive semi-definite matrix and it is invertible if and only if the position of the target is observable along the trajectory. The minimum eigenvalue $\lambda_{1r}$ of the OG can then be taken as an observability metric: it quantifies how far is the target position from being unobservable [7].

As the integrand of the OG is a positive semi-definite matrix, the OG is monotonically increasing in time. Since in this work we are only concerned about maintaining $\lambda_{1r}$ above a minimum threshold, we introduce a forgetting factor in the OG dynamics that makes the information exponentially decaying in absence of new measurements. Also, in order to take into account the sensing limits of the drones, we introduce weights on the information acquired at time $t$. The dynamics of the weighted OG with forgetting factor can then be written as:

$$ \dot{\mathbf{G}}_r = -\rho \mathbf{G}_r + b \sum_{i=1}^{N} w_{ir} \mathbf{\Pi}_{\beta_{ir}} \qquad (10) $$

where $\rho > 0$ is the forgetting factor, $w_{ir} \in [0, 1]$ is a differentiable weight function whose expression will be defined later on, used to encode sensing constraints in the information dynamics, and, as before, $b$ is the information gain associated to the measurement. Note that the OG first derivative only depends on the drone positions and not on their velocities. Hence, any function of the entries of the OG would have relative degree 2 w.r.t. the system inputs.
## B. Perception Awareness

As mentioned in the previous section, the drone sensing limitations are taken into account by weighting the OG. In (10), $w_{ir}$ is a scalar differentiable quantity used by the i-th drone to weight the information acquired about the r-th target. The weight $w_{ir}$ smoothly varies from 1, inside the sensing limit region, to 0, outside the sensing limit region. The information artificially decreases in case the target approaches the maximum sensing range or angle of the FoV. The weight is defined as $w_{ir} = w_{Dir}w_{\beta ir}$, with

$$ w_{Dir} = \begin{cases} \frac{-(\hat{d}_{ir} - D_m^{th})^2}{\sigma_{D_m}^2}, & \text{if } \hat{d}_{ir} < D_m^{th} \\ e^{-\frac{(\hat{d}_{ir} - D_m^{th})^2}{\sigma_{D_m}^2}}, & \text{if } D_m^{th} \le \hat{d}_{ir} \le D_M^{th} \\ e^{-\frac{(\hat{d}_{ir} - D_M^{th})^2}{\sigma_{D_M}^2}}, & \text{if } \hat{d}_{ir} > D_M^{th} \end{cases} \quad (11) $$ $$ w_{\beta ir} = \begin{cases} \frac{-(c_{ir} - \cos(\alpha_M^{th}))^2}{\sigma_{\beta}^2}, & \text{if } c_{ir} < \cos(\alpha_M^{th}) \\ e^{-\frac{(c_{ir} - \cos(\alpha_M^{th}))^2}{\sigma_{\beta}^2}}, & \text{if } c_{ir} \ge \cos(\alpha_M^{th}) \end{cases} $$

where $\hat{d}_{ir} = \|\hat{\mathbf{p}}_{ir}^{\tau+} - \mathbf{p}_i\|$ is the estimated distance from the i-th robot to the r-th target, $D_m^{th}$, $D_M^{th}$, $\alpha_M^{th}$ are parameters that represent respectively the distance and FoV angle at which the weight start to decrease, $\sigma_{D_M}$, $\sigma_{D_m}$, $\sigma_{\beta}$ are standard deviations, $c_{ir}$ is the cosine of the angle between the bearing and the camera optical axis.

As shown in Fig. 2, the weights $w_{Dir}$ and $w_{\beta ir}$ do not (purposely) vanish to zero when approaching the sensing limits. The idea being to allow the drones temporarily losing a measurement when enough information is available. The non-zero gradient of the weights can then be exploited by the drones to possibly move back towards the targets for reacquiring information. This choice also allows drones which are not currently acquiring measurements to use the group-level knowledge about the target position, and their own weight gradient, to obtain a measurement in the future.

<figure><img src="image_4.png" /><figcaption>Figure 2: Perception awareness weights for values $D_m = 0.5$ m, $D_m^{th} = 1$ m, $D_M = 5$ m, $D_M^{th} = 4$ m, $\sigma_{D_M}^2 = 0.5m^2$, $\sigma_{D_m}^2 = 0.1m^2$, $\alpha_M = 50$ deg, $\alpha_M^{th} = 40$ deg, $\sigma_{\beta}^2 = 0.01 rad^2$.</figcaption></figure>

## C. High Order Control Barrier Functions

We briefly introduce some basic concepts about the HOCBFs, see [13] for a more detailed discussion. Consider a generic nonlinear system in control affine form

$$ \dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}) + \mathbf{g}(\mathbf{x})\mathbf{u} \quad (12) $$

with state $\mathbf{x} \in \mathcal{D} \subset \mathbb{R}^n$, input $\mathbf{u} \in \mathcal{U} \subset \mathbb{R}^m$ and $\mathbf{f}$ and $\mathbf{g}$ locally Lipschitz.

**Definition 2.** A continuous function $\alpha : (-b, a) \to (-\infty, \infty)$ is an extended class $\mathcal{K}$ function if it is strictly increasing and $\alpha(0) = 0$.

Given a $\varrho$-th order differentiable function $h : \mathcal{D} \subset \mathbb{R}^n \to \mathbb{R}$ and sufficiently smooth extended class $\mathcal{K}$ functions $\alpha_1, \alpha_2, \dots, \alpha_{\varrho}$, one can define a series of functions

$$ \psi_0(\mathbf{x}) = h(\mathbf{x}), \quad \psi_k(\mathbf{x}) = \left(\frac{d}{dt} + \alpha_k\right) \psi_{k-1}, \quad 1 \le k \le \varrho \quad (13) $$

with the corresponding sets $C_{k-1} = \{\mathbf{x} : \psi_{k-1}(\mathbf{x}) \ge 0\}$. Function $h(\mathbf{x})$ is an HOCBF of order $\varrho$ for (12) if there exist $\varrho$ extended class $\mathcal{K}$ functions $\alpha_1, \dots, \alpha_{\varrho}$ and an open set $\mathcal{D}$ with $C := \cap_{k=1}^{\varrho} C_{k-1} \subset \mathcal{D}$ such that

$$ \sup_{\mathbf{u} \in \mathcal{U}} [L_f \psi_{\varrho-1}(\mathbf{x}) + L_g \psi_{\varrho-1}(\mathbf{x})\mathbf{u} + \alpha_{\varrho} (\psi_{\varrho-1}(\mathbf{x}))] \ge 0. \quad (14) $$

with $L_f \psi_{\varrho-1}(\mathbf{x}) = \frac{\partial \psi_{\varrho-1}}{\partial \mathbf{x}} \mathbf{f}(\mathbf{x})$ and $L_g \psi_{\varrho-1}(\mathbf{x}) = \frac{\partial \psi_{\varrho-1}}{\partial \mathbf{x}} \mathbf{g}(\mathbf{x})$ being the Lie derivatives of $\psi_{\varrho-1}(\mathbf{x})$ for the system (12). This definition generalizes to higher relative degrees the classical CBF definition [8] that only applies to constraints of relative degree $\varrho = 1$. Then, as usual, given a Lipschitz continuous desired input $\mathbf{u}^d$, one can define a minimally invasive controller by solving the following Quadratic Program (QP):

$$ \begin{array}{ll} \min_{\mathbf{u} \in \mathcal{U}} & \frac{1}{2} \|\mathbf{u} - \mathbf{u}^d\|_2^2 \\ \text{s.t.} & L_f \psi_{\varrho-1}(\mathbf{x}) + L_g \psi_{\varrho-1}(\mathbf{x})\mathbf{u} + \alpha_{\varrho} (\psi_{\varrho-1}(\mathbf{x})) \ge 0. \end{array} \quad (15) $$

## V. PERSISTENT MONITORING

In this section, we show how to combine the HOCBFs and the weighted OG with forgetting factor in order to achieve the persistent target monitoring task. We start by formulating the general problem in a centralized form and we then discuss how it can be solved in a distributed way.

### A. Centralized Formulation

As previously mentioned, the goal is to enforce validity of Assumption 2. This can be obtained by ensuring that the minimum eigenvalue $\lambda_{1r}$ of the OG remains over a prescribed threshold. For this purpose, we define the safe set as

$$ C_0^r = \{\zeta \in \mathbb{R}^{3N+(3+9)} : h_r(\zeta) = \lambda_{1r}(\zeta) - \epsilon_{\psi} \ge 0\} \quad (16) $$

with $\epsilon_{\psi} > 0$. The state $\zeta$ consists of the stack of the drone positions, of the target position and of the vectorized OG, i.e.,

$$ \zeta = [\mathbf{p}^T \quad (\mathbf{p}_r^T)^T \quad \text{vec}(\mathbf{G}_r)^T]^T \quad (17) $$

where $\text{vec}(\cdot)$ is the vectorization operator and $\mathbf{p}$ aggregates all the drones positions. The corresponding system dynamics are

$$ \dot{\zeta}(t) = \mathbf{f}(\zeta, t) + \mathbf{g}(\zeta)\mathbf{u} \quad (18) $$

with

$$ \mathbf{f}(\zeta, t) = \begin{bmatrix} \mathbf{0}_{3N} \\ \mathbf{v}_r^T(t) \\ \text{vec}(-\rho \mathbf{G}_r + b \sum_{i=1}^N w_{ir} \mathbf{\Pi}_{\beta ir}) \end{bmatrix} \quad (19) $$

where, with an abuse of notation, we only indicated the direct time dependency, and

$$ \mathbf{g}(\zeta) = [(\mathbf{1}_N \otimes \mathbf{I}_3)^T \quad \mathbf{0}_{3 \times 3N}^T \quad \mathbf{0}_{9 \times 3N}^T]^T. \quad (20) $$
where $\otimes$ represents the Kronecker product and $1_N \in \mathbb{R}^N$ denotes the all-ones vector.

As previously mentioned, a function of the entries of the OG (such as $h_r(\zeta)$ in our case) has relative degree $\varrho = 2$ w.r.t. the system input $\mathbf{u}$, hence the need to resort the HOCBFs. In our case,

$$ 
\begin{aligned}
\psi_{0r}(\zeta) &= \lambda_{1r}(\zeta) - \epsilon_{\psi} \\
\psi_{1r}(\zeta) &= \frac{\partial \lambda_{1r}(\zeta)}{\partial(\mathrm{vec}(\mathbf{G}_r))} \mathrm{vec}(\dot{\mathbf{G}}_r) + \alpha_1^{\psi}(\lambda_{1r}(\zeta) - \epsilon_{\psi})
\end{aligned}
\quad (21) $$

where we used a linear function as extended class $\mathcal{K}$ with $\alpha_1^{\psi} > 0$ and $\frac{\partial \lambda_{1r}}{\partial(\mathrm{vec}(\mathbf{G}_r))} = \mathbf{v}_{1r}^T \otimes \mathbf{v}_{1r}^T$, with $\mathbf{v}_{1r}$ being the eigenvector of the OG associated to $\lambda_{1r}$.

The centralized QP that needs to be solved is

$$ 
\begin{aligned}
\min_{\mathbf{u} \in \mathcal{U}} & \quad \frac{1}{2} \|\mathbf{u} - \mathbf{u}^d\|_2^2 \\
\text{s.t.} & \quad \sum_{i=1}^{N} L_{gi} \psi_{1r}(\zeta) \mathbf{u}_i + L_f \psi_{1r}(\zeta) + \alpha_2^{\psi} \psi_{1r}(\zeta) \ge 0
\end{aligned}
\quad (22) $$

where, again, we chose a linear function as extended class $\mathcal{K}$ with $\alpha_2^{\psi} > 0$. Also,

$$ L_{gi} \psi_{1r}(\zeta) = b \mathbf{v}_{1r}^T \otimes \mathbf{v}_{1r}^T \left( \mathrm{vec}(\mathbf{\Pi}_{\beta_{ir}}) \frac{\partial w_{ir}}{\partial \mathbf{p}_i} + w_{ir} \frac{\partial \mathrm{vec}(\mathbf{\Pi}_{\beta_{ir}})}{\partial \mathbf{p}_i} \right) \quad (23) $$

and

$$ 
\begin{aligned}
L_f \psi_{1r}(\zeta) &= (\mathrm{vec}(\dot{\mathbf{G}}_r)^T \mathbf{H}_{\lambda r} + (\alpha_1^{\psi} - \rho) \mathbf{v}_{1r}^T \otimes \mathbf{v}_{1r}^T) \mathrm{vec}(\dot{\mathbf{G}}_r) \\
&\quad + b \mathbf{v}_{1r}^T \otimes \mathbf{v}_{1r}^T \sum_{i=1}^{N} \left( \mathrm{vec}(\mathbf{\Pi}_{\beta_{ir}}) \frac{\partial w_{ir}}{\partial \mathbf{p}_r^{\tau}} + w_{ir} \frac{\partial \mathrm{vec}(\mathbf{\Pi}_{\beta_{ir}})}{\partial \mathbf{p}_r^{\tau}} \right) \mathbf{v}_r^{\tau} \\
&= c_r(\zeta) + \sum_{i=1}^{N} d_{ir}(\zeta)
\end{aligned}
\quad (24) $$

where

$$ \mathbf{H}_{\lambda r} := \frac{\partial^2 \lambda_{1r}}{\partial \mathrm{vec}(\mathbf{G}_r) \mathrm{vec}(\mathbf{G}_r)^T} = \mathbf{K}_3 \left( \mathbf{Y}_{1r}^{\dagger} \otimes \mathbf{v}_{1r} \mathbf{v}_{1r}^T + \mathbf{v}_{1r} \mathbf{v}_{1r}^T \otimes \mathbf{Y}_{1r}^{\dagger} \right) \quad (25) $$

$\mathbf{Y}_{1r} := \lambda_{1r} \mathbf{I} - \mathbf{G}_r$ and $\mathbf{K}_3$ being the commutation matrix [17]. Also, we split $L_f \psi_{1r}(\zeta)$ in the separable part $\sum_{i=1}^N d_{ir}(\zeta)$ and the non-separable one $c_r(\zeta)$ (the reason will be clearer later), defined as follow:

$$ c_r(\zeta) := (\mathrm{vec}(\dot{\mathbf{G}}_r)^T \mathbf{H}_{\lambda r} + (\alpha_1^{\psi} - \rho) \mathbf{v}_{1r}^T \otimes \mathbf{v}_{1r}^T) \mathrm{vec}(\dot{\mathbf{G}}_r) \quad (26) $$

and

$$ d_{ir}(\zeta) := b \mathbf{v}_{1r}^T \otimes \mathbf{v}_{1r}^T \left( \mathrm{vec}(\mathbf{\Pi}_{\beta_{ir}}) \frac{\partial w_{ir}}{\partial \mathbf{p}_r^{\tau}} + w_{ir} \frac{\partial \mathrm{vec}(\mathbf{\Pi}_{\beta_{ir}})}{\partial \mathbf{p}_r^{\tau}} \right) \mathbf{v}_r^{\tau} \quad (27) $$

The QP problem in (22) is centralized. Our goal is to have each drone solving a local QP using only local quantities, such that the collective solution of the local QPs results in the satisfaction of the centralized constraint in (22). In the next subsection, we show how to solve this problem in a distributed way.

## B. Distributed Persistent Target Monitoring

To guarantee satisfaction of the constraint in (22) first note that, from (24), $L_f \psi_{1r}(\zeta)$ can be split in a part local to each robot, $d_{ir}(\zeta)$, and a part that is not already separated $c_r(\zeta)$. A

possible strategy to satisfy the previous constraint is, for each drone, to consider the following constraint in its local QP:

$$ L_{gi} \psi_{1r}(\zeta) \mathbf{u}_i + d_{ir}(\zeta) \geq -k_i(\zeta) (c_r(\zeta) + \alpha_2^{\psi} \psi_{1r}(\zeta)) \quad (28) $$

where the weights $k_i(\zeta)$ need to sum up to 1, i.e. $\sum k_i = 1$. Then, taking the sum for each robot of the left hand side of the inequality in (28) yields

$$ 
\begin{aligned}
& \sum_{i=1}^{N} L_{gi} \psi_{1r}(\zeta) \mathbf{u}_i + \sum_{i=1}^{N} d_{ir}(\zeta) \\
& \geq - \left( \sum_{i=1}^{N} k_i(\zeta) \right) \left( c_r(\zeta) + \alpha_2^{\psi} \psi_{1r}(\zeta) \right) = - \left( c_r(\zeta) + \alpha_2^{\psi} \psi_{1r}(\zeta) \right) \quad (29)
\end{aligned}
 $$

which satisfies the centralized constraint in (22). The most trivial choice for the weights is $k_i(\zeta) = \frac{1}{N}$, which divides equally the constraint among the robots, but other choices are also possible [11]. In the local constraint in (28), some of the variables are not directly locally available but they can be estimated in a decentralized way:

* $\sum_{i=1}^N w_{ir} \mathbf{\Pi}_{\beta_{ir}}$ : this quantity appears in $\mathrm{vec}(\dot{\mathbf{G}}_r)$ (see (10)) and can be computed in a distributed way by dynamic average consensus [18] and multiplying the average by the number of drones;

* $\mathbf{G}_r$ : Each drone has its own copy $\mathbf{G}_{ir}$ of the information collected about the target $\mathbf{G}_r$, which is obtained by integrating (10). The matrices $\mathbf{G}_{ir}$ starts from the same initial conditions and have the same dynamics up to the consensus error on $\sum_{i=1}^N w_{ir} \mathbf{\Pi}_{\beta_{ir}}$. In order to have consistency across the network we add a consensus term to the OG dynamics:

$$ \dot{\mathbf{G}}_{ir} = -\rho \mathbf{G}_{ir} + \sum_{i=1}^{N} bw_{ir} \mathbf{\Pi}_{\beta_{ir}} + \sum_{j \in \mathcal{N}_i} (\mathbf{G}_{jr} - \mathbf{G}_{ir}). \quad (30) $$

Then, also the quantities $\lambda_{1r}$ and $\mathbf{v}_{1r}$ are available to all the drones;

* $\mathbf{p}_r^\tau, \mathbf{v}_r^\tau$ : every drone has its own posterior estimate $\hat{\mathbf{p}}_{ir}^{\tau+}$ and $\hat{\mathbf{v}}_{ir}^{\tau+}$ provided by the ICF.

**Remark 3.** Due to the non-differentiability of the minimum eigenvalue function, as in previous works we use a smooth approximation as in [19].

A direct implementation of (28) would imply that, when $L_{gi} \psi_{1r}(\zeta)$ approaches zero, the QP may result infeasible. In order to solve this issue, we add a slack variable to obtain a soft constraint.

$$ 
\begin{aligned}
\min_{\mathbf{u}_i \in \mathcal{U}_i, \delta_{ir}} & \quad \frac{1}{2} \|\mathbf{u}_i - \mathbf{u}_i^d\|_2^2 + \frac{1}{2} K_\delta w_{ir} \delta_{ir}^2 \\
\text{s.t.} & \quad L_{gi} \psi_{1r}(\zeta) \mathbf{u}_i + d_{ir}(\zeta) + \delta_{ir} \geq -\frac{1}{N} (c_r(\zeta) + \alpha_2^{\psi} \psi_{1r}(\zeta)) \\
& \quad r = 1, \dots, M
\end{aligned}
\quad (31) $$

Notice that the slack variable in the cost function is weighted by the product of a high gain $K_\delta$ and the weight $w_{ir}$. The reason of this is two-fold: 1) to avoid numerical problems when $L_{gi} \psi_{1r}(\zeta)$ is very small (because of a very small weight $w_{ir}$); 2) to relax the constraint for drones which are not very near to the target. This in practice means that if some drones are already observing the target, the drones with very small
weight will ignore the constraint and only track the desired input (representative of any additional task).

For the formation to track multiple targets, we simply take the intersection of the safe sets corresponding to each target, i.e. we add a linear inequality constraint for each target and extend the state $\zeta$ with the other targets state. Notice that, with the proposed formulation, if the collected information becomes high enough, since the constraint is satisfied at the current time, the drones could stop following a target $r$ and just track the desired input. In this situation, the weight of each robot w.r.t. target $r$ could become very small and it may happen that no drone would then be able to reach the target again. In order to avoid this issue, we add another CBF for ensuring that $h_{wr} := \sum_{i=1}^{N} w_{ir} - \epsilon_w \ge 0$ for $r = 1, ..., M$ and $\epsilon_w > 0$. The additional CBF constraint can be added to the local QP as in the previous case:

$$ \begin{array}{ll} \min_{\mathbf{u}_i \in \mathcal{U}_i, \delta_{\psi_{ir}}, \delta_{w_{ir}}} & \frac{1}{2} \|\mathbf{u}_i - \mathbf{u}_i^d\|_2^2 + \frac{K_\delta}{2} \sum_{r=1}^{M} w_{ir} \delta_{\psi_{ir}}^2 + \frac{K_\delta}{2} \sum_{r=1}^{M} w_{ir} \delta_{w_{ir}}^2 \\ \text{s.t.} & L_{gi} \psi_{1r}(\zeta) \mathbf{u}_i + d_{ir}(\zeta) + \delta_{\psi_{ir}} \ge -\frac{1}{N} (c_r(\zeta) + \alpha_2^\psi \psi_{1r}(\zeta)) \\ & \frac{\partial w_{ir}}{\partial \mathbf{p}_i} \mathbf{u}_i + \frac{\partial w_{ir}}{\partial \mathbf{p}_r^\tau} \mathbf{v}_r^\tau + \delta_{w_{ir}} \ge -\frac{\alpha_1^w}{N} \left( \sum_{i=1}^{N} w_{ir} - \epsilon_w \right) \\ & r = 1, \dots, M \end{array} \quad (32) $$

where $\frac{1}{N} \sum_{i=1}^{N} w_{ir}$ is obtained through average consensus.

**Remark 4.** Depending on the design of the weights, this constraint does not necessarily imply that one of the drones is forced to continuously observe the target. Rather, it ensures that the drones remain close enough to the target so that they can exploit the gradient information in the weights for approaching and measuring again the target whenever necessary.

## VI. SIMULATION RESULTS

In this section we validate the proposed approach via a series of simulations. We consider $N$ drones localizing $M$ target ground robots, which are unable to localize themselves. We show three representative scenarios: 1) a single target moving with non-constant velocity, 2) the same target motion but with the drones having a secondary task of formation control, 3) a multi-target scenario, with targets moving at non-constant velocity. For each case, we perform 20 simulations, with the initial drone positions sampled from a uniform distribution near the origin. Knowledge of the fact that the targets are moving on the $xy$ plane is never used by the algorithm (which indeed would also work for a generic 3D motion). Each drone runs an ICF for each target and solves the QP (32). For each case, we report the plots of average results for the CBFs $\psi_{0r}$, $\psi_{1r}$, $h_{wr}$ and of the norm of the position estimation error $\|e_p(t)\|_2$ across all drones and all simulations. We also plot the minimum and maximum values at each time instant showing a shadowed area between the two. Note that if the CBFs remain non-negative then the corresponding constraints are satisfied. A red dashed line is highlighting the minimum threshold at zero. We check that at $t = 0$ each target is visible by at least 2 drones. The drones run a collision avoidance algorithm using CBFs as in [9], but as the topology of the communication graph is fixed,

it is not implemented in a distributed way. The minimum inter-agent distance is set to $1.0 m$, and the velocities of the drones are limited to $\|\mathbf{u}_i\| \in [-3, 3] m/s$. The weights parameters used for all the simulations are reported in the caption of Fig. 2. Other parameters common to all the simulations are the following: forgetting factor $\rho = 0.7$, threshold $\epsilon_\psi = 0.1$, threshold $\epsilon_w = 1$, slack variable weight $K_\delta = 10^5$, number of consensus iterations per step $K_c = 1$, information gain $b = 2$. For all targets, we initialize the estimated position $\hat{\mathbf{p}}_{ir}^{\tau-}$ to a random guess and the velocity $\hat{\mathbf{v}}_{ir}^{\tau-}$ to zero, $\mathbf{W}_{ir}^{-}$ to $\mathbf{I}_3$ and $\mathbf{G}_r$ to $\epsilon_\psi \mathbf{I}_3$. The system covariance $\mathbf{Q}$ is diag$(0.01 \cdot \mathbf{I}_3, \mathbf{I}_3)$. The measurements acquired by the drones are affected by Gaussian noise acting on the image plane with zero mean and covariance $\mathbf{R} = 5 \cdot 10^{-5} \mathbf{I}_2$. The reader can find attached to the paper a video with representative simulations.

### A. Case 1: Single target, no additional tasks

In this case, we consider $N = 6$ and $M = 1$. The drones do not have an additional task besides the target estimation one (namely $\mathbf{u}_i^d = \mathbf{0} \ \forall i \in \{1, \dots, 6\}$). The trajectory of the target, starting from the origin, is the eight-shape (Figs. 3 and 4e) defined by $\mathbf{p}_1^\tau(t) = [A \sin(\omega t), A \sin(\omega t) \cos(\omega t), 0]^T$, with $\omega = 0.12 \text{ rad/s}$, $A = 10 \text{ m}$. Notice that the velocity is far from being constant: hence, two non-colinear bearings are necessary as per Remarks 1 and 2. In each of the 20 realizations, the drones initial positions are generated by uniformly sampling a box of sizes $8 \times 8 \times 1 \text{ m}$ centered at $[0, 0, 2]^T \text{ m}$.

The results are reported in Fig. 4. Figures 4a, 4b and 4c depict the mean CBFs. In these plots, three peaks are clearly visible: the first is due to initial conditions, since the target starts at the origin where it can be sensed by the majority of the drones with high weights $w_{i1}$ (see Fig.4c, which shows the sum of the weights with offset given by $\epsilon_w$), and hence the information rapidly grows. The second and third peaks refer, respectively, to the target going back from the origin after half a period, where it is visible again by the drones which did not follow it, and the target completing a period of its motion and passing again through the origin. In between the peaks, the constraints are active and few drones follow the target. The actual number of moving drones depends also on $\epsilon_w$, which can be tuned depending on the application. For example, in this case it can be used to impose that at least two drones follow the target by choosing $\epsilon_w > 1$. The trajectories of drones and target at the final instant of one of the 20 simulations are depicted in Fig. 4e. As one can see, the CBFs have non-negative values, although some slight violations may happen because of estimation noise and the use of slack variables. Fig. 4d depicts the average of the norm of the position error with maximum and minimum values for each time instant. In correspondence of the peaks in the CBFs plots, one can observe a slight decrease of the error, because the target is being observed by more drones.

### B. Case 2: single target, formation control task

In this case, we consider the same scenario as before, but adding as secondary task a bearing-only formation control [20], which does not constrain scale and barycenter of the formation
and which is used to provide the desired inputs $\mathbf{u}_i^d$. The results are reported in Fig. 5, while Fig. 3 depicts the trajectories of a simulation of case 2 at the final instant. From Figs. 5a, 5b and 5c, we can draw the same considerations as in the previous case, with the CBFs having in general higher values. This is because the drones observing the target tend to steer the formation closer to the target. This is confirmed also by Fig. 5d, where we can see that the estimation error remains lower than the previous case. When the target moves away from the origin, 2 or 3 drones follow it and the rest of the group implements the input given by the formation control task. In Fig. 5e, we also show the average of the norm of the bearing errors related to the formation task, with maximum and minimum values for each time $t$. The bearing error initially rapidly decreases, and it then remains limited, although not zero on average. The reason being that the formation control provides a *desired* input which is then filtered by the CBFs. This is also due to the suboptimality of the distributed CBFs here implemented w.r.t. the centralized QP.

<figure><img src="image_5.png" /><figcaption>Figure 3: Trajectories at the final instant of simulation examples of case 2 (left) and case 3 (right). The blue 'X' represent drones which are not sensing any target, the blue thin lines represent the FoVs.</figcaption></figure>

### C. Case 3: multi-target, no additional tasks

In this case, we consider $N = 10$ and $M = 3$ with $\mathbf{u}_i^d = \mathbf{0} \quad \forall i \in \{1, \dots, 10\}$. The target initial positions are $\mathbf{p}_1^T(0) = [1, 0, 0]^T m$, $\mathbf{p}_2^T(0) = [-0.5, 0.866, 0]^T m$ and $\mathbf{p}_3^T(0) = [-0.5, -0.866, 0]^T m$, and they move with sinusoidal motion in the $+x$, $+y$, and $-x$ directions respectively (see Fig.3). Hence, again, they do not satisfy the constant velocity condition. The results are reported in Fig. 6, where in each subfigure the top plot refers to target 1, the middle plot to target 2 and the bottom plot to target 3. Initially, the targets are seen by the majority of the drones, hence the minimum eigenvalue of the OG rapidly increases and the weights related to the targets are high, as it can be seen from Figures 6a, 6b and 6c. After these initial peaks, the CBFs decrease approaching zero without crossing it (slight exceptions are possible again due to estimation noise and the use of slack variables). This phase corresponds to the situation in which the drones have to “split” and move from their original positions to follow different targets. Also, in this case the minimum number of drones required to estimate the state of a target respecting the constraints is 2. The final time of a simulation is depicted in Fig. 3.

### D. Discussion

From the reported results, it emerged that the multi-target tracking cases still present some limitations. Specifically, when the drones need to split to track different targets and the drones are centered w.r.t. the targets when the constraints become active, some undesirable oscillations may occur in the drone motion. This issue arises because each drone lacks knowledge of the inputs applied by the other drones: indeed, a *centralized* implementation of the proposed strategy would not encounter the same problem. Better performance in this sense (and also in terms of the optimality of the secondary task) could be achieved by using distributed optimization techniques for finding the optimal solution of the centralized QP with coupled time-varying linear inequality constraints. The formulation of distributed CBFs presented in [21] seems a promising direction which we will investigate in future works.

Another limitation of the approach is the influence of estimation errors on the fulfillment of the constraints. While some robustness of the control law to noise in the model can be shown, studying its robustness w.r.t. estimation errors is much more complex and a common open challenge in active sensing.

## VII. CONCLUSIONS AND FUTURE WORKS

In this paper, we proposed a distributed persistent monitoring scheme for estimating the state of one or multiple moving target(s) from bearing measurements by employing an Information Consensus Filter. The filter is uniformly globally exponentially stable if a Persistency of Excitation condition is met. The main contribution of this work is to guarantee that such a PE condition is met also in presence of sensing constraints while potentially achieving other tasks of interest. This is achieved by relying on two main tools, namely the decentralized High Order Control Barrier Functions, used to enforce the invariance of the safe set, and the weighted Observability Gramian with forgetting factor, which is used to quantify the persistency of excitation. The approach has been validated via numerical simulations. In addition to the discussion provided in Sect. VI-D, in the future we plan to consider more complex secondary tasks, as well as a time-varying graph topology with a connectivity maintenance constraint.

## REFERENCES

[1] R. Olfati-Saber, “Kalman-consensus filter: Optimality, stability, and performance,” in *Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference*. IEEE, 2009, pp. 7036–7042.

[2] A. T. Kamal, J. A. Farrell, and A. K. Roy-Chowdhury, “Information weighted consensus filters and their application in distributed camera networks,” *IEEE Transactions on Automatic Control*, vol. 58, no. 12, pp. 3112–3125, 2013.

[3] G. Battistelli and L. Chisci, “Kullback–leibler average, consensus on probability densities, and distributed state estimation with guaranteed stability,” *Automatica*, vol. 50, no. 3, pp. 707–718, 2014.

[4] M. Jacquet, M. Kivits, H. Das, and A. Franchi, “Motor-level n-mpc for cooperative active perception with multiple heterogeneous uavs,” *IEEE Robotics and Automation Letters*, vol. 7, no. 2, pp. 2063–2070, 2022.

[5] F. Morbidi and G. L. Mariottini, “Active target tracking and cooperative localization for teams of aerial vehicles,” *IEEE transactions on control systems technology*, vol. 21, no. 5, pp. 1694–1707, 2012.
<figure><img src="image_6.png" /><figcaption>Figure 4: Case 1: mean, maximum and minimum values for each $t$.</figcaption></figure>

<figure><img src="image_7.png" /><figcaption>Figure 5: Case 2: mean, maximum and minimum values for each $t$.</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>Figure 6: Case 3: mean, maximum and minimum values for each $t$.</figcaption></figure>

[6] C. Freundlich, S. Lee, and M. M. Zavlanos, "Distributed active state estimation with user-specified accuracy," *IEEE Transactions on Automatic Control*, vol. 63, no. 2, pp. 418-433, 2017.

[7] A. J. Krener and K. Ide, "Measures of unobservability," in *Proceedings of the 48th IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference*. IEEE, 2009, pp. 6401-6406.

[8] A. D. Ames, S. Coogan, M. Egerstedt, G. Notomista, K. Sreenath, and P. Tabuada, "Control barrier functions: Theory and applications," in *2019 18th European control conference (ECC)*. IEEE, 2019, pp. 3420-3431.

[9] L. Wang, A. D. Ames, and M. Egerstedt, "Safety barrier certificates for collisions-free multirobot systems," *IEEE Transactions on Robotics*, vol. 33, no. 3, pp. 661-674, 2017.

[10] B. Capelli and L. Sabattini, "Connectivity maintenance: Global and optimized approach through control barrier functions," in *2020 IEEE International Conference on Robotics and Automation (ICRA)*. IEEE, 2020, pp. 5590-5596.

[11] L. Lindemann and D. V. Dimarogonas, "Barrier function based collaborative control of multiple robots under signal temporal logic tasks," *IEEE Transactions on Control of Network Systems*, vol. 7, no. 4, pp. 1916-1928, 2020.

[12] W. Xiao and C. Delta, "High order control barrier functions," *IEEE Transactions on Automatic Control*, 2021.

[13] X. Tan, W. S. Cortez, and D. V. Dimarogonas, "High-order barrier functions: Robustness, safety, and performance-critical control," *IEEE Transactions on Automatic Control*, vol. 67, no. 6, pp. 3021-3028, 2021.

[14] T. Hamel and C. Samson, "Riccati observers for position and velocity bias estimation from direction measurements," in *2016 IEEE 55th conference on decision and control (CDC)*. IEEE, 2016, pp. 2047-2053.

[15] F. Pukelsheim, *Optimal design of experiments*. SIAM, 2006.

[16] P. Salaris, M. Cognetti, R. Spica, and P. Robuffo Giordano, "Online optimal perception-aware trajectory generation," *IEEE Transactions on Robotics*, vol. 35, no. 6, pp. 1307-1322, 2019.

[17] J. R. Magnus, "On differentiating eigenvalues and eigenvectors," *Economic Theory*, vol. 1, pp. 179-191, 1985.

[18] S. S. Kia, B. Van Scoy, J. Cortes, R. A. Freeman, K. M. Lynch, and S. Martinez, "Tutorial on dynamic average consensus: The problem, its applications, and the algorithms," *IEEE Control Systems Magazine*, vol. 39, no. 3, pp. 40-72, 2019.

[19] R. Spica and P. Robuffo Giordano, "Active decentralized scale estimation for bearing-based localization," in *2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. IEEE, 2016, pp. 5084-5091.

[20] S. Zhao and D. Zelazo, "Bearing rigidity and almost global bearing-only formation stabilization," *IEEE Transactions on Automatic Control*, vol. 61, no. 5, pp. 1255-1268, 2015.

[21] X. Tan and D. V. Dimarogonas, "Distributed implementation of control barrier functions for multi-agent systems," *IEEE Control Systems Letters*, vol. 6, pp. 1879-1884, 2021.