Control Protocols for Range-Based Navigation of a Networked Group of Underwater Vehicles

This paper tackles the problem of formation reconstruction for a team of vehicles based on the knowledge of the range between agents of a subset of the participants. One main peculiarity of the proposed approach is that the relative velocity between agents, which is a fundamental data to solve the problem, is not assumed to be known in advance neither directly communicated. For the purpose of estimating this quantity, a collaborative control protocol is designed in order to mount the velocity data in the motion of each vehicle as a parameter through a dedicated control protocol, so that it can be inferred from the motion of the neighbor agents. Moreover, some suitable geometrical constraints related to the agents' relative positions are built and explicitly taken into account in the estimation framework providing a more accurate estimate. The issue of the presence of delays in the transmitted signals is also studied and two possible solutions are provided explaining how it is possible to get a reasonable range data exchange to get the solution both in a centralized fashion and in a decentralized one. Numerical examples are presented corroborating the validity of the proposed approach.


INTRODUCTION
Localization is one of the most important basic abilities for an autonomous vehicle to perform autonomously a wide number of tasks (Ferri et al., 2017;Simetti et al., 2017;Antonelli et al., 2018), so that an accurate and reliable localization algorithm is a key practical tool for the success of mission in many applications of underwater robotics.
In essence, the localization problem is often addressed exploiting geometrical relations between the pose of the vehicle and the sensors, so that the issue of solving the localization problem may be strongly related to the environment of the given application. Sensor technology strongly depends on the environment, e.g., the Global Navigation Satellite System (GNSS), Attitude Heading Reference Systems (AHRS), radar-based tracking systems, accelerometers, gyros, and compass devices. This makes the issue of the underwater localization problem more challenging, and it has been considerably studied in the past years. Underwater acoustic-based trilateration solutions as long base line (LBL) systems have been studied as well, but they require complex deployment operations (Scherbatyuk, 1995).
Localization is a long-time debated research area in robotics and beyond, and different aspects have been studied over time. In this paper, we consider the relative localization problem for a team of agents, that is, the formation reconstruction problem in a multi-vehicle framework. This is a peculiar problem in the research area of localization, which has been recently considered by several authors for its importance in various applications. In the paper by Soares et al. (2013), the authors propose a formation keeping under severe communication and localization constraints, which is a typical condition of the underwater environment. In Sarras et al. (2017), the authors adopt an observer-based approach to treat the problem of multi-vehicle collaborative localization using time-varying range and relative velocity measurements, while in Halsted and Schwager (2017) a method of estimating the shape of an indoor environment using the echos of acoustic pulses among the robots is studied. Indeed, when an underwater mission is performed by a team of robots, it is often fundamental to know the relative positions and orientations in order to be able to correctly merge the data of the environment (e.g., merging pieces of map) collected by each individual robot (see, e.g., Roumeliotis and Bekey, 2000).
From a theoretic standpoint, the range-based estimation problems have been recently considered also in a single vehicle framework (Bayat et al., 2016). The challenges of dealing with single range measurements come from the fact that they are a non-linear algebraic map of the vehicles' positions hence the observability analysis requires the tools of local and weakly local observability (Hermann and Krener, 1977), but this approach suffers from several difficulties (Gadre and Stilwell, 2004;Ross and Jouffroy, 2005;Jouffroy and Reger, 2006). However, an alternative approach has been recently investigated recurring to a reformulation of the problem, which requires the observability of a linear time varying system (see, e.g., Batista et al., 2011;De Palma et al., 2017) so that a number of useless drawbacks of the local approach are avoided.
In this paper, we afford the relative positions reconstruction problem for a team of collaborative robots using local data. Collaborative navigation based on single-range data has been studied in the underwater environment (Fallon et al., 2010;Soares et al., 2013;Webster et al., 2013) as well as in more general settings (Cao et al., 2011). Indeed, since the milestone paper by Sanderson (1997), the area of cooperative navigation and localization has been significantly explored. One first significant attempt to the decentralized collective localization problem is explained in Roumeliotis and Bekey (2002); to achieve this goal, data processed during each collective localization session are propagated among all the robots in the group. This approach is further investigated in Mourikis and Roumeliotis (2006), where the Relative Position Measurement Graph (RPMG), i.e., the weighted directed graph representing the network of robot-to-robot exteroceptive measurements, is introduced and used as a key tool for the analysis of cooperative localization. The distributed acoustic navigation problem for Autonomous Underwater Vehicles (AUVs) is explored in Bahr et al. (2009), where the authors use acoustic ranging and data exchange based on dead-reckoning and range-only measurements provided by acoustic modems that are mounted on each vehicle to achieve cooperative positioning. In the paper by Allotta et al. (2014), the use of AUVs with low-cost instrumentation is explored (namely, each of them is equipped with a low-cost IMU, a compass and depth sensor, but only one of them, the master, has a high accuracy navigation sensor such as the DVL), and acoustic modems for communication are used as sensors of relative distance to achieve an innovative cooperative localization algorithm. In Soares et al. (2017), the authors optimize the nonconvex maximum-likelihood estimator in the presence of range measurements contaminated with Gaussian noise, and obtain a convergent, accurate, and distributed positioning algorithm that outperforms the extended Kalman filter. However, this topic has been largely explored by several authors, and the interested reader may refer to Arai et al. (2002) The research activity reported in this paper stems from the above considerations and is strongly inspired by the experience gained within a European project (Antonelli et al., 2018). The goal is to extend the preliminary results achieved by the same authors in De Palma et al. (2015) and De Palma et al. (2019) as follows. One of the main novelties with respect to De Palma et al. (2015) is relative to the following fact. Based on the consideration that communications networks in the underwater environment do not perform well, we want to avoid the direct communication between vehicles of the relative velocity by setting a suitable agreed control protocol in which it is possible to encapsulate the data which one vehicle wishes to communicate as a parameter that can be easily estimated using the relative motion by any of its neighbors. As opposed to the approach in Mourikis and Roumeliotis (2006), in this paper the solution proposed relies on intra-vehicle ranges only rather than relative positions of vehicles. As a further peculiar feature of the approach proposed by the authors of this paper, we further use topology-based relations among the unknown variables as an additional constraint and this results in reduction of the overall estimate uncertainty. Further, in this paper we explicitly account for delays in range measurements acquisition. Indeed, the technology underneath underwater networks is typically acoustic and communication delays may be significant and their impact may not be neglected. The solution provided in this paper exploits the intuitive idea of a neat time-division protocol to prevent any delay-related issue in the localization solution provided that an upper bound is available.
The paper is structured as follows: after a brief summary of notation and terminology in section 2, we provide the general problem statement in section 3, where section 4 is dedicated to the localization-oriented control protocol. In section 5, the observer design is performed, and section 5.1 is fully dedicated to the projection approach, which allows to improve the estimate precision. In section 6, the issue of delays in measurements is faced, and in section 7, two possible communication protocols are provided both in the case of a single "leader" agent performing the elaboration (thus only one agent collecting all the estimates) or any agent of the network. In section 8, a wide simulation activity is reported and discussed, showing the effectiveness of the proposed approach. Section 9 closes the paper summarizing the results achieved in the paper.

NOTATION AND GRAPH THEORY TERMINOLOGY
In the following, we introduce the notation adopted in the paper and some tools from graph theory (Godsil and Royle, 2001), which are useful for a mathematical treatment of the problem. We use the symbol ⊗ to denote the Kronecker product between two matrices, which is defined as follows. For a pair of matrices A ∈ R n×m and B ∈ R p×q : We use diag(A 1 , . . . , A n ) to denote a block-diagonal matrix with matrix diagonal entries A i . A graph G is the collection of a set V = {1, . . . , n} called set of nodes and another set E ⊆ V × V, which is called the set of edges. For a given i ∈ V, the set N i = {j ∈ V :(j, i) ∈ E} is called the set of its neighbors. A path P between node i and node j is a collection of nodes and edges of G connecting i and j; a graph G = (V, E) is connected if there exists a path connecting each h, g ∈ V. A cycle is analogously defined with the additional condition that i = j. A cycleC is linearly independent from a preassigned set of cycles if at least one edge inC is not present in the union of the edge sets of the cycles.

PROBLEM FORMULATION
Let x i ∈ R 3 for i = 1, 2, . . . , n denote the position of n agents and v i ∈ R 3 their velocity. Each agent is able to know its own velocity with reference to the common frame I so that, unless specified, we assume that the velocity is expressed in this common frame I. We assume that if two agents are able to measure the range between themselves, they are connected, so that it is possible to define a connection graph. Inspired by the work of Mourikis and Roumeliotis (2006), we refer to it as to an RPMG, which we assume to be a simple graph G with node set V = {1, . . . , n} and the edges set E. We further assume that if two agents are able to measure the range between themselves, they can establish a communication link to exchange data, so that it is possible to consider G also a communication graph. The evolution of the agents can be computed using simple kinematic equations: so thatż where z ij in Equation (3) denotes the relative positions among those agents able to exchange information. All agents are assumed to be able to acquire measurements of their relative Euclidean distance y ij in Equation (6), with the goal of estimating z ij performing an elaboration of the relative range measurements y ij and local data. A fundamental difference between this problem statement and the one afforded in the paper by De Palma et al. (2015) is that here we do not assume to exchange the velocity data through a dedicated underwater network, but we encapsulate this information as parameters of an agreed control protocol and infer the velocity value using a range-based Kalman filter as detailed in the following. We consider this choice of the problem statement a significant step forward for all those applications where only range information exchange is possible. From now on we work under the assumption that the communication graph is time invariant. This choice is instrumental to keep the problem simple and the associated solution clear. The authors believe that it is a mild assumption considering that the resulting localization procedure requires a bounded amount of time. It is equivalent to assume that nodes that are neighbors at the initial timet keep this communication alive during the whole time span, while other nodes that may fall in the communication range aftert are not included in the elaboration. Furthermore, from a practical point of view, it should be emphasized that acoustic underwater communications degrades drastically after certain threshold distances. If a group of underwater vehicles keeps its formation during a mission within such a threshold distance (most common case), the quality of the communications can be assumed to remain good and the communication links can be considered constant. Finally, even if a communication link (i,j) was lost, the proposed strategy could still be adopted by deleting the corresponding state variable z ij .
We now describe a strategy to improve the estimation when cycles are present in the communication graph. Indeed, the relative positions may be not independent, but they can be subject to geometric constraints if they belong to the same cycle. Based on the consideration that the sum of all the vectors representing the relative positions of agents belonging to a cycle must necessarily be zero, each set of independent cycles corresponds to a set of independent geometric constraints on the relative positions. Considering a connected graph with n nodes and m edges, any cycle basis can be mapped into a set of m−n+1 additional relations. A team of n = 4 agents with m = 5 links is depicted in Figure 1. It is possible to set m − n + 1 = 2 additional relations, namely that can be rewritten as Dz * = 0 3×1 with The above example can be easily generalized, and it is possible to write a general setting as follows. For a team of n nodes and RPMG edge set E, the additional relations can be expressed as: Each geometrical relation associated with (11) can be encapsulated in the state estimation procedure in order to improve the estimation quality.

LOCALIZATION-ORIENTED CONTROL LAW
In this section, a motion control scheme for range-based relative localization is proposed. Using this strategy, it is possible to infer the relative velocity of agents from the motion measurements. Consider the following control law for each vehicle: where K ∈ R is a positive constant. According to such schema, the motion of each vehicle i depends only on the relative positions with its neighbors, namely z ij with j ∈ N i . It is worth noting that in our framework Equation (15) cannot be directly implemented (as the actual relative positions z ij are not known to vehicle i), but we replace the estimated relative positionsẑ ij instead: leading to the following relative velocities: Details about the specific computation of the estimated relative positionsẑ ij to be used in (16) are provided in the next section.
When agents adopt this control law, the system (5-6) becomes: As a final remark, it is interesting to note that the control law in Equation (15) has the same structure of a consensus protocol as those described in Olfati-Saber and Murray (2004) and Ren and Beard (2007). Control strategies based on such kind of protocols have been widely studied for the coordination or formation control of a team of agents (Leonard et al., 2007;Ren and Cao, 2010). In this paper, we do not seek control objectives but we rather use Equation (16) as a localization-oriented control protocol that each vehicle must follow at each t = kT s , being T s a fixed time interval. Therefore, within each interval the agents keep their velocity constant. Indeed, in this paper such control law is adopted to make the motion informative of each agent's position and velocity, and hence make the range-based relative localization of a networked group of underwater vehicles solvable in finite time so that it can be executed as a routine inside a mission when localization is needed. This is useful when, during a cooperative mission, the relative localization accuracy of the agents decreases; in this case, the proposed localization-oriented control law can be activated so as to improve the accuracy of the relative localization.
The advantage of such solution with respect to the work presented in De Palma et al. (2015) is that by adopting the motion control scheme for range-based relative localization in (16), there is no need for the agents to share their velocity information in the communication channel. Indeed, an agent is able to derive the velocity of the other agents from the knowledge of the adopted control law and the estimated relative positions. This result in a significant reduction of the communication load over the network. The results achieved in this paper are particularly relevant in underwater applications where the bandwidth is often limited due to the acoustic communications.

OBSERVER DESIGN
The estimation of the relative positions z ij (t) in Equations (18) with d ij (t) defined as Frontiers in Robotics and AI | www.frontiersin.org Equation (20) allows to compute The termȳ ij (t) defined in Equation (23) as well as the term d ij (t) defined in (21) are both known quantities, so that the new linear output equation in Equation (24) can be considered. Consequently, the original non-linear model (Equations 5 and 6) can be expressed as a Linear Time-Varying (LTV) model Thus, the estimation of z ij (t) in (25, 26) can be addressed exploiting the standard linear system theory. It should be noticed that the output matrix d ⊤ ij (t) of the LTV model depends on the control input, hence the observability depends on the agents' relative velocity v ij . It can be proven that a sufficient condition for the observability of the original system (5-6) on [t 0 , t] is the invertibility of the observability Gramian of the LTV system (25, 26) defined as: The reader should refer to Indiveri et al. (2016) for a detailed analysis of the observability properties of such a system. Let us consider the discrete time formulation of the LTV system given by: where ω(k) and ǫ(k) are assumed to be zero mean, Gaussian, white, and uncorrelated process and measurements noises with covariances Q(k) and R(k), respectively, and T s represents the sampling time. A standard Kalman filter can be applied to the model in Equations (28) and (29), leading to the following equations: In the considered scenario, thanks to the intra-vehicles acoustic communications, each agent is able to know the estimationsẑ ij and their covariances P ij . Therefore, each agent can improve the estimation accuracy exploiting the additional geometric constraints (11). To this aim, we can benefit from the projection approach described in the following subsection.

Constraint Exploitation for the Estimate Improvement
Assuming to know the m Kalman filter estimatesẑ ij , it is possible to incorporate the constraint (11) in the estimation framework resorting to the approach described in Simon (2006). Let us define the Kalman filter estimateẑ * aŝ and its posterior covariance as An estimateẑ * p satisfying the constraint (11) can be derived projecting the Kalman filter estimate onto the constraint surface; this would lead to the following solution: where U is the projection operator such that U 2 = U, DU = 0 3(m−n+1)×3m , and W ∈ R 3m×3m is any positive definite weighting matrix. As proven in Simon and Chia (2002), if the weight matrix W in Equation (40) is chosen as W = P * −1 , then the estimateẑ * p in Equation (39) is minimum variance, namely being P * p the error covariance ofẑ * p ; however, if the weight matrix W is chosen as W = I, then the constrained estimatê z * p in Equation (39) is always closer to the true state than the unconstrained estimate, namely ||ẑ * −ẑ * p || ≤ ||ẑ * −ẑ * ||.
Choosing the weight matrix W = P * −1 (k), the estimateẑ * p (k) becomes: and resulting posterior covariance is given by From theẑ * p resulting from (39), it is possible to extract the singlê z ij that appears in the control and estimation equations (16,17,18,20,21,28,25,31,32). Hence, the minimum variance estimatê z * p is actually used by each agent to set its velocity according to the control law (Equation 16). Therefore, we assume that this velocity is kept constant for the fixed time interval T s , namely it does not change until a new estimation is available.

Remark
It is worth noting that the outputȳ ij (t) defined in Equation (23) depends on the first measurement y ij (t 0 ). This dependency may affect the robustness of the solution as a single erroneous measurement (e.g., an outlier or a fault signal) at t = t 0 would jeopardize the output. This issue can be overcome by periodically resetting the measurement y(t 0 ) with y(t). This would also prevent possible uncertainties in the knowledge of v ij (t) from causing an unbounded bias in the displacement d ij (t) in Equation (21) used to computeȳ ij (t). In the discrete time case, this would correspond to periodically mapping y ij (0) → y ij (k * ) as if the measurement had started at step k * while the state estimateẑ ij (k+ 1|k + 1) follows its update dynamics. The results presented in the following section refer to the discrete time case with periodic mapping of the initial measurement y ij (0) with y ij (k − 1) (i.e., k * = k − 1). Consequently, the displacement in Equation (31)

DELAYS IN RANGE MEASUREMENTS ACQUISITION
One key point to have in mind when dealing with underwater networks is that acoustic communications may be subject to relatively large delays. In particular, communication latency is due to the physics of the communication channel as well as to the specific networking protocol employed. This latter component of the delay may be eventually reduced accepting higher packet loss probabilities. While details about the assessment of the communication latency are not addressed in this work, it should be noted that delays may be significant for larger distances and should be accounted for in the estimation framework. Indeed, this is the case within the approach described in this work where the delay needs to be known. In this framework, the range measurements available during each step of the estimation process will be y ij (t − τ ij ), rather than y ij (t), having denoted with τ ij the time delay in the measurement acquisition due to the acoustic communication network. This arises the problem of how it is possible to obtain the actual range y ij (t) from the knowledge of the delayed measurement y ij (t − τ ij ), and the time delay τ ij , in order to properly perform the observer for the relative position estimation z ij . Let consider the intra-vehicle range y ij (t): and its time derivative: Equation (46) allows computing y ij (t) from the knowledge of y ij (t − τ ij ) and τ ij as: Exploiting Equation (47), time delays in the measurements are taken into account mitigating their effects on the estimation process. It is worth highlighting that the sampling time T s of the Kalman filter should be properly chosen.
As a final remark, it is worth noting that it is not possible to implement Equation (47) as it is because the actual relative positions z ij are not known, and we use the current estimationŝ z ij instead: The numerical integration of Equation (48) leads to the following discrete-time equation: where dT denotes the integration time. Notice that in spite of the lack of an analytical proof of convergence of Equation (49) to the true measurement y ij , all the numerical results confirm the effectiveness of this approach. The overall control and estimation procedure is illustrated in Algorithm 1. Summarizing, at each time step the last available constrained estimates of z ij are used by the control law of each vehicle using Equation (16). Then, the measurements y ij are acquired: in case of delays, the current y ij is estimated through Equation (49). Finally, the observer updates the estimates of the variables z ij using the constrained Kalman filter solution.

COMMUNICATION PROTOCOLS
In this section, we describe the communication policy that we adopted to perform the range data exchange among the agents during the intersampling period. This step is instrumental to make the computation of Equations (32), (36) possible at each sampling time, and in turn the projection (Equations 43, 44). Several approaches are possible; we propose two alternative solutions, which we refer to as centralized approach and decentralized approach. In the centralized approach, only one agent, a leader agent, is expected to perform the computation of Equations (32)-(36) and (43), (44) so that the communication policy is organized in order to make the data flow to the leader for the twofold task of reconstructing the topology of the RPMG established and collecting a complete set of range measurement to perform the estimation of Equations (32)-(36). If necessary or useful, the leader agent sends back the resulting estimated positions among the agents using the same scheme reversed. In the decentralized approach, all agents have the capability of performing the computation of (32)-(36) and (43), (44) and hence the communication policy is oriented to spread the range data among agents to distribute them to all, so that each agent performs the computation of the positions estimation. It is worth noting that the term centralized/decentralized is related to the computation of Equations (32)-(36), and hence to the fact that the "holder" of the estimated value is only one agent or any one of the network.
Regardless of the strategy adopted, an issue to consider is that the RPMG cannot be known in advance, and the topology identification of it is instrumental to the computation of Equations (32)-(36). In this respect, we assume that the number of the vehicles n and a preassigned labeling of the agents is known in advance, while the connection topology is unknown to any vehicle and it must be reconstructed as well using any approach.
We assume that all agents involved are equipped with synchronized clocks so as to use One-Way Travel Time (OWTT) range measurement schemas. Then, a Time-Division Multiple Access (TDMA) scheme can be employed to access the communication channel. Under this hypothesis, the communication among agents is unidirectional; this choice is conservative in order to avoid the chance of packet collisions and the management of the resulting loss of data. It should be noted that the duration of the time slots depends on the available bit rate and on the specific communication protocol. Examples of acoustic sensors commonly used in underwater environment are the middle frequency (MF) modems (18-34 kHz) by Evologics (Kebkal et al., 2017). They have been recently used for underwater positioning purposes during geotechnical survey experiments performed within a European project (Abreu et al., 2016). Such modems are characterized by a nominal bit rate in the range 3.10-3.85 kbps, hence compatible with the application at hand.
It is now worth mentioning that the amount of time needed for two agents to communicate using acoustic signals may be significant for large distances and in this paper it is accounted for as described in section 6. Indeed, sound speed underwater is approximately 1500 m/s, namely about six orders of magnitude lower than the speed of electromagnetic signals in air.
According to all the previous considerations, we established our communication policy under the following assumptions. We refer to Figure 2 as to a description of the idea in the case of n = 4 and RPMG as in Figure 3. The two strategies are put in a pseudo-algorithm form (in the form of a flowchart) depicted in Figures 4, 5.
We assume that vehicles are organized to send packets one by one. The agents are labeled from the beginning and they follow their labeling in order to send broadcast packets according to the agreed protocol (which depends on the type of approach, this is detailed in the following). Each packet is received only by the neighboring agents and it takes a non-zero travel time to reach the receiver, so we set equal to δt the maximum travel time (which depends on the sensor range and environmental parameters). All agent are aware of the starting time of the estimation procedure, sayt. Agent i sends its packet at timet + (i − 1) · δt and this packet reaches the agent j, j ∈ N i within the time span (t + (i − 1) · δt,t + i · δt). This is periodically repeated at each t + κT s .
In the centralized approach, the range data that are needed to run the filter can be distributed in the team of n members as illustrated in Figure 4: agents sequentially (one in each time slot) broadcast a data packet containing their identifying label, a time stamp, all the edges it is aware of, and all the corresponding range data and measurements delays. After all agents but one (i.e., after n − 1 time slots) execute the protocol, the leader agent collects knowledge about the whole connection topology. Hence, the leader agent knows all the information, i.e., relative distances (6), required to solve the estimation problem taking into account the additional geometric constraints (11) associated with the connection topology. Once computed the estimatesẑ * p of the m relative positions using the collected information, the leader agent broadcasts to all agents a data packet containing  such estimates. Overall, 2n − 2 time slots are required to complete one estimation step. This kind of scaling appears to be most likely acceptable for most applications involving a limited number of vehicles.
In the decentralized approach, considering that the communication graph is unknown to the agents, the communication policy is implemented with the aim of retrieving the graph topology and spreading the range data to all agents. Each agent during its time slot broadcasts a data packet containing its label, a time stamp, the set of links already identified, and the corresponding range data and measurements delays. All agents receiving the ping, decode the data packet, and identify the link between itself and the transmitter agent. This is repeated until all agents collect knowledge about the whole connection topology. In the worst case, 2n − 2 communications slots are required to ensure that all agents have identified the connection topology. At this point, each agent can perform the estimationẑ * p of the relative positions. The main differences between the two approaches can be deduced by the schemes in Figures 4, 5, and we briefly comment them in the following. In the centralized approach, only one agent perform the elaboration, and it can be useful when the team is heterogeneous and some agents have higher computational capacity than others. However, the centralized approach requires a larger amount of communicated data when the estimated state is transmitted to all agents.

SIMULATIONS
The proposed range-based mutual localization for a team of underwater vehicles is here tested on the RPMG in Figure 3 relative to a group of n = 4 agents and m = 4 communication links. The corresponding geometric constraints are as follows: Frontiers in Robotics and AI | www.frontiersin.org The velocity inputs of each agent are assigned according to the localization-oriented control law in Equation (16) with K = 0.1: v 1 (k) = −K(z 12 (k) + z 31 (k)); v 2 (k) = −K(z 12 (k) + z 24 (k)); v 3 (k) = −K(z 31 (k) + z 43 (k)); v 4 (k) = −K(z 24 (k) + z 43 (k)). The agents are located in the following initial positions: x 1 (0) = (0, 0, 1) ⊤ m; x 2 (0) = (10, 0, 2) ⊤ m; x 3 (0) = (0, 10, 3) ⊤ m; x 4 (0) = (10, 10, 4) ⊤ m. Without loss of generality, the range measurements are assumed to be acquired with different time delays τ ij , namely τ 12 = 0.1 s, τ 24 = 0.2 s, τ 31 = 0.3 s, andτ 43 = 0.4 s, whereas a sampling time T s = 0.4 s has been considered. At each sampling time T s , the actual range y ij (t) is derived from the knowledge of the delayed measurement y ij (t − τ ij ), and the time delay τ ij according to Equation (49). The resulting trajectories are shown in Figure 6. It is worth remarking that the proposed agents velocities v i guarantee the observability of the system (Equations 25 and 26). Indeed, it can be verified, by direct calculation, that the motion generated by the control   Figure 7 shows the rank of the Gramian along the trajectory, and a few range acquisitions are needed to get a full rank Gramian matrix. Therefore, given the observability of the system, the states z ij can be estimated using the Kalman observer in Equations (32)-(36), with covariance Q = 0.9 10 −5 · diag(1, 1, 1)m 2 , covariance R = 0.25m 2 , and initial condition given bŷ z ij (0) ∼ N (z ij (0), P ij (0)), P ij (0) = 4 · diag(1, 1, 1)m 2 . (51) The estimateẑ * = (ẑ ⊤ 12ẑ ⊤ 24ẑ ⊤ 43ẑ ⊤ 13 ) ⊤ ∈ R 12 obtained using the Kalman filter is reported in Figure 8A. This estimate violates the equality constraint (50). A constrained state estimate can be obtained projecting the standard Kalman filter estimateẑ * onto the constraint surface through Equations (43), (44). This leads to the projected estimation illustrated in Figure 8B. Figure 9 reports the norm of the equality constraints, FIGURE 9 | Equality constraint Dẑ * z * = 0. Frontiers in Robotics and AI | www.frontiersin.org Dẑ * z * . It is worth noting that the unconstrained Kalman estimate (red line) does not satisfy exactly the constraint, whereas the constrained Kalman estimate (blue line) satisfies the equality constraint, namely Dẑ * z * = 0. Moreover, as expected, the constrained estimate is also characterized by a reduced covariance, i.e., P * p − P * < 0. Indeed, the maximum eigenvalue of the matrix P * p − P * , shown in Figure 10, is always negative (not positive), confirming the improvement obtained by exploiting the additional information provided by the geometric constraints.
Note that regarding the norm of the estimation error, even if the weight W is chosen as W = P * −1 (k) rather than W = I, the projected estimates still provide better results with respect to the corresponding unconstrained Kalman estimates. The norm of the estimation error for both estimates is reported in Figure 10.
The benefits of including the geometric constraints into the estimation framework are more evident in Figure 11, where the estimation error and the corresponding uncertainty region of each component of the state z * ∈ R 12 are shown for both estimates, unconstrained and constrained. The estimation errors and the uncertainty region for the first component of the state z * ∈ R 12 are also depicted in Figure 12. It is interesting to note that, as already highlighted, the uncertainty region of the constrained estimates is smaller than the one related to the standard Kalman estimate. As a final remark, it is worth noting that the estimates in Figures 8, 11, 12, as well as the equality constraint in Figure 9, have sharp leaps whenever a range update is processed, i.e., every T s seconds. Moreover, the management of the delays in range measurements as described in section 6 allows to correctly process the measurements without compromise the convergence of the estimations.
Regardless of the specific approach adopted for the acoustic communications among agents, i.e., centralized or decentralized, the estimation of the relative positions z * = (z ⊤ 12 z ⊤ 24 z ⊤ 43 z ⊤ 13 ; z ⊤ 23 ) ⊤ ∈ R 15 is based on the Algorithm 1. Figure 16 reports the ultimate constrained Kalman filter estimatê z * p of the relative motions. It is worth remarking that, given the global observability of the motion, even if the estimations are initialized with a value far from the real one (see Equation 52), the resultingẑ pij converge to z ij . This is an interesting feature because the proposed localization-oriented control law can actually be activated when the relative localization accuracy of agents is poor. Indeed, adopting such control strategy the whole agents network improves significantly its formation accuracy. Frontiers in Robotics and AI | www.frontiersin.org

CONCLUSIONS
In this paper, the relative localization estimation problem for a team of vehicles is studied based on the knowledge of the range between agents of a subset of the participants. One main peculiarity of the proposed approach is that the relative velocity between agents, which is a fundamental data to solve the problem, is not assumed to be known in advance neither directly communicated. For this reason, a collaborative control protocol is designed in order to encapsulate the velocity data in the motion of each vehicle as a parameter through a dedicated control protocol, so that it can be inferred from the motion of the neighbor agents. Moreover, some suitable geometrical constraints associated with the agents' (unknown) positions are built and explicitly accounted for in the estimation schema providing a more accurate estimate. The issue of possible delays in the transmitted signals is also studied and two possible solutions are provided explaining how it is possible to get a reasonable range data exchange to get the solution both in a centralized fashion and in a decentralized one. Finally, the validity of the proposed approach is shown through numerical simulations.

DATA AVAILABILITY STATEMENT
The datasets generated for this study are available on request to the corresponding author.