# Adaptive Observer From Body-Frame Relative Position Measurements for Cooperative Localization

Nicola De Carli, Esteban Restrepo, Paolo Robuffo Giordano

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

## ► To cite this version:

Nicola De Carli, Esteban Restrepo, Paolo Robuffo Giordano. Adaptive Observer From Body-Frame Relative Position Measurements for Cooperative Localization. IEEE Control Systems Letters, In press, 10.1109/LCSYS.2024.3410893. [hal-04610053](https://hal.science/hal-04610053)

HAL Id: hal-04610053

https://hal.science/hal-04610053

Submitted on 12 Jun 2024

HAL is a multi-disciplinary open access archive for the deposit and dissemination of scientific research documents, whether they are published 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
<figure><img src="image_1.png" /></figure>

HAL
open science

# Adaptive Observer From Body-Frame Relative Position Measurements for Cooperative Localization

Nicola De Carli, Esteban Restrepo, Paolo Robuffo Giordano

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

## To cite this version:

Nicola De Carli, Esteban Restrepo, Paolo Robuffo Giordano. Adaptive Observer From Body-Frame Relative Position Measurements for Cooperative Localization. IEEE Control Systems Letters, 2024, 8, pp.1337-1342. 10.1109/LCSYS.2024.3410893. hal-04610053

HAL Id: hal-04610053

https://hal.science/hal-04610053v1

Submitted on 12 Jun 2024

HAL is a multi-disciplinary open access archive for the deposit and dissemination of scientific research documents, whether they are published 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_3.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Adaptive Observer from Body-Frame Relative Position Measurements for Cooperative Localization

Nicola De Carli, Esteban Restrepo, Paolo Robuffo Giordano

***Abstract***— In this work, we propose an observer scheme to estimate in a common frame the position and yaw orientation of a group of robots from *body-frame* relative position measurements. The state of the robots is represented by their position and yaw orientation. The graph representing the sensing interaction among the robots is directed and it is only required to be weakly connected in addition to satisfy certain *persistency of excitation* conditions. Our proposed observer scheme consists of three distinct components. Firstly, an adaptive observer is used to estimate the relative yaw corresponding to each persistently exciting edge. Subsequently, an observer is used to estimate the yaw orientation in a common frame based on these estimated relative yaws. Finally, a third observer is employed to estimate the positions in this common frame, using the estimated yaw orientations. The stability of the whole system is investigated and numerical simulations validate the theoretical findings.

## I. INTRODUCTION

Cooperative localization from relative sensing is a relevant topic in the multi-robot community [1]–[6]. It is especially important to deploy highly autonomous systems in unstructured environments, such as inside buildings, underwater, underground, or even in deep space, where centralized sensing facilities, such as GPS and Motion Capture, are not available and the robots can only rely on local onboard sensing and communication with neighboring robots. Cooperative control challenges often demand substantial coordination among robots, mandating a shared comprehension of specific physical quantities. This often entails establishing a consensus on a *shared frame* to facilitate the exchange of information initially captured in their *local frames* (e.g. for mapping or target tracking applications). Absence of shared orientation necessitates either global orientation estimation [1]–[4] or coordinate frame alignment [7], [8]. Previous studies primarily address orientation estimation when relative orientation measurements are available [1]–[4], [9]. Fewer works, however, tackle scenarios where solely body-frame relative position or bearing measurements are accessible [5], [10]–[12]. Typically, agents’ orientation can be estimated uniquely up to a coordinated rotation. Estimating this unknown coordinated rotation becomes feasible if at least one agent in the network measures its orientation relative to the world frame.

In prior works [1], [2], [9], [13], algorithms are proposed to estimate the global orientation of individual agents from relative orientation measurements, demonstrating almost global convergence. These algorithms rely on defining linear dynamics depending on a matrix which is shown to be *similar*

to a Laplacian matrix. The authors consider both the case in which the orientation belongs to the unit sphere $S^1$ [1] and the case in which the orientation belongs to $SO(3)$ [2], [9], [13]. Addressing noise-affected relative orientation measurements, [4] introduces a locally optimal maximum likelihood estimator.

In another approach by [5], an observer estimates the position and yaw orientation of a group of robots in a shared frame using body-frame bearing measurements. This method uses gradient descent for *infinitesimally bearing rigid* formations, exhibiting local convergence. Meanwhile, works such as [10], [11], present algorithms for estimating orientation in a shared frame from body-frame bearing measurements, relying on an *undirected sensing graph* to recover relative orientations among robots from reciprocal measurements. In [12], an algorithm based on distributed Riemannian gradient descent was proposed to estimate the full 3D rotation in a specific setup in which either the edges are symmetric (i.e. *undirected sensing graph*) or for any edge $e_{ij}$ there is an auxiliary node $k$ which is measured by both the robots $i$ and $j$.

In this work, we introduce an observer to estimate the yaw orientation and positions of a fleet of Unmanned Aerial Vehicles (UAVs) operating in $\mathbb{R}^3 \times S^1$ using body-frame relative position measurements. While our approach assumes the communication graph to be undirected, it differs from previous works [10]–[12] by not requiring the sensing graph to be undirected as well. This relaxed assumption is crucial for practical robotic applications, considering constraints posed by sensors with limited field of view (FoV). Instead, we introduce a *persistency of excitation* (PE) condition, requiring the robots to have non-zero velocity on the $x-y$ plane, which is a mild assumption in robotic applications. Our scheme comprises three distinct steps:

1. An adaptive observer estimating relative yaw corresponding to persistently exciting edges derived from body-frame relative position measurements.

2. A modification of the observer introduced in [1] to estimate the robots' yaw in a shared frame from the estimated relative yaws. Notably, our adaptation accounts for time-varying orientation and imperfect measurements, unlike [1].

3. An observer estimating positions in a shared frame, using estimated yaw orientations and body-frame relative position measurements.

We point out that considering only planar orientation is justified by the fact that typically, the attitude of UAVs

The authors are with CNRS, Univ Rennes, Inria, IRISA, Rennes, France.
{nicola.de-carli, prg}@irisa.fr; esteban.restrepo@inria.fr

This work was supported by the ANR-20-CHIA-0017 project "MULTI-SHARED"
# Adaptive Observer from Body-Frame Relative Position Measurements for Cooperative Localization

Nicola De Carli, Esteban Restrepo, Paolo Robuffo Giordano

***Abstract***— In this work, we propose an observer scheme to estimate in a common frame the position and yaw orientation of a group of robots from *body-frame* relative position measurements. The state of the robots is represented by their position and yaw orientation. The graph representing the sensing interaction among the robots is directed and it is only required to be weakly connected in addition to satisfy certain *persistency of excitation* conditions. Our proposed observer scheme consists of three distinct components. Firstly, an adaptive observer is used to estimate the relative yaw corresponding to each persistently exciting edge. Subsequently, an observer is used to estimate the yaw orientation in a common frame based on these estimated relative yaws. Finally, a third observer is employed to estimate the positions in this common frame, using the estimated yaw orientations. The stability of the whole system is investigated and numerical simulations validate the theoretical findings.

## I. INTRODUCTION

Cooperative localization from relative sensing is a relevant topic in the multi-robot community [1]–[6]. It is especially important to deploy highly autonomous systems in unstructured environments, such as inside buildings, underwater, underground, or even in deep space, where centralized sensing facilities, such as GPS and Motion Capture, are not available and the robots can only rely on local onboard sensing and communication with neighboring robots. Cooperative control challenges often demand substantial coordination among robots, mandating a shared comprehension of specific physical quantities. This often entails establishing a consensus on a *shared frame* to facilitate the exchange of information initially captured in their *local frames* (e.g. for mapping or target tracking applications). Absence of shared orientation necessitates either global orientation estimation [1]–[4] or coordinate frame alignment [7], [8]. Previous studies primarily address orientation estimation when relative orientation measurements are available [1]–[4], [9]. Fewer works, however, tackle scenarios where solely body-frame relative position or bearing measurements are accessible [5], [10]–[12]. Typically, agents’ orientation can be estimated uniquely up to a coordinated rotation. Estimating this unknown coordinated rotation becomes feasible if at least one agent in the network measures its orientation relative to the world frame.

In prior works [1], [2], [9], [13], algorithms are proposed to estimate the global orientation of individual agents from relative orientation measurements, demonstrating almost global convergence. These algorithms rely on defining linear dynamics depending on a matrix which is shown to be *similar*

to a Laplacian matrix. The authors consider both the case in which the orientation belongs to the unit sphere $S^1$ [1] and the case in which the orientation belongs to $SO(3)$ [2], [9], [13]. Addressing noise-affected relative orientation measurements, [4] introduces a locally optimal maximum likelihood estimator.

In another approach by [5], an observer estimates the position and yaw orientation of a group of robots in a shared frame using body-frame bearing measurements. This method uses gradient descent for *infinitesimally bearing rigid* formations, exhibiting local convergence. Meanwhile, works such as [10], [11], present algorithms for estimating orientation in a shared frame from body-frame bearing measurements, relying on an *undirected sensing graph* to recover relative orientations among robots from reciprocal measurements. In [12], an algorithm based on distributed Riemannian gradient descent was proposed to estimate the full 3D rotation in a specific setup in which either the edges are symmetric (i.e. *undirected sensing graph*) or for any edge $e_{ij}$ there is an auxiliary node $k$ which is measured by both the robots $i$ and $j$.

In this work, we introduce an observer to estimate the yaw orientation and positions of a fleet of Unmanned Aerial Vehicles (UAVs) operating in $\mathbb{R}^3 \times S^1$ using body-frame relative position measurements. While our approach assumes the communication graph to be undirected, it differs from previous works [10]–[12] by not requiring the sensing graph to be undirected as well. This relaxed assumption is crucial for practical robotic applications, considering constraints posed by sensors with limited field of view (FoV). Instead, we introduce a *persistency of excitation* (PE) condition, requiring the robots to have non-zero velocity on the $x-y$ plane, which is a mild assumption in robotic applications. Our scheme comprises three distinct steps:

1. An adaptive observer estimating relative yaw corresponding to persistently exciting edges derived from body-frame relative position measurements.

2. A modification of the observer introduced in [1] to estimate the robots’ yaw in a shared frame from the estimated relative yaws. Notably, our adaptation accounts for time-varying orientation and imperfect measurements, unlike [1].

3. An observer estimating positions in a shared frame, using estimated yaw orientations and body-frame relative position measurements.

We point out that considering only planar orientation is justified by the fact that typically, the attitude of UAVs

The authors are with CNRS, Univ Rennes, Inria, IRISA, Rennes, France.
{nicola.de-carli, prg}@irisa.fr; esteban.restrepo@inria.fr

This work was supported by the ANR-20-CHIA-0017 project "MULTI-SHARED"
can be retrieved from Inertial Measurement Units (IMUs) measurements using well-established observers such as [14]. However, in scenarios like indoors, the reliability of the magnetometer, which offers yaw measurements, can diminish, resulting in yaw estimation drift.

The paper is organized as follows. In section II, we give the necessary preliminaries and the statement of the problem. In section III, we present the proposed observer and we study its stability properties. In section IV and V respectively, we show the simulation results and we give the conclusions and future directions.

## II. PRELIMINARIES

Similarly to previous works [5], [7], we consider a group of N UAVs modeled as simple first order kinematic system with known body-frame velocities and yaw rate commands $\mathbf{u}_i := [\mathbf{v}_i^\top \quad \omega_i]^\top \in \mathbb{R}^4$:

$$ \begin{bmatrix} \dot{\mathbf{p}}_i \\ \dot{\psi}_i \end{bmatrix} = \begin{bmatrix} \mathbf{R}_i & \mathbf{0} \\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} \mathbf{v}_i \\ \omega_i \end{bmatrix} \quad (1) $$

where $\mathbf{p}_i \in \mathbb{R}^3$ is the robot position, $\psi_i \in (-\pi, \pi]$ is the yaw angle and $\mathbf{R}_i := \mathbf{R}_z(\psi_i) \in SO(3)$ is the associated rotation matrix around the z-axis, with $SO(3) := \{\mathbf{R} \in \mathbb{R}^{3 \times 3} | \mathbf{R}^\top \mathbf{R} = \mathbf{I}_3, \det(\mathbf{R}) = 1\}$. Additionally, we assume $\mathbf{v}_i \in \Upsilon_i \subset \mathbb{R}^3$ (and $\omega_i \in W_i \subset \mathbb{R}$), where $\Upsilon_i$ (and $W_i$) is a compact set, and both $\mathbf{v}_i$ and $\omega_i$ are continuously differentiable.

Each robot is assumed to be capable of exchanging data over a communication channel and to have a camera sensor and a sensor providing relative distance measurements (e.g., an RGB-D camera). These sensors combined provide the relative position of robot j in the i-th robot body-frame:

$$ {}^i\mathbf{p}_{ij} := \mathbf{R}_i^\top (\mathbf{p}_j - \mathbf{p}_i) \quad (2) $$

We model the interactions among the robots using a directed sensing graph $\mathcal{G}_s := (\mathcal{V}, \mathcal{E}_s)$ and an undirected communication graph $\mathcal{G}_c := (\mathcal{V}, \mathcal{E}_c)$, where $\mathcal{V} = \{0, 1, ..., N-1\}$ is the vertex set and $\mathcal{E}_* \subseteq \mathcal{V} \times \mathcal{V}$ is the edge set. A directed sensing edge $e_{sk} = (i, j) \in \mathcal{E}_s$ implies that robot i can sense robot j, the opposite is not necessarily true. This assumption is motivated by the fact that employing a camera sensor naturally induces a directed sensing graph due to FoV limitations. We consider the communication graph to correspond to the undirected counterpart of the sensing graph, hence, if robot i can measure robot j then the two robots can communicate in a bidirectional way and there exist both the edges $e_{ck} = (i, j) \in \mathcal{E}_c$ and $e_{ck} = (j, i) \in \mathcal{E}_c$.

## III. OBSERVER DESIGN

For robots to be able to cooperate, it is of paramount importance to be localized one relative to the other. Furthermore, it is also highly desirable to share a common frame in which to express shared physical quantities to facilitate the exchange of locally available information. In this section, we present a cascaded observer designed to estimate the position and orientation of robots within a common frame.

The proposed observer is composed by the following three systems in cascade: (i) an adaptive observer which is used

to estimate the relative yaw orientation $\psi_{ij} := \psi_j - \psi_i$, among each couple of neighboring robots from the body-frame relative position measurements, (ii) an observer which uses the estimated relative yaws to estimate the yaw $\psi_i$ of each robot in a common frame and (iii) an observer which uses the estimated yaw measurements in a common frame and the body-frame position measurements to estimate the position $\mathbf{p}_i$ of each robot in a common frame. The stability of each observer and of their interconnection is studied.

### A. Relative State Observer

In this section, we formulate the adaptive observer used to estimate the relative yaw orientation among each pair of neighboring robots. First of all, we consider the relative state among two robots given by ($^i\mathbf{p}_{ij}, \psi_{ij}$) and we write down the relative state dynamics:

$$ \begin{aligned} {}^i\dot{\mathbf{p}}_{ij} &= -\mathbf{v}_i - \mathbf{S}_{e3}\omega_i^i\mathbf{p}_{ij} + {}^i\mathbf{R}_j\mathbf{v}_j \\ \dot{\psi}_{ij} &= \omega_{ij} \end{aligned} \quad (3) $$

where $\mathbf{S}_{e3} := [\mathbf{e}_3]_\times^1$, $^i\mathbf{R}_j := \mathbf{R}_i^\top \mathbf{R}_j = \mathbf{R}_z(\psi_{ij})$ and $\omega_{ij} := \omega_j - \omega_i$.

We point out that $^i\mathbf{p}_{ij}$ is directly measured, $\mathbf{v}_j$ and $\omega_j$ can be communicated, the only unknown is $\psi_{ij}$, which affects the dynamics of $^i\mathbf{p}_{ij}$. The idea is then to reformulate the dynamics of $^i\mathbf{p}_{ij}$ for the relative orientation to appear in a more convenient way by changing parameterization so that we can formulate an adaptive observer. In particular, we define $\mathbf{z}_{\psi_{ij}} := [\cos(\psi_{ij}) \quad \sin(\psi_{ij})]^\top \in \mathbb{S}^1$, which is a representation of the relative orientation among the two robots using sphere coordinates, with $\mathbb{S}^d := \{\mathbf{x} \in \mathbb{R}^{d+1} | \|\mathbf{x}\| = 1\}$.

We can now rewrite the relative position dynamics as:

$$ {}^i\dot{\mathbf{p}}_{ij} = -\mathbf{v}_i - \mathbf{S}_{e3}\omega_i^i\mathbf{p}_{ij} + v_{jz}\mathbf{e}_3 + \bar{\mathbf{V}}_j\mathbf{z}_{\psi_{ij}} \quad (4) $$

with

$$ \bar{\mathbf{V}}_j(t) := \begin{bmatrix} v_{jx}(t) & -v_{jy}(t) \\ v_{jy}(t) & v_{jx}(t) \end{bmatrix} \quad (5) $$

and $v_{jx}$, $v_{jy}$ and $v_{jz}$ representing respectively the x, y and z components of $\mathbf{v}_j$. It is worth noting that, as $\bar{\mathbf{V}}_j$ is a linear transformation of $\mathbf{v}_j$, and given the compactness of $\Upsilon_j$ and the continuous differentiability of $\mathbf{v}_j$, $\bar{\mathbf{V}}_j$ is uniformly bounded and globally Lipschitz over $\Upsilon_j$. Consequently, both $\|\bar{\mathbf{V}}_j(t)\|$ and $\|\dot{\bar{\mathbf{V}}}_j(t)\|$ are uniformly bounded. Contrary to the basic adaptive setup, the unknown $\mathbf{z}_{\psi_{ij}}$ is not constant, but its dynamics are given by

$$ \dot{\mathbf{z}}_{\psi_{ij}} = \begin{bmatrix} -\omega_{ij} \sin(\omega_{ij}t) \\ \omega_{ij} \cos(\omega_{ij}t) \end{bmatrix} = \mathbf{\Omega}_{ij} \mathbf{z}_{\psi_{ij}} \quad (6) $$

with skew-symmetric $\mathbf{\Omega}_{ij} := \begin{bmatrix} 0 & -\omega_{ij} \\ \omega_{ij} & 0 \end{bmatrix}$.

Then, we formulate an adaptive observer as follows:

$$ \begin{aligned} {}^i\dot{\mathbf{p}}_{ij} &= -\mathbf{v}_i - \mathbf{S}_{e3}\omega_i^i\mathbf{p}_{ij} + v_{jz}\mathbf{e}_3 + \bar{\mathbf{V}}_j\hat{\mathbf{z}}_{\psi_{ij}} - k_{pe}^i\mathbf{e}_{p_{ij}} \\ \dot{\mathbf{z}}_{\psi_{ij}} &= \mathbf{\Omega}_{ij}\hat{\mathbf{z}}_{\psi_{ij}} - k_{\psi}\bar{\mathbf{V}}_j^\top i\mathbf{e}_{p_{ij}} \end{aligned} \quad (7) $$

with $^i\mathbf{e}_{p_{ij}} = {}^i\hat{\mathbf{p}}_{ij} - {}^i\mathbf{p}_{ij}$ and $k_{pe}, k_{\psi} > 0$.

$^1[\cdot]_\times$ is defined such that $[\cdot]_\times \mathbf{y} = \mathbf{x} \times \mathbf{y}$ is the vector cross product.
can be retrieved from Inertial Measurement Units (IMUs) measurements using well-established observers such as [14]. However, in scenarios like indoors, the reliability of the magnetometer, which offers yaw measurements, can diminish, resulting in yaw estimation drift.

The paper is organized as follows. In section II, we give the necessary preliminaries and the statement of the problem. In section III, we present the proposed observer and we study its stability properties. In section IV and V respectively, we show the simulation results and we give the conclusions and future directions.

## II. PRELIMINARIES

Similarly to previous works [5], [7], we consider a group of N UAVs modeled as simple first order kinematic system with known body-frame velocities and yaw rate commands $\mathbf{u}_i := [\mathbf{v}_i^\top \quad \omega_i]^\top \in \mathbb{R}^4$:

$$ \begin{bmatrix} \dot{\mathbf{p}}_i \\ \dot{\psi}_i \end{bmatrix} = \begin{bmatrix} \mathbf{R}_i & \mathbf{0} \\ \mathbf{0} & 1 \end{bmatrix} \begin{bmatrix} \mathbf{v}_i \\ \omega_i \end{bmatrix} \quad (1) $$

where $\mathbf{p}_i \in \mathbb{R}^3$ is the robot position, $\psi_i \in (-\pi, \pi]$ is the yaw angle and $\mathbf{R}_i := \mathbf{R}_z(\psi_i) \in SO(3)$ is the associated rotation matrix around the z-axis, with $SO(3) := \{\mathbf{R} \in \mathbb{R}^{3 \times 3} | \mathbf{R}^\top \mathbf{R} = \mathbf{I}_3, \det(\mathbf{R}) = 1\}$. Additionally, we assume $\mathbf{v}_i \in \Upsilon_i \subset \mathbb{R}^3$ (and $\omega_i \in W_i \subset \mathbb{R}$), where $\Upsilon_i$ (and $W_i$) is a compact set, and both $\mathbf{v}_i$ and $\omega_i$ are continuously differentiable.

Each robot is assumed to be capable of exchanging data over a communication channel and to have a camera sensor and a sensor providing relative distance measurements (e.g., an RGB-D camera). These sensors combined provide the relative position of robot j in the i-th robot body-frame:

$$ {}^i\mathbf{p}_{ij} := \mathbf{R}_i^\top (\mathbf{p}_j - \mathbf{p}_i) \quad (2) $$

We model the interactions among the robots using a directed sensing graph $\mathcal{G}_s := (\mathcal{V}, \mathcal{E}_s)$ and an undirected communication graph $\mathcal{G}_c := (\mathcal{V}, \mathcal{E}_c)$, where $\mathcal{V} = \{0, 1, ..., N-1\}$ is the vertex set and $\mathcal{E}_* \subseteq \mathcal{V} \times \mathcal{V}$ is the edge set. A directed sensing edge $e_{sk} = (i, j) \in \mathcal{E}_s$ implies that robot i can sense robot j, the opposite is not necessarily true. This assumption is motivated by the fact that employing a camera sensor naturally induces a directed sensing graph due to FoV limitations. We consider the communication graph to correspond to the undirected counterpart of the sensing graph, hence, if robot i can measure robot j then the two robots can communicate in a bidirectional way and there exist both the edges $e_{ck} = (i, j) \in \mathcal{E}_c$ and $e_{ck} = (j, i) \in \mathcal{E}_c$.

## III. OBSERVER DESIGN

For robots to be able to cooperate, it is of paramount importance to be localized one relative to the other. Furthermore, it is also highly desirable to share a common frame in which to express shared physical quantities to facilitate the exchange of locally available information. In this section, we present a cascaded observer designed to estimate the position and orientation of robots within a common frame.

The proposed observer is composed by the following three systems in cascade: (i) an adaptive observer which is used

to estimate the relative yaw orientation $\psi_{ij} := \psi_j - \psi_i$, among each couple of neighboring robots from the body-frame relative position measurements, (ii) an observer which uses the estimated relative yaws to estimate the yaw $\psi_i$ of each robot in a common frame and (iii) an observer which uses the estimated yaw measurements in a common frame and the body-frame position measurements to estimate the position $\mathbf{p}_i$ of each robot in a common frame. The stability of each observer and of their interconnection is studied.

### A. Relative State Observer

In this section, we formulate the adaptive observer used to estimate the relative yaw orientation among each pair of neighboring robots. First of all, we consider the relative state among two robots given by ($^i\mathbf{p}_{ij}$, $\psi_{ij}$) and we write down the relative state dynamics:

$$ \begin{aligned} {}^i\dot{\mathbf{p}}_{ij} &= -\mathbf{v}_i - \mathbf{S}_{e3}\omega_i^i \mathbf{p}_{ij} + {}^i\mathbf{R}_j \mathbf{v}_j \\ \dot{\psi}_{ij} &= \omega_{ij} \end{aligned} \quad (3) $$

where $\mathbf{S}_{e3} := [\mathbf{e}_3]_\times^1$, $^i\mathbf{R}_j := \mathbf{R}_i^\top \mathbf{R}_j = \mathbf{R}_z(\psi_{ij})$ and $\omega_{ij} := \omega_j - \omega_i$.

We point out that $^i\mathbf{p}_{ij}$ is directly measured, $\mathbf{v}_j$ and $\omega_j$ can be communicated, the only unknown is $\psi_{ij}$, which affects the dynamics of $^i\mathbf{p}_{ij}$. The idea is then to reformulate the dynamics of $^i\mathbf{p}_{ij}$ for the relative orientation to appear in a more convenient way by changing parameterization so that we can formulate an adaptive observer. In particular, we define $\mathbf{z}_{\psi_{ij}} := [\cos(\psi_{ij}) \quad \sin(\psi_{ij})]^\top \in \mathbb{S}^1$, which is a representation of the relative orientation among the two robots using sphere coordinates, with $\mathbb{S}^d := \{\mathbf{x} \in \mathbb{R}^{d+1} | \|\mathbf{x}\| = 1\}$.

We can now rewrite the relative position dynamics as:

$$ {}^i\dot{\mathbf{p}}_{ij} = -\mathbf{v}_i - \mathbf{S}_{e3}\omega_i^i \mathbf{p}_{ij} + v_{jz}\mathbf{e}_3 + \bar{\mathbf{V}}_j\mathbf{z}_{\psi_{ij}} \quad (4) $$

with

$$ \bar{\mathbf{V}}_j(t) := \begin{bmatrix} v_{jx}(t) & -v_{jy}(t) \\ v_{jy}(t) & v_{jx}(t) \end{bmatrix} \quad (5) $$

and $v_{jx}$, $v_{jy}$ and $v_{jz}$ representing respectively the x, y and z components of $\mathbf{v}_j$. It is worth noting that, as $\bar{\mathbf{V}}_j$ is a linear transformation of $\mathbf{v}_j$, and given the compactness of $\Upsilon_j$ and the continuous differentiability of $\mathbf{v}_j$, $\bar{\mathbf{V}}_j$ is uniformly bounded and globally Lipschitz over $\Upsilon_j$. Consequently, both $\|\bar{\mathbf{V}}_j(t)\|$ and $\|\dot{\bar{\mathbf{V}}}_j(t)\|$ are uniformly bounded. Contrary to the basic adaptive setup, the unknown $\mathbf{z}_{\psi_{ij}}$ is not constant, but its dynamics are given by

$$ \dot{\mathbf{z}}_{\psi_{ij}} = \begin{bmatrix} -\omega_{ij} \sin(\omega_{ij}t) \\ \omega_{ij} \cos(\omega_{ij}t) \end{bmatrix} = \mathbf{\Omega}_{ij} \mathbf{z}_{\psi_{ij}} \quad (6) $$

with skew-symmetric $\mathbf{\Omega}_{ij} := \begin{bmatrix} 0 & -\omega_{ij} \\ \omega_{ij} & 0 \end{bmatrix}$.

Then, we formulate an adaptive observer as follows:

$$ \begin{aligned} {}^i\dot{\mathbf{p}}_{ij} &= -\mathbf{v}_i - \mathbf{S}_{e3}\omega_i^i \mathbf{p}_{ij} + v_{jz}\mathbf{e}_3 + \bar{\mathbf{V}}_j\hat{\mathbf{z}}_{\psi_{ij}} - k_{pe}^i \mathbf{e}_{p_{ij}} \\ \dot{\mathbf{z}}_{\psi_{ij}} &= \mathbf{\Omega}_{ij}\hat{\mathbf{z}}_{\psi_{ij}} - k_{\psi}\bar{\mathbf{V}}_j^\top \mathbf{e}_{p_{ij}} \end{aligned} \quad (7) $$

with $^i\mathbf{e}_{p_{ij}} = {}^i\hat{\mathbf{p}}_{ij} - {}^i\mathbf{p}_{ij}$ and $k_{pe}, k_{\psi} > 0$.

$^1[\cdot]_\times$ is defined such that $[\mathbf{x}]_\times \mathbf{y} = \mathbf{x} \times \mathbf{y}$ is the vector cross product.
Moreover, if both the directed edges (i, j) and (j, i) exist, the update of the relative yaw can benefit from both edges. One can consider that $z_{\psi_{ji}} = Z z_{\psi_{ij}}$, with $Z := \begin{bmatrix} 1 & 0 \\ 0 & -1 \end{bmatrix}$. Then, the update for the bidirectional edges can be written as:

$$ 
\begin{aligned}
{}^i \dot{\mathbf{p}}_{ij} &= -\mathbf{v}_i - \mathbf{S}_{e3} \omega_i^i \mathbf{p}_{ij} + v_{jz} \mathbf{e}_3 + \bar{\mathbf{V}}_j \hat{\mathbf{z}}_{\psi_{ij}} - k_{pe}^i \mathbf{e}_{p_{ij}} \\
{}^j \dot{\mathbf{p}}_{ji} &= -\mathbf{v}_j - \mathbf{S}_{e3} \omega_j^j \mathbf{p}_{ji} + v_{iz} \mathbf{e}_3 + \bar{\mathbf{V}}_i Z \hat{\mathbf{z}}_{\psi_{ij}} - k_{pe}^j \mathbf{e}_{p_{ji}} \\
\dot{\mathbf{z}}_{\psi_{ij}} &= \mathbf{\Omega}_{ij} \hat{\mathbf{z}}_{\psi_{ij}} - k_{\psi} (\bar{\mathbf{V}}_j^{\top i} \mathbf{e}_{p_{ij}} + Z \bar{\mathbf{V}}_i^{\top j} \mathbf{e}_{p_{ji}})
\end{aligned} 
\quad (8) $$

**Remark 1.** We point out that, from two reciprocal measurements, if $\mathbf{p}_j - \mathbf{p}_j$ is not aligned with the z-axis, it is also possible to algebraically compute the relative yaw among the two robots. However, since, in practice, measurements are affected by noise, we avoid the algebraic computation.

In the following stability analysis, we consider the case of a single directed edge, but the proof for the bidirectional edge follows the same lines.

We define the yaw error $e_{\psi_{ij}} := \hat{\mathbf{z}}_{\psi_{ij}} - \mathbf{z}_{\psi_{ij}}$, where $e_{\psi_{ij}}$ is the chordal distance on $\mathbb{S}^1$. Then, the error dynamics are described by the following linear time-varying system:

$$ \begin{bmatrix} {}^i \dot{\mathbf{e}}_{p_{ij}} \\ \dot{\mathbf{e}}_{\psi_{ij}} \end{bmatrix} = \begin{bmatrix} -k_{pe} \mathbf{I}_3 \\ -k_{\psi} \bar{\mathbf{V}}_j^\top \end{bmatrix} \begin{bmatrix} {}^i \mathbf{e}_{p_{ij}} \\ \mathbf{e}_{\psi_{ij}} \end{bmatrix} + \begin{bmatrix} \mathbf{0} \\ \mathbf{\Omega}_{ij} \mathbf{e}_{\psi_{ij}} \end{bmatrix} \quad (9) $$

**Theorem 1.** Since $\|\bar{\mathbf{V}}_j(t)\|$ and $\|\dot{\mathbf{V}}_j(t)\|$ are uniformly bounded and assuming that, there exists $T, \mu > 0$, such that $\bar{\mathbf{V}}_j(t)$, as given by (5), satisfies the persistency of excitation condition

$$ \int_t^{t+T} \bar{\mathbf{V}}_j^\top(\tau) \bar{\mathbf{V}}_j(\tau) d\tau \geq \mu \mathbf{I}_2 \quad \forall t \geq 0, \quad (10) $$

then, $(e_{p_{ij}}, e_{\psi_{ij}}) = (\mathbf{0}, \mathbf{0})$ is a globally exponentially stable equilibrium point of (9).

**Proof.** The proof uses conventional adaptive control reasoning [15], [16]. We outline the main steps demonstrating that the dynamics of $z_{\psi_{ij}}$ do not impact the convergence proof. Consider the quadratic Lyapunov function $V({}^i e_{p_{ij}}, e_{\psi_{ij}}) := \frac{1}{2} {}^i e_{p_{ij}}^\top {}^i e_{p_{ij}} + \frac{1}{2k_{\psi}} e_{\psi_{ij}}^\top e_{\psi_{ij}}$. Its derivative is given by

$$ \begin{aligned} \dot{\mathbf{V}} = {}^i e_{p_{ij}}^\top \bar{\mathbf{V}}_j e_{\psi_{ij}} - k_{pe} \| {}^i e_{p_{ij}} \|^2 - e_{\psi_{ij}}^\top \bar{\mathbf{V}}_j^{\top i} e_{p_{ij}} \\ + \frac{1}{k_{\psi}} e_{\psi_{ij}}^\top \mathbf{\Omega}_{ij} e_{\psi_{ij}} = -k_{pe} \| {}^i e_{p_{ij}} \|^2 \le 0 \end{aligned} \quad (11) $$

where we used the fact that $\mathbf{\Omega}_{ij}$ is skew-symmetric. The negative semi-definiteness of $\dot{\mathbf{V}}$, implies uniform stability of the origin. Leveraging Barbalat's lemma demonstrates convergence to zero of $e_{p_{ij}}$. The PE condition (10) ensures convergence to zero of the error $e_{\psi_{ij}}$. Global exponential stability follows from the linear time-varying nature of the system (see the Persistency of Excitation lemma in [15]). □

**Remark 2.** Condition (10) is satisfied if the velocity on the xy-plane is non-zero for a time interval of non-zero measure over each period $T$. In fact,

$$ \bar{\mathbf{V}}_j(t)^\top \bar{\mathbf{V}}_j(t) = \begin{bmatrix} v_{jx}^2(t) + v_{jy}^2(t) & 0 \\ 0 & v_{jy}^2(t) + v_{jx}^2(t) \end{bmatrix}, \quad (12) $$

which is clearly positive definite if the velocity on the xy-plane is non-zero.

**Remark 3.** We point out that, given the update law (8) for $\hat{\mathbf{z}}_{\psi_{ij}}$, in presence of PE, the error $e_{\psi_{ij}}$ will asymptotically go to zero, but the norm of $\hat{\mathbf{z}}_{\psi_{ij}}$ will not necessarily be 1 at each instant, meaning that it will not represent a valid orientation. One can, anyway, extract the closest angle $\psi_{ij}$ representing the orientation after renormalizing $\hat{\mathbf{z}}_{\psi_{ij}}$.

## B. Common Frame Yaw Observer

In the preceding section, we introduced an observer able to estimate the relative orientation among neighboring robots. While knowledge of their relative orientation allows for coordination between two robots, it often falls short, necessitating awareness of position and orientation within a shared frame. Among its advantages, for instance, is the ability to adapt to a time-varying graph; upon the creation of a new edge, two robots can initialize their relative orientation in an informed way.

To estimate the yaw orientations in a common frame, we leverage the algorithm proposed in [1], designed for estimating the yaw within a shared frame based on relative yaw measurements. In our context, we substitute the relative yaw measurements with estimated relative yaw orientations obtained using (8) and, moreover, we consider time-varying orientations.

In the design of this observer, we represent the orientation of the i-th robot as a unit complex number $z_i := e^{i\psi_i} \in \mathbb{C}$, where $i = \sqrt{-1}$, and, similarly, the relative orientation between two robots as $z_{ij} := e^{i\psi_{ij}} = [1 \ i] z_{\psi_{ij}} \in \mathbb{C}$. While equivalent to the representation used in the previous section, we adopt this representation for analytical simplicity of the proof and coherence with [1].

The objective of the observer is articulated as follows: for a common complex value $\alpha \in \mathbb{C}$ and robot orientations $z_i(t)$, devise an estimation law such that $\angle \hat{z}_i \to \angle z_i + \angle \alpha \ \forall i$ as $t \to \infty$, i.e. the estimated robot orientations should converge to the real ones up to a common rotation.

The proposed estimation law takes the form:

$$ \dot{\hat{z}}_i = i\omega_i \hat{z}_i + k_z \sum_{j \in \mathcal{N}_i} (\bar{z}_{ij} \hat{z}_j - \hat{z}_i) \quad (13) $$

where $k_z > 0$ and $\bar{z}_{ij}$ represents the complex conjugate of $z_{ij}$. It differs with respect to [1] for the fact that the orientation of the robots is time-varying. Defining $\hat{z} := [\hat{z}_1, \dots, \hat{z}_N]^\top$ and $\omega := [\omega_1, \dots, \omega_N]^\top$, the observer dynamics can be written in matrix form as:

$$ \dot{\hat{z}} = i \operatorname{diag}(\omega) \hat{z} - k_z \mathbf{L}_c \hat{z} = (i \operatorname{diag}(\omega) - k_z \mathbf{L}_c) \hat{z} \quad (14) $$

where

$$ (\mathbf{L}_c)_{ij} := \begin{cases} |\mathcal{N}_i| & \text{if } i = j \\ -\bar{z}_{ij} & \text{if } j \in \mathcal{N}_i \\ 0 & \text{otherwise} \end{cases} \quad (15) $$

and $|\mathcal{N}_i|$ is the cardinality of the neighbors set of the i-th robot.
Moreover, if both the directed edges (i, j) and (j, i) exist, the update of the relative yaw can benefit from both edges. One can consider that $z_{\psi_{ji}} = Z z_{\psi_{ij}}$, with $Z := \begin{bmatrix} 1 & 0 \\ 0 & -1 \end{bmatrix}$. Then, the update for the bidirectional edges can be written as:

$$ 
\begin{aligned}
{}^i \dot{\mathbf{p}}_{ij} &= -\mathbf{v}_i - \mathbf{S}_{e3} \omega_i^i \mathbf{p}_{ij} + \mathbf{v}_{jz} \mathbf{e}_3 + \bar{\mathbf{V}}_j \hat{\mathbf{z}}_{\psi_{ij}} - k_{pe}^i \mathbf{e}_{p_{ij}} \\
{}^j \dot{\mathbf{p}}_{ji} &= -\mathbf{v}_j - \mathbf{S}_{e3} \omega_j^j \mathbf{p}_{ji} + \mathbf{v}_{iz} \mathbf{e}_3 + \bar{\mathbf{V}}_i Z \hat{\mathbf{z}}_{\psi_{ij}} - k_{pe}^j \mathbf{e}_{p_{ji}} \\
\dot{\mathbf{z}}_{\psi_{ij}} &= \mathbf{\Omega}_{ij} \hat{\mathbf{z}}_{\psi_{ij}} - k_{\psi} (\bar{\mathbf{V}}_j^{\top i} \mathbf{e}_{p_{ij}} + Z \bar{\mathbf{V}}_i^{\top j} \mathbf{e}_{p_{ji}})
\end{aligned} 
\quad (8) $$

**Remark 1.** We point out that, from two reciprocal measurements, if $\mathbf{p}_j - \mathbf{p}_j$ is not aligned with the z-axis, it is also possible to algebraically compute the relative yaw among the two robots. However, since, in practice, measurements are affected by noise, we avoid the algebraic computation.

In the following stability analysis, we consider the case of a single directed edge, but the proof for the bidirectional edge follows the same lines.

We define the yaw error $e_{\psi_{ij}} := \hat{\psi}_{\psi_{ij}} - z_{\psi_{ij}}$, where $e_{\psi_{ij}}$ is the chordal distance on $\mathbb{S}^1$. Then, the error dynamics are described by the following linear time-varying system:

$$ \begin{bmatrix} {}^i \dot{\mathbf{e}}_{p_{ij}} \\ \dot{\mathbf{e}}_{\psi_{ij}} \end{bmatrix} = \begin{bmatrix} -k_{pe} \mathbf{I}_3 \\ -k_{\psi} \bar{\mathbf{V}}_j^{\top} \end{bmatrix} \begin{bmatrix} {}^i \mathbf{e}_{p_{ij}} \\ \mathbf{e}_{\psi_{ij}} \end{bmatrix} + \begin{bmatrix} \mathbf{0} \\ \mathbf{\Omega}_{ij} \mathbf{e}_{\psi_{ij}} \end{bmatrix} \quad (9) $$

**Theorem 1.** Since $\|\bar{\mathbf{V}}_j(t)\|$ and $\|\dot{\bar{\mathbf{V}}}_j(t)\|$ are uniformly bounded and assuming that, there exists $T, \mu > 0$, such that $\bar{\mathbf{V}}_j(t)$, as given by (5), satisfies the persistency of excitation condition

$$ \int_t^{t+T} \bar{\mathbf{V}}_j^{\top}(\tau) \bar{\mathbf{V}}_j(\tau) d\tau \geq \mu \mathbf{I}_2 \quad \forall t \geq 0, \quad (10) $$

then, $(e_{p_{ij}}, e_{\psi_{ij}}) = (0, 0)$ is a globally exponentially stable equilibrium point of (9).

**Proof.** The proof uses conventional adaptive control reasoning [15], [16]. We outline the main steps demonstrating that the dynamics of $z_{\psi_{ij}}$ do not impact the convergence proof. Consider the quadratic Lyapunov function $V({}^i e_{p_{ij}}, e_{\psi_{ij}}) := \frac{1}{2} {}^i e_{p_{ij}}^{\top} {}^i e_{p_{ij}} + \frac{1}{2k_{\psi}} e_{\psi_{ij}}^{\top} e_{\psi_{ij}}$. Its derivative is given by

$$ \begin{aligned} \dot{\mathbf{V}} = {}^i e_{p_{ij}}^{\top} \bar{\mathbf{V}}_j e_{\psi_{ij}} - k_{pe} \| {}^i e_{p_{ij}} \|^2 - e_{\psi_{ij}}^{\top} \bar{\mathbf{V}}_j^{\top i} e_{p_{ij}} \\ + \frac{1}{k_{\psi}} e_{\psi_{ij}}^{\top} \mathbf{\Omega}_{ij} e_{\psi_{ij}} = -k_{pe} \| {}^i e_{p_{ij}} \|^2 \le 0 \end{aligned} \quad (11) $$

where we used the fact that $\mathbf{\Omega}_{ij}$ is skew-symmetric. The negative semi-definiteness of $\dot{\mathbf{V}}$, implies uniform stability of the origin. Leveraging Barbalat's lemma demonstrates convergence to zero of $e_{p_{ij}}$. The PE condition (10) ensures convergence to zero of the error $e_{\psi_{ij}}$. Global exponential stability follows from the linear time-varying nature of the system (see the Persistency of Excitation lemma in [15]). □

**Remark 2.** Condition (10) is satisfied if the velocity on the xy-plane is non-zero for a time interval of non-zero measure over each period $T$. In fact,

$$ \bar{\mathbf{V}}_j(t)^{\top} \bar{\mathbf{V}}_j(t) = \begin{bmatrix} v_{jx}^2(t) + v_{jy}^2(t) & 0 \\ 0 & v_{jy}^2(t) + v_{jx}^2(t) \end{bmatrix}, \quad (12) $$

which is clearly positive definite if the velocity on the xy-plane is non-zero.

**Remark 3.** We point out that, given the update law (8) for $\hat{\mathbf{z}}_{\psi_{ij}}$, in presence of PE, the error $e_{\psi_{ij}}$ will asymptotically go to zero, but the norm of $\hat{\mathbf{z}}_{\psi_{ij}}$ will not necessarily be 1 at each instant, meaning that it will not represent a valid orientation. One can, anyway, extract the closest angle $\psi_{ij}$ representing the orientation after renormalizing $\hat{\mathbf{z}}_{\psi_{ij}}$.

## B. Common Frame Yaw Observer

In the preceding section, we introduced an observer able to estimate the relative orientation among neighboring robots. While knowledge of their relative orientation allows for coordination between two robots, it often falls short, necessitating awareness of position and orientation within a shared frame. Among its advantages, for instance, is the ability to adapt to a time-varying graph; upon the creation of a new edge, two robots can initialize their relative orientation in an informed way.

To estimate the yaw orientations in a common frame, we leverage the algorithm proposed in [1], designed for estimating the yaw within a shared frame based on relative yaw measurements. In our context, we substitute the relative yaw measurements with estimated relative yaw orientations obtained using (8) and, moreover, we consider time-varying orientations.

In the design of this observer, we represent the orientation of the i-th robot as a unit complex number $z_i := e^{i\psi_i} \in \mathbb{C}$, where $\iota = \sqrt{-1}$, and, similarly, the relative orientation between two robots as $z_{ij} := e^{i\psi_{ij}} = [1 \ \iota] z_{\psi_{ij}} \in \mathbb{C}$. While equivalent to the representation used in the previous section, we adopt this representation for analytical simplicity of the proof and coherence with [1].

The objective of the observer is articulated as follows: for a common complex value $\alpha \in \mathbb{C}$ and robot orientations $z_i(t)$, devise an estimation law such that $\angle \hat{z}_i \to \angle z_i + \angle \alpha \ \forall i$ as $t \to \infty$, i.e. the estimated robot orientations should converge to the real ones up to a common rotation.

The proposed estimation law takes the form:

$$ \dot{\hat{z}}_i = \iota \omega_i \hat{z}_i + k_z \sum_{j \in \mathcal{N}_i} (\bar{z}_{ij} \hat{z}_j - \hat{z}_i) \quad (13) $$

where $k_z > 0$ and $\bar{z}_{ij}$ represents the complex conjugate of $z_{ij}$. It differs with respect to [1] for the fact that the orientation of the robots is time-varying. Defining $\hat{z} := [\hat{z}_1, \dots, \hat{z}_N]^{\top}$ and $\omega := [\omega_1, \dots, \omega_N]^{\top}$, the observer dynamics can be written in matrix form as:

$$ \dot{\hat{z}} = \iota \mathrm{diag}(\omega) \hat{z} - k_z \mathbf{L}_c \hat{z} = (\iota \mathrm{diag}(\omega) - k_z \mathbf{L}_c) \hat{z} \quad (14) $$

where

$$ (\mathbf{L}_c)_{ij} := \begin{cases} |\mathcal{N}_i| & \text{if } i = j \\ -\bar{z}_{ij} & \text{if } j \in \mathcal{N}_i \\ 0 & \text{otherwise} \end{cases} \quad (15) $$

and $|\mathcal{N}_i|$ is the cardinality of the neighbors set of the i-th robot.
Now, we present the following Proposition, introduced in [1] and, to better understand the following developments, we provide a sketch of the proof.

**Proposition 1** ([1]). Zero is a simple eigenvalue of $L_c(t)$ with a corresponding eigenvector $z(t)$ if and only if the associated digraph has a spanning tree. Moreover, every eigenvalue, except for the zero eigenvalue, has strictly negative real part.

Define the matrix $D_z := \operatorname{diag}(z_1, ..., z_N) \in \mathbb{C}^{N \times N}$.
Notice that, since $z_i$ is a unit complex number, then $|z_i| = 1$.
Moreover, $D_z$ is a nonsingular matrix and its inverse is $D_z^{-1} = D_z^H$, where we indicated with $H$ the conjugate transpose. As $D_z$ is a nonsingular matrix, we define the similarity transformation $L = D_z^H L_c D_z$. Since $D_z$ is a diagonal matrix, the off-diagonal entries of $L$ are:

$$ (L)_{ij} = (D_z^H)_{ii} (L_c)_{ij} (D_z)_{jj} = -e^{-i\psi_i} e^{-i\psi_{ij}} e^{i\psi_j} \quad (16) \\ = -e^{i(-\psi_i - \psi_{ij} + \psi_j)} = -e^{i0} = -1 $$

In contrast, the diagonal entries are invariant under this transformation (i.e. $e^{-i\psi_i}|_{\mathcal{N}_i}e^{i\psi_i} = |_{\mathcal{N}_i}|$). This demonstrates that, $L$ is the classical unweighted Laplacian matrix, which, as well-known [17], [18], is positive semi-definite and if the graph has a spanning tree, then 0 is a simple eigenvalue corresponding to the right eigenvector $1_N = [1, ..., 1]^\top \in \mathbb{R}^N$. Since $L_c$ and $L$ are similar matrices, then zero is a simple eigenvalue of $L_c$ as well. Furthermore, since $0 = \hat{L}1_N = D_z^H L_c D_z 1_N = D_z^H L_c z$, it follows that, $z$ is the right eigenvector associated to the zero eigenvalue.

**Remark 4.** From (8), each robot gets an estimate of the relative orientation with respect to both the in- and out-neighboring robots in the directed graph $\mathcal{G}_s$. Hence, the Laplacian $L$ corresponds to the undirected graph $\mathcal{G}_c$ and the condition for zero to be a simple eigenvalue of $L_c$ reduces to the graph $\mathcal{G}_c$ being connected.

The following theorem extends the one from [1] to the case in which the yawrate of the robots may be different from zero.

**Theorem 2.** The observer (14) globally exponentially converges to $\hat{z}(t) = (z^H(0)\hat{z}(0)/N)z(t)$, if and only if the graph is connected.

*Proof.* Consider the change of coordinates $\hat{z} = D_z y$. Notice that, $y_i = \bar{z}_i \hat{z}_i$ corresponds to the rotation error on $\mathbb{S}^1$, where $y_i = 1$ represents the identity rotation $z_i = \hat{z}_i$. Unlike [1], we consider non-zero yaw rates for the robots, resulting in (i) an additional term in the dynamics (14) and (ii) a time-varying change-of-coordinates matrix. Consequently:

$$ \begin{aligned} \dot{\hat{z}} &= D_z \dot{y} + \dot{D}_z y = D_z \dot{y} + i \operatorname{diag}(\omega) \operatorname{diag}(z) y \\ &= D_z \dot{y} + i \operatorname{diag}(\omega) D_z y \end{aligned} \quad (17) $$

From (17) and (14)

$$ \begin{aligned} \dot{y} &= -k_z D_z^H L_c D_z y + i D_z^H \operatorname{diag}(\omega) D_z y - i D_z^H \operatorname{diag}(\omega) D_z y \\ &= -k_z L y \end{aligned} \quad (18) $$

for which the consensus subspace {$y_1 = y_2 = ... = y_N$} is a globally exponentially stable equilibrium set [19]. In particular, since $L$ is a symmetric Laplacian, $y(t) \rightarrow (1_N^T y(0)/N)1_N$, which corresponds to the same rotation error for each angle $\hat{z}_i$, i.e. $1_N^T y(0)/N = \alpha$. By applying again the change of coordinates:

$$ \begin{aligned} \hat{z}(t) &\rightarrow D_z(t) \left( \frac{1}{N} 1_N^T y(0) \right) 1_N \\ &= \left( \frac{1}{N} 1_N^T D_z(0)^H \hat{z}(0) \right) D_z(t) 1_N = \left( \frac{1}{N} z^H(0) \hat{z}(0) \right) z(t) \end{aligned} \quad (19) $$

We also note that, $\hat{z}(t) = 0$ is an undesired equilibrium of the system, as it does not represent a valid orientation. But $\hat{z}(t) \rightarrow 0$ if and only if $z^H(0)\hat{z}(0) = 0$. $\square$

The stability proof in Theorem 2 relies on Proposition 1, assuming perfect measurements $z_{ij}$. However, using $\hat{z}_{ij}$ from (8) in place of $z_{ij}$ may render the estimated relative orientations unrealizable. Consequently, the existence of a $z^* := [z_1^* \dots z_N^*]^\top \in \mathbb{C}^N$ compatible with all estimated $\hat{z}_{ij} \forall (i, j) \in \mathcal{E}$ is not guaranteed. Specifically, within a graph containing cycles, while (16) may hold for edges within a spanning tree, this might not hold for the remaining edges outside the spanning tree.

Let us denote $\hat{L}_c$ as the matrix derived similarly to (15) but using the estimated $\hat{z}_{ij}$ and $\hat{L} := D_z^H \hat{L}_c D_z^*$, in which $D_z^* := \operatorname{diag}(z_1^*, ..., z_N^*)$. For edges within cycles, instead of (16), we might encounter $(D_z^H \hat{L}_c D_z^*)_{ij} = -e^{i\hat{\psi}_{ij}}$ where $\hat{\psi}_{ij} = \psi_j^* - \psi_i^* - \hat{\psi}_{ij}$, with $\psi_i^* = \angle z_i^*$ and $\psi_j^* = \angle z_j^*$. Consequently,

$$ (\hat{L})_{ij} := \begin{cases} |\mathcal{N}_i| & \text{if } i=j \\ -1 & \text{if } (i,j) \in \mathcal{G}_{\tau} \\ -e^{i\hat{\psi}_{ij}} & \text{if } (i,j) \in \mathcal{G}_c \\ 0 & \text{otherwise} \end{cases} \quad (20) $$

where we used $\mathcal{G}_{\tau}$ and $\mathcal{G}_c$ to denote respectively a subgraph of $\mathcal{G}_c$ representing the considered spanning tree and a subgraph representing the remaining cycle edges. As $e^{i\hat{\psi}_{ij}} = e^{-i\psi_{ji}}$ and $|e^{i\hat{\psi}_{ij}}| = 1$, $\hat{L}$ emerges as a Hermitian weakly diagonally dominant matrix with positive elements on the diagonal, establishing its positive semidefiniteness. Consequently, (18), and hence (14), remain stable also using imperfect measurements $\hat{z}_{ij}$.

### C. Position Observer

In this section, we design an observer for the position of each robot within a common frame. This observer is added in cascade with respect to (14). The primary objective of this observer can be defined as follows: for a common translation $\bar{p} \in \mathbb{R}^3$, a common rotation $R_\alpha$ (where $\alpha \in \mathbb{C}$ remains consistent with the preceding section), alongside the real position $p_i$ and orientation $\angle z_i$, the aim is to devise an estimation law ensuring that $\hat{p}_i \rightarrow \bar{p} + R_\alpha^\top p_i \forall i$ as $t \rightarrow \infty$

<footnote n="2">²diag(·) represents a diagonal matrix of the input arguments and
blkdiag(·) represents a block diagonal matrix of the input arguments</footnote>
Now, we present the following Proposition, introduced in [1] and, to better understand the following developments, we provide a sketch of the proof.

**Proposition 1** ([1]). Zero is a simple eigenvalue of $L_c(t)$ with a corresponding eigenvector $z(t)$ if and only if the associated digraph has a spanning tree. Moreover, every eigenvalue, except for the zero eigenvalue, has strictly negative real part.

Define the matrix $D_z := \operatorname{diag}(z_1, ..., z_N) \in \mathbb{C}^{N \times N}$.
Notice that, since $z_i$ is a unit complex number, then $|z_i| = 1$.
Moreover, $D_z$ is a nonsingular matrix and its inverse is $D_z^{-1} = D_z^H$, where we indicated with $H$ the conjugate transpose. As $D_z$ is a nonsingular matrix, we define the similarity transformation $L = D_z^H L_c D_z$. Since $D_z$ is a diagonal matrix, the off-diagonal entries of $L$ are:

$$ (L)_{ij} = (D_z^H)_{ii} (L_c)_{ij} (D_z)_{jj} = -e^{-i\psi_i} e^{-i\psi_{ij}} e^{i\psi_j} \quad (16) \\ = -e^{i(-\psi_i - \psi_{ij} + \psi_j)} = -e^{i0} = -1 $$

In contrast, the diagonal entries are invariant under this transformation (i.e. $e^{-i\psi_i}|_{\mathcal{N}_i}e^{i\psi_i} = |_{\mathcal{N}_i}|$). This demonstrates that, $L$ is the classical unweighted Laplacian matrix, which, as well-known [17], [18], is positive semi-definite and if the graph has a spanning tree, then 0 is a simple eigenvalue corresponding to the right eigenvector $1_N = [1, ..., 1]^\top \in \mathbb{R}^N$. Since $L_c$ and $L$ are similar matrices, then zero is a simple eigenvalue of $L_c$ as well. Furthermore, since $0 = \hat{L}1_N = D_z^H L_c D_z 1_N = D_z^H L_c z$, it follows that, $z$ is the right eigenvector associated to the zero eigenvalue.

**Remark 4.** From (8), each robot gets an estimate of the relative orientation with respect to both the in- and out-neighboring robots in the directed graph $\mathcal{G}_s$. Hence, the Laplacian $L$ corresponds to the undirected graph $\mathcal{G}_c$ and the condition for zero to be a simple eigenvalue of $L_c$ reduces to the graph $\mathcal{G}_c$ being connected.

The following theorem extends the one from [1] to the case in which the yawrate of the robots may be different from zero.

**Theorem 2.** The observer (14) globally exponentially converges to $\hat{z}(t) = (z^H(0)\hat{z}(0)/N)z(t)$, if and only if the graph is connected.

*Proof.* Consider the change of coordinates $\hat{z} = D_z y$. Notice that, $y_i = \bar{z}_i \hat{z}_i$ corresponds to the rotation error on $\mathbb{S}^1$, where $y_i = 1$ represents the identity rotation $z_i = \hat{z}_i$. Unlike [1], we consider non-zero yaw rates for the robots, resulting in (i) an additional term in the dynamics (14) and (ii) a time-varying change-of-coordinates matrix. Consequently:

$$ \begin{aligned} \dot{\hat{z}} &= D_z \dot{y} + \dot{D}_z y = D_z \dot{y} + i \operatorname{diag}(\omega) \operatorname{diag}(z) y \\ &= D_z \dot{y} + i \operatorname{diag}(\omega) D_z y \end{aligned} \quad (17) $$

From (17) and (14)

$$ \begin{aligned} \dot{y} &= -k_z D_z^H L_c D_z y + i D_z^H \operatorname{diag}(\omega) D_z y - i D_z^H \operatorname{diag}(\omega) D_z y \\ &= -k_z L y \end{aligned} \quad (18) $$

for which the consensus subspace {$y_1 = y_2 = ... = y_N$} is a globally exponentially stable equilibrium set [19]. In particular, since $L$ is a symmetric Laplacian, $y(t) \rightarrow (1_N^T y(0)/N)1_N$, which corresponds to the same rotation error for each angle $\hat{z}_i$, i.e. $1_N^T y(0)/N = \alpha$. By applying again the change of coordinates:

$$ \begin{aligned} \hat{z}(t) &\rightarrow D_z(t) \left( \frac{1}{N} 1_N^T y(0) \right) 1_N \\ &= \left( \frac{1}{N} 1_N^T D_z(0)^H \hat{z}(0) \right) D_z(t) 1_N = \left( \frac{1}{N} z^H(0) \hat{z}(0) \right) z(t) \end{aligned} \quad (19) $$

We also note that, $\hat{z}(t) = 0$ is an undesired equilibrium of the system, as it does not represent a valid orientation. But $\hat{z}(t) \rightarrow 0$ if and only if $z^H(0)\hat{z}(0) = 0$. $\square$

The stability proof in Theorem 2 relies on Proposition 1, assuming perfect measurements $z_{ij}$. However, using $\hat{z}_{ij}$ from (8) in place of $z_{ij}$ may render the estimated relative orientations unrealizable. Consequently, the existence of a $z^* := [z_1^* \dots z_N^*]^\top \in \mathbb{C}^N$ compatible with all estimated $\hat{z}_{ij} \forall (i, j) \in \mathcal{E}$ is not guaranteed. Specifically, within a graph containing cycles, while (16) may hold for edges within a spanning tree, this might not hold for the remaining edges outside the spanning tree.

Let us denote $\hat{L}_c$ as the matrix derived similarly to (15) but using the estimated $\hat{z}_{ij}$ and $\hat{L} := D_z^H \hat{L}_c D_z^*$, in which $D_z^* := \operatorname{diag}(z_1^*, ..., z_N^*)$. For edges within cycles, instead of (16), we might encounter $(D_z^H \hat{L}_c D_z^*)_{ij} = -e^{i\hat{\psi}_{ij}}$ where $\hat{\psi}_{ij} = \psi_j^* - \psi_i^* - \hat{\psi}_{ij}$, with $\psi_i^* = \angle z_i^*$ and $\psi_j^* = \angle z_j^*$. Consequently,

$$ (\hat{L})_{ij} := \begin{cases} |\mathcal{N}_i| & \text{if } i=j \\ -1 & \text{if } (i,j) \in \mathcal{G}_{\tau} \\ -e^{i\hat{\psi}_{ij}} & \text{if } (i,j) \in \mathcal{G}_c \\ 0 & \text{otherwise} \end{cases} \quad (20) $$

where we used $\mathcal{G}_{\tau}$ and $\mathcal{G}_c$ to denote respectively a subgraph of $\mathcal{G}_c$ representing the considered spanning tree and a subgraph representing the remaining cycle edges. As $e^{i\hat{\psi}_{ij}} = e^{-i\psi_{ji}}$ and $|e^{i\hat{\psi}_{ij}}| = 1$, $\hat{L}$ emerges as a Hermitian weakly diagonally dominant matrix with positive elements on the diagonal, establishing its positive semidefiniteness. Consequently, (18), and hence (14), remain stable also using imperfect measurements $\hat{z}_{ij}$.

### C. Position Observer

In this section, we design an observer for the position of each robot within a common frame. This observer is added in cascade with respect to (14). The primary objective of this observer can be defined as follows: for a common translation $\bar{p} \in \mathbb{R}^3$, a common rotation $R_\alpha$ (where $\alpha \in \mathbb{C}$ remains consistent with the preceding section), alongside the real position $p_i$ and orientation $\angle z_i$, the aim is to devise an estimation law ensuring that $\hat{p}_i \rightarrow \bar{p} + R_\alpha^\top p_i \forall i$ as $t \rightarrow \infty$

<footnote n="2">²diag(·) represents a diagonal matrix of the input arguments and
blkdiag(·) represents a block diagonal matrix of the input arguments</footnote>
Each robot implements the following update law:

$$
\begin{equation}
\begin{aligned}
\dot{\hat{\mathbf{p}}}_i = \hat{\mathbf{R}}_i \mathbf{v}_i - k_p \left[ \sum_{j \in \mathcal{N}_i^{\text{out}}} (\hat{\mathbf{p}}_i - \hat{\mathbf{p}}_j + \hat{\mathbf{R}}_i{}^i \mathbf{p}_{ij}) \right. \\
\left. \qquad + \sum_{j \in \mathcal{N}_i^{\text{in}}} (\hat{\mathbf{p}}_i - \hat{\mathbf{p}}_j - \hat{\mathbf{R}}_j{}^j \mathbf{p}_{ji}) \right]
\end{aligned}
\tag{21}
\end{equation}
$$

where $k_p > 0$, $\mathcal{N}_i^{\text{in}}$ and $\mathcal{N}_i^{\text{out}}$ represent the in- and out-neighbors of the i-th robot, while $\hat{\mathbf{R}}_i$ and $\hat{\mathbf{R}}_j$ are obtained from observer (14) and the corresponding angle commu-cated among neighboring robots.

Let us denote $\bar{\mathbf{R}} := \text{blkdiag}(\{\mathbf{R}_i\}_{i=1}^N) \in \mathbb{R}^{3N \times 3N}$ and $\bar{\mathbf{R}}_{\mathcal{E}} := \text{blkdiag}(\{\mathbf{R}_k\}_{k=1}^{|\mathcal{E}|}) \in \mathbb{R}^{3|\mathcal{E}| \times 3|\mathcal{E}|}$, where $\mathbf{R}_k$ is the rotation matrix corresponding to the initial node of the edge $e_k := (i, j)$ and we use the 'hat' to indicate the estimated counterpart. The dynamics of the observer can be expressed in matrix form as follows:

$$
\dot{\hat{\mathbf{p}}} = \hat{\mathbf{R}}\mathbf{v} - k_p \mathbf{E}_3 (\mathbf{E}_3^\top \hat{\mathbf{p}} - \hat{\mathbf{R}}_{\mathcal{E}}^b \mathbf{p}_{\mathcal{E}}) \quad (22)
$$

where $\mathbf{E}_3 := \mathbf{E} \otimes \mathbf{I}_3$ is the incidence matrix of the directed sensing graph $\mathcal{G}_s$, $\otimes$ is the Kronecker product and $^b\mathbf{p}_\mathcal{E} := \bar{\mathbf{R}}_\mathcal{E}^\top \mathbf{E}_3^\top \mathbf{p}$. Similarly, we define for later use $\mathbf{p}_\mathcal{E} := \mathbf{E}_3^\top \mathbf{p}$. Notice that, after convergence of the yaw observer, it holds $\hat{\mathbf{R}}_i = \mathbf{R}_\alpha^\top \mathbf{R}_i$ and, by denoting $\bar{\mathbf{R}}_\alpha := \mathbf{I}_N \otimes \mathbf{R}_\alpha$, it follows:

$$
\hat{\mathbf{R}}_{\mathcal{E}}^{b} \mathbf{p}_{\mathcal{E}} = \bar{\mathbf{R}}_{\alpha}^{\top} \bar{\mathbf{R}}_{\mathcal{E}} \bar{\mathbf{R}}_{\mathcal{E}}^{\top} \mathbf{p}_{\mathcal{E}} = \bar{\mathbf{R}}_{\alpha}^{\top} \mathbf{E}_{3}^{\top} \mathbf{p}. \quad (23)
$$

Before proceeding with the main theorem, we define some quantities which will be used later. Let us denote $\bar{\mathbf{L}}_3 := \bar{\mathbf{L}} \otimes \mathbf{I}_3 = \mathbf{E}_3 \mathbf{E}_3^\top$, where the Laplacian matrix $\bar{\mathbf{L}}$ has components:

$$
(\bar{\mathbf{L}})_{ij} := \begin{cases} |\mathcal{N}_i^{\text{in}}| + |\mathcal{N}_i^{\text{out}}| & \text{if } i = j \\ 2 & \text{if } j \in \mathcal{N}_i^{\text{in}} \wedge j \in \mathcal{N}_i^{\text{out}} \\ 1 & \text{if } j \in \mathcal{N}_i^{\text{in}} \oplus j \in \mathcal{N}_i^{\text{out}} \\ 0 & \text{otherwise} \end{cases}
\tag{24}
$$

with $\oplus$ denoting the exclusive disjunction. Also, define the yaw estimation error $e_\psi := \hat{z}_\psi - z_{\alpha\psi}$, with $\hat{z}_\psi := [\cos(\hat{\psi}_1), \sin(\hat{\psi}_1), \dots, \cos(\hat{\psi}_N), \sin(\hat{\psi}_N)]^\top$ and $z_{\alpha\psi} := [\cos(\psi_1 - \alpha), \sin(\psi_1 - \alpha), \dots, \cos(\psi_N - \alpha), \sin(\psi_N - \alpha)]^\top$, as well as $\bar{\mathbf{R}}_{\alpha\mathcal{E}} := \mathbf{I}_{|\mathcal{E}|} \otimes \mathbf{R}_{\alpha}$.

**Theorem 3.** Consider the estimation error up to a common rotation $e_p := \hat{\mathbf{p}} - \bar{\mathbf{R}}_\alpha^\top \mathbf{p}$ and the observer dynamics (22). Under the assumptions that $\mathcal{G}_c$ is connected and the inputs, as well as the measurements (2), are uniformly bounded and continuously differentiable, then, the set $\mathcal{S} := \{e_{p1} = e_{p2} = \dots = e_{pN}\}$ is input-to-state stable (ISS) with respect to the yaw error $e_\psi$.

*Proof.* The error dynamics can be written as:

$$
\begin{align}
\dot{e}_p &= (\hat{\mathbf{R}} - \bar{\mathbf{R}}_{\alpha}^{\top} \bar{\mathbf{R}}) \mathbf{v} - k_p \mathbf{E}_3 (\mathbf{E}_3^{\top} \hat{\mathbf{p}} - \hat{\mathbf{R}}_{\mathcal{E}}^{b} \mathbf{p}_{\mathcal{E}}) \nonumber \\
&\quad + k_p \mathbf{E}_3 \bar{\mathbf{R}}_{\alpha \mathcal{E}}^{\top} \bar{\mathbf{R}}_{\mathcal{E}}^{b} \mathbf{p}_{\mathcal{E}} - k_p \mathbf{E}_3 \bar{\mathbf{R}}_{\alpha \mathcal{E}}^{\top} \bar{\mathbf{R}}_{\mathcal{E}}^{b} \mathbf{p}_{\mathcal{E}} \nonumber \\
&= -k_p \bar{\mathbf{L}}_3 e_p + (\hat{\mathbf{R}} - \bar{\mathbf{R}}_{\alpha}^{\top} \bar{\mathbf{R}}) \mathbf{v} + k_p \mathbf{E}_3 (\hat{\mathbf{R}}_{\mathcal{E}} - \bar{\mathbf{R}}_{\alpha \mathcal{E}}^{\top} \bar{\mathbf{R}}_{\mathcal{E}})^b \mathbf{p}_{\mathcal{E}} \nonumber \\
&= -k_p \bar{\mathbf{L}}_3 e_p + (\text{blkdiag}\{\bar{\mathbf{V}}_i\} + k_p \mathbf{E}_3 \text{blkdiag}\{^i \bar{\mathbf{P}}_{ij}\} \mathbf{E}_{2\otimes}^{\top}) e_{\psi} \nonumber \\
&= -k_p \bar{\mathbf{L}}_3 e_p + \mathbf{B} e_{\psi} \tag{25}
\end{align}
$$

where, in the second line, we added and subtracted $k_p \mathbf{E}_3 \bar{\mathbf{R}}_{\alpha \mathcal{E}}^{\top} \bar{\mathbf{R}}_{\mathcal{E}}^{b} \mathbf{p}_{\mathcal{E}}$, in the third equality, we used an analogous transformations to (5), i.e.

$$
{}^i \bar{\mathbf{P}}_{ij}(t) := \begin{bmatrix} {}^i p_{ijx}(t) & - {}^i p_{ijy}(t) \\ {}^i p_{ijy}(t) & {}^i p_{ijx}(t) \end{bmatrix}, \quad (26)
$$

to obtain linearly the orientation error $e_\psi$. Also, $\mathbf{E}_{2\otimes} := \mathbf{E} \otimes \mathbf{I}_2$, where

$$
(\mathbf{E}_{\otimes})_{ik} := \begin{cases} 1 & \text{if node } i \text{ is the head of edge } e_k \\ 0 & \text{otherwise} \end{cases} \tag{27}
$$

is the out-incidence matrix [18]. In the last equality, we defined $\mathbf{B}(t) := \text{blkdiag}\{\bar{\mathbf{V}}_i\}(t) + k_p \mathbf{E}_3 \text{blkdiag}\{^i \bar{\mathbf{P}}_{ij}\}(t) \mathbf{E}_{2\otimes}^\top$. Let us denote $\mathbf{U} := \mathbf{1}_N \otimes \mathbf{I}_3$ and let $e_p^\perp := e_p - \mathbf{U}\mathbf{U}^\top e_p$ be the error orthogonal to the consensus subspace $\mathcal{S}$ and $\bar{e}_p := \mathbf{U}^\top e_p$ be the average error (i.e. a common translation). Then, the system dynamics can be written as:

$$
\dot{e}_p^\perp(t) = -k_p \bar{\mathbf{L}}_3 e_p^\perp(t) + (\mathbf{I}_{3N} - \mathbf{U}\mathbf{U}^\top) \mathbf{B}(t) e_\psi(t) \quad (28)
$$

$$
\dot{e}_p(t) = \mathbf{U}^\top \mathbf{B}(t) e_\psi(t) \quad (29)
$$

where we used the fact that $\mathbf{U}^\top \bar{\mathbf{L}}_3 = \bar{\mathbf{L}}_3 \mathbf{U} = \mathbf{0}$ for undirected graphs [18]. The unforced system (28) (i.e. for $e_\psi(t) = \mathbf{0}$) is globally exponentially stable and since $\mathbf{B}(t)$ is uniformly bounded and continuously differentiable, then (28) is ISS with respect to $e_\psi(t)$ (see [20, Corollary 5.16]). Furthermore, as shown in the previous section, $e_\psi(t)$ is bounded $\forall t \ge 0$ and $e_\psi(t) \to 0$ as $t \to \infty$, implying that $e_\psi(t)$ is integrable, and, as a consequence, $\bar{e}_p(t)$ remains bounded. $\square$

IV. SIMULATION RESULTS

This section presents simulation results validating our
theoretical findings using a group of 8 quadrotors, whose
sensing graph is depicted in Fig. 1. The quadrotors execute a

<figure><img src="image_3.png" /><figcaption>Fig. 1: Sensing graph</figcaption></figure>

formation control task with a modified version of the control
law from [1] to suit a directed sensing graph,

$$
v_i = k_c k_i \mu_i \quad (30)
$$

with

$$
\mu_i := \sum_{j \in \mathcal{N}_i^{\text{in}}} ({}^i \mathbf{p}_{ij} - \hat{\mathbf{R}}_i^\top (\mathbf{p}_j^* - \mathbf{p}_i^*)) + \sum_{j \in \mathcal{N}_i^{\text{out}}} (\hat{\mathbf{R}}_j^\top (\mathbf{p}_i^* - \mathbf{p}_j^*) - {}^j \mathbf{p}_{ji}), \quad (31)
$$

$k_c > 0$ and $k_i = 1$ if $\|\boldsymbol{\mu}_i\| \le \nu_{\max}$, $k_i = \nu_{\max}/\|\boldsymbol{\mu}_i\|$ otherwise, where $\nu_{\max}$ is the maximum velocity norm. The yaw is controlled independently using a spline with random coefficients. The initial estimated positions of the drones are drawn from a gaussian distribution centered around the real position of the drones and with standard deviation of $1m$, instead the initial common frame yaw estimate $\hat{z}$ is drawn
Each robot implements the following update law:

$$
\begin{equation}
\begin{aligned}
\dot{\hat{\mathbf{p}}}_i = \hat{\mathbf{R}}_i \mathbf{v}_i - k_p \left[ \sum_{j \in \mathcal{N}_i^{\text{out}}} (\hat{\mathbf{p}}_i - \hat{\mathbf{p}}_j + \hat{\mathbf{R}}_i^i \mathbf{p}_{ij}) \right. \\
\left. \qquad + \sum_{j \in \mathcal{N}_i^{\text{in}}} (\hat{\mathbf{p}}_i - \hat{\mathbf{p}}_j - \hat{\mathbf{R}}_j^j \mathbf{p}_{ji}) \right]
\end{aligned}
\tag{21}
\end{equation}
$$

where $k_p > 0$, $\mathcal{N}_i^{\text{in}}$ and $\mathcal{N}_i^{\text{out}}$ represent the in- and out-neighbors of the i-th robot, while $\hat{\mathbf{R}}_i$ and $\hat{\mathbf{R}}_j$ are obtained from observer (14) and the corresponding angle commu-cated among neighboring robots.

Let us denote $\bar{\mathbf{R}} := \text{blkdiag}(\{\mathbf{R}_i\}_{i=1}^N) \in \mathbb{R}^{3N \times 3N}$ and $\bar{\mathbf{R}}_{\mathcal{E}} := \text{blkdiag}(\{\mathbf{R}_k\}_{k=1}^{|\mathcal{E}|}) \in \mathbb{R}^{3|\mathcal{E}| \times 3|\mathcal{E}|}$, where $\mathbf{R}_k$ is the rotation matrix corresponding to the initial node of the edge $e_k := (i, j)$ and we use the 'hat' to indicate the estimated counterpart. The dynamics of the observer can be expressed in matrix form as follows:

$$
\dot{\hat{\mathbf{p}}} = \hat{\mathbf{R}}\mathbf{v} - k_p \mathbf{E}_3 (\mathbf{E}_3^\top \hat{\mathbf{p}} - \hat{\mathbf{R}}_{\mathcal{E}}^b \mathbf{p}_{\mathcal{E}}) \quad (22)
$$

where $\mathbf{E}_3 := \mathbf{E} \otimes \mathbf{I}_3$ is the incidence matrix of the directed sensing graph $\mathcal{G}_s$, $\otimes$ is the Kronecker product and $^b\mathbf{p}_\mathcal{E} := \bar{\mathbf{R}}_{\mathcal{E}}^\top \mathbf{E}_3^\top \mathbf{p}$. Similarly, we define for later use $\mathbf{p}_\mathcal{E} := \mathbf{E}_3^\top \mathbf{p}$. Notice that, after convergence of the yaw observer, it holds $\hat{\mathbf{R}}_i = \mathbf{R}_\alpha^\top \mathbf{R}_i$ and, by denoting $\bar{\mathbf{R}}_\alpha := \mathbf{I}_N \otimes \mathbf{R}_\alpha$, it follows:

$$
\hat{\mathbf{R}}_{\mathcal{E}}^{b} \mathbf{p}_{\mathcal{E}} = \bar{\mathbf{R}}_{\alpha}^{\top} \bar{\mathbf{R}}_{\mathcal{E}} \bar{\mathbf{R}}_{\mathcal{E}}^{\top} \mathbf{p}_{\mathcal{E}} = \bar{\mathbf{R}}_{\alpha}^{\top} \mathbf{E}_{3}^{\top} \mathbf{p}. \quad (23)
$$

Before proceeding with the main theorem, we define some quantities which will be used later. Let us denote $\bar{\mathbf{L}}_3 := \bar{\mathbf{L}} \otimes \mathbf{I}_3 = \mathbf{E}_3 \mathbf{E}_3^\top$, where the Laplacian matrix $\bar{\mathbf{L}}$ has components:

$$
(\bar{\mathbf{L}})_{ij} := \begin{cases} |\mathcal{N}_i^{\text{in}}| + |\mathcal{N}_i^{\text{out}}| & \text{if } i = j \\ 2 & \text{if } j \in \mathcal{N}_i^{\text{in}} \wedge j \in \mathcal{N}_i^{\text{out}} \\ 1 & \text{if } j \in \mathcal{N}_i^{\text{in}} \oplus j \in \mathcal{N}_i^{\text{out}} \\ 0 & \text{otherwise} \end{cases}
\quad (24)
$$

with $\oplus$ denoting the exclusive disjunction. Also, define the yaw estimation error $e_\psi := \hat{z}_\psi - z_{\alpha\psi}$, with $\hat{z}_\psi := [\cos(\hat{\psi}_1), \sin(\hat{\psi}_1), \dots, \cos(\hat{\psi}_N), \sin(\hat{\psi}_N)]^\top$ and $z_{\alpha\psi} := [\cos(\psi_1 - \alpha), \sin(\psi_1 - \alpha), \dots, \cos(\psi_N - \alpha), \sin(\psi_N - \alpha)]^\top$, as well as $\bar{\mathbf{R}}_{\alpha\mathcal{E}} := \mathbf{I}_{|\mathcal{E}|} \otimes \mathbf{R}_{\alpha}$.

**Theorem 3.** Consider the estimation error up to a common rotation $e_p := \hat{\mathbf{p}} - \bar{\mathbf{R}}_\alpha^\top \mathbf{p}$ and the observer dynamics (22). Under the assumptions that $\mathcal{G}_c$ is connected and the inputs, as well as the measurements (2), are uniformly bounded and continuously differentiable, then, the set $\mathcal{S} := \{e_{p1} = e_{p2} = \dots = e_{pN}\}$ is input-to-state stable (ISS) with respect to the yaw error $e_\psi$.

*Proof.* The error dynamics can be written as:

$$
\begin{align}
\dot{e}_p &= (\hat{\mathbf{R}} - \bar{\mathbf{R}}_\alpha^\top \bar{\mathbf{R}}) \mathbf{v} - k_p \mathbf{E}_3 (\mathbf{E}_3^\top \hat{\mathbf{p}} - \hat{\mathbf{R}}_{\mathcal{E}}^b \mathbf{p}_{\mathcal{E}}) \nonumber \\
&\quad + k_p \mathbf{E}_3 \bar{\mathbf{R}}_{\alpha\mathcal{E}}^\top \bar{\mathbf{R}}_{\mathcal{E}}^b \mathbf{p}_{\mathcal{E}} - k_p \mathbf{E}_3 \bar{\mathbf{R}}_{\alpha\mathcal{E}}^\top \bar{\mathbf{R}}_{\mathcal{E}}^b \mathbf{p}_{\mathcal{E}} \nonumber \\
&= -k_p \bar{\mathbf{L}}_3 e_p + (\hat{\mathbf{R}} - \bar{\mathbf{R}}_\alpha^\top \bar{\mathbf{R}}) \mathbf{v} + k_p \mathbf{E}_3 (\hat{\mathbf{R}}_{\mathcal{E}} - \bar{\mathbf{R}}_{\alpha\mathcal{E}}^\top \bar{\mathbf{R}}_{\mathcal{E}})^b \mathbf{p}_{\mathcal{E}} \nonumber \\
&= -k_p \bar{\mathbf{L}}_3 e_p + (\text{blkdiag}\{\bar{\mathbf{V}}_i\} + k_p \mathbf{E}_3 \text{blkdiag}\{^i \bar{\mathbf{P}}_{ij}\} \mathbf{E}_{2\otimes}^\top) e_\psi \nonumber \\
&= -k_p \bar{\mathbf{L}}_3 e_p + \mathbf{B} e_\psi \tag{25}
\end{align}
$$

where, in the second line, we added and subtracted $k_p \mathbf{E}_3 \bar{\mathbf{R}}_{\alpha\mathcal{E}}^\top \bar{\mathbf{R}}_{\mathcal{E}}^b \mathbf{p}_{\mathcal{E}}$, in the third equality, we used an analogous transformations to (5), i.e.

$$
{}^i \bar{\mathbf{P}}_{ij}(t) := \begin{bmatrix} {}^i p_{ijx}(t) & - {}^i p_{ijy}(t) \\ {}^i p_{ijy}(t) & {}^i p_{ijx}(t) \end{bmatrix}, \quad (26)
$$

to obtain linearly the orientation error $e_\psi$. Also, $\mathbf{E}_{2\otimes} := \mathbf{E} \otimes \mathbf{I}_2$, where

$$
(\mathbf{E}_{\otimes})_{ik} := \begin{cases} 1 & \text{if node } i \text{ is the head of edge } e_k \\ 0 & \text{otherwise} \end{cases} \tag{27}
$$

is the out-incidence matrix [18]. In the last equality, we defined $\mathbf{B}(t) := \text{blkdiag}\{\bar{\mathbf{V}}_i\}(t) + k_p \mathbf{E}_3 \text{blkdiag}\{^i \bar{\mathbf{P}}_{ij}\}(t) \mathbf{E}_{2\otimes}^\top$. Let us denote $\mathbf{U} := \mathbf{1}_N \otimes \mathbf{I}_3$ and let $e_p^\perp := e_p - \mathbf{U}\mathbf{U}^\top e_p$ be the error orthogonal to the consensus subspace $\mathcal{S}$ and $\bar{e}_p := \mathbf{U}^\top e_p$ be the average error (i.e. a common translation). Then, the system dynamics can be written as:

$$
\dot{e}_p^\perp(t) = -k_p \bar{\mathbf{L}}_3 e_p^\perp(t) + (\mathbf{I}_{3N} - \mathbf{U}\mathbf{U}^\top) \mathbf{B}(t) e_\psi(t) \quad (28)
$$

$$
\dot{e}_p(t) = \mathbf{U}^\top \mathbf{B}(t) e_\psi(t) \quad (29)
$$

where we used the fact that $\mathbf{U}^\top \bar{\mathbf{L}}_3 = \bar{\mathbf{L}}_3 \mathbf{U} = \mathbf{0}$ for undirected graphs [18]. The unforced system (28) (i.e. for $e_\psi(t) = \mathbf{0}$) is globally exponentially stable and since $\mathbf{B}(t)$ is uniformly bounded and continuously differentiable, then (28) is ISS with respect to $e_\psi(t)$ (see [20, Corollary 5.16]). Furthermore, as shown in the previous section, $e_\psi(t)$ is bounded $\forall t \ge 0$ and $e_\psi(t) \to 0$ as $t \to \infty$, implying that $e_\psi(t)$ is integrable, and, as a consequence, $\bar{e}_p(t)$ remains bounded. $\square$

IV. SIMULATION RESULTS

This section presents simulation results validating our
theoretical findings using a group of 8 quadrotors, whose
sensing graph is depicted in Fig. 1. The quadrotors execute a

<figure><img src="image_4.png" /><figcaption>Fig. 1: Sensing graph</figcaption></figure>

formation control task with a modified version of the control
law from [1] to suit a directed sensing graph,

$$
\mathbf{v}_i = k_c k_i \mu_i \quad (30)
$$

with

$$
\mu_i := \sum_{j \in \mathcal{N}_i^{\text{in}}} ({}^i \mathbf{p}_{ij} - \hat{\mathbf{R}}_i^\top (\mathbf{p}_j^* - \mathbf{p}_i^*)) + \sum_{j \in \mathcal{N}_i^{\text{out}}} (\hat{\mathbf{R}}_j^\top (\mathbf{p}_i^* - \mathbf{p}_j^*) - {}^j \mathbf{p}_{ji}), \quad (31)
$$

$k_c > 0$ and $k_i = 1$ if $\|\boldsymbol{\mu}_i\| \le \nu_{\max}$, $k_i = \nu_{\max}/\|\boldsymbol{\mu}_i\|$ otherwise, where $\nu_{\max}$ is the maximum velocity norm. The yaw is controlled independently using a spline with random coefficients. The initial estimated positions of the drones are drawn from a gaussian distribution centered around the real position of the drones and with standard deviation of $1m$, instead the initial common frame yaw estimate $\hat{z}$ is drawn
from a uniform distribution in $(-\pi, \pi]$. The relative yaw estimates $\hat{z}_{\psi_{ij}}$ are initialized based on the initial common frame yaw estimate, ensuring consistency with the graph cycles. Consequently, $L_c$ will be a similar matrix to a Laplacian, with an eigenvalue exactly at 0.

The relative state observer (8) is run @200Hz with gains $k_{pe} = 20$ and $k_{\psi} = 600$, the common frame yaw observer (15) @100Hz with gain $k_z = 40$ and the common frame position observer (21) @50Hz with gain $k_p = 1$. Results for the estimation of relative yaw, yaw in a common frame, and position in a common frame, as well as the formation error norm are depicted in Fig. 2.

<figure><img src="image_4.png" /><figcaption>Fig. 2: The figures respectively illustrate the following: (a) the norm of the estimation error on the relative yaw for each edge, (b) the norm of the estimation error on the yaw in a common frame for each robot, (c) the norm of the estimation error on the position in a common frame for each robot, and (d) the norm of the formation error for each robot.</figcaption></figure>

V. CONCLUSIONS AND FUTURE WORKS

This work presents a novel observer for robots to estimate their position and yaw in a common frame from relative position measurements. We achieve this through a three-part observer that leverages persistency of excitation, eliminating the need for restrictive assumptions about the sensing graph. Future work will focus on extending this approach

to bearing measurements and exploring a single-observer implementation.

REFERENCES

[1] B.-H. Lee and H.-S. Ahn, "Distributed formation control via global orientation estimation," *Automatica*, vol. 73, pp. 125–129, 2016.

[2] —, "Distributed estimation for the unknown orientation of the local reference frames in n-dimensional space," in *2016 14th International Conference on Control, Automation, Robotics and Vision (ICARCV)*. IEEE, 2016, pp. 1–6.

[3] 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.

[4] N. Boumal, A. Singer, and P.-A. Absil, "Robust estimation of rotations from relative measurements by maximum likelihood," in *52nd IEEE Conference on Decision and Control*. IEEE, 2013, pp. 1156–1161.

[5] F. Schiano, A. Franchi, D. Zelazo, and P. R. Giordano, "A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs," in *2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. IEEE, 2016, pp. 5099–5106.

[6] N. De Carli, P. Salaris, and P. R. Giordano, "Multi-robot active sensing for bearing formations," in *2023 International Symposium on Multi-Robot and Multi-Agent Systems (MRS)*. IEEE, 2023, pp. 184–190.

[7] E. Montijano, D. Zhou, M. Schwager, and C. Sagues, "Distributed formation control without a global reference frame," in *2014 American Control Conference*. IEEE, 2014, pp. 3862–3867.

[8] K.-K. Oh and H.-S. Ahn, "Formation control and network localization via orientation alignment," *IEEE Transactions on Automatic Control*, vol. 59, no. 2, pp. 540–545, 2013.

[9] Q. Van Tran and H.-S. Ahn, "Distributed formation control of mobile agents via global orientation estimation," *IEEE Transactions on Control of Network Systems*, vol. 7, no. 4, pp. 1654–1664, 2020.

[10] Q. Van Tran, H.-S. Ahn, and B. D. Anderson, "Distributed orientation localization of multi-agent systems in 3-dimensional space with direction-only measurements," in *2018 IEEE Conference on Decision and Control (CDC)*. IEEE, 2018, pp. 2883–2889.

[11] X. Li, X. Luo, and S. Zhao, "Globally convergent distributed network localization using locally measured bearings," *IEEE Transactions on Control of Network Systems*, vol. 7, no. 1, pp. 245–253, 2019.

[12] S. Leonardos, K. Daniilidis, and R. Tron, "Distributed 3-d bearing-only orientation localization," in *2019 IEEE 58th Conference on Decision and Control (CDC)*. IEEE, 2019, pp. 1834–1841.

[13] B.-H. Lee, S.-M. Kang, and H.-S. Ahn, "Distributed orientation estimation in so (d) and applications to formation control and network localization," *IEEE Transactions on Control of Network Systems*, vol. 6, no. 4, pp. 1302–1312, 2018.

[14] R. Mahony, T. Hamel, and J.-M. Pflimlin, "Nonlinear complementary filters on the special orthogonal group," *IEEE Transactions on Automatic control*, vol. 53, no. 5, pp. 1203–1218, 2008.

[15] R. Marino and P. Tomei, *Nonlinear control design: Geometric, adaptive & robust*. Prentice Hall, 1995.

[16] G. Besançon, "Remarks on nonlinear adaptive observer design," *Systems & control letters*, vol. 41, no. 4, pp. 271–280, 2000.

[17] M. Mesbahi and M. Egerstedt, *Graph theoretic methods in multiagent networks*. Princeton University Press, 2010.

[18] F. Bullo, *Lectures on network systems*. Kindle Direct Publishing Seattle, DC, USA, 2020, vol. 1, no. 3.

[19] L. Moreau, "Stability of continuous-time distributed consensus algorithms," in *2004 43rd IEEE conference on decision and control (CDC)(IEEE Cat. No. 04CH37601)*, vol. 4. IEEE, 2004, pp. 3998–4003.

[20] F. Bullo, *Contraction theory for dynamical systems*. Francesco Bullo, 2022.
from a uniform distribution in $(-\pi, \pi]$. The relative yaw estimates $\hat{z}_{\psi_{ij}}$ are initialized based on the initial common frame yaw estimate, ensuring consistency with the graph cycles. Consequently, $L_c$ will be a similar matrix to a Laplacian, with an eigenvalue exactly at 0.

The relative state observer (8) is run @200Hz with gains $k_{pe} = 20$ and $k_{\psi} = 600$, the common frame yaw observer (15) @100Hz with gain $k_z = 40$ and the common frame position observer (21) @50Hz with gain $k_p = 1$. Results for the estimation of relative yaw, yaw in a common frame, and position in a common frame, as well as the formation error norm are depicted in Fig. 2.

<figure><img src="image_5.png" /><figcaption>Fig. 2: The figures respectively illustrate the following: (a) the norm of the estimation error on the relative yaw for each edge, (b) the norm of the estimation error on the yaw in a common frame for each robot, (c) the norm of the estimation error on the position in a common frame for each robot, and (d) the norm of the formation error for each robot.</figcaption></figure>

V. CONCLUSIONS AND FUTURE WORKS

This work presents a novel observer for robots to estimate their position and yaw in a common frame from relative position measurements. We achieve this through a three-part observer that leverages persistency of excitation, eliminating the need for restrictive assumptions about the sensing graph. Future work will focus on extending this approach

to bearing measurements and exploring a single-observer implementation.

REFERENCES

[1] B.-H. Lee and H.-S. Ahn, "Distributed formation control via global orientation estimation," *Automatica*, vol. 73, pp. 125–129, 2016.

[2] —, "Distributed estimation for the unknown orientation of the local reference frames in n-dimensional space," in *2016 14th International Conference on Control, Automation, Robotics and Vision (ICARCV)*. IEEE, 2016, pp. 1–6.

[3] 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.

[4] N. Boumal, A. Singer, and P.-A. Absil, "Robust estimation of rotations from relative measurements by maximum likelihood," in *52nd IEEE Conference on Decision and Control*. IEEE, 2013, pp. 1156–1161.

[5] F. Schiano, A. Franchi, D. Zelazo, and P. R. Giordano, "A rigidity-based decentralized bearing formation controller for groups of quadrotor UAVs," in *2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. IEEE, 2016, pp. 5099–5106.

[6] N. De Carli, P. Salaris, and P. R. Giordano, "Multi-robot active sensing for bearing formations," in *2023 International Symposium on Multi-Robot and Multi-Agent Systems (MRS)*. IEEE, 2023, pp. 184–190.

[7] E. Montijano, D. Zhou, M. Schwager, and C. Sagues, "Distributed formation control without a global reference frame," in *2014 American Control Conference*. IEEE, 2014, pp. 3862–3867.

[8] K.-K. Oh and H.-S. Ahn, "Formation control and network localization via orientation alignment," *IEEE Transactions on Automatic Control*, vol. 59, no. 2, pp. 540–545, 2013.

[9] Q. Van Tran and H.-S. Ahn, "Distributed formation control of mobile agents via global orientation estimation," *IEEE Transactions on Control of Network Systems*, vol. 7, no. 4, pp. 1654–1664, 2020.

[10] Q. Van Tran, H.-S. Ahn, and B. D. Anderson, "Distributed orientation localization of multi-agent systems in 3-dimensional space with direction-only measurements," in *2018 IEEE Conference on Decision and Control (CDC)*. IEEE, 2018, pp. 2883–2889.

[11] X. Li, X. Luo, and S. Zhao, "Globally convergent distributed network localization using locally measured bearings," *IEEE Transactions on Control of Network Systems*, vol. 7, no. 1, pp. 245–253, 2019.

[12] S. Leonardos, K. Daniilidis, and R. Tron, "Distributed 3-d bearing-only orientation localization," in *2019 IEEE 58th Conference on Decision and Control (CDC)*. IEEE, 2019, pp. 1834–1841.

[13] B.-H. Lee, S.-M. Kang, and H.-S. Ahn, "Distributed orientation estimation in so (d) and applications to formation control and network localization," *IEEE Transactions on Control of Network Systems*, vol. 6, no. 4, pp. 1302–1312, 2018.

[14] R. Mahony, T. Hamel, and J.-M. Pflimlin, "Nonlinear complementary filters on the special orthogonal group," *IEEE Transactions on Automatic control*, vol. 53, no. 5, pp. 1203–1218, 2008.

[15] R. Marino and P. Tomei, *Nonlinear control design: Geometric, adaptive & robust*. Prentice Hall, 1995.

[16] G. Besançon, "Remarks on nonlinear adaptive observer design," *Systems & control letters*, vol. 41, no. 4, pp. 271–280, 2000.

[17] M. Mesbahi and M. Egerstedt, *Graph theoretic methods in multiagent networks*. Princeton University Press, 2010.

[18] F. Bullo, *Lectures on network systems*. Kindle Direct Publishing Seattle, DC, USA, 2020, vol. 1, no. 3.

[19] L. Moreau, "Stability of continuous-time distributed consensus algorithms," in *2004 43rd IEEE conference on decision and control (CDC)(IEEE Cat. No. 04CH37601)*, vol. 4. IEEE, 2004, pp. 3998–4003.

[20] F. Bullo, *Contraction theory for dynamical systems*. Francesco Bullo, 2022.