See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/324833639

Bearing-based localization and control for multiple quadrotor UAVs

Thesis · January 2018

CITATIONS READS

0 114

1 author:

Fabrizio Schiano
École Polytechnique Fédérale de Lausanne
28 PUBLICATIONS   163 CITATIONS   

SEE PROFILE

Some of the authors of this publication are also working on these related projects:

Towards a Deployable and Sensor-Based Multi-Robot System View project

All content following this page was uploaded by Fabrizio Schiano on 29 October 2018.

The user has requested enhancement of the downloaded file.

 Bearing-based localization and control for multiple
quadrotor UAVs
Fabrizio Schiano

To cite this version:

Fabrizio Schiano. Bearing-based localization and control for multiple quadrotor UAVs. Robotics
[cs.RO]. Université Rennes 1, 2018. English. <NNT : 2018REN1S009>. <tel-01780065>

HAL Id: tel-01780065

https://tel.archives-ouvertes.fr/tel-01780065
Submitted on 27 Apr 2018

HAL is a multi-disciplinary open access L’archive ouverte pluridisciplinaire HAL, est

archive for the deposit and dissemination of sci- destinée au dépôt et à la diffusion de documents
entific research documents, whether they are pub- scientifiques de niveau recherche, publiés ou non,
lished or not. The documents may come from émanant des établissements d’enseignement et de
teaching and research institutions in France or recherche français ou étrangers, des laboratoires
abroad, or from public or private research centers. publics ou privés.
 ANNÉE 2018

THÈSE / UNIVERSITÉ DE RENNES 1

sous le sceau de l’Université Européenne Bretagne Loire
pour le grade de
DOCTEUR DE L’UNIVERSITÉ DE RENNES 1
Mention : Automatique, Productique et Robotique
École doctorale MathSTIC
présentée par
Fabrizio Schiano
préparée à l’unité de recherche IRISA – UMR6074
Institut de Recherche en Informatique et Systeme Aléatoires
et au centre INRIA - Rennes Bretagne Atlantique

Bearing-based Thèse soutenue à Rennes

le 11/01/2018
devant le jury composé de :
Localization and Alcherio Martinoli / Rapporteur
Associate Professor, Ecole Polytechnique Fédérale de
Control for Multiple Lausanne (EPFL), Switzerland
Nicolas Marchand / Rapporteur
Directeur de Recherche CNRS - HDR, GIPSA-lab Gre-
Quadrotor UAVs noble, Control Systems Department, France
Bruno Siciliano / Examinateur
Full professor, Università degli studi di Napoli Federico II,
Italy
Sophie Pinchinat / Examinateur
Full professor, ISTIC (Department of Electrical Enginee-
ring and Computer Science) University of Rennes 1,
France
Pascal Morin / Examinateur
Full Professor, Institut des Systèmes Intelligents et de Ro-
botique, Paris, France
Paolo Robuffo Giordano / Directeur de thèse
Directeur de Recherche CNRS, IRISA-INRIA, Rennes,
France
To my family and to Ida
Always improve yourself
 Résumé en français

Dans la littérature, l’une des premières apparitions des automates fut recensée dans
un texte Daoïste, intitulé Lie Zi (ou Liezi), dans lequel était décrite une rencontre
entre le Roi Mu de Zhou (1023-957 B.C.) et Yan Shi, un ingénieur en mécanique [1].
On dit que l’ingénieur montra au roi sa construction mécanique, d’aspect et de taille
comparables à celles d’un humain. Cet exemple témoigne de l’intérêt immémorial de
l’Homme pour les créations mimant le comportement humain, primant même sur
son désir de reproduire le comportement des animaux ou des insectes. Selon moi,
l’exemple le plus fascinant est celui des animaux volants et des insectes. Depuis ses
débuts, l’Homo Sapiens ne cesse d’observer des corps volants et de s’imaginer voler.
Il lui fallut beaucoup de temps pour réaliser ce rêve, mais, désormais, nous vivons
dans un monde où se déplacer en avion est devenu monnaie courante et où nous ne
sommes qu’à un pas de faire voler nos voitures. Dans cette optique, il semble que la
robotique soit entrée dans une nouvelle ère, dans laquelle les robots sont pleinement
intégrés à nos sociétés. Le lecteur de ce manuscrit pourra d’ailleurs faire le parallèle
avec la révolution numérique du 21ième siècle. Cela fait d’ailleurs plusieurs décennies
que les robots sont utilisés dans des applications civiles et militaires. Aujourd’hui,
ils sont principalement employés par l’industrie, l’armée et l’agriculture, mais ils
sont aussi présents en médecine, sous la forme de nanorobots, dans nos foyers etc.
La liste ne cesse de s’allonger, et, malgré cela, chaque année, de nouvelles manières
d’utiliser les robots, insoupçonnées encore il y a vingt ans, émergent. C’est le cas,
par exemple, des robots cuisiniers1 , serveurs, ou bien de ceux qui seront utilisés pour
administrer des médicaments plus efficacement à des patients [2, 3].
Imiter le comportement d’un seul animal ou insecte est encore le centre d’attention
de grands sujets de recherche [4–6]. Cependant, depuis des siècles, les chercheurs
se sont aussi intéressés au comportement collectif des animaux, des insectes et
des bactéries [7]. Pour cette raison, l’un des objectifs de la communauté robotique
1
http://www.moley.com/

i
est de produire des robots capables d’imiter des comportements présents dans la
nature, tels que l’essaimage (en anglais swarming), le rassemblement (flocking),
l’attroupement (herding), et le rassemblement en bancs (shoaling/schooling) [8–15].
Même en dehors de la robotique, de nombreux chercheurs étudient ces comportements
pour pouvoir les répliquer. Un aspect fascinant de ces derniers est que les agents
de tels groupes semblent souvent agir en suivant seulement leur propre intention,
alors qu’une ruche d’abeilles, une colonie de fourmis ou une migration d’oiseaux
apparaissent particulièrement organisées, comme si leurs membres agissaient dans un
but commun. Les chercheurs comprirent que dans ces situations, les agents suivent
quelques règles simples, le plus souvent sans avoir besoin d’un chef. En suivant
celles-ci, le comportement du groupe semble alors harmonieux. Dans la Fig. 1.1 sont
présentés deux exemples de coopération chez les fourmis de feu qui surgissent grâce
aux principes mentionnés. Dans ces situations, et dans bien d’autres également, les

(a) (b)

Figure 1 – Exemples de comportements collectifs exposés par des fourmis de feu. Fig. 1(a):
un tas de 500 fourmis de feu, composées d’une couche partiellement mouillée de fourmis sur
le fond et les fourmis sèches sur sommet, [16]. Fig. 1.1(b): flottabilité et élasticité du radeau
de fourmis, comme indiqué par immersion essayée par une brindille, [16].

agents (animaux ou insectes) ne disposent que des informations qu’ils ressentent, et

ne peuvent agir que localement (c’est-à-dire par rapport à eux-mêmes). Il n’existe
pas d’information partagée par tous les agents, et aucune forme d’encadrement.
Durant la dernière décennie, un intérêt croissant fut porté aux applications multi-
robots. En particulier, la recherche sur la coordination multi-véhicules débuta vers la
fin des années 1980, dans le domaine de la robotique mobile (se référer à [17,18] pour
un état de l’art dans ce domaine, valable jusqu’à 2006). La plupart des scénarios
multi-robots sont similaires aux comportements de groupe adoptés par certains
animaux et insectes, abordés précédemment. D’ailleurs, l’un des éléments communs à
toute formation organisée est l’objectif principal (par exemple, la construction d’une
ruche, le transport d’un morceau volumineux de nourriture d’un endroit à un autre,
la migration de tout un groupe d’oiseaux etc) à atteindre de manière décentralisée
(chaque agent ne connaît qu’en partie l’état de ses voisins). Pour atteindre cet

ii
objectif, un élément que l’on retrouve dans chaque formation organisée est la faculté
de chaque agent à interagir seulement avec une sous-partie du groupe. Ce sous-partie
du groupe est d’habitude appelé les voisins de l’agent considéré. Certains agents
sont même capables de se repérer par rapport aux autres et de prendre des décisions,
en se basant uniquement sur des informations locales.
En résumé, il arrive souvent que des agents de la formation doivent effectuer des
actions nécessitant des mesures relatives (relatives aux autres agents de la formation,
et non à un référentiel central au groupe). Parmi ces mesures relatives on trouve
les distances relatives, les bearings 2 relatives, les positions relatives et, bien sûr,
des combinaisons des trois. En robotique, ce type de mesure peut être obtenu à
partir de capteurs embarqués, comme, par exemple, des capteurs ultrasonores, des
scanners lasers, des caméras ou des émetteurs/récepteurs radiofréquences. Leur
utilisation, pour extraire des mesures relatives, permet à la structure de contrôle de
s’affranchir de la présence de systèmes de localisation centralisés, comme le dispositif
de capture de mouvement proposé par Vicon, ou le GNSS et l’utilisation de, par
exemple, algorithmes SLAM [19, 20]. Ces concepts nous amènent à un des motifs
principaux du développement des systèmes de multi-agent qui est la possibilité de
décentralisation.
Les solutions décentralisées jouent un rôle significatif dans des applications de
multi-agent puisqu’elles permettent d’utiliser des algorithmes indépendement de
la taille de groupe (scalable algorithms). Cependant, même si l’importance et les
avantages de décentralisation sont evidentes à la communauté de multi-robot, il est
rare de trouver des solutions purement décentralisées pour des systèmes de multi-
robot. En effet, c’est le cas de toutes ces applications qui comptent lourdement sur
des systèmes de localisation centralisés comme le système de capture de mouvement
de Vicon ( Fig. 1.2(a) ) ou le GNSS ( Fig. 1.2(b) ). Ces applications sont très
intéressantes, mais nécessitent des ajustements substantiels afin d’être déployées
dans un scénario réaliste, pour lequel les systèmes centralisés ne sont pas disponibles
(par exemple, dans un bâtiment détruit par un séisme, sous l’eau, sous terre, dans
l’espace, ou dans un endroit où le signal GPS est faible).
En robotique, les robots mobiles terrestres sont encore largement employés dans
les applications multi-robots [19, 21], (voir Fig. 1.2(c) ). Cela est principalement
dû à leur coût relativement faible, leur facilité d’utilisation, mais aussi leur sécurité
(d’habitude) intrinsèque. Pour toutes ces raisons, à leurs débuts, les applications
multi-robots permirent de mettre en avant les atouts des systèmes multi-agents, ainsi
que les verrous technologiques inhérents à leur élaboration. Le principal inconvénient
2
Dans cette Thèse le terme anglais bearing se réfère à une mesure non métrique, obtenue en
temps-réel via l’utilisation d’une caméra montée sur le drone et visualisant les autres drones de la
formation. Cette mesure est alors représentée par le vecteur unitaire pointant vers le drone visualisé.

iii
d’un robot terrestre est que son champ d’action est, le plus souvent, limité à deux
dimensions. Ceci, combiné à l’intérêt croissant des chercheurs pour les drones, a
suscité un fort intérêt pour l’application des concepts des formations multi-robots
terrestres à leurs homologues aériens et, en particulier, ceux capables de décoller et
d’atterrir verticalement (en anglais VTOL UAVs) [22].
Dans cette thèse, nous nous intéressons plus particulièrement aux quadrotors,
des drones de taille réduite, et à l’utilisation d’informations visuelles pour les
coordonner. Cette technologie a d’ailleurs été appliquée avec succès à la fouille de
sites dévastés par des catastrophes naturelles, telles que les séismes de Chirstchurch
en Nouvelle-Zélande (2011), Emilia-Romagna (2012) et Amatrice (2016), ou bien
après le désastre nucléaire de Fukushima Daiichi, au Japon (2011). De plus, les
drones sont fréquemment utilisés pour filmer des événements sportifs comme les
Jeux Olympiques d’hiver de Sotchi, en Russie, ou la coupe d’Europe 2016, ayant
eu lieu en France. Un autre exemple est celui du spectacle de Lady Gaga, organisé
pendant la mi-temps du Super Bowl (2017), et pour lequel 300 drones Shooting Star
de chez Intel furent déployés. Il s’agissait de l’une des premières fois que les drones
étaient utilisés pour un événement télévisuel. Selon l’institut de recherche Gartner3 ,
l’industrie des drones personnels ou commerciaux devrait générer des revenus de
l’ordre de 5 milliards d’euros en 2017 et plus de 9,4 milliards d’euros en 2020 [23].
Malgré cette croissance exponentielle du domaine de la robotique, l’utilisation
d’un dispositif entièrement autonome dans un environnement inconnu et déstruc-
turé constitue toujours, aujourd’hui, un sujet actif de recherche. Il n’est donc pas
surprenant que trois ambitieux projets aient été financés à hauteur de 22,2 millions
d’euros ces six dernières années pour approfondir la thématique de la coordination
multi-robots [24–26]. De la même manière, les compétitions de robotique dédiées
aux applications multi-robots sont de plus en plus fréquentes et attirent toujours
plus d’investissements4 .

Contributions de la Thèse
L’objectif de la thèse est de proposer des innovations pour résoudre les problèmes
mentionnés auparavant (en particulier, le contrôle de la formation et la localisation
par coopération), dans le cas d’un groupe de drones de type quadrotor, équipés
de caméras monoculaires. Un intérêt est porté sur la mise en place d’un contrôle
de la formation entièrement décentralisé et de techniques d’estimation reposant
sur (i) des mesures relatives, obtenues à partir de caméras embarquées, et (ii) une
communication locale utilisant le standard Wi-Fi. Pour que les résultats obtenus
3
www.gartner.com
4
www.mbzirc.com, www.robocup.org, www.eurathlon.eu

iv
 (a) (b)

(c) (d)

Figure 2 – 1.2(a) : Transport coopératif d’un objet rigide avec quatre UAVs. Copyright
d’Université de la Pennsylvanie (USA). 1.2(b) : un drapeau américain fait de 300 drones
Shooting Star par Intel au spectacle de mi-temps du Super Bowl (2017). Copyright d’Intel.
1.2(c) : un groupe de robots terrestres naviguants et déplacent des objets autour d’un
entrepôt. Copyright de systèmes Kiva. 1.2(d) : structure de filament extensible construite
par des robots. Copyright de l’Institute for Computational Design and Construction à
l’Université de Stuttgart.

soient les plus réalistes possibles, certaines limitations classiques des capteurs sont
prises en considération. Une approche alternative permettant de résoudre le problème
de localisation dans un environnement inconnu, à l’aide de capteurs embarqués, est
également envisageable avec des techniques de mapping/SLAM [19]. Cependant,
cela nécessiterait soit une carte détaillée de l’environnement, soit la possibilité
d’exécuter des algorithmes complexes de type SLAM en temps réel, directement sur
les robots. Par ailleurs, nous pensons que la résolution du problème de localisation par
coopération, uniquement à partir d’informations locales (capteurs et communication)
aux drones, offrirait au système plus de souplesse, lui permettrait d’évoluer dans des
environnements inconnus et n’impliquerait pas qu’il faille embarquer de nombreux
capteurs ni une grande puissance de calcul sur chaque drone.
Pour contrôler une formation de drones, il est important d’avoir une quantité mé-
trique de quel récupérer les distances entre les robots. Cependant, cette information
ne peut être obtenue uniquement à partir d’informations visuelles (non-métriques)
d’entrée. Ainsi, dans ce manuscrit, nous présentons également une méthode permet-
tant d’extraire en temps réel l’échelle de la formation (et donc les distances entre les
robots) à l’aide de mesures de bearing et en supposant que les robots ont connais-

v
sance des vitesses linéaires et angulaires exprimées dans leur repère (body-frame).
La dernière hypothèse est, selon nous, assez faible puisque chaque drone doit avoir
connaissance de ces grandeurs, afin de contrôler son vol.
Les résultats théoriques présentés dans ce manuscrit ont été validés de manière
approfondie en simulation et durant des expérimentations. Ces dernières ont été
réalisées avec un groupe de drones de type quadrotors (Annexe B).

Structure de la thèse
Le présent manuscrit est divisé en trois grandes parties. La première (I) contient une
courte introduction sur la robotique qui détaille plus particulièrement les applications
multi-robots, en insistant sur celles ayant recours à des drones. Une synthèse de
ces thématiques est proposée, et plusieurs états de l’art issus de la littérature
sont cités pour appréhender le contexte dans lequel s’inscrivent les travaux. La
deuxième partie (II), quant à elle, constitue le cœur de la thèse. Elle en présente
les contributions principales. Les résultats étayés dans cette seconde partie ont fait
l’objet des publications suivantes : [27–29]. La troisième (III), et dernière, partie
conclut le manuscrit, expose les futurs travaux à mener et s’achève sur deux annexes,
permettant au lecteur d’approfondir certains aspects techniques. Un sommaire est
détaillé ci-dessous.
Aperçu de la Partie I
La première partie contient un état de l’art exhaustif sur le contrôle et la
localisation des formations multi-robots, et introduit des fondamentaux sur la théorie
algébrique des graphes et de la rigidité.
Le Chapitre 2 donne un aperçu des concepts portant sur les formations multi-
robots, et explicite ceux appliqués aux drones. Il inclut notamment une brève
description des problèmes majeurs inhérents au contrôle des formations multi-agents,
en se focalisant sur les problèmes de consensus et de contrôle de la formation. Dans
sa conclusion, ce chapitre décrit le problème de localisation par coopération à partir
de mesures relatives.
Le Chapitre 3 propose une introduction sur la théorie algébrique des graphes.
Celle-ci est fondamentale pour, ensuite, exposer les principes de la théorie de la
rigidité. La dernière partie de ce chapitre approfondit la théorie de la rigidité,
appliquée aux cas de la distance et des contraintes de bearing. Cela permettra
d’aboutir, par la suite, au concept clé de cette thèse, à savoir la matrice de rigidité
bearing (BRM).
Le Chapitre 4 décrit les équations modélisant un seul drone et fournit quelques
détails sur la rigidité bearing en R3 ×S1 , qui seront utilisés dans les chapitres suivants.

vi
 Aperçu de la Partie II
La seconde partie de ce manuscrit décrit les contributions apportées par les
travaux conduits dans le cadre de cette Thèse. Il s’agit de techniques permettant de
contrôler et de localiser, de manière décentralisée, une formation de drones de type
quadrotors dans R3 × S1 . Ensuite, cette méthode est combinée avec un algorithme
assurant la rigidité de la formation, une propriété essentielle pour la convergence de
cette dernière et pour les modèles de localisation. La dernière contribution repose
sur une analyse non-linéaire de l’observabilité d’un système multi-agents composé
de plusieurs quad-rotors. Cette étude montre qu’il est possible d’estimer l’échelle de
la formation uniquement à partir de mesures de bearing et des vitesses linéaires et
angulaires des robots, exprimées dans leur repère.
Le Chapitre 5 aborde la conception d’un algorithme permettant de contrôler
et de localiser, de manière décentralisée, une formation. Il est inspiré de [30, 31]. Cet
algorithme de localisation est capable d’estimer les positions et le lacet des agents.
Cependant, il est important de garder en mémoire que ce dernier n’a pas connaissance
des poses absolues des robots. Par conséquent, il existera systématiquement un
décalage lié à une rototranslation globale du système. Il subsiste également une
ambiguïté d’échelle, liée à l’absence de données de distance (métriques). Celle-ci est
levée par l’introduction d’une unique mesure de distance. Des expérimentations avec
cinq quad-rotors sont présentées à la fin du chapitre.
Le Chapitre 6 expose le problème de maintenance de la rigidité bearing d’une
formation de quadrotors. Cette propriété est d’une importance capitale pour résoudre
les problèmes de contrôle et de localisation de la formation, présentés dans les
chapitres précédents. La stratégie de maintenance mise en place est robuste à des
limitations des capteurs telles que le champ de vision réduit des caméras, leur faible
portée, ainsi que d’éventuelles occlusions entre les agents lors du mouvement de la
formation.
Le Chapitre 7 détaille l’analyse de l’observabilité du système non-linéaire
constitué par plusieurs drones. Celle-ci est préliminaire à la réalisation d’un filtre
de Kalman étendu, qui est implémenté directement dans SE(3), ceci lui permettant
d’estimer les positions et les orientations des agents de la formation. Il est important
de noter que, par rapport à l’algorithme de localisation présenté au Chapitre 5, les
positions sont toujours estimées sans avoir connaissance de la rototranslation globale
du système, mais avec la bonne échelle. Ce résultat est obtenu sous l’hypothèse
(réaliste) que les valeurs d’entrée transmises à chaque agent, dans le repère qui leur
est propre (body-frames), sont connues.
Aperçu de la Partie III
Le Chapitre 8 présente les conclusions des travaux de Thèse et en résume les

vii
contributions. De plus, des pistes de recherche pertinentes à explorer sont proposées et
discutées. Celles-ci sont d’ailleurs actuellement étudiées par l’auteur de ce manuscrit.
L’Annexe A détaille certains concepts mathématiques présentés au Chapitre 6.
L’Annexe B décrit brièvement l’architecture (hardware et software) utilisée
durant les expérimentations présentées dans les Chapitres 5,6,7.

viii
 Abstract

Since humans exist, they have been witnessing the great power of nature. Among
the many fascinating behaviors we find in nature, one which has inspired the work
of researchers from all over the world is the show offered every day by insects
and vertebrates with their collective behaviors. Indeed, one aspect common to all
previous categories is that they give birth to complex cooperative behaviors through
really simple actions. If, to this concept, we add also the environment variable we
are talking about a really specific social network mechanism which is called stigmergy.
Stigmergy is the phenomenon of indirect communication mediated by modifications
of the environment. As an example, it is intriguing to think about ants. Each ant
is mainly able to do two simple actions: (i) leave traces of different pheromone
perfumes in the environment and (ii) follow these traces. Relying only on these local
capabilities ants are capable to give birth to exceptionally complex behaviors such
as the construction of anthills, transportation of food (alone or in groups), sticking
together to form a whole and float in water or resist to an external force.
Since many years researchers from all over the world are trying to understand
the very small details of these behaviors in order to replicate them. Human beings
try to replicate nature because they are aware that nature often presents them with
efficient solutions to very specific problems (e.g., flying between two places, flocking
or moving an object from one point to another). This is made possible by the
fact that nature is constantly solving an optimization problem. This sophisticated
process is usually known as evolution. Among the many disciplines tackling these
problems, we are interested in robotics and specifically in multi-robot applications.
In this broad context, the aim of this Thesis is to give contributions to the state
of the art on the collective behavior of a group of flying robots, specifically quadrotor
UAVs, which can only rely on their onboard capabilities and not on a centralized
system (e.g., Vicon or GNSS) in order to safely navigate in the environment. We
achieve this goal by giving a possible solution to the problems of formation control

ix
and localization from onboard sensing and local communication. We tackle these
problems exploiting mainly concepts from algebraic graph theory and the so-called
theory of rigidity. This allows us to solve these problems in a decentralized fashion,
and propose decentralized algorithms able to also take into account some typical
sensory limitations. The onboard capabilities we referred to above are represented by
an onboard monocular camera and an inertial measurement unit (IMU) in addition
to the capability of each robot to communicate (through RF) with some of its
neighbors. This is due to the fact that an IMU and a camera represent a possible
minimal, lightweight and inexpensive configuration for the autonomous localization
and navigation of a quadrotor UAV. Notice that sensor limitations are present both
in robotics and in nature (e.g., ants have a limited sensing range of the pheromone
traces, birds have a limited field of view and so on).
A first contribution of this Thesis is the design of a formation control technique
that allows the robots to achieve a certain shape only through bearing measurements
coming from onboard monocular cameras and, at least, one distance measurement
(e.g., coming from a rangefinder). In addition to this, the bearing formation can also
be steered in 3D space without changing the bearings between the robots. We also
couple this control algorithm with an estimation of the relative poses between the
robots of the formation which is able to converge also for non-stationary agents. A
second contribution of this Thesis consists in a strategy able to maintain formation
rigidity over time against sensing limitations (limited field of view of the camera,
maximum/minimum range of the camera, and occluded visibility).
Finally, in order to cope with the missing scale information from pure camera
measurements, a third contribution of this Thesis consists in a technique able
to estimate the scale of a formation of quadrotor UAVs only through bearing
measurements and known agent ego-motion (body-frame linear/angular velocity).
All the theoretical developments discussed in this Thesis are corroborated by
simulations and experiments run by using a group of quadrotor UAVs.
The reported results show the effectiveness of proposed techniques in controlling
the motion of multiple quadrotor UAVs only relying on (constrained) onboard
sensing/communication capabilities.

Keywords: multi-robot, formation control, cooperative localization, aerial robotics,

rigidity theory.

x
 Acknowledgements

Here I am, at the end of another journey of my life trying to thank all the people
who have contributed to it. For sure it was the most tiring (my first white hairs are
the proof) but also a fascinating journey of my life. It was the one which changed the
most my way of working and approaching different problems both in my professional
and personal life.

For this, first of all, I should thank my supervisor: Paolo. Before starting the
Ph.D., or sometimes at its beginning, I read that the admiration for your supervisor
is inversely proportional to time, but for me was exactly the opposite. The more
I went through my Ph.D. the more I recognized how lucky I was to be advised,
encouraged and shaken up by him. As any Ph.D., mine was full of highs and lows
and mainly thanks to his advice I was able to get across the main obstacle and learn
from every error. I truly appreciated his ability to always be there when I asked for
his help and to give me more space when I wanted to count mainly on my forces.

Then, I would like to thank Roberto, who guided me for 6 months (and more) in
Boston during my visiting period. When I arrived in Boston I thought it would have
been really hard to end up again with an amazing advisor but it (luckily) happened
again. We addressed difficult topics which I thought to be impossible for me to
deal with, but thanks to his patience I was able to get really a lot from those six
months. As Paolo, he was incredibly available when I needed him and this helped a
lot in giving my Ph.D. a final important boost. The decision of spending six months
working with him was among the best decisions I have made during my entire Ph.D.

Another big thank you goes to the head of the Lagadic team: Francois. He
gave me the opportunity to be part of an amazing, talented team. Thanks for our
occasional, always interesting, talks. With him too, I was truly impressed that
whenever I knocked at his door he was available to listen and to help me.

Thanks to my parents I became who I am right now. For this, I will be always

xi
grateful to you both. I could not ask for something better than you. During the
years of my Ph.D., besides being the father that everyone would ask for, he was a
priceless source of professional advice. I do not think I would be here without him
conveying to me all the passion for math, engineering and so on. My mother is one
(of the two) anchors for stability and balance in my life. She can understand my
mood from the first ten words even if we are speaking over the phone, thousands of
kilometers away from each other.
Thanks to my brother who has always believed in me and who, I know, was
secretly my first supporter.
My grandmother would have been proud of me finishing a Ph.D. even without
knowing what a Ph.D. is, thanks for your continuous admiration and love.
I am grateful that I have many friends in my life but thanking all of them one
by one would be quite hard. Therefore I say already sorry if I forgot you. Thanks to
Philip, one of my best friends. Thanks for listening to my highs, but mainly to my
lows during these three years. For me, you are the proof that friendship does not
need 10 years to be built. Thanks to Bruno, Arturo, Anna, Luca, Mirko for your
support during these years. We are definitely sure that our friendship is immune
to distance. Thanks to Riccardo for his invaluable advice regarding my Ph.D. but
mainly for his friendship. I am really glad to have met you during my Ph.D. Thanks
to Francesco for introducing me to the Ferrari oven and for his friendship which
brought a lot of interesting things in my life and I am sure it will continue to do so.
Thanks to Giovanni for having listened to several problems during my Ph.D. At some
points knocking at the door of the robotics room was almost the normal consequence
of my frustration during experiments. Thanks to Rosa, Fausta and Riccardo for our
serate tranquille at home with lots of food and less of Ph.D.-related-stuff. Thanks
to Carlotta, Luiz, Roberto, Antonella, Claudia and Melania for our amazing times
in Rennes. Thanks to the most recent Italian-Lagadic-subgroup: Marco, Marco and
Claudio. To all of you, I hope that our friendship, started thanks to my period in
Rennes will not end even if our ways will separate us. Thanks to all my friends in
Naples, even talking to you for just 5 minutes helped me a lot sometimes. Thanks
a lot to my Bostonian friends: Claudia, Alessandra, Caterina, Morgane, Johnny,
Fernando, Mirza and Jamie. Without you, my experience in the US would have been
way harder. The same goes for the Ph.D. students in the robotics lab in Boston,
in particular Zac and Bee. Thanks to Nicola for having helped me with a session
of experiments which finished really early (in the morning). Thanks to Pedro for
the lots of coffees shared which brought a lot of nice and interesting conversations.
Thanks to Muhammad for his help in the last session of experiments of my Ph.D. and
for the interesting chats. Thanks to all the members of the Lagadic team, Fabien,
Aurelien, Nicolas, Pol, Bryan, Hadrien, Axel, Firas, Louise, Suman, Aly, Vishnu and

xii
much more who were present in this amazing journey.
The last paragraph is to say GRAZIE to the most important person and the love
of my life, Ida. Without you, I honestly think that my Ph.D. would have been ten
times harder. You are the main source of balance/happiness/joy of my life and the
person who every night listened to me in these years. You motivated me with your
sincere and unconditional love. For this reason, I am and will be immensely grateful.
You all made one of my dreams come true. Thank you.
Fabrizio.

xiii
Acronyms and abbreviations

AGL Above Ground Level

BLOS Beyond Line of Sight

BRM Bearing Rigidity Matrix

DBOM Dynamic Bearing Observability Matrix

EKF Extended Kalman Filter

GNSS Global Navigation Satellite System

IMU Inertial Measurement Unit

LOS Line of Sight

MAS Multi-Agent System

MSL Median Sea Level

RF Radio Frequency

ROS Robot Operating System

SfM Structure from Motion

SLAM Simultaneous Localization and Mapping

UAV Unmanned Aerial Vehicle

VTOL Vertical Take-off and Landing

xiv
 Contents

Contributions de la Thèse . . . . . . . . . . . . . . . . . . . . . . . . . . . iv

Notation xix

Chapter 1 Introduction 1
1.1 Thesis contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.2 Structure of the Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . 6

Part I Preliminaries and state of the art 9

Chapter 2 Multi-robot systems 11

2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.2 Decentralization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.3 Multi-aerial vehicles . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.4 Multi-agent control problems . . . . . . . . . . . . . . . . . . . . . . 20
2.4.1 Consensus and rendezvous . . . . . . . . . . . . . . . . . . . . 21
2.4.2 Formation control . . . . . . . . . . . . . . . . . . . . . . . . 22
2.5 Cooperative localization algorithms . . . . . . . . . . . . . . . . . . . 24

Chapter 3 Algebraic graph theory and graph rigidity 27

3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.2 Graph theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.2.1 Graph theory definitions . . . . . . . . . . . . . . . . . . . . . 29
3.3 Algebraic graph theory . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.3.1 Laplacian matrix and connectivity . . . . . . . . . . . . . . . 32
3.4 Rigidity theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.4.1 The Laman’s theorem and some definitions for the case of
distance constraints . . . . . . . . . . . . . . . . . . . . . . . 36

xv
 3.4.2 The distance-rigidity matrix . . . . . . . . . . . . . . . . . . . 39
3.4.3 The case of bearing constraints . . . . . . . . . . . . . . . . . 44
3.4.4 Similarity between the rigidity matrix and the Jacobian of
robotic manipulators . . . . . . . . . . . . . . . . . . . . . . . 45

Part II Contributions 47

Chapter 4 Main modeling assumptions 49

4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.2 Agent model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.3 Directed bearing rigidity in R3 × S1 . . . . . . . . . . . . . . . . . . 52

Chapter 5 Formation control and localization in R3 × S1 57

5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.1.1 Chapter overview . . . . . . . . . . . . . . . . . . . . . . . . . 60
5.2 Decentralized formation control . . . . . . . . . . . . . . . . . . . . . 60
5.2.1 Rigidity-based control of bearing frameworks in R3 × S1 . . . 61
5.2.2 Rigidity-based localization of time-varying bearing frameworks
in R3 × S1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
5.2.3 Coordinated motions in the null-space of the bearing rigidity
matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
5.2.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
5.3 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
5.4 Experimental results . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
5.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

Chapter 6 Rigidity maintenance 75

6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
6.1.1 Chapter overview . . . . . . . . . . . . . . . . . . . . . . . . . 77
6.2 Cooperative localization from bearing measurements . . . . . . . . . 78
6.3 A bearing rigidity maintenance strategy . . . . . . . . . . . . . . . . 78
6.3.1 Design of the inter-agent weights . . . . . . . . . . . . . . . . 79
6.3.1.1 Minimum/maximum range . . . . . . . . . . . . . . 79
6.3.1.2 Limited field of view . . . . . . . . . . . . . . . . . . 80
6.3.1.3 Occluded visibility . . . . . . . . . . . . . . . . . . . 81
6.3.2 The bearing rigidity eigenvalue . . . . . . . . . . . . . . . . . 83
6.3.3 The bearing rigidity maintenance controller . . . . . . . . . . 87
6.3.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
6.4 Experimental results . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
6.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91

xvi
Chapter 7 Nonlinear observability and estimation for multi-agent
systems 93
7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
7.1.1 Prior work . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
7.1.2 Main contributions . . . . . . . . . . . . . . . . . . . . . . . . 96
7.1.3 Chapter overview . . . . . . . . . . . . . . . . . . . . . . . . . 97
7.2 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97
7.2.1 General notation . . . . . . . . . . . . . . . . . . . . . . . . . 97
7.2.2 Formation, agent and measurement model . . . . . . . . . . . 98
7.2.3 Elements of Riemannian geometry . . . . . . . . . . . . . . . 99
7.2.4 Elements of local nonlinear observability . . . . . . . . . . . . 101
7.3 Dynamic Bearing Observability Matrix . . . . . . . . . . . . . . . . . 102
7.3.1 Matrix R̃A . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
7.3.2 Matrix R̃B . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
7.3.3 Numerical verification of the ranks of R̃A and R̃ . . . . . . . 109
7.4 A multi-agent Extended Kalman Filter . . . . . . . . . . . . . . . . . 110
7.5 Experimental results . . . . . . . . . . . . . . . . . . . . . . . . . . . 112
7.6 Conclusions and future works . . . . . . . . . . . . . . . . . . . . . . 113

Part III Conclusions and Future Work 117

Chapter 8 Conclusions and future work 119

8.1 Summary and contributions . . . . . . . . . . . . . . . . . . . . . . . 119
8.2 Open issues and future perspectives . . . . . . . . . . . . . . . . . . 121

Appendix A Additional technical details associated to Chapt. 6 127

A.1 How to go from (6.15) to (6.16) . . . . . . . . . . . . . . . . . . . . 127
A.2 Useful derivatives for the computation of the derivative of λB6 . . . . 128
A.3 Towards the derivatives of the weights . . . . . . . . . . . . . . . . . 129

Appendix B Simulation and experimental architecture 131

B.1 Simulations architecture . . . . . . . . . . . . . . . . . . . . . . . . . 131
B.2 Real experiments architecture . . . . . . . . . . . . . . . . . . . . . . 132
B.2.1 The quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . 132
B.2.2 The Vicon motion capture system . . . . . . . . . . . . . . . 134

Bibliography 139

Vita 159

xvii
 Notation

General notation conventions

Unless otherwise stated, the main conventions used in the notation of this Thesis
are the following

• Scalar quantities are represented by lowercase symbols such as u, v, and so on.

• Elements of Rn and similar sets are interpreted as column vectors and repre-
sented by bold lowercase symbols such as u, v, and so on.

• ed represents the d-th element of the standard basis in Rn .

• We use the notation (a, b, c) to indicate a vertical concatenation of elements

(scalars, vectors or matrices) and [a b c] for horizontal concatenations.

• In is used to represent the identity matrix of dimension n × n.

• 0n×m is used to represent the n × m matrix with all elements equal to zero. If
m = 1 we also use 0n .

• 1n represents a vector of all ones of dimension n × 1.

• Matrixes of real numbers, i.e. elements of Rn×m , are indicated with capital
letters such as A, B and so on.

• AT denotes the matrix transpose of A.

• sym(A) is an operator that extracts the symmetric component of a matrix A,

i.e., sym(A) = 12 (A + AT ).

• skew(A) is an operator that extracts the skew-symmetric component of a

matrix A, i.e., skew(A) = 12 (A − AT ).

xix
• A† indicates the pseudoinverse of A.

• ⊗ denotes the Kronecker product.

• tr(A) is the trace of a matrix A.

• f˙(·), f¨(·) are the first and second derivative of a scalar function f .

• stack(·) is an operator which returns a matrix containing a vertical stacking of

the arguments.

• Sn represents the n-dimensional sphere defined as Sn = {v ∈ Rn+1 : v T v = 1}.

• h·, ·i represents the Riemannian metric.

• M represents a Riemannian manifold and Tx M is the tangent space of a

manifold M at a point x ∈ M.

• SO(3) denotes the special orthogonal group of dimension three, i.e. the space
of 3-D rotations SO(3) = {R ∈ R3×3 : RT R = I, det(R) = 1}.

• SE(3) denotes the special Euclidean group of dimension three, i.e. the space
of 3-D poses SE(3) = {(p, R) : R ∈ SO(3), p ∈ R3 }. the space of 3-D poses
SE(3) = {(p, R) : R ∈ SO(3), p ∈ R3 }

• W represents an absolute 3-D world reference frame.

• Ai represents a body reference frame attached to the i-th agent.

• Ri ∈ SO(3) represents a rotation matrix transforming from Ai to W.

• Let f : Rn 7→ R, p 7→ f (p) be a generic scalar function of a vector argument.

We indicate with   
∂f (p) 
 ∂p1  

 p 

 ∂f (p)  
 ∂p2  
∇p f (p) = 
 . p

 . 
 .  
 
 ∂f (p)  
∂pn 
p

the column vector built by stacking the partial derivatives of f with respect
to the elements of p. This vector is also called the gradient of f w.r.t p.

xx
Graph theory

• V represents the vertex set which is a finite set of elements: V = {v1 , . . . , vN }.

• E represents the edge set which is a subset of ordered pairs of V 2 , the

 

2-element subsets of V: V 2 = {(vi , vj ) , i = 1, . . . , N, j = 1, . . . , N, i 6= j}.

 

E ⊆ V 2 and G is said to be undirected if (vi , vj ) ∈ E =⇒ (vj , vi ) ∈ E, while

 

it is said to be directed if (vi , vj ) ∈ E =⇒

6 (vj , vi ) ∈ E.

• G = (V, E) represents a graph G with vertex set V and edge set E.

• ek represents the k-th edge associated to the graph G.

• Ni = {j ∈ V| (i, j) ∈ E} ⊂ V represents the set of neighbors of an agent i with

respect to a graph G.

• Oi = {j ∈ V| (j, i) ∈ E} represents the set of agents j for which i is a neighbor

(this distinction is important for directed graphs).

• pi represents the position of agent i in a world frame (usually in R2 or R3 )

• ϕi , ϑi , ψi represent the respectively the roll, pitch and yaw associated to Ri

• qi represents the configuration of the agent i which can be either the pair
(pi , ψi ) or (pi , Ri ) depending on the context

• pij represents the vector (pj − pi ) ∈ R3

• dij represents the actual distance between agents i and j computed as kpj −pi k

• βij represents the bearing unit-norm vector between the agent i and j expressed
in the body frame of the agent i, and it is defined as
pj − pi pij
βij = RiT = RiT ∈ S2
kpj − pi k dij

xxi
 Chapter 1
Introduction

Contents
1.1 Thesis contributions . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.2 Structure of the Thesis . . . . . . . . . . . . . . . . . . . . . . . 6

NE of the first appearances of automata is reported in a Daoist text called

O Lie Zi (or Liezi) in which it is described a rendezvous between King Mu

of Zhou (1023-957 B.C.) and Yan Shi, a mechanical engineer. It seems
that the engineer showed the king a life-size, human-shaped figure of his mechanical
creation [1]. This is an example to show that humans have been interested in
replicating human behaviors for a long time and, maybe, even before they tried
to replicate animal/insect behaviors. Regarding the latter, the best and most
fascinating example is, in my opinion, the one involving flying animals and insects.
Homo Sapiens has seen flying bodies since the beginning of his time and since then
he dreamed of flying. It took some time for Homo Sapiens to figure out how to fly
but right now we live in a world in which airplanes are a completely normal thing
and flying cars are likely to come next. It seems indeed that robotics is now entering
a new era in which robots are more commonly accepted as an integral part of our
society. It can be considered similar to what happened to the digital revolution at
the beginning of the 21st century. Robots are being employed both in civil and
military applications since decades now. At the current time, some of their main
applications are industrial robots, military robots, agricultural robots, household
robots, nanorobots, medical robots and so on. This list seems to expand year after
year and robots are starting to be used in scenarios which we did not even imagine
of two decades ago. Examples of this trend are represented by the robots that now
are used as waiters, cooks1 or the ones which will be used to deliver drugs in human
bodies efficiently [2, 3].
1
http://www.moley.com/

1
Bearing-based localization and control for multiple quadrotors

Replicating a single animal/insect behavior is still the focus of great research [4–6]
but, since decades now, researchers are also being interested in replicating the
collective behaviors of multiple animals, insects and bacteria [7]. For this reason,
one of the goals of the robotic community is to make robots able to mimic behaviors
present in nature such as swarming, flocking, herding and shoaling (schooling) [8–15].
Several researchers, not only in robotics, are interested in studying, understanding
and replicating these behaviors. One of the fascinating aspects of these behaviors is
that the agents of these groups seem often to act only by following their own plan
but yet it seems that a beehive, an ant colony or a bird migration are incredibly
organized as if the agents are pursuing a master plan. Researchers understood that
in these cases it happens that all the agents of the group are following some easy
rules without, usually, the need of a supervisor. By following these rules, they are
capable of giving birth to some specific group behaviors. In Fig. 1.1 there are some
examples of cooperative behaviors exhibited by fire ants which arise thanks to the
mentioned principles. In the case highlighted in Fig. 1.1, and in many others, the

(a) (b)

Figure 1.1 – Examples of collective behaviors exhibited by fire ants. Fig. 1.1(a): a raft of
500 fire ants, composed of a partially wetted layer of ants on the bottom and dry ants on
top, from [16]. Fig. 1.1(b): buoyancy and elasticity of the ant raft, as shown by attempted
submersion by a twig, from [16].

agents (animals/insects) can use only what they sense and act locally (with respect
to themselves). There is neither a global information shared between the agents nor
some sort of supervision happening.
The last decades have witnessed a growing interest in multi-robot applications,
in particular, research about multi-vehicle coordination started around the end of
the 1980s in the field of mobile robotics (refer to [17, 18] for the state of the art up to
2006). Between the multi-robot scenarios, many of them are dealing with cases that
are similar to the formation-type behaviors mentioned above for animals and insects.
We can say that one common aspect of formation-type behaviors is that they have a
particular centralized goal (i.e., build the anthill/beehive, transport a big piece of
food from one point to another, migration of a whole group from one place to another

2
 1. Introduction

and so on) to achieve in a decentralized manner (each agent knows only part of the
state of its neighbors). In order to do that, one aspect that is common to all these
groups is the ability, for each agent, of sensing and communication with a limited
subset of the whole group. This subgroup is usually referred as the neighbors of the
considered agent. Some agents are even able to localize themselves with respect to
other agents of the formation and make decisions based only on local information.
To summarize, often it happens that the agents of the formation have to perform
actions which involve relative measurements (relative to the different agents of
the formation and not to a central unit/reference frame). Examples of relative
measurements are relative distances, relative bearings2 or positions and of course
combinations of the previous are possible. In robotics, this kind of measurements can
be retrieved directly from onboard sensors; some examples are ultrasound sensors,
laser scanners, cameras and radio-frequency transmitters/emitters. Using this type
of sensors to extract relative measurements allows freeing the whole control structure
from the presence of centralized localization systems as Vicon/GNSS and the use
of, for example, SLAM algorithms [19, 20]. These concepts bring us to one of the
main reasons for the development of multi-agent systems which is the possibility of
decentralization.
Decentralized solutions play a significant role in multi-agent applications since
they allow for scalable algorithms (in the sense of computational and communicational
loads) with respect to the group size. However, even if the importance and the
advantages of decentralization are quite clear to the multi-robot community it
seldom happens to come across purely decentralized solutions for multi-robot systems.
Indeed, this is the case of all those applications which are heavily relying on centralized
localization systems such as the Vicon motion capture system (Fig. 1.2(a)) or the
GNSS (Fig. 1.2(b)). These applications have a high value, but they would need
substantial adjustments to be deployed in a real-world scenario where centralized
systems are not available (e.g., inside a collapsed building after an earthquake,
underwater, underground, in weak-GNSS locations or even in deep space).
In robotics, ground (mobile) robots continue to be widely used for multi-robot
applications [19, 21], (see Fig. 1.2(c)). The use of ground robots is mostly due
to their relatively low cost, their ease of use and control and they are (usually)
intrinsic safeness. For all these reasons, at the beginning of multi-robot applications,
they helped understand the main strengths of multi-agent systems along with the
challenges present in their design. The main limitation of a ground robot is its
pervasiveness limited mainly to 2D scenarios. This, and the fast-paced growth of
2
In this Thesis the term bearing refers to the non-metric information that can be instantaneously
recovered from an onboard camera looking at other UAVs in the scene (that is, the unit bearing
vector pointing towards a UAV).

3
Bearing-based localization and control for multiple quadrotors

the UAV technology, induced a strong interest in applying the multi-robot concepts
apprehended through ground robot applications, to aerial robots and in particular
to Vertical Take-Off and Landing (VTOL) UAVs [22].
In this Thesis, we are particularly interested in small-size UAVs, specifically
quadrotors and in their coordination using mainly visual information. These objects
have been successfully used in different scenarios like the investigation of sites after
catastrophic events like the earthquakes of Christchurch (2011) in New Zealand,
Emilia-Romagna (2012) and Amatrice (2016) in Italy or after the Fukushima Daiichi
(2011) nuclear disaster in Japan. They are massively used to film sportive events
like the winter Olympics in Sochi, Krasnodar Krai, Russia and the UEFA Euro 2016
held in France. A recent example which involved 300 Shooting Star drones by Intel
and that has been literally celebrated in the news was the one during Lady Gaga’s
performance for the halftime show at the Super Bowl (2017). This event marked
the first time in which drones have been used in a televised event. According to the
research firm Gartner3 , globally the market revenue coming from the production of
commercial and personal drones should hit 5 billion euros in 2017 and grow more
than 9.4 billion euros in 2020 [23].
Despite the exponential progress in the robotics field, the use of a fully au-
tonomous system in a real-world unknown and unstructured environment is still
a subject of active research. For this reason, it is not surprising that three very
ambitious research projects [24–26] about multi-robot coordination have been funded
by the European Union for a total of about 22.2 millions of Euros in the last six years.
Similarly, robotics competitions focused on multi-robot application are receiving
exponential interest, and funding, all over the world4 .

1.1 Thesis contributions

Keeping these considerations in mind, this Thesis aims at advancing the state-of-the-
art in overcoming the problems mentioned above (in particular formation control
and cooperative localization) for the case of a group of quadrotor UAVs equipped
with monocular cameras. We address the design of fully-decentralized formation
control and estimation techniques based on (i) relative measurements retrievable
through onboard cameras and (ii) local communication through Wi-Fi. To make
the results of this work even closer to a real-world deploying, we also deal with
some typical sensor limitations. An alternative approach to tackle the problem
of localization in unknown environments through onboard sensors is an absolute
localization through mapping/SLAM techniques [19]. This would require though
3
www.gartner.com
4
www.mbzirc.com, www.robocup.org, www.eurathlon.eu

4
 1. Introduction

(a) (b)

(c) (d)

Figure 1.2 – 1.2(a): cooperative transportation of a rigid object with four UAVs. Copyright
of University of Pennsylvania (USA). 1.2(b): an American flag made of 300 Shooting Star
drones by Intel at the halftime show of the Super Bowl (2017). Copyright of Intel. 1.2(c): a
group of ground robots navigating and moving objects around a warehouse. Copyright of
Kiva systems. 1.2(d): tensile filament structure built by multiple robots. Copyright of the
Institute for Computational Design and Construction at the University of Stuttgart.

either a detailed map of the environment or the possibility of running complex SLAM
algorithms in real-time onboard the robots. On the other hand, we believe that
solving the cooperative localization problem relying only on the local skills (local
sensing and local communication) of the quadrotor UAVs provides our system with
a better flexibility allowing it to work in entirely unknown environments without
having to embed on each robot significant sensing/computational capabilities.
In order to solve the formation control for a group of robots, it is important
to have a metric quantity from which to retrieve the distances between the robots.
As well-known, this is not retrievable solely from the non-metric visual inputs.
Therefore, in this Thesis, we also present a method for retrieving online the scale of
the formation (and therefore all the distances between the robots) through bearing
measurements and assuming that the robots know their body-frame linear/angular
velocities. The last assumption is, in our opinion, quite mild since any UAV needs
knowledge of these body-frame quantities in order to control its flight.
All the theoretical results presented throughout the Thesis have been validated
through extensive sets of simulations and experiments. The experiments were
performed using as a platform a group of quadrotor UAVs (Appendix B).

5
Bearing-based localization and control for multiple quadrotors

1.2 Structure of the Thesis

This Thesis is divided into three main parts. The first part (Part I) contains a short
introduction about robotics going more in details into multi-robot applications and
especially the ones involving unmanned aerial vehicles (UAVs). A state of the art
for these topics is provided, and several survey papers are referenced to help the
reader in building up a well-defined context. The second part (Part II) represents
the core of the Thesis and its main contributions. The results illustrated in this part
correspond to the following author’s publications: [27–29]. The third and last part
(Part III) of the Thesis contains its conclusions, future work and two appendices
which will help the reader in the understanding of some details of the Thesis. Below
there is a summary of the content of each part of the Thesis.
Outline of part Part I
This part contains an extensive state-of-the-art for the topics of multi-robot
formation control and localization and the fundamentals of algebraic graph theory
and the theory of rigidity.
Chapt. 2 provides an overview of the multi-robot concepts and specifically of
the ones applied to UAVs. There is a brief description of the main multi-agent
control problems focusing more on the consensus and the formation control problems.
This chapter ends with the description of the cooperative localization problem from
relative measurements.
Chapt. 3 gives an introduction on algebraic graph theory. This is fundamental
for the explanation of the concepts of rigidity theory. The last part of this chapter
is devoted to explaining more the theory of rigidity both in the case of distance and
bearing constraints. This will allow us to arrive at the essential concept, for this
Thesis, of bearing rigidity matrix (BRM).
Chapt. 4 describes the equations regarding the model of the single UAV and
we give some details about bearing rigidity in R3 × S1 which are instrumental for
the following chapters.
Outline of Part II
The second part of this Thesis presents the contributions of this work related to a
technique used for the decentralized formation control and localization of quadrotor
UAVs in R3 × S1 . Then, we couple this method with an algorithm able to maintain
rigidity of the formation, an essential property for the convergence of the formation
control and localization schemes. The last contribution consists in a nonlinear
observability analysis of a multi-agent system composed of multiple quadrotor UAVs.
This study shows that it is possible to estimate the scale of a formation only from
bearing measurements and linear/angular velocities of the robots in their body

6
 1. Introduction

frames.
In Chapt. 5 is addressed a decentralized formation control and localization
algorithm which takes inspiration from [30, 31]. Note that the localization algorithm
can estimate the positions and the yaw angle of the agents up to a global roto-
translation of the whole framework. There is also an ambiguity related to the scale
due to the absence of a metric information5 . This ambiguity is removed with the
introduction of a single distance measurement. Experiments with five quadrotors
are presented at the end of the chapter.
In Chapt. 6 we deal with the problem of maintaining bearing rigidity of a
formation of quadrotor UAVs. This property is of paramount importance to solve the
problems of formation control and localization which we addressed in the previous
chapter. The maintenance strategy can cope with some sensor limitations as limited
field of view of the cameras, their limited range and the possibility of occlusions
between the agents occurring during motion.
In Chapt. 7 is presented an observability analysis of the nonlinear system made
of different UAVs. This analysis is preliminary to the design of an extended Kalman
filter (EKF), implemented directly on SE(3) which is able to estimate the positions
and orientations of the agents of the formation. It is important to note that, with
respect to the localization algorithm proposed in Chapt. 5, the positions are always
estimated up to a rototranslation of the whole framework but with the right scale
without, for the latter, depending on the availability of a distance measurement.
This is obtained by taking advantage of the (realistic) assumption of knowing the
inputs given to each agent in their own body frame.
Outline of Part III
In Chapt. 8 are reported the conclusions of the Thesis and the main contribu-
tions brought to the state-of-the-art are summarized. Moreover, some open issues
are listed and we discuss future directions which would be worth exploring. Note
that some of the proposed future directions are the subject of the author’s current
research.
In Appendix A there are some mathematical details relative to Chapt. 6.
In Appendix B it is briefly described the hardware and software architecture
used to carry out the experiments described in Chapts. 5 to 7.

5
Visual inputs, and therefore bearing measurements are non-metric measurements.

7
 Part I

Preliminaries and
state of the art

9
 Chapter 2
Multi-robot systems

Contents
2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.2 Decentralization . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
2.3 Multi-aerial vehicles . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.4 Multi-agent control problems . . . . . . . . . . . . . . . . . . . . 20
2.4.1 Consensus and rendezvous . . . . . . . . . . . . . . . . . 21
2.4.2 Formation control . . . . . . . . . . . . . . . . . . . . . . 22
2.5 Cooperative localization algorithms . . . . . . . . . . . . . . . . 24

2.1 Introduction
S pointed out in the introduction of this Thesis (Chapt. 1), research on multi-

A agent systems, and in particular multi-robot systems (e.g., quadrotors),

has flourished over the last decades with a number of theoretical and
experimental results. The improvements in this field were also made possible by
the constant technological advancements in onboard sensing, communication and
computing power [32–36]. These systems have many potential advantages with
respect to single-agent systems (e.g., decentralized processing, resilience against
failures of individual agents, faster task completion times and so on).
In a lot of multi-robot scenarios robots are acting either by themselves (with
respect to other robots) or just with a limited awareness of the presence of other robots
in their environment (Fig. 2.1). Since many years the robotics community has been
attracted by the so called multi-agent systems (MASs) [18]. A MAS usually denotes
a group of several agents which are able to communicate and sense the whole group
or part of it. Some multi-agent systems can also give birth to collective behaviors in
order to fulfil different tasks and/or interact with the environment. Several reasons

11
Bearing-based localization and control for multiple quadrotors

Figure 2.1 – An example of four robots working together, in an automotive factory, on the
same car. The robots are fulfilling different tasks and they have a really limited awareness
of other robots and of their environment (from which they are separated through a cage).

pushed researchers in developing such systems in lieu of single agent systems. One of
them is that a MAS could be helpful in solving complex tasks in a robust (to single
robot failures) and highly flexible way. The tasks addressed by multi-robot groups are
usually the ones which would be hard or even impossible for a single agent and where
the use of multiple robots translates in a drastic reduction of time for the completion
of the task. An example is the PATH project at the University of Berkeley1
which demonstrated multiple cars driving together in platoons. This project served
as inspiration for many projects which followed. as the one undertaken by the
VISLAB2 which, between many achievements, completed the Vislab Intercontinental
Autonomous Challenge (VIAC), see Fig. 2.2. More recent examples of multi-robot
applications which have a big impact on the current society are the ones given
by the mobile robots used to move shelves around warehouses at Amazon and
Alibaba, see Fig. 2.3 while refer to [37] for an early description of the Kiva Systems
which is behind the robots at Amazon warehouses. Within the scope of robotics,
the following are between the most promising multi-robot fields of application:
autonomous search and rescue, precision agriculture3 , military systems, mobile sensor
networks, transportation systems (Fig. 2.2), firefighting, cooperative localization,
surveillance [22, 38]4 , medical robotics, exploration, intervention and mapping in
dangerous or inaccessible/unknown areas [19, 32, 39, 40], target tracking [35, 41], large
antenna synthetization with multiple robots, cooperative building of 3D structures
[42, 43], complex cooperative manipulation and transportation [33, 44–46].
1
California Partners for Advanced Transit and Highways, www.path.berkeley.edu
2
www.vislab.it
3
http://www.precisionhawk.com/
4
https://www.aeryon.com/

12
 2. Multi-robot systems

(a) (b)

Figure 2.2 – Fig. 2.2(a): convoy of vehicles driving for the Vislab Intercontinental Au-
tonomous Challenge (VIAC) which one of the first examples of autonomous driving chal-
lenges. Fig. 2.2(b): the route covered during the challenge which lasted 100 days in which
15.926 km were covered, from Parma (Italy) to Shanghai (China).

(a) (b)

Figure 2.3 – Multiple robots moving shelves around a warehouse at Amazon (Fig. 2.3(a))
and Alibaba5 (Fig. 2.3(b)).

Anywhere on page where float appears Another reason for the development
of MASs is the understanding that it would be easier to control ten agents with
limited capabilities than embed one agent with the capabilities of the aggregate
ten-agents-system6 . When designing a control algorithm for a group of agents it is
important to make them rely on their onboard capabilities and not on a central unit
otherwise a single failure could compromise the functioning of the whole system. In
brief, the goal of research about MASs is to design control techniques which allow
these systems to exploit all the strengths mentioned above7 .
Among the multi-robot community, at the end of 1990s and beginning of 2000s,
5
www.flashhold.com
6
It is interesting to notice that interest in MASs really blossomed at the beginning of the 2000s.
This was also the time in which it happened the switch, in microprocessor manufacturing, from
single-core CPUs to multi-core CPUs. The reason of going from single-core to multi-core CPUs is
the same which drove the development of MASs: researchers realized that it was easier to build a
processor with multiple cores than one core with the same capabilities of a multi-core processor.
7
In this work the term multi-agent system is often used as a synonym of multi-robot system. If
one wants to be more precise a multi-robot system can be seen in general as a subset of multi-agent
systems.

13
Bearing-based localization and control for multiple quadrotors

research about the coordination of multiple aircraft and especially UAVs started
to become really active [47]. As already said, a central part of this work is about
the decentralized formation control and estimation for a group of quadrotor UAVs.
For this reason the following sections describe more into details the state-of-the-
art of these topics, exploring first the concept of decentralization and giving a
classification of the different UAVs, pointing out the differences between fixed-wing
and rotary-wing UAVs.
However, just before diving into these notions, we will shortly describe a concept
which is instrumental for the rest of this work: decentralization.

2.2 Decentralization

Beside multi-agent systems and robotics, decentralization plays a fundamental role

in many domains such as economics, finance, politics and society. One basic and
practical example of decentralization is the one of a group of people transporting a
heavy load which would be impossible to even lift for one person (see Fig. 2.4(a)).
Another example would be the one in which ants cooperate to transport a piece of
food [44, 46], (see Fig. 2.4(b)). In these cases usually there is no central unit and
there are communication capabilities but usually communication is not used for the
whole task but only in its critical parts (e.g., people are not talking all the time
while transporting a heavy load). In the same way there are sensing capabilities but
usually they are not exploited for the whole duration of the task and/or they are
changing over time depending on the relative configuration of the agents (e.g. people
are not looking all the time at the other members of the group while transporting a
heavy load). The key feature in all these cases is decentralization.

(a) (b)

Figure 2.4 – Examples of decentralized behaviors in humans and ants. Fig. 2.4(a): a group
of people performing in the 25th castells competition at Tarraco Arena ring in Tarragona,
Spain. Fig. 2.4(b): cockerelli ants retrieving a piece of fig, from [44]

14
 2. Multi-robot systems

On the other hand, according to [48], a centralized system is such when at least
one agent needs to sense a global information or communicate with all the other agents
at once. In control theory terms a centralized system is one in which the controller is
able to communicate and control with all the agents of the formation [49–51]. This
kind of structure (see Fig. 2.5(a) for an example) is usually easier to implement than
a decentralized structure but it carries with it different problems:

• The complexity of the algorithm scales with the number of agents of the
formation

• The control structure is not robust, in the sense that the presence of a central
control unit identifies also a single point of failure.

• The agents of the formation need to deal with a quantity of information that
grows with the size of the agent group.

A decentralized system (see Fig. 2.5(b) for an example) instead scales well against
the number of agents, it is more resilient to external threats and allows the agents
to deal only with a limited quantity of information which, in general, does not grow
with the group size8 .

(a) (b)

Figure 2.5 – Examples of schemes of a centralized (Fig. 2.5(a)) and decentralized (Fig. 2.5(a))
system

2.3 Multi-aerial vehicles

When we refer to an Unmanned Aerial Vehicle (UAV) we indicate a flying vehicle
which does not have a human pilot onboard9 . UAVs can be classified according to
8
Note that decentralized is often used as a synonym of distributed.
9
International Civil Aviation Organization (ICAO). Unmanned Aircraft Systems (UAS); ICAO:
Montreal, QC, Canada, 2011.

15
Bearing-based localization and control for multiple quadrotors

Category Normal Normal

Class &
& Normal Operating Mission Example
Weight,
Weight, Employment Altitude, Radius Platform
w (kg)
w (kg) h (ft) (km)
Tactical Unit Luna,
Small h ≤ 5000
Class I (employs launch 50 (LOS) Hermes
w>20 AGL
w<150 system) 90
ScanEagle,
Skylark,
Mini Tactical Unit h ≤ 3000 Raven,
25 (LOS)
2≤w≤20 (manual launch) AGL DH3,
Aladin,
Strix
Tactical Patrol/-
section, Individ- h ≤ 200 Black
Micro w<2 5 (LOS)
ual (single oper- AGL Widow
ator)
Sperwer,
Iview
Class II
Tactical Forma- h ≤ 10000 200 250, Her-
150≤w Tactical
tion AGL (LOS) mes 450,
≤600
Aerostar,
Ranger
Strike/ Strategic/ Unlimited
Class III h ≤ 65000
Combat National (BLOS)
w>600
Strategic/ Unlimited
Global
HALE h ≤ 65000
National (BLOS)Hawk
Predator
A, Preda-
tor B,
Operational/ h ≤ 45000 Unlimited
MALE Heron,
Theater MSL (BLOS)
Heron TP,
Hermes
900

Table 2.1 – NATO Unmanned Aerial Systems UASs Classification Guide. September 2009
JCGUAV meeting

many different features [52]. An example of classification upon weight is reported

in table 2.1. However, classification of UAVs is outside of the scope of this work
and therefore we only deal with the difference between UAVs which are defined
fixed-wing (e.g., Fig. 2.6(a)) and rotary-wing (e.g., Fig. 2.6(b)) and we do it from
an high level perspective.

Each of the two categories has its own benefits and downsides. The advantages
and disadvantages of fixed-wing UAVs, with respect to rotary-wing UAVs, are

16
 2. Multi-robot systems

(a) (b)

Figure 2.6 – Examples of fixed and rotary wing UAVs. Fig. 2.6(a): the MQ-9 Reaper, from
General Atomics. Fig. 2.6(b): the Mavic Pro, from DJI.

reported in table 2.2.

Advantages Disadvantages
Simpler structure and therefore less Usual need of a runway or a launcher
maintenance required to take-off and landing
More efficient aerodynamics which
translates in longer flight times at No hovering capabilities
higher speeds
Normally not suited for indoor ap-
Capability of carrying greater pay- plications due to their low degree of
loads for longer distances manoeuvrability when compared to
rotary wing UAVs

Table 2.2 – Some of the advantages and disadvantages of fixed-wing UAVs

Regarding rotary-wing UAVs, their advantages and disadvantages, with respect

to fixed-wing UAVs, are reported in table 2.3.

Advantages Disadvantages
Ability of vertical take-off and landing
Limited flight time
(VTOL)
More complex mechanics and elec-
Hovering capability tronics and therefore more mainte-
nance needed
Higher manoeuvrability, especially in-
Lower cruising speed
door

Table 2.3 – Some of the advantages and disadvantages of rotary-wing UAVs

In both categories there is ongoing research to overcome the listed disadvantages.

Examples include fully-actuated multi-rotor UAVs [53] as also VTOL fixed-wing

17
Bearing-based localization and control for multiple quadrotors

UAVs [54]10 . There are works, as [38], which envision to use swarms of both
fixed-wing and rotary-wing UAVs.
However, even if UAVs are being applied in many fields the regulations related
to these objects are still at an embryological state, refer to [55] for a survey and
to Fig. 2.7 for a global overview of the UAV regulations up to October 2016. In
addition to Fig. 2.7 it is also interesting to have a look at the global distribution of
first releases of UAV regulations in the world in Fig. 2.8 to understand the pioneering
countries of the world in the field of UAV regulations.

Figure 2.7 – Global overview of current status of UAV regulations on a country-level

resolution (status: October 2016), from [55]

Figure 2.8 – Global distribution of first releases of UAV regulations on country level resolution
(status: October 2016), from [55]

Based on the advantages of rotary-wing UAVs listed in table 2.3 and especially
because of their relatively low-cost and high pervasiveness in the 3D space we chose to
use quadrotor UAVs for our research. The spectrum of application of quadrotor UAVs
10
When a UAV is a hybrid between a fixed-wing and rotary-wing UAV is usually addressed as
compound .

18
 2. Multi-robot systems

is really broad and, according to [55], beside many military applications, between the
quadrotor UAVs civil application there are: high-resolution surface reconstruction in
the geosciences, documentation of cultural heritage and archaeological sites, precision
agriculture and forest change detection, support for disaster management, surveying
and mapping, land administration and wildlife observation. Beside these we can add
package delivery, events filming (e.g., concerts, sport events) and the film industry.
The fields of application of single UAVs can be seen as a subset of the ones of
group of UAVs. Indeed, multiple UAVs can fulfil the tasks assigned to a single UAV
in a fraction of the time a single UAV would take [56]. Beside these tasks there are
others which are only suitable for multiple UAVs and that are therefore impossible
for a single UAVs. These are all the tasks in which cooperation is not only better but
fundamental such as aerial mobile manipulation [57], cooperative load-carrying [58]
and cooperative construction [42, 59, 60] or really specific cooperative tasks [61].

(a) (b)

(c) (d)

Figure 2.9 – 2.9(a): two quadrotors assembling a structure, from [56]. 2.9(b): concept of
flying-hand for transportation of a rigid object with four UAVs commanded through human
hand movements, from [57]. 2.9(c): two quadrotors tying a knot to build a o build a 7.4 m
long rope bridge one can walk on, from [60]. 2.9(d): three quadrotors attached to a net used
to juggle and catch a ball through, from [61]

All the works referenced in Fig. 2.9, together with others such as [34, 62, 63],
achieve quite impressive results but they are all relying on a centralized system
to retrieve the poses of the involved agents. Again, the aim of this Thesis is to

19
Bearing-based localization and control for multiple quadrotors

find decentralized algorithms for (i) the formation control and (ii) cooperative
localization of a group of quadrotor UAVs without the need of a centralized system
and therefore relying only on the local skills of the robots. Some results in this sense
are starting to appear in the literature [64, 65]. Hence, in the following sections a
comprehensive state-of the-art of these two topics, specifically for UAVs, is given.

Different classifications have been proposed over the years to put some order
within the realm of multi-agent systems (e.g., [66] focuses on the sensing capabilities
and the interaction topology of the agents while [67] ). For the next sections we
adopted a terminology (and a structure) similar to [48].

2.4 Multi-agent control problems

A good starting point to have an idea about some of the most common multi-agent
control problems in the literature is Fig. 2.10.

Figure 2.10 – Main multi-agent control problems

Following the structure of [48], we go through the same problems but focusing
more on the problems of formation control and localization which are instrumental
for this work.

Note that, when dealing with the control of multi-agent systems, due to the
already intrinsic complexity of the problem, it is common to start the design of the
control algorithm by considering the nodes/agents/robots as having simple dynamics
(e.g., simple/double integrators). Therefore, usually the dynamics of the agents is
not taken into account. This does not prevent the addition of the dynamics of the
agent in a second stage of the design of the algorithm.

20
 2. Multi-robot systems

2.4.1 Consensus and rendezvous

Consensus-based algorithms have their roots in computer science [68] but, since many
years now, they are playing a central role in the multi-robot literature [67, 69, 70],
refer to [71] for a survey. The consensus problem, according to [69], is the process of
reaching an agreement concerning a certain quantity of interest depending on the
state of all the agents. Without going too much into details we can think about
the consensus protocol as a solution to the following problem. We have a set of N
agents with an internal state xi ∈ Rm . As we said above, the internal dynamics for
the state evolution of each agent is considered to be the one of a single integrator
and therefore: ẋi = ui (with ui the input to agent i). Let us also associate an
interaction graph G to the formation, where the agents are the vertexes of G. The
problem is to design the control inputs ui so that all the states xi converge to the
same common value x̄:
lim xi (t) = x̄, ∀i (2.1)
t→∞

The main characteristic of the consensus protocol is that it achieves (2.1) by making
use only of relative information with respect to the state of the neighbors, therefore
basing everything on relative sensing and on a decentralized structure of the control
algorithm. A basic solution to this problem is achieved by choosing the inputs ui
such that:
X
ui = (xj (t) − xi (t)) (2.2)
j∈Ni

where Ni is the set of neighbors of agent i. It is also possible to express (2.2) in a

compact form for all the agents as:

ẋ (t) = −(L ⊗ Im )x (t) (2.3)

where x ∈ RN m is the stack of all the xi , and L ∈ RN ×N is the Laplacian associated

to the graph G. More details about the Laplacian matrix can be found in Sect. 3.2.
Another key feature of the consensus protocol is that the convergence is assured
if the graph G is connected (i.e., the second smallest eigenvalue λ2 of the Laplacian
matrix L is non-null, see Sect. 3.3.
The consensus protocol has several applications and depending on them it gets
different names. For example:

• Rendezvous: a consensus protocol to meet at a common point. Its goal is to

uniform the positions of the agents.

• Alignment: a consensus protocol to point in the same direction. Its goal is to

uniform the angles of the agents.

21
Bearing-based localization and control for multiple quadrotors

• Distributed estimation: a consensus protocol to agree on the estimation of

some distributed quantity (e.g., average humidity/temperature in a room)

• Synchronization: a consensus protocol to agree on the same time regardless of

phase shifts or different rates in the clocks.

The consensus protocol is the model on which are based many decentralized laws
for control and/or estimation. A more generic form of the (2.2), neglecting the time
dependence, is:
X
ẋi = ui = ki f (xj − xi ) (2.4)
j∈Ni

where f is a local action function of the relative state between the agent i and
its neighbors. For instance, assume that pi ∈ Rm is the agent position in some
m-dimensional space and let p∗ = (p∗1 , . . . , p∗N ) a set of desired positions. The
dynamics of the agent i is such that ṗi = ui . Then the following law converges to
the desired shape up to, of course, a global translation:
X
(pi − pj ) − p∗i − p∗j


ui = ki (2.5)
j∈Ni

It is important to note that in order to implement the (2.5) there is the need for
the agent i to know its relative position with respect to its neighbors in a common
reference frame.

2.4.2 Formation control

Another possibility of multi-agent control is the so called formation control, which
is of big interest for this work. In general, formation control is the problem of
controlling a group of agents by imposing geometric constraints on their relative
poses. Depending on the states of the robots and on their sensing capabilities one
can impose certain constraints which result in different behaviors (e.g., through
interdistances [72, 73] or angles [74–77] between the agents). For example, in [77]
it is presented a formation controller which is able to keep a formation defined in
terms of bearing constraints and which requires only bearing measurements. The
controller converges almost globally and maintains limited inter-agent distances even
without metric informations. Regarding specifically UAVs, one of the first works
about formation flight control is represented by [78].
Formation control can be also seen as a specialized version of (2.2) and (neglect-
ing the time-dependence) can be expressed with the following, which is a sort of
generalization of the (2.5)
X 
ui = oiij − oid
ij (2.6)
j∈Ni

22
 2. Multi-robot systems

where oiij ∈ Rm is the current relative measurement between i and j and oid m
ij ∈ R is
an offset which encloses the desired formation information. Note that this approach
can be applied easily to both bearing and distance cases. In the first case oiij , oid
ij
would be a bearing while in the second case a distance. Of course other options are
possible depending on the application.
Formation control is still a really active field of research, for example in [79] the
desired formation is defined in terms of both inter-agent distances and angles and
there is no need of a global or common reference frame.
Formation control can be implemented through different architectures depending
on the requirements. Some renowned implementations of formation control are the
leader-follower and flocking paradigms.
Leader-follower, as its name suggests, consists of a type of formation control
in which there is one agent which acts as a leader and the remaining agents are
the followers, see Fig. 2.11(a). In the literature there is plenty of applications to
this paradigm (e.g., target tracking, environment patrolling) but one of its main
disadvantages is that it creates a system with a single point of failure (the leader).
Flocking takes inspiration from nature and it refers to a certain behavior exhibited
by birds. A similar behaviour is showed in groups of fishes and it is referred as
schooling (if the group is swimming in the same direction in a coordinated manner)
or shoaling (if the group of fishes stay together for social reasons). The difference
with respect to the leader-follower behavior is that in this case there is no leader.
On the other side, the thing in common with the leader-follower architecture is
that the agents cannot see all the other agents of the formation. In the case of
birds the formation changes over time as the birds in front get tired they leave their
place to birds which are behind them optimizing the efficiency of the migration or
other goals. [80] represents a recent and complete work specifically on flocking and
rendezvous behaviors.

(a) (b) (c)

Figure 2.11 – Example of leader-follower, flocking and shoaling. Fig. 2.11(a): leader-follower
happening between a mother duck and her ducklings, Fig. 2.11(b): example of flocking of
birds, Fig. 2.11(c): example of shoaling of fishes 11 .

11
www.octavioaburto.com/cabo-pulmo

23
Bearing-based localization and control for multiple quadrotors

2.5 Cooperative localization algorithms

As the problem of formation control, cooperative localization is another fundamental
problem in the multi-robot applications. In this context, with cooperative localization
we refer to the process of estimating the configuration12 of the agents in a common
reference frame from a sparse set of relative measurements between the robots. As
in the rest of the Thesis, the aim is to solve the localization problem without relying
on a centralized localization system as a GNSS or the Vicon motion capture system.
The topic of cooperative localization was firstly investigated in [81]. Many more
followed based on different approaches for the localization, such as probabilistic
approaches [82, 83], based on Kalman filters [84–86] or particle filters [87]. Note
that cooperative localization plays a central role not only in multi-robot systems
but also in many others multi-agent systems as sensor networks [88–91], Structure
from Motion (SfM) [92, 93] in computer vision, and graph drawings [94]13 . This
problem is of paramount importance in all the scenarios highlighted in Sect. 2.1
such as the ones in which the robots need to navigate in an unknown environment
and therefore they need to know their relative configuration. In this cases usually
the absolute configurations (with respect to a centralized system) of the robots is
not needed but the relative one (with respect to a reference frame attached to the
formation) would be enough. The solution to the localization problem can follow
different approaches, which strongly depend on the sensors carried onboard by the
different agents used for the relative measurements. Some work has been done in
position-based localization [99], bearing-based localization [100,101] and a mix of the
two [102, 103]. Our interest lies in the second category because, as we said already,
we assume the robots equipped with monocular cameras.
There is one thing though which is seldom taken into account in the literature
mentioned above. Cooperative localization algorithms need, together with relative
sparse measurements, also the identity associated to these measurements. This
means that each robot when it acquires a measurement it needs also to be capable
to associate the measurement to a specific robot. This operation is often referred as
tagging [104]. The identities of the robots need to be coherent between the robots
of the formation. Here we want to stress out that this operation, according to the
robot sensing capabilities, could be not trivial but it is not in the scope of our work.
Therefore, all the algorithms described in the rest of the Thesis assume that tagging
is an intrinsic capability of the robots along with their sensing capabilities, and

12
The meaning of the term configuration depends on the application. It can refer to positions
(in R2 or R3 ), positions and orientations and so on.
13
Note that some works [95, 96] use the specific term relative mutual localization (RML) to refer
the cooperative localization in scenarios in which the measurements are expressed with respect to
the body frames of the agents [97, 98], like the one of this Thesis.

24
 2. Multi-robot systems

consequently we do not deal explicitly with the measurement-anonymousity problem.

Freeing the proposed algorithms from the need of identities is left as a possible
interesting future work. About this topic, addressed as anonymous localization,
research has been done both for the case of position measurements [95, 105] (in 2D)
and for the one of bearing measurements [96, 106, 107] (respectively in 2D and 3D).
In this chapter we gave some details about multi-agent systems, focusing on
multi-robot systems (e.g., UAVs) and going through the most relevant problems to
this field of research. Deliberately, we gave more emphasis to two of these problems:
formation control and cooperative localization. The reason for this choice is simple:
we believe that the theory of rigidity together with algebraic graph theory represents
the perfect tool to tackle both problems. Therefore, in the following Chapt. 3 we
give some details about these topics which represents the theoretical core on which
this work is based.

25
 Chapter 3
Algebraic graph theory and graph
rigidity

Contents
3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.2 Graph theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.2.1 Graph theory definitions . . . . . . . . . . . . . . . . . . 29
3.3 Algebraic graph theory . . . . . . . . . . . . . . . . . . . . . . . 31
3.3.1 Laplacian matrix and connectivity . . . . . . . . . . . . 32
3.4 Rigidity theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.4.1 The Laman’s theorem and some definitions for the case
of distance constraints . . . . . . . . . . . . . . . . . . . 36
3.4.2 The distance-rigidity matrix . . . . . . . . . . . . . . . . 39
3.4.3 The case of bearing constraints . . . . . . . . . . . . . . 44
3.4.4 Similarity between the rigidity matrix and the Jacobian
of robotic manipulators . . . . . . . . . . . . . . . . . . 45

3.1 Introduction
n the previous Chapt. 2 we gave a brief overview about multi-robot systems

I and we understood that an ideal scenario for multi-robot applications would

be one which is embedded with the following characteristics. There should
be availability of relative poses with respect to any other robot in the group in a
common reference frame. There should be the possibility to communicate with any
other robot in the group with no delays. Each agent should have unlimited memory
and processing power (onboard/offboard). In real-world scenarios this is seldom the

27
Bearing-based localization and control for multiple quadrotors

case. Instead, in many realistic applications, these systems are subject to several
limitations, requirements and constraints such as:

• limited sensing: partial measurement of the other robot states (e.g., distance,
bearing), lack of a common shared frame, occlusions, limited field of view,
limited range

• limited communication: occlusions, limited range, delays, maximum data rate

• limited memory and processing power.

Graphs represent a powerful mathematical structure to encode, in a compact

manner, these limitations. Indeed, they are built to describe and model pairwise
relationship between specific objects. For this reason graph theory plays a central
role in this Thesis and lays the groundwork for the so-called rigidity theory which
helps us solving the problems of formation control and cooperative localization.
Hence, the following sections describe more in details graph theory and the theory
of rigidity, two topics which are broadly used throughout the rest of the Thesis.

3.2 Graph theory

According to [108], the first appearance of graph theory can be found in a paper
by Leonhard Euler (Fig. 3.1) on the Seven Bridges of Königsberg and published in
1736. His work was then followed by Augustin-Louis Cauchy [109] and Gergonne
L’Huilier [110] and gave birth to the mathematical field of topology. Since then,

Figure 3.1 – Leonhard Euler, Swiss mathematician (1707-1783)

graph theory has been massively applied to many distinct areas such as sociology,
computer science, mathematics, biology, chemistry and physics (refer to [111] for a
thorough analysis of graph theoretic methods and their application in the analysis
and design of multi-agent systems).

28
 3. Algebraic graph theory and graph rigidity

First of all, it is important to understand that several graphs can be associated to

a single group of robots to encode different capabilities/properties of the group [112]
and the information/action flow among the robots, such as:

• Sensing graph: it encodes what agents can be sensed by each agent

• Communication graph: it encodes to what agents each agent can communicate

some sort of information

• Action graph: for each control action, encodes what robots are (locally) affected
.

More graphs can be defined depending on the application. In the following

section we go through some definitions of graph theory which are of help for the
next developments.

3.2.1 Graph theory definitions

Let us define:

• V as the vertex set which is a finite set of elements: V = {v1 , . . . , vN }

• V 2 as the 2-element subsets of V defined as

 

V 2 = {(vi , vj ) , i = 1, . . . , N, j = 1, . . . , N, i 6= j}
 

• E as the edge set which is a subset of ordered pairs of V 2 such as E ⊆ V 2

   

Then we indicate with G = (V, E) a graph G with vertex set V and edge set E. In
general a graph G can be directed 1 or undirected Fig. 3.2. G is said to be undirected
if
(vi , vj ) ∈ E =⇒ (vj , vi ) ∈ E,

while it is said to be directed if

(vi , vj ) ∈ E =⇒
6 (vj , vi ) ∈ E

An undirected graph can be thought of as a special case of a directed graph where

if an edge eij ⊆ E then eji ⊆ E. An undirected graph is said to be complete if for
every pair of vertexes there is an edge connecting them. If the graph is directed, it is
complete if each pair of vertexes is connected by a unique edge, one in each direction.
In this Thesis we indicate with K the complete graph associated to a set of nodes.
1
Directed graphs are often referred as digraphs.

29
Bearing-based localization and control for multiple quadrotors

(a) (b)

Figure 3.2 – Examples of undirected (Fig. 3.2(a)) and directed (Fig. 3.2(b)) graphs

A node vj is said adjacent (neighbor ) of vi if (vi , vj ) ∈ E and therefore, given a

node vi , the set Ni defined as

Ni = {j ∈ V| (i, j) ∈ E} ⊂ V (3.1)

represents the set of neighbors2 of an agent i with respect to a graph G. With Oi

we represent the set of agents j for which i is a neighbor defined as

Oi = {j ∈ V| (j, i) ∈ E}, (3.2)

we note that the distinction between Ni and Oi is only relevant in case of directed
graphs.
We can also define the degree of a node vi , indicated as deg(vi ), as the number
of edges to which vi is connected, therefore it holds, for a graph G = (V, E), the
so-called handshaking lemma 3 :

deg(v) = 2|E|.
v∈V

For a directed graph there is the need, with respect to a generic vertex vi , to
define the in-degree (defined as deg − (vi )) and the out-degree (defined as deg + (vi ))
respectively as the number of edges entering and exiting vi . Therefore it holds the
so-called degree sum formula:
 
deg − (v) = deg + (v) = |E|.
v∈V v∈V

If, for every vertex v ∈ V, deg − (v) = deg + (v) the graph is called a balanced directed
graph.
We can also define a path as a finite or infinite sequence of edges which connect
a sequence of distinct vertices. If a path starts and ends in the same vertex the path
2
More formal definitions of neighbors, in different senses, can be found in [113].
3
This formula was first introduced by L. Euler in Seven Bridges of Königsberg.

30
 3. Algebraic graph theory and graph rigidity

is called cycle. An undirected graph is said connected if there exists a path joining
any two vertexes in V. On the other side a directed graph is said strongly connected
if there exists a directed path joining any two vertexes, while it is said to be weakly
connected if there exists an undirected graph connecting any two vertexes in V.
An extension to the concept of graph explained so far can be the one of weighted
graph. Usually the weight is associated to each edge of the graph. The easiest
way to think about weighted graphs is with respect to sensing graphs and a weight
associated to an edge in order to encode, e.g., how reliable is the corresponding
measurement. Usually to each edge is associated a weight between 0 and 1 (where
0 indicates a loss of measurement and 1 maximum reliability) but other choices
are completely legit. The concepts of weighted graphs are heavily exploited in this
Thesis in Chapt. 6.

(a) (b) (c) (d)

Figure 3.3 – Fig. 3.3(a): connected graph, Fig. 3.3(b): weakly connected graph, Fig. 3.3(c):
strongly connected graph, Fig. 3.3(d): disconnected graph

3.3 Algebraic graph theory

Graphs are great tools to encode, in a compact way, different information but it is
possible to boost their power if we associate to them algebraic methods. This, gives
birth to a branch of mathematics called algebraic graph theory. This is probably the
feature which made graphs so widespread in so many different fields of engineering.
One of the most investigated branches of algebraic graph theory is the one which
links graphs to linear algebra. Regarding this, it is important to introduce three
fundamental matrixes: adjacency matrix, degree matrix, incidence matrix.
Given a graph G = (V, E) the adjacency matrix A ∈ R|V|×|V| (where |V| is the
number of nodes) has elements Aij defined as:

1 if the edge eij ∈ E
Aij = (3.3)
0 otherwise.

The degree matrix D ∈ R|V|×|V| has elements Dij defined as:


deg(vi ) if i = j
Dij = (3.4)
0 otherwise.

31
Bearing-based localization and control for multiple quadrotors

Note that to a directed graph are associated an in-degree and an out-degree matrix.
The incidence matrix E, for directed graphs, has elements Eij ∈ R|V|×|E| defined
as4 
 −1 if the edge eij exits from vertex vi

Eij = +1 if the edge eij enters into vertex vi (3.5)

0 otherwise.


Another fundamental matrix in algebraic graph theory is the so-called Laplacian

matrix L ∈ R|V|×|V| (refer to [114, 115]) which can be built in two different ways
starting from the matrixes defined above

L=D−A (3.6)

L = EE T . (3.7)
The two definitions are equivalent and the latter is not dependent on the particular
labelling or orientation chosen for the graph. For an undirected graph G the matrix L
results symmetric and positive semi-definite. This means that all its |V| eigenvalues
λi are real and non-negative. If we order them as

0 = λ1 ≤ λ2 ≤ . . . ≤ λ|V| (3.8)

it holds that the graph G is connected if and only if λ2 > 0 [114, 115] (e.g., refer
to Fig. 3.4 for some examples).
The definitions given above are the most common ones in the literature. There
are different definitions of these matrices but, in this work, when we refer to the
adjacency, degree, incidence and Laplacian matrices we are referring to the definitions
in (3.3) to (3.7)

3.3.1 Laplacian matrix and connectivity

The Laplacian matrix defined in (3.6–3.7) represents therefore a link between combi-
natorial properties of a graph (existence of a path among two agents) and spectral
properties of a matrix associated to a graph (condition on λ2 ). This means that
there is therefore an equivalence between the combinatorial and spectral properties
of graphs which plays a fundamental role for control and estimation purposes. The
power of the Laplacian matrix is that its second smallest eigenvalue λ2 offers a
measure of the degree of connectivity of the graph to which the matrix is associated.
For this reason λ2 is often referred as the connectivity eigenvalue (or algebraic
connectivity). It is important to highlight that λ2 is a global property of the graph
which means that it is influenced by all the nodes (agents) of the graph.
4
Some authors use the opposite notation.

32
 3. Algebraic graph theory and graph rigidity

λ2 = 4 λ2 = 2 λ2 = 0.58 λ2 = 0

Figure 3.4 – Graphs with different connectivity eigenvalues λ2

For example, the Laplacian matrix can be used to study the consensus protocol
(refer to Sect. 2.4.1), one of the major problems in multi-agent applications. It can
be shown, by analyzing (2.3), that the convergence to an arbitrary equilibrium is
related to the spectral properties of the matrix L. In particular L represents the
state-transition matrix in the closed-loop dynamics of the system. Basically the
consensus converges faster as the λ2 increases and vice versa.
Previously, at the beginning of Sect. 3.2, we mentioned that to a specific group
of robots we can associate different graphs. From this we can understand that the
Laplacian matrix can be useful to model the connectivity of different properties of
the group such as sensing, communication or action. For example, if the underlying
communication graph associated to the group of robots is connected it means that
there can be a flow of information between each pair of robots.
Analogously to connectivity and taking inspiration from structural mechanics
one can define a stronger property which allows to study different properties of a
group of agents and it involves not only the graph associated to the agents but also
their relative position in space (2D or 3D). This property is called rigidity and it is
discussed in the following section.

3.4 Rigidity theory

Rigidity has its roots in mechanics, this theory was born as a combinatorial theory for
characterizing the stiffness or flexibility of structures made of rigid bodies connected
by flexible linkages or hinges. One of the first works considered related to the theory
of rigidity and graphs is the so-called Euler-conjecture about polyhedra.
Rigidity theory has found a large number of applications in the context of
formation control (see Sect. 2.4.2) based on, e.g., relative distance measurements [72,
116–119] and relative bearing measurements [76, 112, 120–123], mainly differing in
the assumptions and/or simplifications taken at the design stage. For instance,
regarding the bearing case, the authors of [76, 112] have considered the formation
control of planar kinematic agents by assuming a common reference frame and an
undirected topology for the measurement graph (i.e., all measurements are assumed

33
Bearing-based localization and control for multiple quadrotors

to be reciprocal), and [122] has extended these results to arbitrary dimensions. The
work [121] has instead dropped the assumption of a common reference frame while,
however, retaining that of an undirected measurement topology. Another example
is [124] in which they apply distance-based rigidity to beacon localization.
As mentioned several times, rigidity theory plays a pivotal role also in the dual
problem of cooperative localization (see Sect. 2.5) from local relative measurements [99,
100, 119, 125, 126]. Indeed, the rigidity of the formation is a necessary requirement
for recovering, from the available relative measurements, a consistent solution of the
localization problem in a common shared frame.
It is interesting to recognize how (mechanical) rigidity played some role in the
design of the first working aircraft of history, the one flown by Wright’s brothers
at Kill Devil Hills, North Carolina on December 17, 1903. Indeed, at the Smithso-
nian National Air and Space Museum, a small demo is offered to the visitors for
understanding how, without the rigidity of some parts of the plane the airplane
would have never taken off, see Fig. 3.5. The Wrights were aware of earlier biplane
designs and especially the 1896 Chanute-Herring glider which had a special bracing.
Steel wires crisscrossed between vertical wooden struts that supported the upper
and lower wings, creating a simple, rigid structure. This structure was tested by the
Wrights for the first time on a kite they built in 1899.

(a) (b) (c)

Figure 3.5 – Wright brothers and rigidity. Fig. 3.5(a): shows some drafts by Wilbur Wright
about a kite to test warping for roll control. Fig. 3.5(b): shows a demo available at the
Smithsonian National Air and Space Museum to stress out the difference that a rigid
structure made in the design of the the 1903 Wright Flyer, the craft that ushered in the
age of flight. Fig. 3.5(c): at the same museum, shows the 1903 Wright Flyer, the craft that
ushered in the age of flight.

Rigidity can also play a combining role between two usually separated communi-
ties, the multi-robot and the parallel robots ones. This has been a topic of discussion
in the workshop Rigidity Theory for Multi-agent Systems Meets Parallel Robots held
during the 2017 IFAC congress in Toulouse (France)5 . Intuitively each robot of a

5
For more details refer to the website of the workshop: https://parrigidwrkshp.sciencesconf.
org/.

34
 3. Algebraic graph theory and graph rigidity

multi-agent system could be seen as a passive joint of a virtual mechanical parallel

robot and each measurement between them as a rigid connection.
As aforementioned, the theory of rigidity can be formulated in case of any kind
of geometrical constraints on the edges, distances and bearings are two possible
cases. In this section, we focus on the case of distance constraints because, for the
sake of illustration, they are more intuitive to deal with. Therefore, each edge in the
following graphs represents the distance between the interested nodes. From here
we gradually evolve towards the case of bearing constraints in 3D space, which is
the case considered in all the scenarios of this Thesis and is detailed in Sect. 4.3.
Note that the main results, theorems, and definitions described below for the case of
distance constraints usually apply flawlessly to the case of bearing constraints.
In order to define and understand rigidity we need to introduce the concept
of framework (or formation). A framework is identified, in the case of distance-
constraints in 2D, as a pair (G, p), where p = stack (p1 , ..., pN ), pi ∈ R2 is the
position of the i-th robot and N is the total number of robots of the formation.
In the previous Sect. 3.3.1 we gave some details about connectivity. Rigidity is
a stronger property and it is possible to prove that if a framework is rigid, the
underlying graph is also connected [127], while the vice versa is not true. Roughly
speaking, a framework (G, p) is rigid if satisfying the distance constraints associated
with the edges of G univocally determines the shape (spatial arrangement of the
positions p) of all the agents of the formation up to a roto-translation. An example
to understand better this concept is reported in Fig. 3.6.
Another intuitive way to define rigidity is the following. A framework (G, p)
is rigid if the only motions satisfying all the constraints over the edges in E are
those which would be allowed in case G was the complete graph K.6 This leads
us to a great advantage of rigid graphs which is related to the complexity of a
hypothetical formation control algorithm associated with a group of agents with an
underlying rigid topology. Shortly, one way of controlling N agents is by regulating
all the distances among N agents to some desired values. This would yield to a
complete graph K which has N (N − 1) edges. This translates into a complexity
of the formation control algorithm of O(N 2 ). Instead, if a graph is rigid in two
dimensions, only 2N − 3 edges7 are needed to control all the distances between the
agents and therefore the complexity decreases to O(N ), i.e., linear versus quadratic.
As stated in [72], if a graph is rigid for a certain set of positions, it will be rigid
for almost all other positions. The adverb almost refers to some nongeneric sets of
positions as the ones corresponding to collinear agents8 . This is also why in order to
6
Note that these concepts are formalized in (3.10–3.11).
7
This precise number of edges comes from the Laman’s theorem, defined in Sect. 3.4.1.
8
In the literature, these cases are sometimes referred as degenerate cases.

35
Bearing-based localization and control for multiple quadrotors

(a)

(b)

Figure 3.6 – Examples of rigid and nonrigid frameworks. The framework in Fig. 3.6(a) is
rigid because it cannot be deformed while satisfying the distance constraints over the edges.
The framework in Fig. 3.6(b) is not rigid because it can be deformed by a smooth motion
without violating the distance constraints over the edges.

study rigidity we can employ two different approaches:

• Combinatorial approach: through Laman’s theorem [128]. This approach does

not take into account the positions of the agents and is only function of the
topology of G. Indeed, it relies on the previous concept whose idea is that if a
formation is rigid in a set of positions, it will be rigid almost anywhere else.

• Algebraic approach: through linear algebra and the concept of rigidity ma-
trix [129, 130]. This approach takes also into account the positions of the
agents.

The theory of rigidity brings with it several definitions and theorems and therefore
in the following part of this section, we briefly go through some of them, refer to [72]
and its bibliography for more details.

3.4.1 The Laman’s theorem and some definitions for the case of
distance constraints
In order to introduce the Laman’s theorem we need the concept of induced subgraph
of a graph G = (V, E) [72]. Let V  be a subset of V, then the subgraph of G induced
by V  is the graph G  = (V  , E  ) where E  includes all the edges of E that are incident
on a vertex pair in V  .

36
 3. Algebraic graph theory and graph rigidity

The Laman’s theorem states the following: a graph G = (V, E) modeling a

formation in two dimensions with |V| vertexes and |E| edges is rigid if and only if
there exists a subgraph G  = (V, E  ) with 2|V| − 3 edges such that for every subgraph
V  of V, the induced subgraph G  = (V  , E  ) of G  has a number of edges |E  | which
is: |E  | ≤ 2|V  | − 3.

Laman’s theorem provides a test, applicable to graphs in two dimensions and for
the case of distance constraints, to check whether a graph is rigid. This is why rigid
graphs with a number of edges equal to 2|V| − 3 are sometimes called Laman graphs.9
A theorem for graphs in three dimensions does not exist but a partial extension of
the Laman’s theorem to three dimensions is discussed in [72] and it requires the
graph G of the considered framework G = (V, E) to have 3|V| − 6 edges.

Another important concept in the theory of rigidity is the one of minimal

rigidity [129]. Intuitively, a framework (G, p) is minimally rigid if removing any of its
edges will cause the loss of rigidity, see Fig. 3.7. Furthermore, we can refer to rigid
graphs which are rigid but not minimally rigid as redundantly rigid graphs [131], and
an edge is called a redundant edge if the graph remains rigid after its removal [132].

(a) (b)

Figure 3.7 – Examples of minimally rigid (Fig. 3.7(a)) and redundantly rigid (Fig. 3.7(a))
graphs.

Minimally rigid graphs have been described in details in several works such as [116].
A procedure to build minimally rigid graphs in two dimensions is represented by
the so-called Henneberg construction [129, 133, 134]. As stated in [72], for graphs in
three dimensions there exist some operations that are analogous to the Henneberg
construction, but they are in the form of conjectures [129] and it is therefore not
sure that they represent necessary and sufficient conditions to build and deconstruct
all minimally rigid graphs.

There are several and equivalent formal definitions of rigidity, for example
in [116, 135] the following definition is given. Let us define a rigidity function

9
Sometimes they are also called plane isostatic graphs.

37
Bearing-based localization and control for multiple quadrotors

associated with the framework (G, p) as the function gG : R2|V| → R|E| given by

gG (p) := stack(..., ||pk − pj ||2 , ...). (3.9)

The i − th component of gG (p) corresponds to the value of the geometric constraint

(distance in this case) over the edge ei ∈ E. After defining the rigidity function it is
possible to give the following definition. A framework (G, p) is rigid if there exists a
neighborhood U ⊂ R2|V| of p such that

gG−1 (gG (p)) ∩ U = gK

−1
(gK (p)) ∩ U (3.10)

where K is the complete graph with the same vertices as G. The definition (3.10)
asserts that, in a neighborhood U of p, the possible shapes corresponding to the
values of the constraints over the edges E in G are the same that one would obtain
by considering the edges of the complete graph K. Moreover, if this is true for all
positions p, and not only for a neighborhood of p, the framework is globally rigid
and therefore the following definition.
A framework (G, p) is globally rigid if

gG−1 (gG (p)) = gK

−1
(gK (p)) (3.11)

for all possible values of p. Global rigidity, as it comes out of the last definition,
is a property which is stronger than rigidity. Moreover, in [132] it is shown that
redundant rigidity is a necessary (but not sufficient) condition for global rigidity. A
typical example to understand the difference between a rigid and a globally rigid
graph is reported in Fig. 3.8.10 Strictly speaking, it is important to highlight that
rigidity (minimal or not) does not uniquely define the shape of a formation but it
ensures that if the formation assumes a specific shape, it will not be able to deform
from that shape smoothly. On the other hand, global rigidity ensures that, if the
formation satisfies a set of distance constraints, the shape is univocally (globally)
defined.
In order to have a better understanding of the definition of rigidity in (3.10) we
can refer to Fig. 3.9 which depicts how the rigidity function works in the case of a
rigid graph. Let us assume that we have a certain set p̄ of positions of the agents.
If we apply the function gG to p̄, from the set of R2|V| we will go to the set of R|E| ,
specifically we will end up in gG (p̄), that is, the value of the distances over the edges
for the particular configuration p̄. Due to the definition (3.9) of the rigidity function
this point consists of a stack of ||p̄i − p̄j ||2 . The definition (3.10) expresses the
10
In accordance to the definition of global rigidity given above sometimes rigidity is referred to
as local rigidity in order to highlight that it is a property local to the current positions of the agents.
In this thesis we refer to it simply as rigidity.

38
 3. Algebraic graph theory and graph rigidity

(a) (b) (c)

Figure 3.8 – Global rigidity: Fig. 3.8(a) is rigid but not globally rigid because the same
distance constraints can be satisfied, e.g., also by the graph in Fig. 3.8(b). On the contrary,
the graph in Fig. 3.8(c) is globally rigid. Note also that Fig. 3.8(a) cannot be continuously
deformed into Fig. 3.8(b). Therefore, locally, the graphs in Figs. 3.8(a) and 3.8(b) are rigid
graphs, although not in a global sense.

following concept. For a rigid graph, the set that one gets from the counter-image
gG−1 (p̄) (the shaded area) is the same set that one would get by replacing G with K,
at least in a neighborhood Up̄ of p̄. On the other hand, in the case of global rigidity
this holds for all p (not just in a neighborhood of p̄).

Figure 3.9 – The rigidity function gG in the case of a rigid framework

3.4.2 The distance-rigidity matrix

As pointed out above, an alternative approach to the combinatorial one provided by

the Laman’s theorem for the study the rigidity of a formation is the algebraic one
based on the Rigidity Matrix.
Again, let us consider a framework (G, p) where p is the stack of all the positions

39
Bearing-based localization and control for multiple quadrotors

pi ∈ R2 of the agents. We can define a rigidity function, which is slightly different

from (3.9)
1
gG (p) := stack(..., ||pk − pj ||2 , ...). (3.12)
2
The rigidity matrix is defined as the Jacobian JgG of the rigidity function with
respect to the positions of the agents. This matrix is useful to study some concepts
of graph rigidity which would be impossible to study only through combinatorial
approaches (refer to [72, 116] and related works for more details). Thanks to the
introduction of the rigidity matrix, and therefore within the algebraic approach, one
can study the so-called infinitesimal rigidity. The idea of infinitesimal rigidity plays
a central role in the design of decentralized formation controllers and localization
algorithms, both in the case of distance constraints and in the one of bearing ones.
Infinitesimal rigidity has the goal of studying the flexibility of a framework under
instantaneous (infinitesimal) motions of the agents. It is discussed, with respect to
distance constraints, in [116].
The idea of infinitesimal rigidity is to move infinitesimally the agents while
keeping the rigidity function (3.12) constant up to the first order. Let δp be an
infinitesimal motion of the framework (G, p). Then the Taylor expansion of gG about
p is
gG (p + δp) = gG (p) + JgG δp + h.o.t.,
therefore the rigidity matrix can be also seen as the linear term in the Taylor
expansion of the rigidity function. The rigidity function stays constant up to first
order when JgG δp is null, which is when δp is in the null-space of the rigidity matrix.
In [135] it is stated: a framework with N agents (G, p) is infinitesimally rigid in
the plane if dim(Ker(JgG )) = 3, or equivalently

rank(JgG ) = 2N − 3 (3.13)

Note that infinitesimal rigidity implies rigidity but the vice versa is not true. In
order to understand better these concepts an example of infinitesimally and not
infinitesimally rigid graph is reported in Fig. 3.10. Note that the two graphs are
both rigid and globally rigid. In particular the second graph (Fig. 3.10(b)) is rigid
because of the Laman’s theorem. Indeed the graph has three edges which is equal to
2|V| − 3. The same graph is not rigid because if one would build the rigidity matrix,
in the specific configuration of Fig. 3.10(b), it would be possible to check that the
rank of the matrix is equal to two (instead of three) and therefore (3.13) would not
be satisfied. Geometrically, this (point-wise) loss of rank is due to the fact that, at
first order, agents 1 and 3 can infinitesimally move orthogonally to the edge (1, 3)
without changing their distance, thus (again infinitesimally) deforming the shape of
the formation.

40
 3. Algebraic graph theory and graph rigidity

(a) (b)

Figure 3.10 – Examples of infinitesimally rigid (Fig. 3.10(a)) and not infinitesimally rigid
(Fig. 3.10(b)) graphs. Note that the red color is used to highlight that there is still an edge
between the agents 1 and 3.

Finally, bringing together the definitions of infinitesimal and minimal rigidity

one can define a minimally infinitesimally rigid framework as one which is both
infinitesimally and minimally rigid.
In the case of distance constraints on the plane, if the rank of the rigidity matrix
is 2|V| − 3 (or equivalently 2N − 3) its null-space will be of dimension 3. This,
in two dimensions for distance constraints, makes sense because it means that in
the null-space of the rigidity matrix there are only three vectors which correspond
to agent velocities which do not change the distance constraints. These vectors
correspond to the planar translation (two vectors) and planar rotation (one vector)
of the whole framework in two dimensions.
In [136] it is also specified that the result in (3.13) extends easily to graphs in
three dimensions. In this case, it is possible to prove that a framework is rigid if and
only if the rigidity matrix has a rank of 3|V| − 6. This means that the null-space
has dimension 6 and its vectors correspond to three translations and three rotations
of the whole framework in three dimensions.
We said several times that rigidity plays an essential role in solving the problem
of formation control. Indeed, in [116], after defining the rigidity matrix they design
a gradient-based control law based on this matrix. Thanks to the gradient-like
structure of the controller and on the intrinsic decentralized structure of the rigidity
matrix (each column corresponds to an agent and contains information which is
relative only to the neighbor of the considered agent) the result is a controller
which has a decentralized structure (i.e., the controller for each agent relies just
on quantities dependent on itself or its neighbors). Furthermore, in [116] they
prove that infinitesimal rigidity is a sufficient condition for local asymptotic stability
of a gradient-based controller, for multi-vehicle systems, which uses only local
information.11
11
In [116], the definition of infinitesimal rigidity is the one coming from [135].

41
Bearing-based localization and control for multiple quadrotors

Note that the choice of a controller based on the rigidity matrix and which
is gradient-based is not casual. Indeed, if instead of a gradient-based method, it
would have been chosen, e.g., a Gauss-Newton approach the controller would end
up being dependent not simply on the rigidity matrix JgG (or more precisely on its
transpose) but on the so-called pseudoinverse of the rigidity matrix which can be
 −1
defined as Jg†G = JgTG JgG JgTG . Without going too much into the details, the
inverse operation of a matrix in some way mixes the elements of the matrix to which
is applied. Then, because the pseudoinverse uses the inverse operation, designing
a controller built on this matrix would yield a loss of its decentralized structure.
However, gradient-based methods have also some drawbacks. One of them is the
speed of convergence which is one of the worse among the optimization methods.
The discussion about this topic is not within the scope of this Thesis but interesting
details can be found in [99], where they try to speed up a gradient-based method by
choosing a better, decentralized, stepsize.

Thanks to the introduction of the rigidity matrix we can understand a difference

between connectivity and infinitesimal rigidity. Connectivity and rigidity are both
global properties of the whole group of agents to which the considered graph is
associated. However, connectivity is associated to a graph while infinitesimal rigidity
takes into account also the particular configuration of the robots. Note that our
algorithms rely on the concept of infinitesimal rigidity (for bearing constraints).12
This property becomes evident by looking at two rigidity matrices corresponding
to a group of agents in two dimensions, always in the case of distance constraints.
The matrix in (3.14) corresponds to a graph like the one in Fig. 3.11(a) which is not
rigid and indeed it has a rank which is less than 2|V| − 3.

 
x1 − x2 y1 − y2 x2 − x1 y2 − y1 0 0 0 0
 

 0 0 x2 − x3 y2 − y3 x3 − x2 y3 − y2 0 0 


 0 0 0 0 x3 − x4 y3 − y4 x4 − x3 y4 − y3 

x 1 − x 4 y1 − y4 0 0 0 0 x4 − x1 y4 − y1

(3.14)

12
This can be summarized by saying that infinitesimal rigidity is not a purely topological property
as connectivity.

42
 3. Algebraic graph theory and graph rigidity

(a) (b)

Figure 3.11 – Example of not-rigid (Fig. 3.11(a)) and rigid (Fig. 3.11(b)) graphs in the case
of distance constraints in two dimensions for four agents.

Instead, the following matrix corresponds to a rigid graph like the one in Fig. 3.11(b)
 
x 1 − x 2 y1 − y2 x 2 − x 1 y2 − y1 0 0 0 0
 
 0 0 x 2 − x 3 y2 − y3 x 3 − x 2 y3 − y2 0 0 
 
 0 0 0 0 x 3 − x 4 y 3 − y 4 x 4 − x 3 y4 − y3 
 
 
 x 1 − x 4 y1 − y4 0 0 0 0 x 4 − x 1 y4 − y1 
 
 x −x y −y 0 0 x 3 − x 1 y3 − y1 0 0 
 1 3 1 3 
0 0 x 2 − x 4 y2 − y4 0 0 x 4 − x 2 y4 − y2

(3.15)

The matrix in (3.15) seems to have a rank always greater or equal than 2|V| − 3.
But, if we look closer it is possible to understand that some terms of (3.15) can
become equal to zero even if the underlying graph contains the edge corresponding
to that element. This happens, for example, when agent 1 has the same x-coordinate
(or y-coordinate) of agent 2 and, in general, when two or more agents are collinear
or coincident with each other. In this case the graph would be rigid but not
infinitesimally rigid, as for the example in Fig. 3.10(b). This leads to the conclusion
that infinitesimal rigidity is a stronger property than rigidity. Indeed, global rigidity
can be seen as a sort of rigidity which ensures that the positions of the robot are
nondegenerate (e.g., not all aligned).
In this section, we went through the primary definitions and results about
the theory of rigidity focusing our attention on the case of distance constraints.
We concentrated on distance constraints for two main reasons. The first one is
a historical reason: distance constraints, because of their analogy with rigidity
in mechanical systems, were the first ones to be tackled and investigated by the

43
Bearing-based localization and control for multiple quadrotors

multi-robot community. The second reason is that we simply believe that this case
is more intuitive than the bearing one.

3.4.3 The case of bearing constraints

However, since some years there has been an interest also in applying the theory of
rigidity to scenarios with bearing constraints [76, 100, 112, 121, 122, 137, 138]. This
happened mainly because bearing measurements are often retrievable from cheaper
and more accessible sensors, such as vision-based sensors (e.g., cameras) and angle-
of-arrival sensor (e.g., wireless network devices13 ). Moreover, a formation based
on angles can be scaled up easier than one based only on distances. A difference
between distance-rigidity and bearing-rigidity is that the rigid body motions in the
second case also include a contraction/expansion of the whole frame along with
the usual roto-translation. Indeed, only with bearing measurements there is no
control over the scale of the formation and therefore the size of the desired shape
cannot be controlled. This is one of the reasons why Chapt. 7 deals with the scale
estimation through bearing measurements and known agent ego-motion (body-frame
linear/angular velocity).
Contrary to distance measurements (which are scalar quantities), bearing mea-
surements are vector quantities. Therefore, one has to consider in which frame these
measurements are expressed.
In the literature, two main cases have been considered: all bearing measurements
expressed in a common (world) frame [76, 112], and all bearing measurements
expressed in the local body frames of the agents [30, 31, 77, 140]. This Thesis, as
stated several times, lies in the second group because this is the kind of measurement
retrievable with a monocular camera. A third category could be represented by the
works which analyze both cases, such as [122] in which there is an extension of a
bearing-based formation control from two to arbitrary dimensions. In [122] there
is also the study of some of the connections between distance and bearing rigidity
showing that a framework in R2 is infinitesimally bearing-rigid if and only if is also
infinitesimally distance-rigid14 .
In the case of a bearing measurement βij (between the agents i and j) expressed
with respect to a common reference frame, the vector βij is the 3D unit-norm vector
expressed as
pj − pi pij
βij = = ∈ S2 , (3.16)
kpj − pi k dij
13
Indeed, the direction of arrival of a radio signal can be estimated through different kinds of
antennas and used for radio source localization [139].
14
This result cannot be generalized to R3 or higher dimensions.

44
 3. Algebraic graph theory and graph rigidity

while in the second case of measurements expressed in the body-frames, the bearing
is expressed as
pj − pi pij
βij = RiT = RiT ∈ S2 , (3.17)
kpj − pi k dij
where pij = pj − pi , dij = kpj − pi k and Ri is a rotation matrix that rotates a
vector from the world frame to the body frame. Regarding the second case, more
details are given in Sect. 4.2 while in this section we will continue to focus on the
first case where the considered framework for bearing rigidity is still the pair (G, p).
Note that bearing rigidity is often addressed as parallel rigidity. This is due to an
alternative but equivalent definition of rigidity given by introducing the parallelism
between vectors (as done in [141]). Let us define the following orthogonal projector
matrix for any non-null vector x ∈ Rd (d ≥ 2) as

x xT
Px := Id − . (3.18)
kxk kxk
Px projects the vector x onto the orthogonal complement of x. Thanks to the
definition of this projector matrix we can define two vectors x, y ∈ Rd as parallel to
each other if and only if Px y = 0 or Py x = 0.
Two frameworks (G, p) and (G, p0 ) are bearing equivalent if

P(pi −pj ) p0i − p0j = 0 ∀ei ∈ E.

Two frameworks (G, p) and (G, p0 ) are bearing congruent if

P(pi −pj ) p0i − p0j = 0 ∀ei ∈ K.

Therefore, it is possible to give an alternative definition, to the ones given

in Sect. 3.4.1, of rigidity and global rigidity.
A framework (G, p) is bearing/parallel rigid if there exists a constant  > 0
such that any framework (G, p0 ) that is bearing equivalent to (G, p), and satisfies
kp − p0 k <  is also bearing congruent to (G, p).
A framework (G, p) is globally bearing/parallel rigid if every framework which is
bearing equivalent to (G, p) is also bearing congruent to (G, p).

3.4.4 Similarity between the rigidity matrix and the Jacobian of

robotic manipulators
To make the understanding of the rigidity matrix easier, in this section we want
to point out to the reader a useful similarity between the rigidity matrix and the
well-known Jacobian in the case of robotic manipulators. A typical task in robotics
consists in reaching a certain desired position pd of the end effector. Therefore

45
Bearing-based localization and control for multiple quadrotors

the classical task could be written as p(q) → pd . Where p is the position of

the end-effector of the considered robot and q is a vector which contains all the
joints variables. In this case, one would differentiate the p and have the well-known
equation
ṗ = J (q)q̇ (3.19)

where the matrix J is the Jacobian associated to the manipulator and it contains
the dynamics of the robot.
In our case, the task is to bring the robots from a certain configuration, with
certain bearings βij∗ , to a desired one which will have β d . In the same way, in the
ij
case of distances, the task would be to steer the agents from a configuration with
some distances d∗ij to a desired configuration with ddij . The dual equation of (3.19)
in our case is, using the same notation used so far:

g˙G = JgG ṗ (3.20)

where JgG is the Jacobian of the function gG and therefore the rigidity matrix.
In this chapter, the theory of rigidity has been addressed to give the reader an
introduction to this vast topic. In the next Chapt. 4 we will specialize the theory of
rigidity for the case of bearing rigidity in R3 × S1 (which is the case considered in
this Thesis) also giving an explicit expression of the bearing rigidity matrix which
has a fundamental role in the whole Thesis.

46
 Part II

Contributions

47
 Chapter 4
Main modeling assumptions

Contents
4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.2 Agent model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.3 Directed bearing rigidity in R3 × S1 . . . . . . . . . . . . . . . . 52

4.1 Introduction
N this chapter we bring together the main modeling assumptions and definitions

I which are common to the chapters of this part of the Thesis. More specifically,
Chapts. 5 and 6 share the same modeling assumptions listed in this chapter
while in Chapt. 7 there are some differences which are specifically addressed and
explained in details.

4.2 Agent model

As we said several times, in this Thesis we want to be able to control a group of
N quadrotor UAVs in an unstructured environment only relying on their onboard
capabilities. To fulfill this task, the robots are required to be able to localize them-
selves with respect to other agents of the formation. The literature shows how this
is possible relying on different sensing capabilities and on the capability of commu-
nication among the agents (e.g., by exchanging data over a radio communication
channel). From a theoretical point of view, the most important aspect of these
different sensing modalities is the quantity of information they provide. For instance,
the sensors could provide estimates of distance (e.g., from wireless signal strength),
bearing directions (e.g., with monocular cameras), translations (e.g., with stereo or
depth cameras), or both rotations and translations (e.g., using cameras and two-view

49
Bearing-based localization and control for multiple quadrotors

Structure from Motion [142, 143]). In this Thesis, we assume that each agent can
measure the direction (but not the distance) of a subset of neighboring agents in
its reference frame1 . This setup is arguably the most practical with today’s most
popular hardware, which, due to weight, cost, and power consumption considera-
tions, is usually limited to an Inertial Measurement Unit (IMU) and a monocular
camera [144, 145].
Let W : {OW , XW , YW , ZW } represent a world frame with ZW aligned with
the vertical (gravity) direction and Ai : {OAi , XAi , YAi , ZAi } the i-th body frame
attached to each quadrotor UAV. Following the modeling assumptions of [27, 30, 31,
77, 146], we consider a group of N ‘velocity-controlled’ quadrotor UAVs with agent
dynamics
! ! !
ṗi Ri 0 ui
= (4.1)
ψ̇i 0 1 wi

where pi ∈ R3 is the quadrotor 3D position in W, ψi ∈ S1 the yaw angle about ZW ,

and Ri = Rz (ψi ) ∈ SO(3) is the canonical rotation around the world z-axis. The
quantities ui ∈ R3 and wi ∈ R represent the body-frame linear velocity and yaw
rate which are assumed to be known and controllable. The configuration of the i-th
quadrotor is then denoted with qi = (pi , ψi ) ∈ R3 ×S1 , while q = (p, ψ) ∈ (R3 ×S1 )N
is the configuration of the whole formation, with p = (p1 , . . . , pN ) ∈ R3N and
ψ = (ψ1 , . . . , ψN ) ∈ SN . We stress that, as in [27, 30, 31, 77, 146], the (absolute) yaw
angle ψi is not considered as an available quantity to the i-th quadrotor. Indeed,
additional sensors, such as compasses, would be needed for obtaining a consistent
yaw measurement for all the UAVs of the group with, however, a typically limited
reliability (e.g., compasses would fail to operate soundly indoor or close to strong
magnetic fields). As a consequence, the N quadrotors are not assumed to share, as a
group, a common (global) reference frame where to express local measurements and
control inputs. The reader is referred to Figs. 4.1 and 4.2 for a better understanding
of the introduced quantities. In the first one, for simplicity, the origin of the body
frame of the quadrotor OAi coincides with the origin of the world frame OW while
in the second one a group of three quadrotor UAVs is illustrated.
Each quadrotor is also assumed equipped with an onboard calibrated camera
that allows an agent i to measure the relative bearing vector with respect to an agent
j in visibility, i.e., the 3D unit-norm vector

pj − pi pij
βij = RiT = RiT ∈ S2 , (4.2)
kpj − pi k dij

where pij = pj − pi , dij = kpj − pi k and S2 is the unit sphere, i.e. the space of 3-D
1
We refer to this reference frame as the body-frame of the robot

50
 4. Main modeling assumptions

Figure 4.1 – A quadrotor UAV with the origin of its body frame Ai which is coincident with
the origin of the world reference frame W

Figure 4.2 – Group of three quadrotor UAVs with the corresponding body reference frames
Ai , Aj , Ak and the world frame W

51
Bearing-based localization and control for multiple quadrotors

unit-norm vectors defined as

S2 = {v : v ∈ R3 , kvk = 1} (4.3)

This agent relative bearing βij can be retrieved by ‘derotating’ the actual bearing
measurement among quadrotors i and j (e.g., from an onboard camera) by the roll
and pitch angles which can be typically measured by exploiting the built-in IMU, see
also [77]. Indeed, by exploiting the known roll/pitch angles (φAi , θAi ) the unit vector
βij can be obtained from the measured βAi ,Aj as βij = Ry (θAi )Rx (φAi )βAi ,Aj and
can, thus, be treated as a quantity available to agent i.
The control strategies discussed in the next Chapts. 5 and 6 are based on
model (4.1) and assume that each agent i can measure the (body-frame) bearing
vector βij with respect to other neighboring agents together with its own (body-
frame) velocity commands (ui , wi ) [22, 147]. Instead, Chapt. 7 considers a more
general model which takes into account a rotation matrix which is a function of
the full orientation of the agent and not only of the yaw angle ψi described above.
Moreover, with special focus on the quadrotor platform, we assume that the UAV is
able (through a low-level controller such as, for instance, the one proposed in [148])
to track a smooth reference trajectory (pi (t) , wi (t)) in the four-dimensional space
R3 × S1 .

4.3 Directed bearing rigidity in R3 × S1

In this section there is a specialization of the theory of rigidity, explained in Sect. 3.4,
for the case of bearing constraints. We also give relevant definitions and properties
of directed bearing formations and bearing rigidity in R3 × S1 which are useful in
the next chapters. Many of the introduced concepts are an extension of the SE(2)
case treated in [27, 30, 31] to which the interested reader is referred for full details.
Let G = (V, E) be a directed graph, where V = {1 . . . N } is the vertex set and
E ⊆ V × V the edge set [149]. Presence of an edge ek = (i, j) in E represents the
possibility for agent i to measure the relative bearing βij (4.2) to agent j. Graph G
is designed as directed as we do not require, in general, reciprocity of the relative
bearing measurements. (i.e., agent i may measure agent j but not be measured by
agent j). This way, the typical visibility constraints of onboard cameras due to, e.g.,
limited field of view or occluded line-of-sight, can be directly accommodated at the
modeling stage. We assume, however, that agents i and j can communicate if either
(i, j) ∈ E or (j, i) ∈ E (i.e., the communication graph is taken as the undirected
counterpart of the directed sensing graph G).

52
 4. Main modeling assumptions

(a) (b)

Figure 4.3 – Examples of two frameworks which are bearing equivalent but not bearing
congruent

We let Ni = {j ∈ V| (i, j) ∈ E} represent the usual set of neighbors of an agent i,

and Oi = {j ∈ V| (j, i) ∈ E} the set of agents j for which i is a neighbor2 . We also
let 1N and IN represent a vector of all ones and the identity matrix of dimension
N , respectively.
A slightly different way of defining bearing rigidity, with respect to the defi-
nitions given in Sect. 3.4.3, is the one which involves the concepts of framework
congruency and equivalence. In this case, the difference is that a framework (or
also formation) [30, 31, 150] is the pair (G, q) where q follows the definition given
in Sect. 4.2.
Two frameworks (G, q) and (G, q  ) are denoted bearing equivalent if βij (q) =
βij (q  ) ∀(i, j) ∈ E and bearing congruent if βij (q) = βij (q  ) ∀ i, j ∈ V, i =
 j. A
framework (G, q) is defined bearing rigid (or simply rigid in the following) if there
exists a neighborhood U of q such that any framework (G, q  ), q  ∈ U , that is
bearing equivalent to (G, q) is also bearing congruent to (G, q). Also in this case,
logically, bearing congruency implies bearing equivalence, but the opposite is not
true (see Fig. 4.3). By extension, a framework is instead defined globally bearing
rigid if U = (R3 × S1 )N , i.e., if all frameworks which are bearing equivalent to
(G, q) are also bearing congruent to (G, q). A non-rigid framework is also termed
roto-flexible, and a framework (G, q) is said to be minimally rigid if (G, q) is rigid
and the removal of any edge yields a roto-flexible framework.
As for the rigidity in the case of distance constraints, the notion of bearing
rigidity can also be characterized from an infinitesimal perspective by introducing
the so-called directed bearing function and corresponding directed bearing rigidity
matrix. The directed bearing function (bearing function from now on) associated to
a framework (G, q) is the map βG (q) : (R3 × S1 )N → (S2 )|E|
 
βG (q) = stack βe1 , . . . , βe|E| (4.4)
2
This distinction between sets Ni and Oi , needed because of the directed nature of graph G,
will play an important role in Sects. 6.3.2 and 6.3.3.

53
Bearing-based localization and control for multiple quadrotors

where the notation ei ∈ E is used to represent a directed edge in the graph G

according to any chosen labeling. The world-frame (directed) bearing rigidity matrix
(BRM) is the Jacobian of the bearing function with respect to the agent configuration
q, that is, the matrix
∂βG (q)
BW
G (q) = ∈ R3|E|×4N . (4.5)
∂q

Let N (·) represent the span of the null-space of a matrix. A framework (G, q) is
said to be infinitesimally bearing rigid at some point q if N (BW W
G (q)) = N (B KN (q)),
with KN being the complete directed graph. Otherwise a framework is said to be
infinitesimally roto-flexible. Since it can be shown that dim N (BW KN (q)) = 5 (see,
e.g., [27, 77]), it follows that a framework (G, q) in R × S is infinitesimally rigid if
3 1

and only if the following is satisfied

rank(BW
G (q)) = 4N − 5. (4.6)

For infinitesimally rigid frameworks in R3 × S1 , the 5-dimensional null-space of the

bearing rigidity matrix (BRM) is also well-understood: it corresponds to the three
rigid-body translations, a dilation relative to a reference point, and a coordinated
rotation about a vertical axis passing through a reference point [30, 31, 77]. Equiva-
lently, we can say that the 5-dimensional null-space of matrix BW G (q) is spanned by
those velocities q̇ that do not change the bearing function βG (q) (i.e., the value of
the measured bearing vectors).
The notion of bearing infinitesimally rigidity and, in particular, the properties
of the bearing rigidity matrix and its null-space play a central role in the design of
decentralized bearing formation controller and localization schemes. A role analogous
to the (more common case of) distance-constrained infinitesimal rigidity and distance-
constrained rigidity matrix [99,116,118,119,151]. Note that in [122] there is a theorem
that expresses a relationship between the three kinds of bearing rigidity which is
summarized in Fig. 4.4.

Figure 4.4 – Relationship between the three kinds of bearing rigidity introduced in this
section, from [122]

Extending the results of [31] to the case of frameworks in R3 × S1 , the k-th row
block of the world-frame bearing rigidity matrix BW G in (4.5) associated to edge

54
 4. Main modeling assumptions

ek = (i, j) has expression

 
Pij RiT Pij RiT
∂βij  −0− − −0− −0− . . . −Sβij −0− 
= dij dij  ∈ R3×4N .
∂q
| {z }
| {z } | {z } 3N +i
i j

(4.7)

T is the orthogonal projector onto the orthogonal complement

Here, Pij = I3 − βij βij
of βij , and S = [0 0 1]T × where [·]× indicates the usual skew-symmetric matrix
 

operator.
We then note that the bearing rigidity matrix BWG is a function of interdistances,
relative bearings, and absolute yaw rotations. Indeed, the bearing rigidity matrix
relates changes in the bearing function βG to the world-frame velocities q̇ = (ṗ, ψ̇)
of the framework " #
W ṗ
β̇G = BG (q) . (4.8)
ψ̇

The world-frame rigidity matrix relates variations in the bearing function to

the world-frame agent velocities (ṗ, ψ̇). Exploiting (4.1), one can also define a
body-frame bearing rigidity matrix BG (q) as
" #" # " # " #
diag(Ri ) 0 u u u
β̇G =BW
G (q) = BW
G T (ψ) = BG (q) , (4.9)
0 IN w w w

which explicits the dependance on the body-frame velocity inputs u = stack (. . . ui . . .) ∈

R3N and w = stack (. . . wi . . .) ∈ RN , respectively the linear and angular velocities.
The (3×4N ) k-th row block of BG (q) associated to an edge ek = (i, j) has expression
 
Pij Pij iRj
 −0− − d −0−
dij
−0− −Sβij −0− 
 ij | {z } , (4.10)
| {z } | {z } 3N +i
i j

where iRj = Rz (ψj − ψi ) = Rz (ψij ).

It is worth noting that the body-frame bearing rigidity (4.10) matrix is, again,
a function of measured bearings βij and interdistances dij . However, contrary to
the previous case, it is not a function of absolute yaw rotations ψi , but, instead,
of relative orientations ψij = ψj − ψi among neighboring agents. This fact will be
important for the next developments.
Note that the same considerations we did for the world-frame bearing rigidity
matrix about infinitesimal rigidity extend to the body-frame bearing rigidity matrix
since rank(BG (q)) = rank(BW G (q)), being T (ψ) in (4.9) a square non-singular matrix.

55
Bearing-based localization and control for multiple quadrotors

Now we are finally ready to present to the reader the main contributions of this
Thesis which are divided into three chapters:

• Chapt. 5 presents a formation control and localization algorithm for a group

of N quadrotor UAVs based on the theory of rigidity.

• Chapt. 6 presents a solution to the problem of maintaining rigidity in spite of

sensing limitations like (i) limited field of view of the onboard cameras, (ii)
limited range of the onboard cameras, (iii) possibility of occlusions between
the agents of the formation during motion.

• Chapt. 7 presents a nonlinear observability analysis of the system composed

of N quadrotor UAVs. The observability analysis is the first step towards
the design of an Extended Kalman Filter which can also estimate the scale
of the formation (and therefore the real distances between the robots) using
only bearing measurements and the linear/angular body-frame velocities of
the robots.

56
 Chapter 5
Formation control and localization
in R3 × S1

Contents
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.1.1 Chapter overview . . . . . . . . . . . . . . . . . . . . . . 60
5.2 Decentralized formation control . . . . . . . . . . . . . . . . . . 60
3 1
5.2.1 Rigidity-based control of bearing frameworks in R × S 61
5.2.2 Rigidity-based localization of time-varying bearing frame-
works in R3 × S1 . . . . . . . . . . . . . . . . . . . . . . 63
5.2.3 Coordinated motions in the null-space of the bearing
rigidity matrix . . . . . . . . . . . . . . . . . . . . . . . 65
5.2.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . 66
5.3 Simulation results . . . . . . . . . . . . . . . . . . . . . . . . . . 68
5.4 Experimental results . . . . . . . . . . . . . . . . . . . . . . . . . 69
5.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

n this chapter we consider the problem of controlling a formation of quadrotor

I UAVs equipped with onboard cameras able to measure relative bearings in their
local body frames with respect to neighboring UAVs. The control goal of this
work is twofold: (i) steering the agent group towards a formation defined in terms
of desired bearings, and (ii) actuating the group motions in the ‘null-space’ of the
current bearing formation. The proposed control strategy relies on an extension of
the rigidity theory to the case of directed bearing frameworks in R3 ×S1 (see Sect. 4.3).
This extension allows to devise a decentralized bearing controller which does not
need presence of a common reference frame or of reciprocal bearing measurements
for the agents.

57
Bearing-based localization and control for multiple quadrotors

5.1 Introduction
As stated in the previous chapters, the problem of formation control of multiple
mobile robots has been extensively studied over the last decade in the robotics and
control communities. The basic goal of most formation controllers is to coordinate a
robot team in order to achieve some desired spatial arrangement. Most formation
control schemes differ in their assumptions about, for example:

• the robot mobility: planar robots, aerial vehicles, possible presence of non-
holonomic constraints or underactuation

• the employed sensing technology: local measurements from onboard sensors

such as cameras, lidars, sonars, radio transmitters/receivers, or absolute/global
measurement from centralized facilities as motion capture systems (mainly for
indoor applications) and GNSS (for outdoor applications).

• the overall architecture: centralized versus decentralized coordination schemes.

A challenging scenario that still motivates considerable research efforts is that

of decentralized formation control of mobile robots based only on local sensing, in
which the robots are assumed to only be able to obtain relative measurements with
respect to other robots in the group. Typical examples of relative sensing include
range sensors (such as lidars and ultrasound) for retrieving inter-robot distances, or
bearing sensors (such as cameras) for obtaining the bearing angle to other robots
within visibility. The use of local sensing also entails the possible lack of a common
reference frame to which all the individually collected measurements and control
actions can be expressed, a limitation that poses additional challenges for solving the
formation control problem [67, 152, 153]. As we already mentioned, these scenarios
are motivated by the goal of deploying highly autonomous multi-robot teams in
‘non-trivial’ environments (e.g., inside buildings, underwater, underground, or even
in deep space). Usually, in these scenarios centralized sensing facilities, such as
GNSS, are not available, and the robots can only rely on their ‘local skills’ (local
sensing and computing power, and local communication with neighboring agents).
In Sect. 3.4 we explained that, in all these cases, the correct theoretical framework
for analyzing and controlling the geometrical properties of robot formations defined in
terms of relative measurements has proven to be the theory of formation rigidity [72]
described already in Chapt. 3.
Cameras are a widespread sensing technology which is often found onboard
quadrotor UAVs mainly because of their low weight and limited power consump-
tion [145]. It is then interesting to study how to design suitable vision-based

58
 5. Formation control and localization in R3 × S1

cooperative control strategies to be exploited in more complex applications such as

exploration, coverage, surveillance or target tracking.
Nevertheless, when addressing visual-based formation control in GNSS-denied
environments, the assumptions of a common reference frame and of an undirected
topology for the measurement graph can be hard to realize. For instance, the
requirement of keeping constant mutual visibility among all robot pairs can easily
become unfeasible because of the limited camera field of view, and likewise for the
possible presence of a common reference frame shared by a group of robots that
has only access to local relative measurements. It is then essential to investigate
possible bearing formation control strategies that can relax as much as possible these
assumptions.
In this respect, to the best of our knowledge, the only decentralized bearing
formation controller that does not require the presence of a common reference frame
and of an undirected sensing topology has been proposed in [77] by exploiting
the (body-frame) measured bearings and a single distance measurement among
an arbitrary pair of robots. However, the machinery presented in [77] requires a
very special structure for the chosen measurement graph (the one in Fig. 5.1(a))
which, among others, must contain two special agents able to measure and to be
measured by any other agent in the group. While instrumental, this choice is also
unnecessarily overconstraining since any bearing rigid topology (with, in general,
far less constraining requirements than the ones exploited in [77]), for example the
graphs in Figs. 5.1(b) and 5.1(c) would still allow solving a bearing formation control
problem.

(a) (b) (c)

Figure 5.1 – Three examples of rigid graphs with 5 agents. Fig. 5.1(a) is the overconstrained
graph used in [77]. Fig. 5.1(b) is another example of rigid graph which does not require
all the constraints of the previous one and Fig. 5.1(c) is yet another rigid graph chosen
randomly.

Our contribution in this context has been the generalization of the ideas of [77].
Indeed, we proposed a fully decentralized bearing formation controller that only
requires the presence of a generic (directed) bearing rigid topology. Furthermore,
as in [77], the proposed control strategy is also complemented with the possibility
of steering the quadrotor group along all the bearing-preserving motion directions:
these can be shown to consist of a collective translation, an expansion with respect
to a reference point, and a coordinated rotation relative to a vertical reference axis.

59
Bearing-based localization and control for multiple quadrotors

This possibility is particularly useful when, for instance, we would need to collectively
steer the quadrotor group for navigation or exploration purposes while maintaining
the desired bearing formation optimized for the task at hand.
The two goals discussed above of bearing formation stabilization and group col-
lective steering are here achieved under a minimal number of assumptions compared
to the existing literature. In particular:

• the quadrotors are only assumed able to collect bearing measurements (through
onboard cameras) and to impose motion commands in their (local) body-frames
(which do not need to be aligned or ‘coordinated’ in some special ways),

• the bearing measurements are not necessarily required to be reciprocal, and

the resulting (directed) sensing topology has no special constraints (apart from
the ‘necessary condition’ of yielding a rigid bearing formation),

• a single (but arbitrary) quadrotor pair is additionally assumed able to measure

its inter-distance (needed to retrieve the correct scale of formation which would
be, otherwise, unobservable without employing estimation strategies such as
those described in Chapt. 7 or in [146]). Note that, besides the intrinsic
importance of knowing the scale of the formation, this quantity is then needed
for correctly implementing the coordinated rotation in the null-space of the
bearing rigidity matrix (BRM).

Finally, we also provide an experimental validation of the overall approach

employing a group of four quadrotor UAVs and simulations with the help of Mat-
lab/SIMULINK and the V-REP simulator.

5.1.1 Chapter overview

The rest of the chapter is organized as follows: Sect. 5.2 illustrates the proposed
bearing formation control strategy for robots evolving in R3 × S1 , which can be
representative of the quadrotor case. Subsequently, Sect. 5.3 reports the simulation
results obtained by considering a group of quadrotor UAVs while Sect. 5.4 presents
the experiments with real robots. Finally, Sect. 6.5 concludes the chapter and
discusses some future directions.

5.2 Decentralized formation control

Consider a bearing rigid framework (G, q) in R3 × S1 consisting of N agents with
dynamics (4.1). Let qd be a desired configuration such that (G, qd ) is bearing rigid,
and let bdG = βG (qd ) = (βed1 . . . βed E| ) be the corresponding desired value for the
|

60
 5. Formation control and localization in R3 × S1

bearing function. Our goal is to design a decentralized bearing formation controller

able to accomplish two distinct objectives.
A first objective is the bearing formation stabilization: by acting on the control
inputs (ui , wi ), the controller should steer the N agents towards a configuration
q ∗ equivalent to qd , i.e., such that βG (q ∗ ) = bdG . Because of the framework rigidity,
equivalence will also imply congruency with qd . Hence, the fulfilment of this first
objective will ensure that q(t) reaches the (correct) desired shape modulo a possible
translation, vertical rotation, and scaling (i.e., the motions spanning the null-space
of the bearing rigidity matrix).
A second independent objective, ‘orthogonal’ to the first one, is the possibility
to steer the agent group along the motion directions that do not affect the bearing
rigidity function (for not interfering with the previous formation control goal), i.e.,
such that βG (q(t)) = const. These are spanned by the null space of the rigidity
matrix which, for a rigid framework in R3 × S1 , consists of the aforementioned
collective translation, expansion and coordinated rotation.
Fulfillment of this second objective will then allow to, for instance, collectively
steer the agent group while maintaining a desired bearing formation for navigation
or exploration purposes.
Finally, the formation controller should be decentralized and only based on
information locally available or communicated by 1-hop neighbors.
As explained above, a complete solution to this control problem has been already
proposed in [77] by, however, exploiting the structure of a very special interaction
graph G and a single distance measurement dij = kpi − pj k among an arbitrary pair
of robots. Indeed, measurement of this distance allows to fix (without ambiguities)
the scale of the formation, which is then needed for correctly implementing the
coordinated rotation in the null-space of the bearing rigidity matrix. Coping with
this requirement may be quite challenging from a sensing/technological point of
view because of, e.g., mutual occlusions (for large quadrotor groups or in cluttered
environments) and/or limited camera field of view (with respect to the last two
problems the reader is referred to Chapt. 6).
Therefore, the goal of the rest of the section is to generalize the machinery
presented in [77] to the case of a generic bearing rigid framework. This will be achieved
by suitably combining and extending the results of [30, 31] which have addressed,
in a separated way, the problems of rigidity-based decentralized localization and
control for bearing formations for frameworks in SE(2).

5.2.1 Rigidity-based control of bearing frameworks in R3 × S1

A first contribution of this work is the following.

61
Bearing-based localization and control for multiple quadrotors

Proposition 5.1. The null-space of the bearing rigidity matrix can be explicitly
characterized as
(" # " # " #)
1 p p⊥
N
N (BW
G (q)) = span
3
, ,
0 0 1N (5.1)
= span {n1 , n2 n3 }

where 1N is a vector of all ones of dimension N , 1N3 = 1N ⊗ I3 , p⊥ = (IN ⊗ S)p,

and ⊗ denotes the matrix Kronecker product.

Proof. The proof of Prop. 5.1 that vectors [1TN3 0T ]T and [pT 0T ]T belong to N (BW G )
can be found in [30, 31]. The explicit expression for the last null-space vector in (5.1)
# in [30, 31]) can be shown as follows: consider the k-th element of
(not "present
p ⊥
BWG , which has the following expression
1N

Pij RiT S(pj − pi ) RT (pj − pi )

− Sβij = Pij RiT SRi i − Sβij =
dij dij (5.2)
Pij Sβij − Sβij = Sβij − Sβij = 0,

where the properties RiT SRi = S and Pij Sβij = Sβij were used (the last one
exploits the fact that Sβij ⊥ βij ).

Each null-space vector of this particular basis for N (BW

G (q)) represents one of
the coordinated motions discussed above, namely, three translations along the world
axes, an expansion about OW , and a rotation about a vertical axis passing through
OW .
Remember that, as pointed out in Sect. 4.3, it is also possible to define a body-
frame bearing rigidity matrix BG (q). Let now eF (q) = bdG − βG (q) denote the
bearing formation control error to be regulated to zero for solving the first objective
(formation stabilization). As shown in [30], minimization of keF k can be obtained
by implementing the following scale-free controller based on the body-frame rigidity
matrix,
" # " #
u diag(dij ) 0
= kc BG (q)T bdG , kc > 0 (5.3)
w 0 IN
that results in the i-th agent velocity command
 X X
d i d

 ui = −kc Pij β ij + kc Rj Pji βji

(i, j)∈E (j, i)∈E

X . (5.4)
T d
 w = kc βij Sβij
 i


(i, j)∈E

62
 5. Formation control and localization in R3 × S1

The reader is referred to [31] for an almost global stability proof for frameworks in
SE(2) that can be directly extended to the case under consideration. Furthermore,
the centroid p̄ = 1TN3 p/N and ‘scale’ pT p of the formation can be shown to be
invariant under the action of (5.4).
It is worth noting that controller (5.4) has a decentralized structure depending
only on the interaction graph G and on relative quantities. In particular, it does not
require knowledge of any distance measurement (from which the term scale-free),
nor knowledge of any common reference frame shared by the agent group. However,
controller (5.4) requires communication among agents since, if there exists an edge
(j, i) ∈ E (i.e., an agent j is measuring agent i), agent i needs to receive the bearing
measurement βji and desired bearing βji d from agent j (second term of u ).
i

Furthermore, controller (5.4) also needs access to the relative orientation iRj
among neighboring pairs which is a quantity not available from direct measurements
and here, again, bearing rigidity comes in handy. Indeed, if a framework is bearing
rigid, all the relative orientations among agent pairs are univocally fixed by the
existing inter-agent bearings constraints. Therefore, it is in principle conceivable to
recover/estimate the relative rotations iRj by processing the measured inter-agent
bearings.
This insight has, indeed, been exploited in [77, 121]. In [121] the assumption
of reciprocal measurements for all agent pairs (undirected sensing graph G) allows
for an algebraic computation of all the needed relative orientations. An analogous
solution is exploited in [77] which, instead, relies on a very special construction of the
(directed) sensing graph G. The procedure of [77] cannot be, however, generalized
to generic bearing rigid frameworks such as those considered in this venue. In
order to cope with this problem, we now detail an extension of the localization
algorithm introduced in [30] for obtaining a (decentralized) estimation of the relative
orientations iRj in presence of a generic bearing rigid graph and of non-stationary
agents.

5.2.2 Rigidity-based localization of time-varying bearing

frameworks in R3 × S1

The decentralized localization algorithm proposed in [30] allows the agent group
to estimate their unscaled positions1 and orientations with respect to a common
reference frame by only exploiting the available relative bearing measurements. The
localization algorithm exploits the world-frame bearing rigidity matrix BWG (q).

1
Here, the term ‘unscaled’ means that the agent positions are estimated up to a common scale
factor.

63
Bearing-based localization and control for multiple quadrotors

Let q̂ = (p̂, ψ̂) be an estimation of the true q and define the bearing estimation
error as eL (q, q̂) = βG (q) − βG (q̂). Assuming βG (q(t)) = const, minimization of
keL k can be obtained by this gradient descent based on the bearing rigidity matrix,

p̂˙
" #

˙ = ke BW T
G (q̂) βG (q), ke > 0. (5.5)
ψ̂

Under the action of (5.5), the estimation q̂(t) will converge towards a configuration
equivalent to q. Bearing rigidity of the framework (G, q) will also imply congruency
with q. Therefore, at convergence (eL = 0), the estimated q̂ will reach a configuration
such that (
p̂ = s(IN ⊗ Rz (ψ̄))p + 1N ⊗ t
(5.6)
ψ̂ = ψ + 1N ψ̄

for an arbitrary translation t ∈ R3 , rotation angle ψ̄ ∈ S1 and scaling factor

s ∈ R+ . Any neighboring pair can then replace the unknown iRj with the estimated
ˆ j = Rz (ψ̂j − ψ̂i ) by exchanging the two estimates (ψ̂i , ψ̂j ) over local communication.
iR

The estimator (5.5) is fully decentralized and only requires the bearings in βG (q)
as measured quantities. However, the estimator (5.5) also assumes βG (q(t)) = const
while the inter-agent relative bearings will be in general time-varying under the
action of controller (5.4)2 . Presence of a time-varying βG (q(t)) can clearly prevent
convergence of the estimation error. This issue can be, however, addressed by adding
to (5.5) the following feedforward term for taking into account the agent motion,

p̂˙
" # " #" #
diag(Rz (ψ̂i )) 0 u
˙ = ke BW T
G (q̂) βG (q) + . (5.7)
ψ̂ 0 IN w

Proposition 5.2. If the initial estimation error keL (t0 )k is small enough and s = 1
then (5.7) will guarantee keL (t)k → 0 in case of time-varying bearings βG (q(t)) 6=
const.

Proof. The closed-loop dynamics of the estimation error is

p̂˙
" # " #
u
ėL = BG (q) − BW
G (q̂) ˙ =
w ψ̂
" # (5.8)
u
= −ke BW W T
G (q̂)B G (q̂) βG (q) + (B G (q) − B G (q̂)) .
w

The first term of (5.8) represents the (nominal) closed-loop dynamics of the constant
bearing case, while the second term of (5.8) is a perturbation due to the agent motion.
2
Indeed, βG (q(t)) = const only for stationary agents or for agents moving along the direc-
tions (5.1).

64
 5. Formation control and localization in R3 × S1

Since the nominal closed-loop dynamics is asymptotically stable [30], one can resort
to the theory of perturbed systems [154] for analyzing the stability of (5.8). In
particular, if the perturbation term can be shown to be vanishing with respect to the
estimation error eL , one can conclude local stability of the overall system (5.8) under
mild conditions. Consider the k-th row of the body-frame rigidity matrix (4.10):
this depends on the quantities βij , dij and iRj . By inspection one can then verify
that, when eL = 0 (i.e., when (5.6) holds), iRj = i R̂j , βij = β̂ij and dij = sdˆij .
Assuming s = 1 in (5.6) then results in eL → 0 =⇒ BG (q) − BG (q̂) → 0 which
concludes the proof.

A correct scale (s = 1) of the estimated formation q̂ is then necessary in order to

compensate for the effects of the agent motion in the estimation dynamics. It is well-
known that the formation scale cannot be retrieved from only bearing measurements
without introducing the concepts of nonlinear observability of Chapt. 7 already or
the ones illustrated in [146]. Therefore, here we exploit the presence of the single
pair of agents, indexed as ι and κ, which is assumed able to also measure its relative
distance dικ . One can then consider the following ‘augmented’ cost function,

1
(ke eTL eL + kd (p̂Tικ p̂ικ − d2ικ )2 ), kd > 0 (5.9)
2
meant to enforce the constraint kp̂ικ k = kp̂ι − p̂κ k = dικ in the estimated q̂. As
shown in [30], minimization of (5.9) is obtained by complementing the update
law (5.7) with the additional (decentralized) terms ∓kd (p̂Tικ p̂ικ − d2ικ )p̂ικ in the ι-th
˙ respectively.
and κ-th entries of p̂,
If a distance measurement is not available, the estimator (5.7) will not be able
to recover the correct scale factor s, but it will still track changes in the formation
scale thanks to the feedforward term. If the initial mismatch between actual and
estimated scale is small enough, this is often enough for allowing (5.7) to still provide
a consistent estimation of q. Sect. 5.3 will indeed show results in this sense, by
comparing the control/estimation performance when including/not-including the
single distance measurement dικ .

5.2.3 Coordinated motions in the null-space of the bearing

rigidity matrix
As a final step, we address the fulfilment of the second control objective, that is, the
implementation of the null-space motions spanned by (5.1).
This can be achieved by realizing the world-frame velocity q̇s = n1 ν + n2 λ + n3 w
which imposes to the framework a common linear velocity ν ∈ R3 , an expansion
rate λ ∈ R about OW , and a coordinated rotation with angular speed w about a

65
Bearing-based localization and control for multiple quadrotors

vertical axis passing through OW . The corresponding body-frame velocities (us , ws )

to be added to the formation control inputs (u, w) in (5.3) are then
" # " #
us diag(RiT ) 0
= q̇s . (5.10)
ws 0 IN

While (5.10) realizes the second control objective, it is usually more interesting
to implement an expansion rate and coordinated rotation about a specific point of
interest attached to the formation itself, rather than about the (arbitrary) origin of
the world frame OW . For instance, an often convenient choice is to implement these
motions relative to the formation centroid p̄ = 1TN3 p/N . This can be obtained by
using as basis for N (BWG (q)) the set {n1 , n2 − n1 p̄, n3 − n1 S p̄} which eventually
results in the i-th agent velocity commands
(
usi = RiT (ν + λ(pi − p̄) + wS(pi − p̄))
. (5.11)
ws i = w

An actual implementation of (5.11) would require the (non-available) quantities

(pi , ψi , p̄): exploiting the estimator (5.7), each agent can replace the true (pi , ψi )
with the estimated (p̂i , ψ̂i ). The corresponding average p̄ ˆ = 1TN p̂/N can then be
3
obtained by resorting to any distributed averaging filtering technique such as the
well-known PI average consensus filter (PI-ACE) [70]. Indeed, given a (time-varying)
vector quantity x(t) ∈ RN with each component xi (t) locally available to agent i, the
PI-ACE filter allows every agent to distributedly build an estimation converging to
the average x̄(t) = N
P
i=1 xi (t)/N with a tunable dynamics that can be made faster
than the underlying dynamics of each agent in the system, see also [34, 119, 155] for
some applications in the context of multi-robot distributed control.
It is worth noting that knowledge of the correct scale factor (s = 1) in the
estimated q̂ is not required for implementing the null-space motions associated to
vectors n1 and n2 (translation and expansion), but it is instead required for correctly
implementing the coordinated rotation associated to vector n3 . Indeed, n1 does not
depend on p and n2 is homogeneous in p, with its direction thus unaffected by any
scaling of the agent positions. This is, however, not the case for vector n3 which is
not homogeneous in p. Similarly to the estimation case, if a distance measurement
is not available for fixing the scale of q̂, the coordinated rotation will not be exactly
implemented.

5.2.4 Discussion
We conclude by emphasizing that, as stated at the beginning of the section, the
proposed control/estimation scheme does not require a special topology for the

66
 5. Formation control and localization in R3 × S1

Figure 5.2 – Possible minimal bearing rigid topologies for N ∈ {3, 4, 5, 6} (note that some
arrows are bi-directional)

interaction graph (besides being bearing-rigid); the bearing controller (5.4), the
localization algorithm (5.7), and the null-space motions (5.11) have the same (de-
centralized) expression for all agents only as a function of the measured bearings
and body-frame linear/angular velocities. The only exception is the inclusion of the
distance measurement dικ which adds an additional control term to agents ι and
κ. In this sense, we believe that the work presented in this chapter represents a
significant generalization of the strategy reported in [77] which, to the best of our
knowledge, is the closest related work to our setting, and relied on a much more
constrained design of the agent group.
We also note that the correct formation scale could be retrieved without assuming
the presence of an (additional) distance measurement dικ (and, thus, presence of two
‘special agents’ in the group). Indeed, the unknown robot inter-distances could be
estimated online by processing the measured inter-robot bearings and the (known)
robot own motions similarly to what done in the context of scale estimation for point
features [156, 157]. In this respect, a possible solution will be presented in Chapt. 7.
A different approach has been instead followed by [146] which proposes an active
scale estimation strategy for bearing formations of quadrotor UAVs.
We finally wish to briefly discuss the practical implications of requiring directed
(bearing) rigidity for the robot formation (which, as often stated, is an underlying
necessary condition of the proposed machinery). As well-known, minimal rigidity
requires presence of a |E| = O(N ) number of edges (i.e., of inter-robot measure-
ments/constraints) in the framework vs. the quadratic complexity of the complete
(directed) graph KN (for which the complexity would be N (N − 1) = O(N 2 )). As
illustration, Fig. 5.2 shows some possible minimal bearing rigid topologies for the
case of N ∈ {3 . . . 6} agents for which |E| = 4, 6, 8, 10, respectively.

67
Bearing-based localization and control for multiple quadrotors

5.3 Simulation results

In this section we present some simulation results involving N = 6 quadrotor UAVs.
The simulation is run by considering the full dynamics of 6 quadrotor UAVs simulated
via the 3D physical simulator V-REP [158], for more details about the software
architecture the reader is referred to Appendix B. In this case, the robustness of the
bearing controller is tested against the discrepancies between the nominal agent (4.1)
and the actual quadrotor flight dynamics, as well as against noise and discretization
in the measured bearings (which are sampled at 60 Hz for mimicking an actual
onboard camera).
The initial configuration q(t0 ) and estimated q̂(t0 ) were generated by adding
to the desired qd a uniformly distributed random perturbation of amplitude 1 m
for the positions and 120 deg for the orientation. A graph G with |E| = 20 directed
edges was then randomly generated under the constraint of guaranteeing bearing
rigidity at q(t0 ), q̂(t0 ) and qd , and the following gains were used: kc = 1 in (5.4),
ke = 5 in (5.7) and kd = 10 in (5.9). Finally, in all simulations the null-space
velocity commands (5.11) were activated, for convenience of illustration, only after
convergence to the desired formation bdG : the five coordinated motions were first
actuated one at the time and then all together.
Figs. 5.3 and 5.4 report the simulation results. In particular, the bottom
Fig. 5.3(a) shows the behavior of keF (t)k (the bearing control error, solid blue
line) and of keL (t)k (the estimation error, solid red line), while the top Fig. 5.3(a)
depicts the five null-space velocity commands (ν, λ, w) in (5.11) given by a human
operator through a joypad. It is then possible to verify how (i) both the control and
estimation bearing errors converge to zero (despite their initial large value and despite
the presence of time-varying bearings β(q(t))) and how (ii) the implementation of
the coordinated motions (5.11) has no disturbing effect on the bearing errors (as
expected).
Let λ6 (q) ≥ 0 represent the sixth smallest eigenvalue of the following square
matrix, which we refer to as symmetric bearing rigidity matrix 3

BGW (q) = BW T W
G (q) B G (q). (5.12)

Since for an infinitesimal rigid framework in R3 × S1 it holds rank(BW G (q)) =

4N − 5, the quantity λ6 (q) can be taken as a measure of the framework bearing
rigidity. Indeed, λ6 (q) > 0 if and only if (G, q) is infinitesimal rigid and λ6 (q) = 0
otherwise. Fig. 5.3(b) then reports the behavior of λC E
6 (t) = λ6 (q(t)) and λ6 (t) =
λ6 (q̂(t)), that is, the rigidity measures for the ‘control’ framework (G, q) and the
‘estimation’ framework (G, q̂). One can then check how both frameworks remained
3
Note the notation difference between B and B

68
 5. Formation control and localization in R3 × S1

0.5 0.18
ν, λ, w
0
−0.5 0.16
0 20 40 60
time [s]
8 0.14

λC6 ,λE
6
6
keF k,keL k

0.12
4
0.1
2

0 0.08
0 20 40 60 0 20 40 60
time [s] time [s]
(a) (b)

Figure 5.3 – Results of the simulation. 5.3(a)-top: behavior of the five null-space motion
commands ν(t) (blue, purple, yellow) λ(t) (green) and w(t) (red). 5.3(a)-bottom: behavior
of the bearing control error keF (t)k and of the localization control error keL (t)k. 5.3(b):
behavior of the rigidity eigenvalues λC E
6 (t) (control – blue) and λ6 (t) (estimation – red)

rigid throughout the motion of the agents, thus confirming congruency between q
and qd (correct agent formation), and between q and q̂ (correct agent localization).
As an additional measure of the localization performance, we considered the
quantity eψ = IN − 1N 1TN /N (ψ − ψ̂): this represents the disagreement between

the orientation estimation error and its mean value, and it should vanish in presence
of a correct localization4 as, indeed, reported in Fig. 5.4(b). A converging eψ (t) then
allows to correctly compute the missing terms i Rj in the bearing controller (5.4).
Finally, Fig. 5.4(b) depicts the behavior of the ‘formation scale error’ defined as
ˆ (t)k which, again, converges to zero, as
es (t) = kp(t) − 1N ⊗ p̄(t)k − kp̂(t) − 1N ⊗ p̄
expected, thanks to the additional distance constraint in (5.9).
It is worth noting that the ‘distortions’ present in Fig. 5.4(b) are mainly due
to the higher-order quadrotor dynamics neglected by model (4.1) which, roughly
speaking, introduces an unmodeled lag between commanded and actual velocities.
The proposed control strategy is nevertheless robust enough for coping with these
model inaccuracies.

5.4 Experimental results

This section discusses the experiment conducted for validating the formation con-
troller described in Sect. 5.2.
For the experiment illustrated in this section we used a group of four quadrotor
of the ones described in Sect. B.2. The TeleKyb framework [159] was used for
4
Indeed, a correct localization implies that ψ̂(t) → ψ(t) + 1N ψ̄ (see (5.6)).

69
Bearing-based localization and control for multiple quadrotors

2.5 2

2 1
1.5 0
1
−1
0.5

es
eψ

−2
0
−3
−0.5
−1 −4

−1.5 −5
−2 −6
0 10 20 30 40 50 60 0 10 20 30 40 50 60
time [s] time [s]
(a) (b)

Figure 5.4 – Results of the first simulation. 5.4(a): behavior of the orientation estimation
error eψ (t). 5.4(b): behavior of the formation scale error es (t)

Figure 5.5 – Four quadrotors flying in the flying arena

interfacing our bearing control algorithm with the MK-Quadro low-level controller.
The onboard low-level controller is the one described in [160] and it is in charge of
letting the orientation of the quadrotor track a desired reference. The experiments
were performed in our flying arena which is equipped with a Vicon motion capture
system, employed for reconstructing the body-frame bearing measurements βij that
would have been obtained by an onboard camera (refer to Sect. B.2.2 for more
details).
The reported experiment followed a pattern similar to the simulation results
described in the previous section: (i) regulation towards a desired bearing formation,
(ii) actuation of the null-space motions (5.11), (iii) regulation towards a different de-
sired bearing formation, (iv) actuation of the null-space motions (5.11). Additionally,
we implemented, at every 6 seconds, a random switch among all the possible rigid

70
 5. Formation control and localization in R3 × S1

1 0.4
ν, λ, w
0
0.3
-1
3

E
6 ,λ6
keF k,keL k

0.2

λC
2
0.1
1

0 0
0 20 40 60 80 0 20 40 60 80
time [s] time [s]
(a) (b)

Figure 5.6 – Results of the experiment. Fig. 5.6(a)-top: behavior of the five null-space
motion commands ν(t) (blue, purple, yellow) λ(t) (green) and w(t) (red). Fig. 5.6(a)-
bottom: behavior of the bearing control error keF (t)k and of the localization control error
keL (t)k. Fig. 5.6(b): behavior of the rigidity eigenvalues λC E
6 (t) (control – blue) and λ6 (t)
(estimation – red)

topologies for the sensing graph in order to show the robustness of our approach
also against possible topology changes during motion.

Fig. 5.6–5.7 report the results of the experiment. The UAV formation starts
far from the desired configuration but, after about 20 seconds, the norm of the
formation control error keF (t)k drops below 4% of its initial value (Fig. 5.6(a)). On
the other hand, convergence of the estimator error keL (t)k is quite fast even though
the initial estimated q̂(t0 ) was generated by adding to the real q(t0 ) a uniformly
distributed random perturbation of amplitude 1.5 m for the positions p(t0 ) and
80 deg for the orientations ψ(t0 ). Convergence of the estimated q̂(t) towards a
configuration congruent with q(t) (and with the correct scale) can also be appreciated
in Fig. 5.7 where the orientation estimation error eψ (t) and the formation scale
error es (t) are shown. One can then verify, again, how a consistent estimation
of the orientations ψ̂ and of the formation scale s could be obtained despite the
(unavoidable) non-idealities present in any real implementation.

Finally, as stated before, the underlying graph G switches randomly at every

6 seconds across all the possible rigid topologies for the N = 4 quadrotors (in partic-
ular, we allowed switches among graphs with |E| ∈ {6, 7, 8, 9, 10, 11, 12}). Fig. 5.6(b)
shows the behavior of the rigidity measures λC E
6 (t) and λ6 (t) which ‘jump’ at every
6 seconds (as expected) because of the topology switches. Nevertheless, these topol-
ogy changes (and associated increases/decreases of the control/estimation framework
rigidity) did not negatively affected the overall performance of the proposed bearing
control strategy.

71
Bearing-based localization and control for multiple quadrotors

0.8 3

0.6
2
0.4
eψ

es
0.2 1

0
0
-0.2

-0.4 -1
0 20 40 60 80 0 20 40 60 80
time [s] time [s]
(a) (b)

Figure 5.7 – Results of the experiment. Fig. 5.7(a): behavior of the orientation estimation
error eψ (t). Fig. 5.7(b): behavior of the formation scale error es (t)

5.5 Conclusions
In this chapter we have considered the problem of devising a decentralized control
strategy for controlling a group of quadrotor UAVs able to measure relative bearings
in their own body frames. In particular, we considered two control objectives: (i)
stabilization of the quadrotor formation towards a desired bearing configuration,
and (ii) steering of the whole formation along the motion directions in the null-
space of the bearing rigidity matrix. To this end, a suitable R3 × S1 extension and
combination of the SE(2) directed bearing rigidity control/localization algorithms
introduced in [30, 31] has been developed for the case of non-stationary agents,
together with a full explicit characterization of the null-space of the bearing rigidity
matrix. This allowed to devise a decentralized bearing controller able to meet the
two control objectives without the need of a common reference frame for the agent
group, nor the requirement of reciprocal bearing measurements (i.e., of an undirected
measurement topology). Simulation and experimental results on real quadrotors
have been proposed to illustrate the various features of the approach.
A fully-onboard implementation of the proposed ideas by equipping the UAVs
with onboard cameras for retrieving relative bearings and estimating the quadrotor
body-frame linear/angular velocities (ui , wi ) represents a fundamental future work
of this work and, in general, of this Thesis. By doing this we will be able to free
ourselves from the need of an external motion capture system. Refer to Chapt. 8 for
more details and a future direction regarding this issue. Another logical consequence
of this work is the extension of the ideas presented in [119] for dealing with the
issue of bearing rigidity maintenance in the presence of sensor constraints. Some
of these constraints are the limited field of view and range of the onboard cameras
and the possible occlusions generated by different UAVs. A possible solution to
this problem is given in Chapt. 6 and it would then allow the quadrotor group
to flexibly navigate in cluttered environment under the (controlled) possibility of

72
 5. Formation control and localization in R3 × S1

losing/gaining neighbors while ensuring a minimum level of bearing rigidity for the
formation. Another interesting extension, as we pointed out several times, is the
(decentralized) integration of Structure from Motion (SfM) schemes, such as [146],
able to recover online the missing scale information by processing the measured
bearings and known agent motion (and, thus, avoiding the requirement of a special
agent pair able to additionally measure its inter-distance). An alternative solution
to this problem is presented in Chapt. 7.

73
 Chapter 6
Rigidity maintenance

Contents
6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
6.1.1 Chapter overview . . . . . . . . . . . . . . . . . . . . . . 77
6.2 Cooperative localization from bearing measurements . . . . . . . 78
6.3 A bearing rigidity maintenance strategy . . . . . . . . . . . . . . 78
6.3.1 Design of the inter-agent weights . . . . . . . . . . . . . 79
6.3.2 The bearing rigidity eigenvalue . . . . . . . . . . . . . . 83
6.3.3 The bearing rigidity maintenance controller . . . . . . . 87
6.3.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . 88
6.4 Experimental results . . . . . . . . . . . . . . . . . . . . . . . . . 90
6.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91

N the previous chapters we considered the concept of rigidity and its properties

I with respect to the problems of formation control and estimation when applied
to a group of quadrotor UAVs. In this chapter of the Thesis we are concerned
with a distributed technique which is able to maintain rigidity while moving a
formation of quadrotor UAVs. We asked ourselves the question: what are the
challenges to maintain bearing rigidity in a real-world scenario? The answer to this
question is strictly related to the aforementioned concepts of bearing rigidity and
therefore to the sensors used to retrieve the bearing measurements (e.g., monocular
cameras) and to their limitations. Nevertheless, in this chapter we present the details
of a strategy we adopted to maintain bearing rigidity of a formation of quadrotor
UAVs. The goal is to maintain the rigidity during motion despite the presence of
several sensing constraints. This strategy can be coupled with a formation controller
like the one presented in Chapt. 5. This would allow to have a distributed control
strategy which is able to reach a desired formation and/or steering in the 3D space,

75
Bearing-based localization and control for multiple quadrotors

all this while coping with sensors limitations. The sensing constraints we explicitly
took into account were:

• minimum and maximum range of the camera

• limited field of view of the camera

• possible occlusions caused by the agents of the formation

To this end, a decentralized gradient-based control action was developed, based

on a suitable degree of infinitesimal rigidity linked to the spectral properties of the
bearing rigidity matrix. The approach was first validated in a simulation environment
and then through real experiments employing five quadrotor UAVs.

6.1 Introduction
The use of local sensing (such as onboard cameras or range sensors), however, entails
many challenges. For instance, in the absence of centralized aids, each robot is
typically only able to collect measurements and impose control actions in its local
body-frame: when exchanging information over communication, the group then faces
the need of, e.g., agreeing over some common shared frame where to express any
quantity of interest. Furthermore, local (onboard) sensing also forces to cope with
any sensing limitation (such as limited range/field of view, or occluded visibility)
that can prevent retrieving the needed measurements with respect to neighboring
robots.
While a number of rigidity-based formation control/localization schemes has
been proposed over the years, to the best of our knowledge only a few previous
works (e.g., [31]) have considered the issue of maintaining/preserving rigidity of
the formation (which is a global property like graph connectivity [114, 161]) during
the agent motion despite the possible presence of sensing constraints. Rigidity
maintenance is a fundamental problem since, as explained, losing formation rigidity
prevents convergence of any formation control/localization scheme run by the robot
group.
In this respect, in this chapter we present a work which considers the problem of
bearing rigidity maintenance for a robot group equipped with onboard monocular
cameras able to measure relative bearings with respect to other robots in visibility.
Indeed, cameras are a widespread sensor modality for mobile robots, and the
problem of coordinating the motion/formation of a robot group from only camera
(bearing) measurements has attracted large attention in the robotics and control
community [77, 120–122, 125, 126, 162]. Cameras, however, also suffer from all the

76
 6. Rigidity maintenance

shortcomings listed above: they only provide relative (and unscaled) measurements
in the local body-frame of the measuring agent, and are affected by several sensing
constraints such as limited field of view, limited range, and occluded visibility.
Another big limitation of most of the commercial cameras is the ability to work
only in environments with an appropriate luminosity. Some recent work is being
conducted to solve these problems on quadrotor UAVs [163] by using event-based
cameras (as [164]) but it is outside the scope of this work.
Taking inspiration from [119], in this work we then propose a control strategy
able to ensure maintenance at all times of a minimum level of bearing rigidity for the
robot formation during motion. The controller consists of a decentralized gradient
descent action based on a suitable “degree of bearing rigidity” which is directly related
to the spectral properties of the so-called bearing rigidity matrix already introduced
in Chapt. 4. Furthermore, we assume a directed measurement topology (no need
of reciprocal measurements among neighboring pairs), and explicitly consider three
typical sensing constraints affecting onboard cameras:

(C1): minimum/maximum range,

(C2): limited field of view, and

(C3): possible occluded visibility because of the alignment of multiple robots in the
camera field of view.

Finally, we experimentally validate the proposed machinery with five quadrotor

UAVs navigating in an indoor flying arena. To the best of our knowledge, the
problem of bearing rigidity maintenance and the use of the complex (but quite
realistic) sensing model (C1)–(C3) for camera-based formation control purposes are
two novel contributions of this work.

6.1.1 Chapter overview

The rest of this chapter is organized as follows: Sect. 6.2 quickly recaps the localization
algorithm described in Sect. 5.2.2. Then Sect. 6.3 presents the various details of the
proposed bearing rigidity maintenance strategy together with a discussion on the
issue of decentralization and scalability. Subsequently, Sect. 5.4 reports the results
of our experimental validation with five quadrotor UAVs, and Sect. 6.5 concludes
the chapter and discusses some future directions.

77
Bearing-based localization and control for multiple quadrotors

6.2 Cooperative localization from bearing

measurements

The modeling assumptions of this work are exactly the same of the previous Chapt. 5
and they are specified in Chapt. 4. Moreover, as suggested by (4.10), the implemen-
tation of bearing formation control algorithms for agents evolving in R3 × S1 requires
the availability of the relative orientation iRj and, possibly, distance dij among
neighboring robots (see, e.g., [27, 31, 77]). These quantities, in our context, are not
assumed to be directly measurable. A possible workaround is to then estimate these
(needed) quantities via some bearing-based cooperative localization scheme.

In this respect, in this chapter we use the decentralized localization algorithm for
rigid frameworks in R3 × S1 presented in Sect. 5.2.2. Therefore, letting q̂i = (p̂i , ψ̂i )
represent the estimation of the true agent i configuration qi , we assume, from now
on, that the agent group runs the localization algorithm (of Sect. 5.2.2), and treat q̂i
as a sufficiently good approximation of the true qi (always up to a roto-translation
of the whole framework).

Note that the localization algorithm presented in Sect. 5.2.2 is entirely decentral-
ized and can cope with any time-varying bearing function βG (q(t)) (i.e., the agents
do not need to remain stationary or move in ‘special’ ways) as long as the formation
is infinitesimally bearing rigid. It does, however, require the availability of at least
one inter-agent distance dικ among an arbitrary agent pair (ι, κ) for fixing the
scale of the formation (which, otherwise, would be unobservable only from bearing
measurements). This (possibly single) inter-agent distance can be either considered
as an additional measurement among a special agent pair equipped with a distance
sensor, or it can be estimated online by processing the measured bearings and the
known agent body-frame velocities (ui , wi ) as extensively explained in Chapt. 7
and [146] (thus, without requiring, in this case, any additional sensing capability).

6.3 A bearing rigidity maintenance strategy

The goal of this section is to present an algorithm for bearing rigidity maintenance
able to also take into account some typical limitations of the employed onboard sensors
(cameras), that is, minimum/maximum range, limited field of view, and possible
line-of-sight (visibility) occlusions due to the alignments of multiple robots in the
camera field of view. Our strategy is inspired by the connectivity/(distance) rigidity
maintenance controllers presented in [34, 119, 155] and can be summarized as follows:
the sensing constraints affecting a pair of agents (i, j) are encoded in a suitable scalar

78
 6. Rigidity maintenance

weight 1 wij = wk associated to the edge ek = (i, j), with wk = 0 if the constraint
is not satisfied, and 0 < wk ≤ 1 otherwise2 . Let W = diag(wk I3 ) ∈ R3|E|×3|E| be
a diagonal matrix collecting all the |E| weights wk = wij , and define the weighted
body-frame rigidity matrix as W BG = W BW G T . Any weight wk → 0 will cause
the corresponding k-th row block of W BG , associated to edge ek = (i, j), to
vanish, potentially diminishing the rank of W BG (and, thus, possibly threatening
bearing rigidity of the framework). The agents will then implement a gradient-based
controller able to preserve infinitesimal bearing rigidity of the formation by ensuring
fulfilment of the rank condition rank(W BW G ) = 4N − 5 for the weighted bearing
rigidity matrix. The next sections detail the various steps needed to implement this
gradient control action.

6.3.1 Design of the inter-agent weights

We start addressing the design of the inter-agent weights wij associated to each
edge ek = (i, j) that will encode the sensing constraints among agent pairs. Since,
as explained in Sect. 6.1, in this work we consider the three constraints (C1)
minimum/maximum range, (C2) limited field of view and (C3) occluded visibility.
The weights wij will then be designed as the product of three individual terms

wij = wRij wFij wVij (6.1)

accounting for (C1)–(C3), respectively.

6.3.1.1 Minimum/maximum range

As first constraint, we consider that the employed cameras have a minimum/maxi-

mum range beyond which no detection of neighboring quadrotors is possible (refer
to Fig. 6.1 for a real-world scenario associated to this particular constraint). In
particular dmin , dmax represent respectively the minimum and maximum range of
the camera and, obviously, dmin < dmax . The weight wRij (dij ) is then designed as a
function that smoothly vanishes (with vanishing derivative) when dij → dmin and
dij → dmax , and has a maximum at the midpoint between dmin and dmax . To this
end, we choose the following function



  0  dij < dmin
1 2dij −(dmin +dmax )
wRij (dij ) = 2 + 12 cos π dmax −dmin dmin ≤ dij ≤ dmax (6.2)


 0 dij > dmax
for which a representative shape is shown in Fig. 6.2a.
1
With a small abuse of notation, we use either wk or wij to indicate the weight associated to
an edge ek = (i, j).
2
We also note that the introduction of time-varying weights wij (t) naturally induces a corre-
sponding time-varying edge set E(t) (the set of all edges ek for which wk (t) > 0).

79
Bearing-based localization and control for multiple quadrotors

(a) (b)

Figure 6.1 – Fig. 6.1(a) is a snapshot from the onboard camera mounted on a quadrotor.
It is taken when the robot is close to lose a link (with respect to the robot highlighted in
yellow) due to the maximum range constraint. Fig. 6.1(b) is the graph view corresponding
to the case of of Fig. 6.1(a).

Figure 6.2 – Representative shapes for the weights wRij (dij ) (a), wFij (αij ) (b), g(ηijk ) (c),
and h(δijk ) (d)

6.3.1.2 Limited field of view

A second constraint is related to the limited field of view of the onboard cameras
(refer to Fig. 6.3 for a real-world scenario associated to this particular constraint).
Let oC ∈ S2 be the (constant and known) direction of the camera optical axis in the
quadrotor body-frame and consider the scalar product αij = oTC βij . Clearly, αij = 1
when βij and oC are perfectly aligned, and αij → −1 as the angle between βij and
oC increases. Letting −1 ≤ αmin < 1 represent the camera field of view, we then
design weight wFij as the following function with a maximum for αmax ≤ αij ≤ 1
and smoothly vanishing with vanishing derivative for αij → αmin




 0  αij < αmin
1 α −α
wFij (αij ) = 2 − 12 cos π αmax
ij min
−αmin αmin ≤ αij ≤ αmax (6.3)


 1 αij > αmax

80
 6. Rigidity maintenance

(a) (b)

Figure 6.3 – Fig. 6.3(a) is a snapshot from the onboard camera mounted on a quadrotor. It
is taken when the robot is close to lose a link (with respect to the robot highlighted in yellow)
due to the limited field of view constraint. Fig. 6.3(b) is the graph view corresponding to
the case of of Fig. 6.3(a).

(a) (b)

Figure 6.4 – Fig. 6.4(a) highlights why the occluded visibility constraint is needed. Indeed,
it is possible to notice how the visibility of the robot in red, with respect to the observing
robot, is being occluded by the robot in green. As usual, Fig. 6.4(b) is the graph view
corresponding to the case of Fig. 6.4(a).

A representative shape for the (6.3) is shown in Fig. 6.2b.

6.3.1.3 Occluded visibility

A final constraint considered in this work is that of possible occluded visibility

because of two (or more) quadrotors (close to be) aligned in front of a camera (with,
thus, the closest quadrotor occluding the visibility of the farthest ones). The design
of the weight wVij meant to encode this constraint requires some care and, thus,
we proceed by steps (Fig. 6.4(a) gives the reader a better understanding of this
constraint through a real-world scenario).
Let us first focus on the situation depicted in Fig. 6.5 in which an agent i is
measuring exactly two agents j and k. Our goal is to define a function wVijk that
captures the possible occlusions of agent k (third subscript) on the edge (i, j) (first
two subscripts), and, analogously, a function wVikj capturing the occlusions of j on

81
Bearing-based localization and control for multiple quadrotors

j ijk ⌧0 k
=0 ijk 0
ijk
j k

k j
⌘ijk ⌘ijk
⌘ijk
g(⌘ijk ) 1 g(⌘ijk ) g(⌘ijk ) 1 g(⌘ijk )

i i i
(a) (b) (c)

Figure 6.5 – Scheme which depicts the weights associated to the occluded visibility between
a triple of agents.

the edge (i, k). To this end, let ηijk = βijTβ

ik represent the (cosine of the) angle
between the two bearing measurements of j and k by agent i, and δijk = dik − dij the
relative distance of k and j with respect to i. Note that ηijk = ηikj and δijk = −δikj .
Since, unlike the previous case, we now wish to penalize small angular displace-
ments between j and k with respect to i (which could lead to a possible occlusion),
we introduce the following penalty function



 0  ηijk ≥ ηmax
1 1 ηijk −ηmax
g(ηijk ) =
 2 − 2 cos π ηmax −ηmin ηmin ≤ ηijk ≤ ηmax (6.4)

 1 η ≤η ijk min

where −1 ≤ ηmin < ηmax ≤ 1 are suitable parameters representing the activation/de-
activation of function g(·). Therefore, g(ηijk ) = 0 if agents j and k are close to be
aligned from the vantage point of i (ηijk ≥ ηmax ), while g(ηijk ) = 1 if agents j and
k are far from being aligned (ηijk ≤ ηmin ). Fig. 6.2c shows a representative plot of
function g(ηijk ).
The weight wVijk , associated to the edge (i, j), should be clearly based on the
‘angular penalty’ function g(ηijk ). However, weight wVijk should also take into
account the fact that, regardless of the value of ηijk , agent j can be occluded by
agent k only if agent k is front of j (i.e., if δijk < 0), and symmetrically for the
weight wVikj associated to the edge (k, j). As depicted in Fig. 6.5, a possibility is to
have:

1) wVijk = g(ηijk ) and wVikj = 1 if δijk  0: agent k is in front of j and, thus,

only the edge (i, j) is penalized by g(ηijk ) (Fig. 6.5(a));

82
 6. Rigidity maintenance

2) wVijk = 1 and wVikj = g(ηijk ) if δijk  0: agent j is in front of k and, thus,

only the edge (i, k) is penalized by g(ηijk ) (Fig. 6.5(c));

3) wVijk = wVikj = g(ηijk ) if δijk = 0: agents j and k are at the same distance
from i and, therefore, the two edges (i, j) and (i, k) are equally penalized by
g(ηijk ) (Fig. 6.5(b))

In order to obtain this goal we then introduce a second penalty function




 0  δijk ≤ 0
1 1 δijk −δmax
h(δijk ) = 2 + 2 cos π δmax 0 ≤ δijk ≤ δmax (6.5)


 1 δ ≤δ max ijk

where δmax > 0 is an activation parameter. Fig. 6.2d shows a representative plot of
function h(δijk ). With this choice of h(δijk ), weight wVijk can then be chosen as

wVijk (δijk , ηijk ) = (1 − h(δijk ))g(ηijk ) + h(δijk ), (6.6)

and symmetrically for wVikj (δikj , ηikj ). Because of (6.4–6.5), one can easily verify
that the expression in (6.6) correctly realizes the above-mentioned requirements
1)–3) for weights wVijk and wVikj .
The last step is to generalize this procedure for producing the (cumulative) weight
wVij on the edge (i, j) to be plugged in (6.1). The previous wVijk accounts for the
occlusions on edge (i, j) by a single specific agent k ∈ Ni /{j}. Clearly, if |Ni | < 2
then one should have wVij ≡ 1 (no occlusions are possible if agent i has less than
two neighbors). If, on the other hand, |Ni | ≥ 2 then, in order to take into account
all the possible occlusions on edge (i, j) by any neighbor k ∈ Ni /{j}, one can just
take the product sequence
Y
wVij = wVijk (δijk , ηijk ). (6.7)
k∈Ni /{j}

6.3.2 The bearing rigidity eigenvalue

Having introduced the weighting machinery meant to encode the sensing constraints
considered in this work, we now discuss a suitable measure of bearing rigidity that
is exploited by the rigidity maintenance controller. We first consider the unweighted
case (W = I3|E| ) and then explicitly introduce the weights wij in the design.
As discussed, a framework is infinitesimally bearing rigid if and only if rank(BW
G (q)) =
rank(BG (q)) = 4N − 5. This rank condition can be translated into an equivalent
spectral condition on the eigenvalues of two suitable corresponding matrices. To this
end, define BGW (q) = BW T W T
G (q) B G (q) and BG (q) = B G (q)B G (q) as the 4N × 4N

83
Bearing-based localization and control for multiple quadrotors

world-frame and body-frame (weighted) symmetric rigidity matrices, respectively.

These two matrices are similar 3 since from (4.9) it follows

BG (q) = T T (ψ)BGW (q)T (ψ) (6.8)

and T (ψ) is orthonormal 4 . Letting λi (A) represent the i-th smallest eigenvalue
of a square symmetric matrix A, infinitesimal bearing rigidity then translates into
the condition λ6 (BGW (q)) = λ6 (BG (q)) > 0 (where the similarity between BGW (q)
and BG (q) has been used). Let then λB 6 (q) represent the sixth smallest eigenvalue
of the world/body-frame symmetric rigidity matrix: λB 6 (q) is a natural measure of
infinitesimal rigidity and is denoted from now on as the bearing rigidity eigenvalue.
The purpose of this section is to detail the main properties and explicit expressions
of λB6 (q) and of its gradient with respect to the i-th agent configuration qi , which is
later used in Sect. 6.3.3.
A first observation is formalized by the following Proposition.

Proposition 6.1. The bearing ridigity eigenvalue does not depend on the agent
orientations ψ, i.e., λB B
6 (q) = λ6 (p).

Proof. The proof exploits the definition λB W

6 (q) = λ6 (BG (q)) based on the world-
frame symmetric rigidity matrix. Let p̄ij = pij /dij and P̄ij = In − p̄ij p̄Tij . Exploit-
ing (4.2) and the identity SRi = Ri S, it follows that

Pij RiT = (IN − βij βij

T
)RiT = RiT P̄ij , (6.9)
Sβij = RiT S p̄ij (6.10)

The k-th row block of BW G (q) in (4.7) associated to an edge ek = (i, j) can then be
factorized as
 
P̄ij P̄ij
T  −0− − d −0−
dij
−0− −S p̄ij −0−  T
Ri (ψi )  ij | {z }  = Ri (ψi )B̄k (pij ),
| {z } |{z} 3N +i
i j

(6.11)

where B̄k (pij ) ∈ R3×4N does not depend on ψ. If we define

B̄ = stack(B̄1 , . . . , B̄k ) ∈ R3|E|×4N , (6.12)

3
Two matrixes A, B ∈ Rn×n are similar if there exists a matrix P ∈ Rn×n such that B =
−1
P AP . One of the (several) properties of similar matrixes which is exploited in the math of
this chapter is that the two matrixes have the same eigenvalues (but, in general, not the same
eigenvectors).
4
An orthonormal matrix A ∈ Rn×n is such that AT A = AAT = In and equivalently AT = A−1 .

84
 6. Rigidity maintenance

it follows that the world-frame symmetric bearing rigidity matrix can be actually
rewritten as
BGW (q) = BW T W T
G (q) B G (q) = B̄(p) B̄(p). (6.13)

This shows that BGW (q) = BGW (p) and, as a consequence, that λB B
6 (q) = λ6 (p), thus
concluding the proof.

We now provide two explicit expressions of λB 6 (p) that is instrumental in the

4N
following developments. Let v6 ∈ R represent the world-frame normalized eigen-
vector associated to λB
6 , i.e., such that (using (6.13))

T W
λB T T
6 = v6 BG v6 = v6 B̄ B̄v6 (6.14)

Consider the partition v6 = [vpT1 . . . vpTN vψ1 . . . vψN ]T , where vpi ∈ R3 and vψi ∈ R
are the eigenvector components associated to the position and orientation of agent
i. By exploiting (6.11), the properties P̄ij P̄ij = P̄ij and P̄ij Spij = Spij (see the
proof of Prop. 6.1 and [27]), it is possible to obtain the following expression for λB 6
(applying (6.14))
!
X P̄ij p̄ ij
λB
6 = vpTij 2 vpij + 2vpTij S vψ − vψ2 i p̄Tij S 2 p̄ij (6.15)
dij dij i
(i, j)∈E

where vpij = vpi − vpj .

The expression (6.15) obviously depends on world-frame quantities. An equiva-
lent expression in terms of body-frame quantities can be obtained as follows (look
at Appendix A.1 for more details): let ν6 represent the body-frame normalized
eigenvector, i.e., such that λB T
6 = ν6 BG ν6 . From (6.8) it follows that v6 = T ν6 and,
therefore, vpi = Ri νpi and vψi = νψi . Since P̄ij = Ri Pij RiT and p̄ij = Ri βij (see
Prop. 6.1 and (4.2)), the expression (6.15) can be reformulated as
!
T Pij βij
X X
B T 2 T 2
λ6 = νpij 2 νpij + 2νpij S νψi − νψi βij S βij = lij (6.16)
dij dij
(i, j)∈E (i, j)∈E

with νpij = νpi − iRj νpj . Each term lij in (6.16) is now rewritten in terms of only
body-frame quantities relative to agents i and j (measured bearings βij , interdistances
dij , relative orientations iRj and relative components of the body-frame rigidity
eigenvector ν6 ). Note that, in general, lij 6= lji . Furthermore, by looking at
the (world-frame) expression of lij in (6.15), it follows that ∂lij /∂ψk = 0, ∀k (in
accordance with Prop. 6.1).
Let us now introduce the weights wij into the rigidity eigenvalue λB
6 and proceed
to obtain a closed-form expression for its gradient. As explained at the beginning
of Sect. 6.3, the weights wij can be included by replacing the body-frame symmetric

85
Bearing-based localization and control for multiple quadrotors

bearing rigidity matrix with a weighted counterpart BTG (q)W BG (q) (and analogously
for the world-frame symmetric bearing rigidity matrix B W ). Since W is a diagonal
matrix, repeating the previous steps simply results into

X
λB
6 = wij lij (6.17)
(i, j)∈E

in place of (6.16). Expression (6.17) can be leveraged for obtaining the gradient
of λB6 with respect to the configuration qυ = (pυ , ψυ ) of a specific agent υ in the
group. Recalling the definition of set Oi in (3.2), we first note that the only terms
lij that depend on pυ are those associated to the edges (υ, j), ∀j ∈ Nυ and (j, υ),
∀j ∈ Oυ . The computation of the gradient of these terms with respect to pυ is
based on the general formula for expressing the derivative of an eigenvalue with
respect to a parameter (see, e.g., [155]) dλ6 = ν6T dBG ν6 (for more details refer
to Appendix A.2). Therefore, it can be shown, after some (tedious) steps, that

1
∇pυ lυj = 2Rυ (βυj νpTυj + νpTυj βυj I3 )Pυj νpυj +
d3υj
! (6.18)
νψ T
νψ2 υ 2
+ 2 υ (Pυj − βυj βυj )Sνpυj + Pυj S βυj = 2Rυ Lυj
dυj dυj

and ∇pυ ljυ = −2Rj Ljυ .

The gradient of weights wij is, however, more involved since the weight design
introduces additional dependencies which must be correctly taken into account. Some
details about the gradient of the weights are given in Appendix A.3. Consider again
the configuration qυ = (pυ , ψυ ) of a specific agent υ and the weight partition (6.1):
clearly, the three subweights wRij in (6.2), wFij in (6.3) and wVij in (6.7) depend on
pυ over the same set of edges (υ, j), ∀j ∈ Nυ and (j, υ), ∀j ∈ Oυ as for the terms
lij . However, because of its definition, and unlike the terms lij , the third subweight
wVij depends on pυ also over all the edges (j, m), ∀j ∈ Oυ , ∀m ∈ Nj . Furthermore,
weight wFij depends on ψυ over all the edges (υ, j), ∀j ∈ Nυ , while weights wRij
and wVij do not depend on ψυ (wRij and wVij are functions of interdistances, and
wVij is also function of the angular quantity ηijk which, however, does not depend
on ψ).

Therefore, after some (again tedious) algebra, and by exploiting (6.18), the

86
 6. Rigidity maintenance

gradient of λB
6 with respect to qυ can be expanded as follows

 
B

 ∇ p λ = Rυ (lυj ∇pυ wυj + 2Lυj wυj )+
 υ 6


 j∈N υ

  

  
+ −2Rj wjυ Ljυ + Rm ljm ∇pυ wjm  . (6.19)



 j∈Oυ m∈Nj

 

 B
∇ ψυ λ6 =
 lυj ∇ψυ wυj
j∈Nυ

6.3.3 The bearing rigidity maintenance controller

Having obtained an explicit expression for the gradient ∇qυ λB 6 , we can now present
the proposed bearing rigidity maintenance controller. Similarly to [34, 119], we
assume that the agents need to maintain a minimum level of bearing rigidity during
motion, i.e., guarantee that λB6 (t) > λ6
min > 0 where λmin is a suitable lower threshold
6
depending on the particular application. We then introduce a potential function
Vλ (λB B B
6 ) ≥ 0 such that Vλ (λ6 ) → ∞ as λ6 → λ6
min and V (λB ) → 0 (with vanishing
λ 6
derivative) as λB6 → λ max > λmin . In particular the expression of V (λB ) is the
6 6 λ 6
following


 0 0 ≤ λB6 ≤ λ6
min
 2
Vλ (λB
6)= KV tan aλB 6 +b λmin
6 ≤ λB6 ≤ λ6
max (6.20)


0 max
λ6 ≤ λ6 B

where
0.5π 0.5πλmax
6
a= , b = − . (6.21)
λmax
6 − λ min
6 λ max − λmin
6 6

Fig. 6.6 shows a possible shape for the potential function Vλ (λB
6 ).

Figure 6.6 – A representative shape for the function Vλ (λB min

6 ) with λ6 = 0.01 and λmax
6 = 0.42

87
Bearing-based localization and control for multiple quadrotors

Each agent υ will then follow the anti-gradient of Vλ (λB 6 ) with respect to qυ
which, using (4.1), yields the following body-frame velocity commands

 uυ = −RυT ∇pυ Vλ (λB 6)
(6.22)
 w = −∇ V (λB )
υ ψυ λ 6

By plugging (6.19) in (6.22), we then obtain

 

 ∂V λ 
X
u = − (lυj ∇pυ wυj + 2Lυj wυj )+

υ

∂λB


6



 j∈N υ

  

Rm ljm ∇pυ wjm  . (6.23)
X X
+ −2υRj 2wjυ Ljυ + υ



 j∈Oυ m∈Nj

∂Vλ

 X
w =− B lυj ∇ψυ wυj


 υ

 ∂λ6 j∈N
υ

6.3.4 Discussion
We conclude with some remarks about the bearing rigidity maintenance con-
troller (6.23). Let us first consider the issue of decentralization and scalability:
by analyzing the explicit expressions of the various terms in (6.23) (given in the
previous sections), it is possible to conclude that, in order to implement the rigidity
maintenance action, agent υ needs knowledge of λB B
6 (for evaluating ∂Vλ /∂λ6 ), of
(νpυ , νψυ ), and of

1) βυj , dυj , ψυj , νpj , νψj , ∀j ∈ Nυ ;

2) βjυ , djυ , ψjυ , νpj , νψj , ∀j ∈ Oυ ;

3) βjm , djm , ψjm , νpm , νψm , ∀j ∈ Oυ , ∀m ∈ Nj /{υ}.

Let us assume, for now, that each agent has access to the value of λB 6 and to its own
components of ν6 : one can then verify that 1)–3) consist of (measured) bearings,
interdistances, relative orientations, and components of the eigenvector ν6 , i.e., all
quantities available to the robot group. In particular, the quantities in items 1)–2) are
either locally available to agent υ, or can be computed5 by exploiting communication
with its 1-hop (communication) neighbors j ∈ Nυ (the agents measured by υ) and
j ∈ Oυ (the agents measuring υ). The quantities in item 3) (which are ultimately
due to weights wVij in (6.7)) are instead relative to the 1-hop neighbors m ∈ Nj /{υ}
of any agent j ∈ Oυ . Therefore, agent υ needs to receive this information from some
5
We recall that, as discussed in Sect. 6.2, the interdistances dij and relative orientations ψij
can be computed by any neighboring pair by exchanging the local estimates p̂i , p̂j , ψ̂i , ψ̂j .

88
 6. Rigidity maintenance

of its 2-hop communication neighbors. In any case, since the amount of information
in 1)–3) is constant per neighbor, an upper bound of the communication complexity
for an agent υ is O(|Nυ | + |Oυ | · maxj∈Oυ |Nj |). We can then conclude that the
rigidity maintenance controller (6.23) admits a decentralized implementation by,
however, assuming a 2-hop communication model.
A second remark is about the availability, for each agent υ, of λB 6 (a global
quantity) and of its own eigenvector components (νpυ , νψυ ): although these quantities
are not directly measurable, it is in principle conceivable to estimate them in a
decentralized way by adapting the estimators presented in [34,155] for the connectivity
case, and ported in [119] to the (distance) rigidity case. Indeed, these methods
essentially require an explicit characterization of the null-space of the bearing rigidity
matrix (which is well understood, see [27]), the use of some PI consensus filters, and
the suitable exploitation of the structure of the symmetric bearing rigidity matrix.
The estimation of λB 6 and of (νpυ , νψυ ) is, however, left for future extensions of this
work and in the following results we just assume availability of these quantities.
A final remark is about the well-posedness of controller (6.23): as well-known,
the derivative of an eigenvalue is not well-defined for multiplicities larger than one
(repeated eigenvalues), since in these cases one cannot reliably find/estimate a unique
eigenvector to be plugged in the derivative computation [165]. This difficulty, which
affects any method based on the optimization of a single eigenvalue associated to
some matrix of interest, has already been recognized in [34,119,155] without, however,
proposing an explicit solution to deal with it. A possible workaround is to replace
the eigenvalue to be optimized with a ‘smoothed’ version [166] which is well-behaved
for multiplicities larger than
q one. Following [146], a possibility is to replace λB6 with
the quantity λ̄B = 4N (λB p B
P p
i=6 i ) where λi is the i-th eigenvalue of the symmetric
bearing rigidity matrix, and p  0. Indeed, when λB B
6  λ7 one has λ̄ ≈ λ6
B B
√
while when, instead, λB B B B
6 ≈ λ7 ≈ λm  λm+1 it is λ̄ ≈
B p
m − 6λB 6 . Therefore,
B B
maximization of λ̄ results into maximization of λ6 with however the advantage
of λ̄B being always differentiable. By evaluating Vλ (·) on λ̄B , the controller (6.22)
then becomes
  p−1
∂Vλ T X4N λB
u = − R i
∇pυ λB


 υ

∂ λ̄B υ i=6 λ̄B i

 B p−1 . (6.24)
 ∂V 4N λ
 wυ = − λ
X
i
∇ψυ λB


i
∂ λ̄B i=6 λ̄B

Evaluation of (6.24) by an agent υ would require the same quantities listed in 1)–3)
and, in order to evaluate ∂Vλ /∂ λ̄B and ∇qυ λB i , the additional availability of λi
B

and of (νpi υ , νψi υ ) (the υ-th components of the eigenvector ν i associated to λi ) for
i = 6 . . . 4N . Therefore any implementation of (6.24) would be decentralized but

89
Bearing-based localization and control for multiple quadrotors

not scalable, as the amount of information per agent would increase with the size of
the whole agent group.

6.4 Experimental results

We employed a group of five quadrotors of the ones described in Sect. B.2. The
TeleKyb framework [159] was used for implementing the low-level flight control
receiving the body-frame velocity commands (6.24), and for exchanging data via
Wi-Fi with the other robots in the group and the ground station. Each quadrotor
was also equipped with an onboard camera, refer to Fig. B.5(b). The UAVs were
flying in an indoor room equipped with the Vicon motion capture system (Fig. 6.7),
refer to Sect. B.2 for more details. This was used for obtaining the ground truth and
for reconstructing the body-frame bearing measurements βij that would have been
obtained by the onboard cameras. Indeed, as others [105], we found the problem
of detecting and tracking (in a reliable way) each quadrotor during flight to be
non-trivial, and then exploited the onboard cameras only for the sake of verifying
that, during motion, all the neighboring pairs remained indeed well visible in the
corresponding camera field of view as predicted by the proposed machinery.
The on-board low-level controller ensures that the orientation of the UAV tracks
the desired reference. As usual, this is accomplished by neglecting the couplings
among the three body axes, and by treating each individual rotation (roll, pitch, yaw)
as a separate channel modeled as a double integrator with input the correspondent
body torque. The adopted controller is then a simple PID with saturated integral
term in order to prevent wind-up issues. Readings from the on-board gyros are
exploited as velocity feedback, while a complementary filter provides estimates of
the UAV attitude by fusing together accelerometer and gyro readings from the IMU.
The experimental results are shown in Figs. 6.8 and 6.9: at the beginning the
UAVs just followed the control action (6.24) and, as a consequence, the value of
λ̄B (t) was maximized (until the vertical dashed line in Fig. 6.8(a)). Then two human
operators started acting on two UAVs by sending two velocity commands added
to (6.24) with the aim of triggering as much as possible the loss/gain of neighbors
because of the considered sensing constraints. One can then note how presence
of these two additional velocity inputs (which are representative of any external
planner/algorithm in charge of steering the robot formation in the environment) did
not threaten bearing rigidity of the formation thanks to the maintenance action (6.24)
(indeed, as shown in Fig. 6.8(a), λ̄(t) remains larger than the chosen threshold
λ̄min = 0.01). This is also confirmed in Fig. 6.9(a) where the evolution of λB 6 (t) for
the unweighted framework is reported. Finally, Fig. 6.8(b) shows the number of
edges |E| over time, thus confirming the (intentionally induced) time-varying nature

90
 6. Rigidity maintenance

Figure 6.7 – A group of five quadrotor UAVs flying in our flying arena at INRIA Rennes,
Bretagne Atlantique, France

(a) (b)

Figure 6.8 – 6.8(a): behavior of the p-norm λ̄, of the rigidity eigenvalue λB
6 6.8(b): the
number of edges |E| of the considered framework.

of the sensing topology during motion, and Fig. 6.9(b) reports two of the graph
topologies encountered during the experiment. The formation also goes through a
rearranging process of its topology (Fig. 6.8(b)) by virtue of the always continuous
evolution of the weights associated to its edges (Fig. 6.10).

6.5 Conclusions
In this chapter we illustrated a decentralized gradient-based controller able to enforce
bearing rigidity maintenance for a group of quadrotor UAVs while coping with three
typical sensing constraints of onboard cameras: minimum/maximum range, limited
field of view, and possible occluded visibility. The proposed control action exploited a

91
Bearing-based localization and control for multiple quadrotors

(a) (b)

Figure 6.9 – 6.9(a): behavior of the unweighted rigidity eigenvalue λB

6 , 6.9(b): two graph
topologies corresponding to the maximum (left, |E| = 17) and minimum (right, |E| = 14)
number of edges during the experiment.

Figure 6.10 – Behavior of the weights over the edges in E during the reported experiment.
The reader should notice the continuous nature of the weights

closed-form expression of λB 6 , the sixth smallest eigenvalue of the symmetric bearing

rigidity matrix, and of its gradient with respect to the agent configuration. Moreover,
a proof of independence of the the bearing rigidity eigenvalue with respect to the
agent orientations ψ was given. To the best of our knowledge, this represents the first
work addressing the problem of bearing rigidity maintenance for a formation of UAVs
by also explicitly taking into account (complex) sensing constraints. The reported
experimental results showed the effectiveness of the proposed control strategy.
Several extensions of this work are possible: as pointed out in the previous
chapter, a fully onboard implementation is still missing (refer to Chapt. 8 for more
details). Furthermore, we would like to address the open points listed in Sect. 6.3.4,
that is, decentralized estimation of the components of the body-frame eigenvector
ν6 , and the scalability issue of exploiting the p-norm λ̄ in place of λB
6 . Finally, we
are also considering the possibility of extending the proposed weighting machinery
for including a collision avoidance action among quadrotors (which is now not
guaranteed), similarly to what done in [119].

92
 Chapter 7
Nonlinear observability and
estimation for multi-agent systems

Contents
7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
7.1.1 Prior work . . . . . . . . . . . . . . . . . . . . . . . . . . 94
7.1.2 Main contributions . . . . . . . . . . . . . . . . . . . . . 96
7.1.3 Chapter overview . . . . . . . . . . . . . . . . . . . . . . 97
7.2 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97
7.2.1 General notation . . . . . . . . . . . . . . . . . . . . . . 97
7.2.2 Formation, agent and measurement model . . . . . . . . 98
7.2.3 Elements of Riemannian geometry . . . . . . . . . . . . 99
7.2.4 Elements of local nonlinear observability . . . . . . . . . 101
7.3 Dynamic Bearing Observability Matrix . . . . . . . . . . . . . . 102
7.3.1 Matrix R̃A . . . . . . . . . . . . . . . . . . . . . . . . . 103
7.3.2 Matrix R̃B . . . . . . . . . . . . . . . . . . . . . . . . . 105
7.3.3 Numerical verification of the ranks of R̃A and R̃ . . . . 109
7.4 A multi-agent Extended Kalman Filter . . . . . . . . . . . . . . 110
7.5 Experimental results . . . . . . . . . . . . . . . . . . . . . . . . . 112
7.6 Conclusions and future works . . . . . . . . . . . . . . . . . . . . 113

n this chapter we still consider the problem of localization in multi-agent forma-

I tions with bearing only measurements but specifically analyze the fundamental
observability properties for dynamic agents. The current well-established ap-
proach is based on the so-called rigidity matrix, and its algebraic properties (e.g., its
rank and nullspace). This method is typically motivated using first-order derivatives,
and shows, among other facts, that the global scale of the formation is not observable.

93
Bearing-based localization and control for multiple quadrotors

This chapter shows that current results represent an incomplete view of the problem.
In particular, we show that

• current methods are a particular instantiation of nonlinear observability theory

• we can introduce the concept of the dynamic bearing observability matrix from
higher order derivatives to study the observability of dynamic formations

• the global scale is, in fact, generally observable when the agents move according
to known inputs.

We use tools from Riemannian geometry and Lie group theory to tackle, in a
general and principled way, the general formulation of the localization problem with
states that include both rotations and translations. Finally, we verify our theoretical
results by deriving and applying, in both simulations and real experiments on UAVs,
a centralized Extended Kalman Filter on Lie groups that is able to estimate the
global scale of a moving formation.

7.1 Introduction
As already discussed in Chapt. 5 and Sect. 2.5, a fundamental problem in the multi-
robot applications is the one of localization.1 This problem becomes of paramount
importance, for instance, when a team of robots has to navigate autonomously in an
unknown environment, or needs to collaborate on a physical task (e.g., transporting a
load [57,167,168]). In this case, the robots need a precise localization with respect to
other robots of the formation. In this chapter, as in the whole Thesis, we consider the
case of bearing measurements coming from onboard calibrated monocular cameras.
Our goal is to show that, despite the very limited information provided by this
type of measurements (direction of relative translations alone), it is possible to
reconstruct the full 3-D pose (rotation and translation) of the agents, including the
global scale, up to a global gauge ambiguity. We demonstrate this 1) theoretically
through an application of nonlinear observability analysis and Riemannian geometry,
and 2) practically by applying an Extended Kalman Filter in both simulations and
experiments.

7.1.1 Prior work

The problem of localization from bearing-only measurements has appeared in a
variety of domains, such as (to cite a few) sensor network localization [88,124,169] and
1
Remember that in this Thesis with localization we refer to the process of determining the
configuration of the agents in a common reference frame from a sparse set of relative measurements
between them, and without the aid of an external centralized system (such as Vicon/GNSS).

94
 7. Nonlinear observability and estimation for multi-agent systems

formation control [170–172] in controls and robotics, Structure from Motion [92, 93]
in computer vision, and graph drawings [94] in discrete mathematics. Most of the
literature has focused on the development of distributed algorithms (especially in
the sensor network and robotics communities), but centralized solutions have also
been considered (mostly in the computer vision community). In this chapter, rather
than specific algorithms, we are interested in analyzing the fundamental aspects of
the localization problem. In this direction, for our case of interest involving bearing-
only measurement, there has been a considerable amount of work for developing a
theory of rigidity [27, 28, 31, 72, 126, 126], which can predict what information can be
recovered from the available measurements (i.e., whether the solution is “unique”).
While most of these works considered only agents in 2-D, recent work has also
studied the 3-D case [173]. The commonly accepted result is that when the number
and connectivity of the measurement graph is sufficiently high (that is, when the
graph is rigid ), then, for static agents, the solution to the bearing-only localization
problem is unique up to a rototranslation and a contraction/expansion of the whole
formation. This is determined by considering the nullspace of a so-called rigidity
matrix. Most of the existing works, however, do not explicitly consider the case of
dynamic agents. At a high level, one could expect that if the agents know their own
velocities in their own local frames (e.g., because they control them, or measure
them using the onboard IMU), then they could use this metric information to avoid
the scale ambiguity. Recent works [146] have pursued this idea, but do not provide
a full, rigorous analysis rooted in nonlinear observability analysis and Riemannian
geometry. One disadvantage of [146] is the presence of two estimators in series
for estimating the scale of the formation (one filter estimates the distances over a
selection of edges, and the second filter recovers a correctly scaled estimation of the
formation configuration). Stability of this cascaded structure is difficult to prove
(indeed, nothing is said in [146]), while our algorithm achieves the same result with
only one single EKF. On the other hand, [146] is able to determine the optimal
motion for the agents in order to maximize the observability of the scale factor. We
plan to exploit the ideas in [146] for a similar characterization in the context of our
EKF estimation. Nonetheless, ideas related to this approach have been successfully
explored in the context of single-agent Simultaneous Localization and Mapping
(SLAM) [144, 174] and localization from distance measurements [175].

In this regard, nonlinear observability (the problem of determining if the state of a

nonlinear dynamical system can be reconstructed by knowing its inputs and outputs)
is a classical topic in automatic controls [102, 154, 176], and it now constitutes
textbook material [177]. However, it has never been explicitly applied to the problem
of localization from bearing-only measurements. On the other hand, Riemannian
geometry has been applied in the context of geometric control and estimation of

95
Bearing-based localization and control for multiple quadrotors

mechanical systems in general [178, 179], and quadrotors in particular [167, 180, 181].
A Riemannian geometry formulation has also been used for multiagent localization
with unscaled relative poses [182], but it has never been applied together with
nonlinear observability analysis to the bearing-only case.
Finally, in this work, we propose a validation of our theoretical results using
an Extended Kalman Filter (EKF) for statistical filtering of states evolving on
Riemannian manifolds. The main advantage of the EKF formulation is that it
is relatively easy to derive; in fact, both centralized [183] and decentralized [184]
implementations have been proposed specifically for multiagent systems, although
without considering states evolving on Riemannian manifolds. On the other hand, it
is known that the EKF is not optimal for nonlinear systems. Developing filtering
techniques with optimality guarantees on Riemannian manifolds (and Lie groups in
particular) is still an active field of research [185, 186]. Since the goal of this work is
merely to use filtering as a validation of the theoretical derivations, we opted for a
straight (although suboptimal), centralized (as opposed to distributed) application
of the EKF, albeit with the explicit consideration of the Riemannian geometry of
the states.

7.1.2 Main contributions

In this work, we make several contributions to the state of the art

• we show how the study of rigidity is, in fact, a particular instance of classical
nonlinear observability analysis;

• using this insight, we propose the notion of dynamic bearing observability

matrix (DBOM), which extends the standard notion of rigidity matrix for the
case of moving agents with known inputs;

• by (numerically) analyzing the rank of the DBOM, we show that the global
scale of the formation is generally observable;

• we show how tools from Riemannian geometry can be employed to carry out
the observability analysis for states evolving in the space of rigid body motions
SE(3);

• we derive and apply a centralized Extended Kalman Filter (EKF) on Rieman-

nian manifolds that empirically verifies the theory (i.e., that shows that the
global scale can be indeed recovered).

Overall, we show how bearing-only measurements (which, taken individually, do

not contain any scale information) and local linear and angular velocity information

96
 7. Nonlinear observability and estimation for multi-agent systems

can be used to recover the entire state of the agents. The recovered state is
up to a common rotation and translation (since all measurements are relative
and do not have any relation to external reference systems, this last ambiguity
appears to be unavoidable, even with dynamic agents). Moreover, by explicitly
using the Riemannian geometry of the space of poses (which is based, among
other elements, on the use of rotations matrices) throughout the chapter (both
for the observability analysis and the statistical filter), we avoid the problems
given by other representations (e.g., the singularities of Euler angles, and the non-
uniqueness of quaternions). Finally, in this work, we do not perform a full, analytical
characterization of the nullspace of the DBOM, and we do not consider distributed
filtering solutions. However, these are interesting future directions that are enabled
by the present work.

7.1.3 Chapter overview

This chapter is organized as follows. Sect. 7.2 reviews notions from several areas
that are necessary to carry out our analysis. In Sect. 7.3 we introduce the novel
concept of Dynamic Bearing Observability Matrix. Sect. 7.4 illustrates our EKF
design. Finally, Sect. 7.5 reports experimental results with a group of quadrotor
UAVs, followed by Sect. 7.6 that concludes the chapter and gives possible future
directions.

7.2 Preliminaries

7.2.1 General notation

The localization problem was solved in the previous Chapts. 5 and 6 through the
estimator described in Sect. 5.2.2 and by assuming that at least one distance was
known (either measured or estimated through [146]). As shown in Sects. 5.3, 5.4
and 6.4 this solution works well, but something better can be done if we think of
employing only one EKF which would estimate all the poses of the agents of the
formation with the right scale and not the cascade of two estimators like in [146].
To design this EKF, as we said in the introduction of this chapter, we conducted
an observability analysis which shows that the poses of the agents (with the right
scale) are actually observable. This will then motivate the convergence of the
designed EKF. We decided to conduct the observability analysis directly on the
Lie group SE(3). This was done for the sake of completeness, to avoid singularity
problems related to the orientation and to use a representation of the orientation
which is also global. Moreover, this will allow us to extend this analysis to complete
models of the quadrotors (also with their full dynamics) and/or to other robots too.

97
Bearing-based localization and control for multiple quadrotors

Note that the representation of the pose of the quadrotor as an element of SE(3) is
done both for the observability analysis and for the localization while the control
can still be done through the model detailed in Sect. 4.2 once the localization phase
is over. Then, as usual, let W represent an absolute 3-D world reference frame, and
Ai represent a body reference frame attached to the i-th agent.

7.2.2 Formation, agent and measurement model

This chapter shares the same formation model in terms of graph theory of the
previous Chapts. 5 and 6 which is described in Sect. 4.3. But, as just said, in this
chapter there is a different modeling assumption with respect to Chapt. 4. Indeed,
here we represent the orientation of the single agents in a different way. More
specifically, we model the state of an agent i ∈ V as a pose qi = (pi , Ri ), where
pi ∈ R3 represents the translation of the origin of Ai expressed in W, and Ri ∈ SO(3)
represents the rotation transforming directions from Ai to W. Note that here Ri is
a function of the full orientation of the rigid body and not only of the yaw angle ψi
as in Sect. 4.2. We denote the space of rigid poses as SE(3) (the detailed definition
of SO(3) and its geometry is postponed to Sect. 7.2.3). Let us denote with {em }3m=1
the standard R3 basis. We also let 1N and IN represent a vector of all ones and
the identity matrix of dimension N , respectively. The operator stack(·) returns a
matrix containing a vertical stacking of the arguments. We assume a simple first
order model for the 6-D dynamics of each agent
   
q̇i = ṗi , Ṙi = Ri vi , Ri ŵi = (7.1)
3 
X  3 
X 
= Ri ek , 0 vik + 0, Ri êk wik , (7.2)
k=1 k=1

where vi , wi ∈ R3 represent, respectively, the linear and angular velocities expressed

in Ai , and vik , wik represent their components along the ek basis vector. We use
this model for generality, but the results of this work could be easily specialized to
other cases (e.g., considering only the positions of the agents, or just the 2-D yaw
angle, as done in the majority of previous works).
In this work, as in the previous chapters, we assume that each robot is equipped
with a sensor (onboard calibrated camera) that allows it to measure the relative
bearing vector (4.2) where the Ri is now the full rotation matrix.
As in [27,30,31,146], we assume that we have only available the inputs {vi , wi }i∈V ,
and the measurements {βij }(i,j)∈E . In particular, we do not have access neither
to the absolute states qi , nor to the global reference frame W. Throughout the
nonlinear observability analysis we will refer to the different components of the

98
 7. Nonlinear observability and estimation for multi-agent systems

vector βij as:

βijm = eTm βij ∈ R, m ∈ {1, 2, 3}. (7.3)

Remark 7.1. While we will individually consider each one of the three elements of
each bearing βij as a separate output, in reality, the fact that βij ∈ S2 implies that,
in general, only two outputs are algebraically independent. The effect of this is that
the bearing rigidity matrix that we will derive will contain more rows than strictly
needed (i.e., some rows will be automatically linearly dependent). However, this does
not change the result of the rank-based observability test.

7.2.3 Elements of Riemannian geometry

This section covers the basic Riemannian geometry notions that are used in the
derivations below. We will be mostly concerned with three manifolds: the Euclidean
space R3 , the space of 3-D rotations SO(3) = {R ∈ R3×3 : RT R = I, det(R) = 1},
the space of 3-D poses SE(3) = {(p, R) : R ∈ SO(3), p ∈ R3 }, and the space of N
3-D poses SE(3)N . These manifold are in fact Lie groups, but we will not make use
of this fact.

Tangent spaces We denote as Tx M the tangent space of a manifold M at a

point x ∈ M. The tangent space at a point can be identified as the vector space
spanned by the tangents of the curves passing through that point; for instance, if
R(t) : I → SO(3) is a parametrized curve in SO(3) defined on some interval I ⊂ R
around zero, then Ṙ(0) ∈ TR(0) SO(3). For R3 , the tangent space at each point can
be identified with R3 itself. For SO(3) however, we first need to define the usual hat
(·)∧ and vee (·)∨ operators between R3 and the set of skew-symmetric matrices in
R3×3 as follows, with v = [v1 , v2 , v3 ]T :
 
0 −v3 v2

∨
v R∧ = R  v3 0 −v1  = Rv̂, V R∨ = RT V (7.4)
 

−v2 v1 0

where V is any vector V ∈ TR SO(3). Note that, if R is not present in the superscript,
or if the R = I, the definitions of hat and vee operators are the classical ones present
in the literature. It can be shown that the tangent space of SO(3) is given by

TR SO(3) = {Rv̂ : v ∈ R3 }. (7.5)

We define a basis for TR SO(3) as {eR∨ 3

m }m=1 ; it follows that any vector V ∈ TR SO(3)
can be expressed as a vector v ∈ R3 of local coordinates in this basis with the relation
v = V R∨ . For instance, for the curve R(t) defined above, letting R0 = R(0) we will
have Ṙ(0) = wR0 ∧ for some w ∈ R3 , or, equivalently, w = Ṙ(0)R0 ∨ . In the case

99
Bearing-based localization and control for multiple quadrotors

where R(t) represents a physical time-varying rotation, using the convention given
in Sec. 7.2.2, the vector w coincides with a vector of angular velocities expressed in
the body frame (see also (7.1)). The tangent space of Tq SE(3) can be identified with
the direct sum R3 ⊕ TR SO(3) (i.e., a tangent for SE(3) is simply a tangent for R3
together with a tangent for SO(3)). Similarly, the tangent space of SE(3)N is simply
the direct sum of N copies of Tq SE(3). A representation in local coordinates of a
vector Tq SE(3) can be obtained by stacking the local coordinate representation of
each rotational component (as discussed above), with the translational components.

Riemannian metrics A Riemannian metric h, i smoothly assigns an inner product

to each tangent space. The standard Riemannian metric for R3 is the usual inner
product. The standard Riemannian metric for SO(3) is defined as

1
hRv̂1 , Rv̂2 i = tr(v̂1T v̂2 ) = v1T v2 , (7.6)
2
where Rv̂1 , Rv̂2 ∈ TR SO(3) are two tangent vectors. For SE(3), we use the metric
given by the sum of the two previous metrics, and for SE(3)N , the sum of the metrics
for each copy of SE(3).

Gradients and how to compute them The gradient of a differentiable function

f (x), f : M → R computed at a point x0 on a manifold M is defined as the unique
tangent vector ∇x f (x0 ) such that, for all curves x(t) with x(0) = x0 ,

d

h∇x f (x0 ), ẋ(0)i = f x(t)  . (7.7)
dt t=0

For R3 , it can be shown that this definition coincides with the more common
definition as a vector of partial derivatives. For SO(3), we can use (7.7) to compute
gradients of any arbitrary function f in a few steps. First, we consider a fictitious
d


parametrized curve R(t) in SO(3). Then we use the chain rule to compute dt f R(t) ,
the derivative of the function along the curve. It can be shown that this derivative
(when it exists) can always be written as

d
f (R) = tr(M T Ṙ) = tr skew(RT M )T RT Ṙ ,


(7.8)
dt
where M is some matrix in R3×3 which in general depends on R, skew(A) =
1 T
2 (A − A ) is an operator that extracts the skew-symmetric component of a matrix,
and the explicit dependency on t has been omitted for brevity. The first equality in
(7.8) comes from the fact that the differential of a map (gradients are a particular
case of differentials) are always linear maps [187], and they can be expressed as
linear functionals using the trace operator [188]. The second equality in (7.8) comes
from the characterization of TR SO(3) given in (7.5) and the fact that tr(S v̂) = 0

100
 7. Nonlinear observability and estimation for multi-agent systems

for any symmetric matrix S ∈ R3×3 and v ∈ R3 . Comparing (7.8) with (7.7), we
then obtain ∇R f (R0 ) = 2R skew(RT M ) (this is equivalent to the formula given,
e.g., in [189]); in local coordinates, this becomes 2 skew(RT M )∨ . Informally, we
refer to this set of steps as the trace trick. For SE(3) and SE(3)N , the computation
of the gradient reduces to a separate computation for each component.

Remark 7.2. In this chapter we represent rotations using rotation matrices. Com-
pared to other representations (such as Euler angles or quaternions), this represen-
tation is unambiguous, does not have singularities, and, as shown above, provides a
relatively straightforward way to compute gradients (see, e.g., [181] for additional
insights).

7.2.4 Elements of local nonlinear observability

Let q = (q1 , . . . , qN ) ∈ SE(3)N (according to remark 2), β = stack({βij }) ∈ R3|E| ,
and the input vector fields gvik , gwik on SE(3)N which are obtained by appropriately
padding with zeros the corresponding vectors in (7.2). Then, the dynamical model
of the entire network can be considered as a nonlinear system with affine inputs:
3
XX
q̇ = (gvik vik + gwik wik ),
i∈V k=1 (7.9)
β = h(q),

where h : SE(3)N → R3|E| , h = stack({hij }).

The goal of nonlinear observability theory [174] applied to problem (7.9) is to
determine what parts of the state q can be reconstructed from the outputs β and
the inputs {vi , wi }. Following the notation of [174], we indicate the kth order
Lie derivative of a function h along the vector fields f1 , . . . , fk as Lkf1 ,...,fk h. The
definition of Lie derivative is given by letting L0 h = h, and then, recursively:

dLkf1 ,...,fk h = ∇q Lkf1 ,...,fk h, (7.10)

Lk+1 k
f1 ,f2 ,...,fk+1 h = hdLf1 ,...,fk h, fk+1 i, (7.11)

where h, i is the Riemannian metric on SE(3)N described in Sect. 7.2.3, and dLkf1 ,...,fk h
is a shorthand notation for the gradient of a Lie derivative. In nonlinear observability,
the function h is set to be an output of the system, and the vector fields f1 , . . . , fk
are taken to be the input vector fields.
In our case, we consider each βijm (that is, each element of β) as a separate
output of the system. In order to carry out the local nonlinear observability analysis
at a particular configuration q, it is necessary to define the subspace

dΩ = span {dLkf1 ,...,fk βijm } ,

(7.12a)

101
Bearing-based localization and control for multiple quadrotors

where k ∈ {0, 1, . . .}, (i, j) ∈ E, m ∈ {1, 2, 3}, (7.12b)

f1 , . . . , fk ∈ {gvik , gwik }. (7.12c)

As discussed in [177, Sect. 1.9], it is sufficient to consider Lie derivatives up to the

order k = 6N − 1 (where 6N is the dimension of the system (7.9)). In practice, we
will numerically verify that k = 1 is already sufficient under general conditions to
show that the global scale of the system can be recovered. We define dΩ⊥ to be the
annihilator of dΩ, that is, the subspace of Tq SE(3)N such that

hv, ni = 0 ∀v ∈ dΩ, n ∈ N (7.13)

The annihilator tells us the locally unobservable modes of the system [174, 177], that
is, what variations of the state q cannot be observed under any choice of the inputs
(and their Lie brackets). In practice, to find dΩ⊥ and compute its dimension, we
need to switch to a local coordinate representation. To avoid introducing additional
notation, we redefine dLkf1 ,...,fk h to be the vector in R6N of local coordinates (as
opposed to an abstract tangent vector in Tq SE(3)N ). Similarly, we redefine dΩ as a
matrix (the original subspace dΩ is given by the row span of this matrix):

dΩ = stack {dLkf1 ,...,fk βijm } ,

(7.14)

where the indexes are the same as in (7.12). Intuitively, the matrix dΩ generalizes
the classical observability matrix used for linear systems [154, 176, 177]. Finally, the
annihilator dΩ⊥ is redefined to be the nullspace of dΩ, dΩ⊥ = null(dΩ).
In the next section we will give the details of the computation of dΩ for Lie
derivatives of order up to k = 1 for our system (7.9).

7.3 Dynamic Bearing Observability Matrix

In this section we introduce the notion of Dynamic Bearing Observability Ma-
trix 2 (DBOM). We define the DBOM R̃ to be equal to the matrix dΩ computed
with the gradients of Lie derivatives of order up to k = 1. More explicitly:

R̃ = stack({dL0 βijm }, {dL1f1 βijm }) = stack(R̃A , R̃B ), (7.15)

where the matrix R̃A and R̃B contain the gradients of the Lie derivative of order,
respectively, k = 0 and k = 1, and will be described in detail in Sects. 7.3.1 and 7.3.2.
We anticipate here that the matrix R̃A is equivalent to the traditional bearing
rigidity matrix first introduced in [150], and then expanded upon in various works
(e.g., [27, 28, 173]). However, here we
2
We chose this name to stress that the scale of a formation based on bearing measurements is
retrievable through dynamic information.

102
 7. Nonlinear observability and estimation for multi-agent systems

1. derive it for full 6-D states in SE(3), using a clear interpretation with respect
to the Riemannian geometry of the space, and

2. give it the interpretation of a first step in a full nonlinear observability analysis.

Intuitively, since this matrix includes only zeroth order Lie derivatives, its properties
tell us which parts of the state can be estimated without providing any input (static
agents). As expect from previous works, and compatibly with the intuition, global
scaling (contraction/expansion) of the formation generate tangent vectors that are
in the nullspace of R̃A , meaning that the global scale is not observable.
The matrix R̃B constitutes the main novelty in our analysis. Intuitively, since
this matrix includes first order Lie derivatives, its properties tell us what can be
estimated by moving the agents with constant inputs. As we will numerically
verify in the following, this matrix contributes to reducing the dimension of the
nullspace of R̃ by one. The direction that is removed corresponds exactly to the
contraction/expansion motion. This is compatible with the intuition above: when
agents move, there is a parallax effect that can be exploited to get an estimate of
the unknown scales.

7.3.1 Matrix R̃A

In order to compute the matrix R̃A , since the zeroth-order Lie derivatives L0 βijm are
simply equal to the function themselves, we can directly focus on computing their
gradients ∇q L0 βijm . For this purpose, we will use the trace-trick method. Assuming
that βij moves along a fictitious curve βij (t), we compute the following:

d 0
L βijm = h∇pi L0 βijm , ṗi i + h∇pj L0 βijm , ṗj i
dt
h∇Ri L0 βijm , Ṙi i + h∇Rj L0 βijm , Ṙj i = eTm β̇ij (7.16)

which can be written also as

 
T pij d pij
eTm β̇ij = eT
m Ṙi + eT
m . (7.17)
dij dt dij
∂β
The ∇pi L0 βijm comes from the computation of ∂piji which is reported for clarity
here
RiT −Pij RiT
      
∂βij ∂ T pij ∂pi ∂pi
= Ri = 2 − dij − pij = (7.18)
∂pi ∂pi dij dij ∂pi ∂dij dij
T is the orthogonal projector onto the orthogonal complement
where Pij = I3 − βij βij
∂β
of βij . In the same way one can compute ∂pijj which gives birth to ∇pj L0 eTm βij .

On the other hand, regarding the gradient ∇Ri L0 βijm we need to focus our
attention on the part of eTm β̇ij which multiplies the Ṙi .

103
Bearing-based localization and control for multiple quadrotors

Before going into the details of the computation of ∇Ri L0 βijm we want to
highlight some useful identities. Let u, v ∈ Rd represent two generic vectors and
A, B ∈ Rd×d two generic matrices. Then, the following identities are satisfied

uT v = tr(uT v) = tr(vuT ) (7.19)

tr(ûT v̂) = − tr(ûv̂) (7.20)
tr(AB) = tr(BA) (7.21)

and that
Ṙi = Ri ŵi =⇒ ṘiT = ŵiT RiT =⇒ ṘiT Ri = ŵiT . (7.22)
Therefore, we can write
     
T T pij T T pij T pij T T T pij T
em Ṙi = tr em Ṙi = tr Ṙi e = tr Ṙi Ri Ri e =
dij dij dij m dij m
= tr ŵiT RiT Ri βij eTm = tr ŵiT βij eTm = − tr ŵi βij eTm = − tr βij eTm ŵi

(7.23)

The last term of (7.23) does not represent an inner product because it is missing
1
a 2. We can therefore rewrite the (7.23) as

1
− tr βij eTm ŵi = − tr 2βij eTm ŵi



(7.24)
2
the argument of the trace in the (7.24) has the shape of a tangent vector of SO(3)
and therefore we can finally extract the gradient ∇Ri L0 βijm through the trace trick
described in Sect. 7.2.3.
So, we can extract the desired gradients (in local coordinates):

1
∇pi L0 eTm βij = − eTm Pij RiT ,


(7.25)
dij
1
∇pj L0 eTm βij = + eTm Pij RiT ,


(7.26)
dij

∨ T
∇Ri L0 eTm βij = − skew 2βij eTm


, (7.27)

∇Rj L0 eTm βij = 0,

(7.28)

Equations (7.25)–(7.28) then give the (1 × 6N ) row block of R̃A associated to the
edge (i, j) ∈ E, which has the following form

eTm Pij RiT eTm Pij RiT

 

∨ T
R̃Aijm = −0− − − skew 2βij eTm −0− −0− −0− ,
dij dij

(7.29)

104
 7. Nonlinear observability and estimation for multi-agent systems

where the blocks are ordered following the ordering of the translational and rotational
states in q. Therefore the matrix R̃A can be seen as
 
R̃A = stack ∇q L0 βijm = stack R̃Aijm , (i, j) ∈ E, m ∈ {1, 2, 3}


(7.30)

Notice that the block row in (7.29) is expressed in the frame W. It is also possible
to express it in the local frame using the same ideas as [28].

7.3.2 Matrix R̃B

In order to compute the matrix R̃B , it is necessary to first compute the first-order
Lie derivatives L1f1 βijm = h∇q L0 βijm , f1 i, where f1 ∈ {gvık , gwık }ı∈V . Note that,
for a given i, j, all these Lie derivative are zero except for f1 ∈ {gvik , gwik , gvjk }
(for f1 = gwjk , the Lie derivative is zero due to (7.28)). We can therefore consider
only the latter ones. For instance, let us focus on the case f1 = gvik . To compute
the gradient of L1gv βijm we can employ again the trace-trick method described
ik
in Sect. 7.2.3.
d 1
L βij = h∇pi L1gv βijm , ṗi i + h∇pj L1gv βijm , ṗj i+
dt gvik m ik ik

+ h∇Ri L1gv βijm , Ṙi i + h∇Rj L1gv βijm , Ṙj i (7.31)

ik ik

Notice that, however, while expanding above we will obtain terms that depend on β̇ij .
Similarly to what we mentioned in Sect. 7.2.3, one can show that this dependency is
linear. More explicitly, we can rewrite (7.31) as

d 1 ¯ p L1g βijm , ṗj i+

¯ p L1g βijm , ṗi i + h∇
L βij = h∇
dt gvik m i vi
k
j vi
k
¯ 1
+ h∇Ri Lgv βijm , Ṙi i + h∇ ¯ R L1g βijm , Ṙj i + Kij β̇ij , (7.32)
ik j vik

where Kij is a matrix in R3×3 . We can exploit (7.32) to compute the rank of
the DBOM R̃, while avoiding the explicit computation of the terms in Kij , thus
simplifying the analytical expressions involved; this is because we can collect all the


{Kij }(i,j)∈E into a 3|E| × 3|E| matrix K = diag {Kij } , and then rewrite (7.15) as
" #
R̃A
R̃ = , (7.33)
R̃C + K R̃A

where R̃C is defined in the same way as R̃B , but by using the modified (and
¯ q instead of the full gradients ∇q . Eq. (7.33) implies
analytically simpler) gradients ∇
that
" # " #" #! " #
R̃A I 0 R̃A R̃A
rank = rank = rank (7.34)
R̃B K I R̃C R̃C

105
Bearing-based localization and control for multiple quadrotors

Hence, for our purposes, we can compute the rows of R̃C instead of those of R̃B
. As for the R̃A in the (7.30), the R̃C can be written as
 
¯ q L1 βijm = stack R̃C


R̃C = stack ∇ f1 ijm ,
(7.35)
where (i, j) ∈ E, m ∈ {1, 2, 3}, f1 ∈ {gvık , gwık }ı∈V

where R̃Cijm is

R̃Cijm =

¯ q L1 βijm , ∇
stack ∇ ¯ q L1 βijm , ∇
¯ q L1 βijm ,
gv i1 gv gv
i2 i3

¯ q L1g
∇ ¯ q L1g
βijm , ∇ ¯ q L1g
βijm , ∇ βijm , (7.36)
vj vj vj
1 2 3

∇¯ q L1g βijm , ∇ ¯ q L1g βijm , ∇ ¯ q L1g βijm ,

wi wi wi
1 2 3

¯ qL
∇ 1
β ijm , ¯
∇ L
q gw
1
β ijm , ¯
∇ L
q gw
1
β ijm .
gw j1 j2 j3

Just to give an example, the first argument of the stack(·) operator in the (7.36)
would be
∇q L1gv βijm =
ik

stack −0 − ∇ ¯ p L1 βijm − 0 − ∇ ¯ p L1 βijm (7.37)
i gvi j gvi
k k
T
¯ R L1 βijm − 0 − ∇
−0 − ∇ ¯ R L1 βijm .
i gv ik j gv ik

The other terms of the (7.36), of course, will have the same structure.
In order to show an analytical expression of the matrix R̃C we need to rewrite
the (7.32) as
 
d 1 d 1 T T
L βij = − em Pij Ri Ri ek =
dt gvik m dt dij
    
∂ 1 ∂ 1
− · ṗi + · ṗj eTm Pij RiT Ri ek +
∂pi dij ∂pj dij
1 T h  T  i
T
em β̇ij βij + βij β̇ij RiT Ri ek +
dij | {z }
Ṗij
1 1 T (7.38)
− eTm Pij ṘiT Ri ek − e Pij RiT Ṙi ek =
dij dij m
" ! ! #
Ri βij Ri βij
− · ṗi + − 2 · ṗj eTm Pij RiT Ri ek +
d2ij dij
1 T T T 1 T T T
+ em β̇ij βij Ri R i e k + e βij β̇ij R i R i ek +
dij dij m
1 1 T
− eTm Pij ṘiT Ri ek − e Pij RiT Ṙi ek .
dij dij m
Focusing on the right hand side of the (7.38)

106
 7. Nonlinear observability and estimation for multi-agent systems

• from the first two terms, which multiply respectively ṗi , ṗj , we will extract
∇¯ p L1g βijm , ∇
¯ p L1g βijm
i vik j v ik

T , are the terms

• the third and fourth term, which multiply respectively β̇ij , β̇ij
which give birth to the part of the matrix K corresponding to gvik

• from the fifth and sixth term, which multiply respectively ṘiT , Ṙi , we will
¯ R L1 βijm
extract the ∇ i gv ik

Extracting ∇¯ p L1g βijm , ∇

¯ p L1g βijm from (7.38) is straightforward and therefore
i vi j vi
k k
we will focus on the computation of ∇ ¯ R L1g βijm . The first term to consider is
i v ik

1 T 1 1
em Pij ṘiT Ri ek = − tr 2eTm Pij ŵiT RiT Ri ek = − tr 2ek eTm Pij ŵiT =



−
dij 2dij 2dij
1 1 1
tr 2ŵiT ek eTm Pij = tr 2ŵi ek eTm Pij = tr 2ek eTm Pij ŵi .




−
2dij 2dij 2dij

(7.39)

The second term to consider is

1 T 1
em Pij RiT Ṙi ek = − tr 2eTm Pij ŵi ek =


−
dij 2dij
(7.40)
1 1
tr 2ŵi ek eTm Pij = − tr 2ek eTm Pij ŵi .



−
2dij 2dij

Thus, from (7.39–7.40) it is possible to notice that

1 T 1 T ¯ R L1 βijm = 0
− em Pij ṘiT Ri ek − e Pij RiT Ṙi ek = 0 =⇒ ∇ (7.41)
dij dij m i gvi
k

and therefore, for f1 = gvik , we have:

" #T
¯ p L1g 1 T T
∇ i βijm = − 2 em Pij Ri Ri ek Ri βij (7.42)
vi
k dij
" #T
¯ p L1 βijm 1 T
∇ j gv = e Pij RiT Ri ek Ri βij (7.43)
ik d2ij m
¯ R L1 βijm = 0T
∇ (7.44)
i gv ik

¯ R L1 βijm = 0T
∇ (7.45)
j gv ik

where the (7.45) is zero due to the fact that L1gv βijm does not depend on Rj .
ik

107
Bearing-based localization and control for multiple quadrotors

d 1
Analogously to (7.38), the following holds with respect to dt Lgvjk βijm

 
d 1 d 1 T T
L βij = e Pij Ri Rj ek =
dt gvjk m dt dij m
" ! ! #
Ri βij Ri βij
· ṗi + − 2 · ṗj eTm Pij RiT Rj ek +
d2ij dij
(7.46)
1 1 T
− eTm β̇ij βij
T T
Ri Rj e k − T T
e βij β̇ij Ri Rj ek +
dij dij m
1 1 T
+ eTm Pij ṘiT Rj ek + e Pij RiT Ṙj ek .
dij dij m

Focusing on the right hand side of the (7.46)

• from the first two terms, which multiply respectively ṗi , ṗj , we will extract
∇¯ p L1 βijm , ∇
¯ p L1 βijm
i gv
jk j gv jk

T , are the terms

• the third and fourth term, which multiply respectively β̇ij , β̇ij
which give birth to the part of the matrix K corresponding to gvjk

• from the fifth and sixth term, which multiply respectively ṘiT , Ṙj , we will
¯ R L1 βijm , ∇
extract the ∇ ¯ R L1 βijm .
i gv jk j gv jk

¯ p L1g βijm , ∇
Also in this case, extracting ∇ ¯ p L1g βijm from (7.46) is straightfor-
i vj j vj
k k
ward and therefore we will focus on the computation of ∇¯ R L1g βijm , ∇
¯ R L1g βijm .
i v jjk v jk

For this purpose, let us write the following (with RiT Rj = iR

j)

1 T 1   1  
em Pij ṘiT Rj ek = tr 2eTm Pij ṘiT Rj ek = tr 2ṘiT Rj ek eTm Pij =
dij 2dij 2dij
1 1
tr 2ŵiT iRj ek eTm Pij = tr 2 iRj ek eTm Pij ŵiT



=
2dij 2dij

(7.47)

and
1 T 1   1
em Pij RiT Ṙj ek = tr 2eTm Pij RiT Ṙj ek = tr 2ek eTm Pij RiT Rj ŵj =


dij 2dij 2dij
1 1
tr 2ŵj ek eTm Pij iRj = tr 2 iRjT Pij em eTk ŵjT .



=
2dij 2dij

(7.48)

108
 7. Nonlinear observability and estimation for multi-agent systems

from the (7.47–7.48), through the trace-trick described in Sect. 7.2.3, we can then
¯ R L1g βijm ,∇
extract the ∇ ¯ R L1g βijm . Accordingly, for f1 = gv , we have:
i v jk j v jk jk

" #T
¯ p L1 Ri βij T T
∇ i gvj βijm = e Pij Ri Rj ek (7.49)
k d2ij m
" #T
¯p L 1 R i β ij T T
∇ j gvj βijm = − e Pij Ri Rj ek (7.50)
k d2ij m

∨ T
 
¯ R Lg βijm =
1 1 T T
∇ i skew 2Ri Rj ek em Pij (7.51)
vj
k dij

T
 
¯ R L1 βijm = 1 skew 2RT Ri Pij em eT ∨
∇ (7.52)
j gvj j k
k dij

Regarding the case f1 = gwik , the Lie derivative L1gw βijm depends on the states
ik

only through βij . Since we have separated the contribution of β̇ij in (7.32), we have
that ∇¯ q L1 βijm = 0. Finally, as already mentioned, all the other first order Lie
gw
ik

derivatives are zero, so they cannot contribute to the rank of R̃.

7.3.3 Numerical verification of the ranks of R̃A and R̃

Sects. 7.3.1 and 7.3.2 provided a complete analytical expression of the different terms
of the matrix R̃. Notice that these matrices (and their ranks) depend only on the
position and rotations of the agents and on their velocities (i.e., inputs).

After building the matrix (7.33) we found that, for a formation of 3 agents in
random positions, the rank of the R̃A is equal to 11 (6N − 7) while the one of the R̃
to 12 (6N − 6). As an example, we plot in Fig. 7.1 the last 8 singular values of the
two matrices as a function of time along the trajectories of the experiment which
will be discussed in Sect. 7.5. Note that, as with any dynamic observability result,
this result does not imply that the global scale can be recovered under any arbitrary
choice of inputs. Instead, it is necessary to choose inputs that “excite” this mode
(intuitively, where the agents move approximately perpendicularly to the bearing).
This is in line with what was found in [146] and in any other work concerned with
Structure from Motion.

Note that at the beginning of this chapter we claimed that the machineries
exploited in this work are based only on bearing measurements while in the previous
terms, almost everywhere, it appears the distance dij . This quantity is retrievable
through the EKF which is the topic of next Sect. 7.4

109
Bearing-based localization and control for multiple quadrotors

singular values R̃A singular values R̃

0.5 1.2

1
0.4

0.8
0.3
0.6
0.2
0.4

0.1
0.2

0 0
0 50 100 0 50 100
time [s] time [s]

Figure 7.1 – Behavior of the last 8 singular values of the matrix R̃A (left) and of the matrix
R̃ (right) during the experiment described in Sect. 7.5

7.4 A multi-agent Extended Kalman Filter

This section describes the design of an Extended Kalman Filter (EKF) on Lie groups
to empirically verify the ideas of the previous sections. In particular, the EKF will
provide an estimate q̂i = (p̂i , R̂i ) of the configuration qi of each agent. There are,
however, three issues that need to be addressed. First, as already seen, the state
of the system can be estimated only up to a global rotation and translation. We
fix this ambiguity by choosing a moving reference frame that moves with the first
agent. Of course, the uncertainty associated with this agent will always be zero
by construction. A more representative choice would probably be to consider an
“average” reference frame placed at the centroid of the formation; this, however,
would significantly complicate the derivation of the filter, and it is therefore outside
the scope of this work. Second, actual implementations of any filter must work
in discrete time. We therefore need to discretize our dynamical model (7.1). As
in [181], we use a simple 1-step Euler forward approach, which is equivalent to
assuming constant input velocites vi , wi between discretization instants. With this
assumption, our system (7.1) becomes
" #
i,k + dtvi,k
p
qi,k+1 =
∧  = fi (qi,k , ui,k ), (7.53)
expRi,k dtRi,k wi,k

where ui,k = stack(vi,k , wi,k ), dt is the length of time of the discretization interval,
and expR (·) is the exponential map at R ∈ SO(3); the exponential map can be
computed as expR (Rŵ) = R expI (ŵ), where the exponential at the identity expI ,
which corresponds to the matrix exponential, can be computed using the Rodriguez’s
formula [142]. Moreover, we define an operator expqk (τk ) with qk = (pk , Rk ) ∈ SE(3)
and τk = (vk , wk ) ∈ Tq SE(3). This operator for the position part of qk corresponds
to the operation qk + vk dt and for the rotation part of qk corresponds to the usual

110
 7. Nonlinear observability and estimation for multi-agent systems

expRk (ŵk ) (this notation simplifies the filter equations). Note that (7.53) is used for
i > 1; for agent i = 1 we have qi,k = 0, given our choice of the reference frame. Note
also that the output function hij in (4.2), corresponding to the edge (i, j), should be
redefined to explicitly take into account the measurement noise (denoted with oij )
pj − pi + oij
h̃ij (qi , qj , oij ) = RiT (7.54)
kpj − pi + oij k
Similarly, we define f̃i (qi , ui , ni ) = f (qi , ui + ni ), where ni ∈ Tqi SE(3) is the noise.
The overall system for the entire network is then

qk = f˜(qk−1 , uk , nk ), βij = h̃(qj , qj , ok ), (7.55)

with uk = stack ({ui,k }), and where we added process and measurement noises
nk , ok (ok = stack ({oij }) introduced in (7.54)), which are assumed to be zero mean
multivariate Gaussian with covariance (block diagonal) matrixes Qk , Rk .
The EKF is based on the linearization of the system (7.55):
 
∂f  ∂h 
Fk = , Hk = , (7.56)
∂q x̂k−1|k−1 , ∂q x̂k|k−1 ,ok =0
u ,n =0
 k k 
∂f 
 ∂h 
Lk = , Mk = . (7.57)
∂n  x̂k−1|k−1 , ∂o x̂k|k−1 ,ok =0
uk−1 ,nk =0

In our specific case, the matrix F is block diagonal with blocks given by
!
ṗi + dtṘ v
i i + dtR v̇
i i
Fi q̇i = ∂f
∂qi q̇i =
i
. (7.58)
DRi Ṙi∨ + dtDwi ẇi
where DRi = DRi expRi (dtRi ŵi ) and Dwi = Dwi expRi (dtwi ) denote the differen-
tials of the exponential map with respect to R and w, respectively; details on the
implementation of these differential can be found in [190]. The matrix Hk is actually
the same as the matrix R̃A computed in Sect. 7.3.1. The matrices Lk and Mk can
be computed similarly to Fk and Hk .
The prediction step of the EKF is given by the state estimate

q̂k|k−1 = f (q̂k−1|k−1 , uk−1 ) (7.59)

and the covariance matrix estimate

T
Pk|k−1 = Fk−1 Pk−1|k−1 Fk−1 + Lk−1 Qk−1 LTk−1 . (7.60)

The update step of the filter consists of the state estimate


q̂k|k = expq̂k|k−1 Pk|k−1 HkT +

 (7.61)
+ Hk Pk|k−1 HkT + Mk Rk MkT −1 ỹk
| {z }
Sk

111
Bearing-based localization and control for multiple quadrotors

where ỹk is the difference between the measured and estimated bearings.
Moreover, the covariance matrix estimate is given by

Pk|k = I − Pk|k−1 HkT + Sk−1 Hk Pk|k−1

(7.62)

In the next Sect. 7.5 it is shown a result of our (centralized) EKF.

7.5 Experimental results

In order to validate the presented ideas, extensive simulations in a Matlab/Simulink

environment have been performed considering cases with both process and mea-
surement noise. In addition to simulations also experiments have been performed
with real quadrotor UAVs. In this section an experiment with three agents (two real
quadrotor UAVs and a fixed virtual agent) will be presented. The experiments were
performed in the flying arena described in Sect. B.2.2. Furthermore, we used two
of the quadrotors described in Sect. B.2 and a third virtual agent of the formation
which fix the ambiguity discussed in Sect. 7.4 has been placed, for convenience, in
the origin of the motion capture system reference frame. This agent will be the one
with respect to which the other two UAVs will refer their measurements.
During the experiment reported in Fig. 7.2 the agent 2 was moving mainly back
and forth on the x axis while the agent 3 was following an ellipsoidal trajectory.
A user was also able to give velocity inputs to the UAVs through a joystick. The
trajectories described above allow to highlight the fact that, in order to collectively
estimate the scale, the agents need to move along exciting trajectories [146]. Indeed,
from Figs. 7.2(a) and 7.2(b) it is possible to notice that the variance associated to
the coordinates which are only ligthly excited (y, z coordinates on the agent 2 and
3) remains higher than the ones which are strongly excited (x coordinates on the
agents 2 and 3). Fig. 7.2(c) shows the behavior of the true and estimated distance
between the agents 2 and 3 and it is clear that the EKF, even starting far from the
true values (both for positions and orientations (Fig. 7.2(d))) it is able to recover
the true distance d23 , hence the scale of the formation.
The orientation error showed in Fig. 7.2(d) is defined as:

1  
Rerri = tr I − R̂iT Ri , ∀i ∈ V (7.63)
2

where R̂i represents the estimation of the real rotation matrix Ri corresponding to
the agent i.

112
 7. Nonlinear observability and estimation for multi-agent systems

5 5
0 0
x

x
-5 -5
0 20 40 60 80 100 120 0 20 40 60 80 100 120

5 10
5
y

y
0 0
0 20 40 60 80 100 120 0 20 40 60 80 100 120

4 2
2 1

z
z

0 0
0 20 40 60 80 100 120 0 20 40 60 80 100 120
time [s] time [s]
(a) (b)
d23 0.6
5
true
estimated
distance [m]

4
0.4

Rerr
3
0.2
2

1 0
0 20 40 60 80 100 120 0 20 40 60 80 100 120

time [s] time [s]

(c) (d)

Figure 7.2 – Fig. 7.2(a) shows the behavior of the real position (x, y, z) (in meters) of the
agent 2, in black, and the estimated one with the associated covariance respectively in red
(x), green (y) and blue (z); Fig. 7.2(b) is the dual of Fig. 7.2(a) for the agent 3; Fig. 7.2(c)
shows the behavior of the true (blue) and estimated (red) distance d23 between the agents 2
and 3. Fig. 7.2(d) shows the behavior of the orientation error introduced in (7.63)

7.6 Conclusions and future works

In this chapter we considered the problem of scale estimation and localization with
bearing-only measurements and known agent velocities. By applying nonlinear
observability theory and Riemannian geometry, we extended existing results on
the theory of rigidity and introduced the notion of Dynamic Bearing Observability
Matrix (DBOM). Using experiments with two quadrotor UAVs, we have shown that
the global scale is indeed observable and that it can be recovered by employing a
centralized EKF implemented directly on SE(3).
The preliminary results of this work open several interesting future research
direction, such as3

• providing a full analytical characterization of the nullspace of the Dynamic

Bearing Observability Matrix and critical agent configurations

3
As for the previous chapters, a fully onboard implementation is missing, refer to Chapt. 8 for
more details.

113
Bearing-based localization and control for multiple quadrotors

• verifying if it is possible to achieve additional insights by including higher

order Lie derivatives

• designing input signals that minimize the uncertainty in localization (using [146]
as inspiration),

• studying distributed implementations of the EKF (building upon, e.g., [184,

191]) and, finally,

• applying similar ideas to different types of agents (e.g. UAVs with full dynamics,
or unicycle dynamics) and measurements (e.g., distance-only),

• extending the proposed ideas to the case of multi-agent systems with unknown
input as in [192].

114
 7. Nonlinear observability and estimation for multi-agent systems

Summary
The material presented in the previous chapters has been published to different
international conferences:

• The work presented in Chapt. 5 has been published in [27] and a related video
can be found at https://goo.gl/ZwwMGa

• The work presented in Chapt. 6 has been published in [28] and a related video
can be found at https://goo.gl/z4v7Bu

• The work presented in Chapt. 7 has been published in [29] and a related video
can be found at https://goo.gl/jqB1kX

115
 Part III

Conclusions and Future Work

117
 Chapter 8
Conclusions and future work

N this last chapter, we wish to review the main theoretical and experimental

I results achieved in the Thesis and point out some issues which are still left
open. Regarding the latter, we also intend to indicate possible directions to
follow for further investigation and research.

8.1 Summary and contributions

The goal of this Thesis was mainly to study the problems of decentralized formation
control and localization of a group of quadrotor UAVs through relative measurements.
The relative measurements on which we focused our attention were 3D bearings, that
is, what one can recover from monocular cameras mounted onboard the robots. Note
that in the whole Thesis we assumed that the underlying topology of the sensing
graph associated with the formation is of a directed type to make our approach more
prone to a real-world application. On the other hand, the underlying communication
graph is supposed to be undirected, that is, if there is an edge, we assume that the
robots connected through that edge are able to communicate with each other.
In Chapt. 5 we showed how to stabilize, through a decentralized controller, a
group of quadrotor UAVs towards a desired formation specified in terms of bearing
constraints. This was done by extending the theory of rigidity from SE(2) to the
case of R3 × S1 and by combining some control and estimation algorithms. All
this was done, up to our knowledge for the first time, in the case of non-stationary
agents under the assumption that the formation remains infinitesimally bearing rigid.
Furthermore, we fully characterized the null-space of the BRM. This allowed us to
steer the group of quadrotors in 3D space through motions which do not alter the
specified bearing constraints. Specifically, these motions consist of the so-called rigid-
body-motions which consist of collective roto-translation and expansion/contraction
of the whole formation.

119
Bearing-based localization and control for multiple quadrotors

Having understood the importance of bearing rigidity in the formation control

and localization problems, we worked on the maintenance of this essential property.
Therefore, in Chapt. 6, we proposed a bearing maintenance strategy which required
the analytical expression of the rigidity eigenvalue and of its derivatives with respect
to the configuration which we detailed in closed-form in the Thesis. This strategy
was built on our previous results and was able to cope with some (realistic) sensor
limitations as limited field of view of the camera, limited range of the camera and
lastly the possibility of occlusions between the quadrotors during motion (a complex
constraint which is quite seldom considered in the literature).
Bearing-only localization intrinsically does not allow to remove the scale ambiguity
because bearings consist of non-metric information. Indeed, our localization scheme
through bearing measurements, used for both the contributions mentioned above,
suffered of the assumption that at least one agent needs to be able to measure its
distance with respect to another agent. Because of this and under the (realistic)
assumption that each robot can know its ego-motion (body frame linear/angular
velocity) we decided to conduct an observability analysis of the whole system. This
showed that the scale of the formation is indeed observable. To this end, we designed
an EKF, directly on SE(3) which was able to estimate the poses of the agents of the
formation and therefore delete the ”scale ambiguity”. Note that the finding that the
scale is observable is in line with other works such as [146] and with the literature
of Structure from Motion [92, 93]. However, unlike the majority of other works,
we tackled these problems with tools from nonlinear observability and Riemannian
geometry.
Again, all the presented theoretical claims were supported by extensive simula-
tions and experimental campaigns. The experiments were run in our flying arena
(see Sect. B.2) with multiple quadrotor UAVs.
Note that, during his Ph.D., the author extensively contributed to the setup of all
the needed software/hardware infrastructure which was not in place at the beginning
of his Ph.D. These experimental activities allowed him to get in contact with several
implementation issues, such as network issues, limited flying time and the choice of
the right software infrastructure which is responsible for the overall control of the
UAVs. Due to this experience, the author believes that substantial work is still needed
to allow a group of flying robots for robust autonomous navigation in an unknown
environment without a centralized localization system and relying only on their
local skills. As we mentioned, a good example of the implementation issues which
continues to be a significant bottleneck is the limited flying time of quadrotor UAVs.
Indeed, current battery technology only enables flights of 10-30 min, depending
on many factors such as wind and payload. There is much research going on with
battery refueling on UAVs with some solutions being battery swapping [193, 194] or

120
 8. Conclusions and future work

automated landing on a recharging platform as presented in [195]. We believe that

improvements on this side of the technology are also fundamental for deploying a
swarm of UAVs in real-world missions.
On the other hand, the numerous technological advancements regarding vision
sensors, computational power and miniaturization of the electronics give big hopes
of seeing these systems becoming a robust reality in the not-so-far future.

8.2 Open issues and future perspectives

However, as we mentioned, even if our approach presented many promising results
it also highlighted some limitations both theoretical and practical. Note that some
difficulties are specific to flying robots and especially quadrotor UAVs and this is the
reason why in a part of the multi-agent community they are not even mentioned.
Entirely onboard implementation
As we pointed out in the conclusions of Chapts. 5 to 7 one limitation of this work
(and in our opinion the most important) is that we did not implement a fully onboard
solution to retrieve bearing measurements from onboard cameras. This obstacle
was due to the complex nature of the problems of robust detection, tracking, and
identification, in real-time of the agents of the formation. However, we believe to have
found the right tool to tackle this problem: the AprilTag detector [196, 197]. This
tool is used extensively in different applications (such as exploration and mapping for
ground robots [198], camera calibration [199,200] and autonomous landing on moving
platforms for UAVs [201]) but not yet, up to our knowledge, in scenarios like the one
described in this Thesis. An alternative to the AprilTag fiducial markers is to use
the WhyCon localization system proposed in [64] and already successfully employed
in several works as [202,203]. For all these reasons, during the experiments described
in Chapts. 5 to 7 the bearing measurements were retrieved from the Vicon motion
capture system. Note that the development of a fully onboard implementation is the
topic of current research of the author (see Fig. 8.1). The previous problem is also
related to the one of onboard computational power of each quadrotor. At the time of
writing, as explained in Appendix B, we equipped our robots with an ODROID-XU4.
However, we think that the new, and more powerful, NVIDIA JETSON TX2 module
(Fig. 8.2) could represent a significant improvement to our system and this is why
we are looking into switching to a platform with this computer module onboard.
Collision avoidance
Another aspect which we did not take into account and plays a pivotal role in
all kinds of formation control tasks is the one of collision avoidance. It is impossible
to think about a real-world application which is not embedded with a collision

121
Bearing-based localization and control for multiple quadrotors

(a) (b)

Figure 8.1 – First tests with the AprilTag fiducial markers. Fig. 8.1(a): two quadrotor UAVs
flying in our flying arena with some Apriltag [197] mounted on them. Note that we plan to
reduce the size of the fiducial markers used in our applications to make them more compact
and easy to embed on each UAV. Fig. 8.1(b): a snapshot of an onboard camera which shows
the detection of an AprilTag fiducial marker mounted on another quadrotor UAV and the
consequent translation to a bearing vector.

Figure 8.2 – The NVIDIA JETSON TX2 Module, from http://www.nvidia.com

avoidance technique. However, in this work collision avoidance is not taken into
account in the design of the controller. Despite the capability of the maintenance
strategy (Chapt. 6) of making an edge disappear when two agents become too close,
this (unfortunately) does not ensure (in general) the avoidance of collision between
the robots. This is because the interested edge could be one which is not necessary
to maintain the rigidity of the formation over a certain threshold. Indeed, consider
the scenario in which two agents start to become too close, but the rigidity of the
framework is still well over the desired threshold. In this case, the weight associated
with the corresponding edge will gradually vanish leaving the considered agents
without any mean of avoiding each other. This is an intrinsic property of our design
of the weights. Indeed, it encodes the concept that the disappearance of an edge

122
 8. Conclusions and future work

corresponds to the vanishing of the capability of a robot to sense the other robot
to which it is connected. For this reason, we believe that our framework needs an
explicit consideration of the collision avoidance problem. In the literature, there are
many works [204] which deal with this problem, and we realized that finding the
right method is of paramount importance. As a starting point towards this aim, we
plan to adapt to our scenario the collision avoidance strategies used in [34,62,63,205]
which allow preventing inter-agent and obstacle collisions. Another choice would
be following up what proposed in [122] where, besides extending their results of
stabilization of bearing-only formations to arbitrary dimensions, they also provide a
sufficient condition to ensure collision avoidance between any pair of agents under
the action of their controller.

Stochasticity of the control system

Another interesting direction is the explicit modeling of the effect of noise

on our algorithms. Noise was added in our simulations and it was intrinsically
present in all the experiments, but no formal study was conducted to claim specific
robustness properties of the proposed methods. This direction was partially followed
in Chapt. 7 in which we took into account (see the equation (7.55)) both process
and measurement noises. Therefore, we are aware that to go towards a real-world
application we need to deal with the stochasticity of the overall system which
could include not entirely reliable communication, tracking of the other robots
through cameras and so on. Regarding the communication, during our experiments,
we experienced several problems with the WiFi interferences, and therefore an
excellent future direction would be to investigate alternatives to this communication
technology. Regarding the problem of tracking other quadrotors, one of the dilemmas
we would need to deal with is the presence of outliers and therefore choose the
right algorithm (between the many which are present in the literature) is essential.
A well-known way to deal with outliers, which would be our first shot at tackling
these problems is the random sample consensus (RANSAC) algorithm [206] (and its
extensions), which has been successfully explicitly used for AprilTags in [207]. All
this could compromise the designed controller and therefore study the robustness of
the controller is essential. A specific study of the gradient-based control law used to
steer a formation to the desired bearing formation which could be taken as a starting
point for future developments is [118]. In this work, they try to give a solution to the
specific problem of measurements discrepancies. It is highlighted that this issue can
sometimes produce undesirable collective motions of the formation, and therefore
dealing with this problem is also of paramount importance.

Multiplicity of the rigidity eigenvalue

Both in Chapts. 5 and 6 we relied on the sixth smallest eigenvalue λ6 of the

123
Bearing-based localization and control for multiple quadrotors

symmetric bearing rigidity matrix BGW (q) as a measure of rigidity of the considered
framework (G, q). Already in Sect. 6.3.4 we alluded to the fact that what we said is
valid as soon as the rigidity eigenvalue has multiplicity equal to one. This is easy to
understand if we remember that we exploited, in our computations, several times
the well-known formula
λi = viT Avi (8.1)

where λi is an eigenvalue of the matrix A and vi is the corresponding eigenvector.

Now suppose that we design a controller which is based on the λi and that this
eigenvalue has multiplicity equal to two. This means that there are two different
eigenvectors vi1 , vi2 corresponding to this eigenvalue and this would mean that even
a linear combination of these two vectors is an eigenvector corresponding to the
considered eigenvalue λi . Therefore, it will likely happen that the controller, at
each time step, would switch between different eigenvectors, with all the possible
instabilities causes by this discontinuing switching. This is a very relevant problem
which was already encountered, as we already said, in [34,119,155] without proposing
a solution. To find a solution, we believe that it could be helpful to understand,
in the scenarios analyzed in this Thesis, what motivates the occurrence of multiple
eigenvalues and if it is associable to a specific structure of the framework and/or
a specific topology of the graph G. For this reason, we believe that this problem
represents an additional future direction of this work.
Agent dynamics
In the multi-robot literature, as in this Thesis, we often encounter works1 in
which the dynamics of the single agent is neglected and therefore each agent is seen
as a simple-integrator. Hence, we believe that another interesting future direction,
also highlighted in Sect. 7.6, is extending our results to systems with different types
of agents, especially second-order integrators, in order to be able to include also the
dynamics associated to each agent.
Final remarks
A last open issue, which is a result of the solution of the open directions mentioned
above, is the need of tackling more complex problems than the simple steering of
the formation by a human operator in an indoor room equipped with a Vicon
motion capture system. As we pointed out many times, this would include, but
it is not limited to, exploration and mapping in collapsed buildings, cooperative
transportation, surveillance, and so forth. Furthermore, moving outdoor, in an
uncontrolled environment is a big step which is needed to make these systems
undoubtedly more useful for humankind. A first step towards all these aims would
1
Especially when dealing with multiple quadrotor UAVs.

124
 8. Conclusions and future work

be to enhance the framework described in Chapt. 6 with the one of Chapt. 7 for the
scale estimation.
Overall, we believe that the contributions of this Thesis allow the field of multi-
robot coordination in unknown environments to take a step further in the under-
standing of the concepts of theory of rigidity and its application to the decentralized
formation control and localization. These problems revealed to be challenging and
at the same time full of potential for real-world applications which we sincerely hope
to see in the near future.

125
 Appendix A
Additional technical details
associated to Chapt. 6

Contents
A.1 How to go from (6.15) to (6.16) . . . . . . . . . . . . . . . . . . 127
A.2 Useful derivatives for the computation of the derivative of λB
6 . 128
A.3 Towards the derivatives of the weights . . . . . . . . . . . . . . . 129

HIS appendix includes additional technical details about the derivations

T relative to Chapt. 6. The mathematical details presented here are not

essential for the understanding of the maintenance strategy but give to the
interested reader and idea on how we proceeded to arrive to certain results.

A.1 How to go from (6.15) to (6.16)

In this section we want to show how to go from (6.15), which is reported below for
convenience,
!
X P̄ij p̄ ij
λB
6 = vpTij 2 vpij + 2vpTij S vψ − vψ2 i p̄Tij S 2 p̄ij (A.1)
dij dij i
(i, j)∈E

to the (6.16) reported below

!
X Pij βij
λB
6 = νpTij 2 νpij + 2νpTij S νψ − νψ2 i βij
T 2
S βij . (A.2)
dij dij i
(i, j)∈E

This is really important because, as explained in Sect. 6.3.2, the (A.1) contains
quantities expressed in the world frame (not available to the i-th agent) while (A.2)
contains quantities only relative to agents i and j.

127
Bearing-based localization and control for multiple quadrotors

Therefore, we can rewrite (A.1), knowing that vpi = Ri νpi and vψi = νψi , as
"
X
T P̄ij
λB


6 = Ri νpi − Rj νpj 2 Ri νpi − Rj νpj +
dij
(i, j)∈E


T p̄ij 2 T 2
2 Ri νpi − Rj νpj S νψ − νψi βij S βij =
dij i
" (A.3)
X
T P̄ij
νpi − iRj νpj RiT 2 Ri νpi − iRj νpj +


dij
(i, j)∈E

i

T T p̄ij 2 T 2
2 νpi − Rj νpj Ri S νψ − νψi βij S βij .
dij i

where the term vψ2 i p̄Tij S 2 p̄ij has been rewritten as

vψ2 i p̄Tij S 2 p̄ij = νψi RiT p̄Tij Ri S 2 RiT p̄ij Ri νψi = νψi RiT βij
T 2
S βij Ri νψi = νψ2 i βij
T 2
S βij .
(A.4)
Noticing that

RiT P̄ij Ri = RiT I − p̄ij p̄T T

ij Ri = I − βij βij = Pij (A.5)

and with νpij = νpi − iRj νpj we have that (A.1) is equal to the (A.2) as wanted.

A.2 Useful derivatives for the computation of the

derivative of λB
6

This section contains some useful derivatives for the computation of the λB
6 in Chapt. 6.

The distance dij between two robots can be written as

dij = kpj − pi k = ((pj − pi ) · (pj − pi ))1/2 (A.6)

therefore, its derivative will be

1 d
d˙ij = ((pj − pi ) · (pj − pi ))−1/2 ((pj − pi ) · (pj − pi )) (A.7)
2 dt

(A.7) follows from (A.6) by an easy application of the chain rule. We further
have
d
[(pj − pi ) · (pj − pi )] = 2(pj − pi ) · (ṗj − ṗi ) (A.8)
dt
substituting (A.8) into (A.7) yields

pj − pi
d˙ij = ((pj − pi ) · (pj − pi ))−1/2 (pj − pi ) · (ṗj − ṗi ) = · (ṗj − ṗi ). (A.9)
dij

128
 A. Additional technical details associated to Chapt. 6

If we multiply (on the left) (A.9) by Ri RiT we have

∂dij ∂dij
d˙ij = Ri βij (ṗj − ṗi ) = ṗi + ṗj = −Ri βij ṗi + Ri βij ṗj (A.10)
∂pi ∂pj
and therefore it follows
∂dij ∂dij
= −Ri βij , = Ri βij . (A.11)
∂pi ∂pj
In the same way we can find
∂ 1 Ri βij ∂ 1 Ri βij
= 2 , =− 2 (A.12)
∂pi dij dij ∂p d
j ij dij
and
∂ 1 Ri βij ∂ 1 Ri βij
2 =2 3 , 2 = −2 3 . (A.13)
∂pi dij dij ∂pj dij dij
Regarding the derivative of βij , it holds
∂βij ∂βij ∂βij
β̇ij = ṗi + ṗj + ψ̇i . (A.14)
∂pi ∂pj ∂ψi
Note that (A.14) is valid only if the matrix Ri considered is a function of one angle
ψi . where
∂βij Pij Ri ∂βij Pij Ri ∂βij
=− , = , = −Sβij (A.15)
∂pi dij ∂pj dij ∂ψi
Furthermore, to compute Ṗij we need to use the following

d T

d T

h 
T
 i
T
Ṗij = I − βij βij =− βij βij = − β̇ij βij + βij β̇ij (A.16)
dt dt
and combine it with the (A.14–A.15).

A.3 Towards the derivatives of the weights

In Sect. 6.3.2, specifically in (6.19), we refer to the gradients of the weights wij
with respect to pi , pj , ψi , ψj without giving further details. This section is intended
to give some more details about the math involved in the computation of these
weights. This should give the interested reader a hint of the rationale behind the
math exploited by the controller proposed in (6.23).
Let us start from the (6.17) which is reported below for convenience
X
λB
6 = wij lij . (A.17)
(i, j)∈E

Remember that wij is composed of different weights

wij = wRij wFij wVij (A.18)

129
Bearing-based localization and control for multiple quadrotors

therefore the ∇qi λB

6 would be equal to
X
∇qi λ B



6 = ∇qi wRij wFij wVij lij + wRij ∇qi wFij wVij lij +
(i, j)∈E (A.19)


wRij wFij ∇qi wVij lij + wRij wFij wVij (∇qi lij ) .

From the (A.19) it is then clear that to compute the gradient of the λB 6 when
the weights are involved, we need to compute the terms ∇qi wRij , ∇qi wFij , ∇qi wVij .
Remember that qi = (pi , ψi ) and therefore each of the previous terms will break
down to a ∇pi (·) and a ∇ψi (·).
As an example, let us look at the gradient of the weight wFij (αij ) defined in (6.3).
In particular it will be

∂   ∂ ∂
wFij (αij ) = wF αij . (A.20)
∂pi ∂αij ij ∂pi

The first term of the right-hand-side of the (A.20) depends just from the shape we
decided for the function expressed by (6.3) and it involves really easy derivatives.
On the other side, the second term of the right-hand-side of the (A.20), because
αij = oTC βij , it will be just equal to the first equation of (A.15) 1 . The same thing
goes for the derivative with respect to pj and for ψi it holds

∂   ∂ ∂
wFij (αij ) = wFij αij . (A.21)
∂ψi ∂αij ∂ψi

where the second term of the right-hand-side of the (A.21) is equal to the third
equation of (A.15).

1
Where oC ∈ S2 is the (constant and known) direction of the camera optical axis in the
quadrotor body-frame

130
 Appendix B
Simulation and experimental
architecture

Contents
B.1 Simulations architecture . . . . . . . . . . . . . . . . . . . . . . 131
B.2 Real experiments architecture . . . . . . . . . . . . . . . . . . . 132
B.2.1 The quadrotor . . . . . . . . . . . . . . . . . . . . . . . 132
B.2.2 The Vicon motion capture system . . . . . . . . . . . . 134

n this appendix we present the overall software/hardware architecture used

I throughout this work both for the simulations and for the experiments. Note
that, during his Ph.D., the author has been extensively involved in the setup
of the whole architecture both on the software and on the hardware level.

B.1 Simulations architecture

The simulation architecture can be divided into two main parts depending on how
the single quadrotor UAV is modeled:

(1) Quadrotor UAV modeled as a simple integrator using only Matlab and
Simulink to run each simulation

(2) Quadrotor UAV modeled with a more complex model through the V-REP robot
simulator [158] and by using the TeleKyb framework to run each simulation
(Figs. B.1 and B.2). For more details about TeleKyb the reader is referred
to [159].

In both cases Matlab and Simulink represent the main programming environment
where we implemented:

131
Bearing-based localization and control for multiple quadrotors

• the formation control and estimation algorithms described in Chapts. 5 and 6

• the observability analysis (with the corresponding EKF) described in Chapt. 7.

In the case (2) above TeleKyb was used to interface the control/estimation algorithms
with the V-REP simulator. Using TeleKyb also in simulations allowed us to have
a similar architecture between simulations and real experiments. This enabled
us to switch easily between the simulation and experimental architectures. The
module which provides the state of the formation is different, of course, between the
simulation architecture and the experimental one. In the first case, it is the V-REP
simulator (or Matlab) which provides the state, while during real experiments is
the Vicon motion capture system which tracks the pose of real quadrotor UAVs.
The communication between the modules present in Fig. B.1 is made possible
through the Robotic Operating System (ROS1 ) and, more in details:

• the bridge described in [208] has been used to build a communication between
V-REP and ROS

• another bridge 2 has been used to build a communication between Matlab,

Simulink and ROS

B.2 Real experiments architecture

As explained in Appendix B.1 the overall architecture used for the real experiments
is similar to the one used in simulations. It is possible to appreciate this similarity
by comparing Fig. B.1 with Fig. B.3. From Fig. B.3 it is also possible to notice that
we used a Vicon motion capture system (see Sect. B.2.2) to track the pose of the
quadrotor UAVs. The Vicon was used for obtaining a reliable ground truth with a
sub-millimeter accuracy, to retrieve the linear/angular velocities of the real robots
and, when an edge was existing, the body-frame bearing measurements βij between
the considered robots which would have been obtained by the onboard cameras. To
mimic bearing measurements coming from a camera we also exploited the option to
decrease the frequency at which the Vicon was giving data.

B.2.1 The quadrotor

The quadrotor platform used for the experiments of this work consists of a MK-
Quadro by Mikrokopter3 . The usual Mk-Quadro was extended with an ODROID-
XU4 computing device (Fig. B.4) running Ubuntu (14.04 or 16.04) ROS and TeleKyb.
1
http://www.ros.org/
2
http://wiki.ros.org/matlab_ros_bridge
3
http://www.mikrokopter.de/en/home

132
 B. Simulation and experimental architecture

Figure B.1 – Scheme of the software architecture used during a simulation when the V-REP
robot simulator is used to model the quadrotor UAVs

For the experiments described in Chapt. 6 each quadrotor was also equipped with
an onboard camera (a Flea FL3-U3-32S2C-CS by FLIR, previously Point-Grey4 ) as
shown in Fig. B.5(b). Note that this camera was not used to retrieve the bearing
measurements during the experiments but only as an additional ground-truth and to
verify that the maintenance strategy of Sect. 6.3 and the associated weight functions
are working correctly.
Note also that in order to have a smooth trajectory and to match better the
dynamics capabilities of the UAVs we used a fourth order linear filter for the inputs
given to each UAV:
....f ...
p i = −k1 p fi − k2 p̈fi − k3 (ṗfi − ṗi ) − k4 (pfi − pi ) (B.1)
.... ...
where with p fi , p fi , p̈fi , ṗfi we indicate respectively the filtered snap, jerk, acceler-
ation and velocity given to the i-th UAV. The gains k1 , k2 , k3 , k4 were chosen in
order to have the poles in −7,−6,−5,−4 and therefore have a convenient settling
time.
Moreover, note that towards the end of this work we decided to upgrade our system
from using TeleKyb to a set of packages based on GenoM3 [209] for implementing
4
https://www.ptgrey.com/

133
Bearing-based localization and control for multiple quadrotors

Figure B.2 – Screenshot of a simulation of six quadrotor UAVs in the V-REP robot simulator

the low-level flight control. This set of packages is mainly developed at LAAS-CNRS
of Toulouse. This choice was done in order to build a common better tool for
flying UAVs and mainly because of the many improvements that were made over
the TeleKyb framework. This new framework has been successfully used for the
experiments showed in Chapt. 7. However, the high level part of the algorithm
described in Chapt. 7 (as in the other chapters) was still implemented through
Matlab/Simulink.

B.2.2 The Vicon motion capture system

During the experiments, in order to get the pose (position and orientation) of the
robots in the environment we used a Vicon motion capture system. This system
is made of multiple infrared cameras (Fig. B.7), a router to receive all the camera
measurements and a software (Vicon Tracker) to analyze these measurements. We
used this system in two different configurations according to the room in which the
experiments were conducted:

• for early-stage tests we used a room with a flying volume of about 7m x 4.5m
x 2.5m of about 7 m x 4.5 m x 2.5 m (Fig. B.6(a)) equipped with a Vicon
system made of eight Bonita 10 cameras (Fig. B.7(a))

• for final experiments we used a room with a bigger flying volume (with respect
to the previous one) of about 6.5 m x 6.5 m x 3 m (Fig. B.6(b)) equipped with

134
 B. Simulation and experimental architecture

Figure B.3 – Scheme of the overall architecture used during the experiments with real
quadrotor UAVs and the Vicon motion capture system

Figure B.4 – Odroid XU4, from http://www.hardkernel.com/

a Vicon system made of eight Bonita 10 cameras (Fig. B.7(a)) and four Vero
1.3 cameras (Fig. B.7(b))5 .

A Bonita 10 camera has the following specifications

5
The experiments described in Chapts. 5 to 7 were performed in the room with a bigger flying
volume, in Fig. B.6(b).

135
Bearing-based localization and control for multiple quadrotors

(a) (b)

Figure B.5 – The MkQuadro without (Fig. B.5(a)) and with (Fig. B.5(b)) a Point Grey
camera mounted on top of it.

(a) Room with 8 Bonita cameras (b) Room with 8 Bonita and 4 Vero cameras

Figure B.6 – Screenshots of the coverage tool present in the Vicon Tracker software which
show the two available rooms for conducting experiments, at INRIA Rennes Bretagne
Atlantique.

(a) Bonita camera (b) Vero camera

Figure B.7 – Vicon cameras used during the experiments, from https://www.vicon.com/

136
 B. Simulation and experimental architecture

• frame rate: 250 fps

• resolution: 1 megapixel

• angle of view wide: 70.29◦ x 70.29◦

• angle of view narrow: 26.41◦ x 26.41◦ ,

while a Vero v1.3 camera has the following specifications

• frame rate: 250 fps

• resolution: 1.3 megapixel

• angle of view wide: 60.8◦ x 50.3◦

• angle of view narrow: 32.7◦ x 26.4◦ .

137
 Bibliography

[1] N. Joseph, “Science and Civilization in China: Volume 2, History of Scientific

Thought,” 1991.

[2] S. M. Douglas, I. Bachelet, and G. M. Church, “A logic-gated nanorobot for

targeted transport of molecular payloads,” Science, vol. 335, no. 6070, pp. 831–
834, 2012.

[3] B. Jang, E. Gutman, N. Stucki, B. F. Seitz, P. D. Wendel-Garcia, T. Newton,

J. Pokki, O. Ergeneman, S. Pane, Y. Or et al., “Undulatory locomotion of
magnetic multilink nanoswimmers,” Nano letters, vol. 15, no. 7, pp. 4829–4833,
2015.

[4] H.-T. Lin, G. G. Leisk, and B. Trimmer, “Goqbot: a caterpillar-inspired soft-

bodied rolling robot,” Bioinspiration & biomimetics, vol. 6, no. 2, p. 026007,
2011.

[5] G. C. de Croon, M. Groen, C. De Wagter, B. Remes, R. Ruijsink, and B. W. van

Oudheusden, “Design, aerodynamics and autonomy of the delfly,” Bioinspiration
& biomimetics, vol. 7, no. 2, p. 025003, 2012.

[6] M. Hutter, C. Gehring, D. Jud, A. Lauber, C. D. Bellicoso, V. Tsounis,

J. Hwangbo, K. Bodie, P. Fankhauser, M. Bloesch et al., “Anymal-a highly
mobile and dynamic quadrupedal robot,” in IEEE International Conference on
Intelligent Robots and Systems, 2016, pp. 38–44.

[7] K. E. Peyer, E. C. Siringil, L. Zhang, M. Suter, and B. J. Nelson, “Bacteria-

inspired magnetic polymer composite microrobots,” in Conference on Biomimetic
and Biohybrid Systems. Springer, 2013, pp. 216–227.

139
Bearing-based localization and control for multiple quadrotors

[8] M. Dorigo, V. Maniezzo, and A. Colorni, “Ant system: optimization by a colony

of cooperating agents,” IEEE Transactions on Systems, Man, and Cybernetics,
Part B (Cybernetics), vol. 26, no. 1, pp. 29–41, 1996.

[9] A. Martinoli, A. J. Ijspeert, and F. Mondada, “Understanding collective aggrega-

tion mechanisms: From probabilistic modelling to experiments with real robots,”
Robotics and Autonomous Systems, vol. 29, no. 1, pp. 51–63, 1999.

[10] E. Bonabeau, M. Dorigo, and G. Theraulaz, Swarm intelligence: from natural

to artificial systems. Oxford university press, 1999, no. 1.

[11] F. Mondada, G. C. Pettinaro, A. Guignard, I. W. Kwee, D. Floreano, J.-L.

Deneubourg, S. Nolfi, L. M. Gambardella, and M. Dorigo, “Swarm-bot: A new
distributed robotic concept,” Autonomous Robots, vol. 17, no. 2-3, pp. 193–221,
2004.

[12] A. Martinoli, K. Easton, and W. Agassounon, “Modeling swarm robotic systems:

A case study in collaborative distributed manipulation,” The Int. J. of Robotics
Research, vol. 23, no. 4-5, pp. 415–436, 2004.

[13] M. Dorigo, M. Birattari, and T. Stutzle, “Ant colony optimization,” IEEE

computational intelligence magazine, vol. 1, no. 4, pp. 28–39, 2006.

[14] J. Halloy, G. Sempo, G. Caprari, C. Rivault, M. Asadpour, F. Tâche, I. Saïd,

V. Durier, S. Canonge, J. M. Amé et al., “Social integration of robots into groups
of cockroaches to control self-organized choices,” Science, vol. 318, no. 5853, pp.
1155–1158, 2007.

[15] D. Karaboga and B. Akay, “A survey: algorithms simulating bee swarm intelli-
gence,” Artificial intelligence review, vol. 31, no. 1-4, pp. 61–85, 2009.

[16] N. J. Mlot, C. A. Tovey, and D. L. Hu, “Fire ants self-assemble into waterproof
rafts to survive floods,” Proceedings of the National Academy of Sciences, vol.
108, no. 19, pp. 7669–7673, 2011.

[17] L. E. Parker et al., “Current state of the art in distributed autnomous mobile
robotics.” in DARS, 2000, pp. 3–14.

[18] R. M. Murray, “Recent research in cooperative control of multi-vehicle systems,”

ASME Journal on Dynamic Systems, Measurement, and Control, vol. 129, no. 5,
pp. 571–583, 2006.

[19] A. Howard, L. E. Parker, and G. S. Sukhatme, “Experiments with a large hetero-

geneous mobile robot team: Exploration, mapping, deployment and detection,”
The Int. J. of Robotics Research, vol. 25, no. 5-6, pp. 431–447, 2006.

140
 Bibliography

[20] Z. Yan, N. Jouandeau, and A. A. Cherif, “A survey and analysis of multi-robot

coordination,” International Journal of Advanced Robotic Systems, vol. 10, no. 12,
p. 399, 2013.

[21] B. Charrow, N. Michael, and V. Kumar, “Cooperative multi-robot estimation

and control for radio source localization,” The Int. J. of Robotics Research,
vol. 33, no. 4, pp. 569–580, 2014.

[22] M. Schwager, B. J. Julian, M. Angermann, and D. Rus, “Eyes in the sky:

Decentralized control for the deployment of robotic camera networks,” Proceedings
of the IEEE, vol. 99, no. 9, pp. 1541–1561, 2011.

[23] “Forecast: Personal and Commercial Drones, Worldwide, 2016,” https://www.

gartner.com/doc/3557717, accessed: 2017-09-04.

[24] “ARCAS: Aerial Robotics Cooperative Assembly System,” http://www.

arcas-project.eu/, accessed: 2017-09-04.

[25] “SHERPA: Smart collaboration between Humans and ground-aErial Robots

for imProving rescuing activities in Alpine environments,” http://www.
sherpa-project.eu/sherpa/, accessed: 2017-09-04.

[26] “AEROWORKS: Collaborative Aerial Workers,” http://www.aeroworks2020.

eu/, accessed: 2017-09-04.

[27] F. Schiano, A. Franchi, D. Zelazo, and P. Robuffo Giordano, “A rigidity-based

decentralized bearing formation controller for groups of quadrotor UAVs,” in
IEEE International Conference on Intelligent Robots and Systems, 2016.

[28] F. Schiano and P. Robuffo Giordano, “Bearing rigidity maintenance for forma-
tions of quadrotor uavs,” in IEEE International Conference on Robotics and
Automation, 2017.

[29] F. Schiano and R. Tron, “The Dynamic Bearing Observability Matrix, Nonlinear
Observability and Estimation for Multi-Agent Systems,” in IEEE International
Conference on Robotics and Automation, 2018.

[30] D. Zelazo, A. Franchi, and P. Robuffo Giordano, “Rigidity Theory in SE(2) for
Unscaled Position Estimation using only Bearing Measurements,” in 2014 ECC,
Strasbourg, France, Jun. 2014, pp. 2703–2708.

[31] D. Zelazo, P. Robuffo Giordano, and A. Franchi, “Bearing-Only Formation

Control Using an SE(2) Rigidity Theory,” in IEEE International Conference on
Decision and Control, Osaka, Japan, 2015, pp. 6121–6126.

141
Bearing-based localization and control for multiple quadrotors

[32] D. Fox, J. Ko, K. Konolige, B. Limketkai, D. Schulz, and B. Stewart, “Distributed

multirobot exploration and mapping,” Proc. of the IEEE, vol. 94, no. 7, pp. 1325–
1339, 2006.

[33] N. Michael, J. Fink, and V. Kumar, “Cooperative manipulation and transporta-

tion with aerial robots,” in 2009 Robotics: Science and Systems, Seattle, WA,
Jun. 2009.

[34] P. Robuffo Giordano, A. Franchi, C. Secchi, and H. H. Bülthoff, “A passivity-

based decentralized strategy for generalized connectivity maintenance,” The Int.
J. of Robotics Research, vol. 32, no. 3, pp. 299–323, 2013.

[35] K. Hausman, J. Müller, A. Hariharan, N. Ayanian, and G. S. Sukhatme,

“Cooperative multi-robot control for target tracking with onboard sensing,” IEEE
Transactions on Automatic Control, vol. 34, no. 13, pp. 1660–1677, 2015.

[36] E. Montijano, E. Cristofalo, D. Zhou, M. Schwager, and C. Sagues, “Vision-

based distributed formation control without an external positioning system,”
IEEE Transactions on Automatic Control, vol. 32, no. 2, pp. 339–351, Jul. 2016.

[37] P. R. Wurman, R. D’Andrea, and M. Mountz, “Coordinating hundreds of

cooperative, autonomous vehicles in warehouses,” AI magazine, vol. 29, no. 1,
p. 9, 2008.

[38] A. Jaimes, S. Kota, and J. Gomez, “An approach to surveillance an area using
swarm of fixed wing and quad-rotor unmanned aerial vehicles uav (s),” in IEEE
International Conference on System of Systems Engineering. IEEE, 2008, pp.
1–6.

[39] A. Franchi, L. Freda, G. Oriolo, and M. Vendittelli, “The sensor-based random

graph method for cooperative robot exploration,” IEEE/ASME Transactions on
Mechatronics, vol. 14, no. 2, pp. 163–175, 2009.

[40] A. Renzaglia, L. Doitsidis, A. Martinelli, and E. B. Kosmatopoulos, “Multi-robot

three-dimensional coverage of unknown areas,” The Int. J. of Robotics Research,
vol. 31, no. 6, pp. 738–752, 2012.

[41] F. Rafi, S. Khan, K. Shafiq, and M. Shah, “Autonomous target following by

unmanned aerial vehicles,” in Proceedings of SPIE, vol. 6230, 2006, pp. 325–332.

[42] Q. Lindsey, D. Mellinger, and V. Kumar, “Construction of cubic structures with

quadrotor teams,” in 2011 Robotics: Science and Systems, Los Angeles, CA, Jun.
2011.

142
 Bibliography

[43] H. Joo, C. Son, K. Kim, K. Kim, and J. Kim, “A study on the advantages on
high-rise building construction which the application of construction robots take
(iccas 2007),” in International Conference on Control, Automation and Systems
(ICCAS). IEEE, 2007, pp. 1933–1936.

[44] S. Berman, Q. Lindsey, M. S. Sakar, V. Kumar, and S. Pratt, “Study of group

food retrieval by ants as a model for multi-robot collective transport strategies,”
in Robotics: Science and Systems, 2010.

[45] K. Sreenath and V. Kumar, “Dynamics control and planning for cooperative
manipulation of payloads suspended by cables from multiple quadrotor robots,”
rn, vol. 1, no. r2, p. r3, 2013.

[46] Z. Wang and M. Schwager, “Force-amplifying n-robot transport system (force-

ants) for cooperative planar manipulation without communication,” The Int. J.
of Robotics Research, vol. 35, no. 13, pp. 1564–1586, 2016.

[47] P. R. Chandler, M. Pachter, and S. Rasmussen, “Uav cooperative control,” in

American Control Conference, 2001. Proceedings of the 2001, vol. 1. IEEE,
2001, pp. 50–55.

[48] G. Antonelli, “Interconnected dynamic systems: An overview on distributed

control,” IEEE Control Systems, vol. 33, no. 1, pp. 76–88, 2013.

[49] C. Belta and V. Kumar, “Abstraction and control for groups of robots,” IEEE
Transactions on robotics, vol. 20, no. 5, pp. 865–875, 2004.

[50] G. Antonelli and S. Chiaverini, “Kinematic control of platoons of autonomous

vehicles,” IEEE Transactions on Robotics, vol. 22, no. 6, pp. 1285–1292, 2006.

[51] G. Antonelli, “Stability analysis for prioritized closed-loop inverse kinematic

algorithms for redundant robotic systems,” IEEE Transactions on Robotics,
vol. 25, no. 5, pp. 985–994, 2009.

[52] A. Korchenko and O. Illyash, “The generalized classification of unmanned air ve-
hicles,” in Actual Problems of Unmanned Air Vehicles Developments Proceedings
(APUAVD), 2013 IEEE 2nd International Conference. IEEE, 2013, pp. 28–34.

[53] M. Ryll, H. H. Bülthoff, and P. R. Giordano, “Modeling and control of a

quadrotor uav with tilting propellers,” in IEEE International Conference on
Robotics and Automation, 2012, pp. 4606–4613.

[54] A. Vuruskan, B. Yuksek, U. Ozdemir, A. Yukselen, and G. Inalhan, “Dynamic

modeling of a fixed-wing vtol UAV,” in International Conference on Unmanned
Aircraft Systems (ICUAS). IEEE, 2014, pp. 483–491.

143
Bearing-based localization and control for multiple quadrotors

[55] C. Stöcker, R. Bennett, F. Nex, M. Gerke, and J. Zevenbergen, “Review of the

current state of uav regulations,” Remote Sensing, vol. 9, no. 5, p. 459, 2017.

[56] F. Augugliaro, S. Lupashin, M. Hamer, C. Male, M. Hehn, M. W. Mueller, J. S.

Willmann, F. Gramazio, M. Kohler, and R. D’Andrea, “The flight assembled
architecture installation: Cooperative construction with flying machines,” IEEE
Control Systems, vol. 34, no. 4, pp. 46–64, 2014.

[57] G. Gioioso, A. Franchi, G. Salvietti, S. Scheggi, and D. Prattichizzo, “The flying

hand: A formation of UAVs for cooperative aerial tele-manipulation,” in IEEE
International Conference on Robotics and Automation, 2014, pp. 4335–4341.

[58] D. Mellinger, M. Shomin, N. Michael, and V. Kumar, “Cooperative grasping

and transport using multiple quadrotors,” in Distributed autonomous robotic
systems. Springer, 2013, pp. 545–558.

[59] Q. Lindsey, D. Mellinger, and V. Kumar, “Construction with quadrotor teams,”

Autonomous Robots, vol. 33, no. 3, pp. 323–336, 2012.

[60] A. Mirjan, F. Augugliaro, R. D’Andrea, F. Gramazio, and M. Kohler, “Building

a bridge with flying robots,” in Robotic Fabrication in Architecture, Art and
Design 2016. Springer, 2016, pp. 34–47.

[61] R. Ritz, M. W. Müller, M. Hehn, and R. D’Andrea, “Cooperative quadrocopter

ball throwing and catching,” in IEEE International Conference on Intelligent
Robots and Systems, 2012, pp. 4972–4978.

[62] P. Robuffo Giordano, A. Franchi, C. Secchi, and H. H. Bülthoff, “Bilateral

teleoperation of groups of UAVs with decentralized connectivity maintenance,”
in Robotics: Science and Systems, Los Angeles, CA, Jun. 2011.

[63] D. Lee, A. Franchi, H. I. Son, H. H. Bülthoff, and P. Robuffo Giordano, “Semi-

autonomous haptic teleoperation control architecture of multiple unmanned
aerial vehicles,” IEEE/ASME Transactions on Mechatronics, vol. 18, no. 4, pp.
1334–1345, 2013.

[64] T. Krajnik, M. Nitsche, J. Faigl, P. Vanek, M. Saska, L. Preucil, T. Duckett, and

M. Mejail, “A practical multirobot localization system,” Journal of Intelligent &
Robotic Systems, vol. 76, no. 3-4, pp. 539–562, 2014.

[65] G. Loianno, C. Brunner, G. McGrath, and V. Kumar, “Estimation, control, and

planning for aggressive flight with a small quadrotor with a single camera and
imu,” IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 404–411, 2017.

144
 Bibliography

[66] K.-K. Oh, M.-C. Park, and H.-S. Ahn, “A survey of multi-agent formation
control,” Automatica, vol. 53, pp. 424–440, 2015.

[67] R. Olfati-Saber, J. A. Fax, and R. M. Murray, “Consensus and cooperation in

networked multi-agent systems,” Proc. of the IEEE, vol. 1, no. 95, pp. 215–233,
2007.

[68] N. A. Lynch, Distributed algorithms. Morgan Kaufmann, 1996.

[69] R. Olfati-Saber and R. M. Murray, “Consensus problems in networks of agents

with switching topology and time-delays,” IEEE Transactions on automatic
control, vol. 49, no. 9, pp. 1520–1533, 2004.

[70] R. A. Freeman, P. Yang, and K. M. Lynch, “Stability and convergence properties

of dynamic average consensus estimators,” in IEEE International Conference on
Decision and Control, San Diego, CA, Dec. 2006, pp. 338–343.

[71] W. Ren, R. W. Beard, and E. M. Atkins, “A survey of consensus problems in

multi-agent coordination,” in American Control Conference, 2005. Proceedings
of the 2005. IEEE, 2005, pp. 1859–1864.

[72] B. D. O. Anderson, C. Yu, B. Fidan, and J. M. Hendrickx, “Rigid graph control

architectures for autonomous formations,” IEEE Control Systems Magazine,
vol. 28, no. 6, pp. 48–63, 2008.

[73] Y. Hou, “Control of formations with non-rigid and hybrid graphs,” Ph.D.
dissertation, The Australian National University, 2016.

[74] M. Basiri, A. N. Bishop, and P. Jensfelt, “Distributed control of triangular

formations with angle-only constraints,” Systems & Control Letters, vol. 59, no. 2,
pp. 147–154, 2010.

[75] A. N. Bishop, “A very relaxed control law for bearing-only triangular formation
control,” IFAC Proceedings Volumes, vol. 44, no. 1, pp. 5991–5998, 2011.

[76] A. N. Bishop, I. Shames, and B. D. O. Anderson, “Stabilization of rigid for-

mations with direction-only constraints,” in IEEE International Conference on
Decision and Control, Orlando, FL, Dec. 2011, pp. 746–752.

[77] A. Franchi, C. Masone, V. Grabe, M. Ryll, H. H. Bülthoff, and P. Robuffo

Giordano, “Modeling and control of UAV bearing-formations with bilateral high-
level steering,” The Int. J. of Robotics Research, Special Issue on 3D Exploration,
Mapping, and Surveillance, vol. 31, no. 12, pp. 1504–1525, 2012.

145
Bearing-based localization and control for multiple quadrotors

[78] L. E. Parker, “Designing control laws for cooperative agent teams,” in IEEE
International Conference on Robotics and Automation. IEEE, 1993, pp. 582–587.

[79] K. Fathian, D. I. Rachinskii, T. H. Summers, M. W. Spong, and N. R. Gans,

“Distributed formation control under arbitrarily changing topology,” in American
Control Conference (ACC), 2017. IEEE, 2017, pp. 271–278.

[80] B. A. Francis and M. Maggiore, Flocking and rendezvous in distributed robotics.

Springer, 2016.

[81] R. Kurazume, S. Nagata, and S. Hirose, “Cooperative positioning with multiple

robots,” in IEEE International Conference on Robotics and Automation, 1994,
pp. 1250–1257.

[82] D. Fox, W. Burgard, H. Kruppa, and S. Thrun, “Collaborative multi-robot

localization,” in Mustererkennung 1999. Springer, 1999, pp. 15–26.

[83] ——, “A probabilistic approach to collaborative multi-robot localization,” Au-

tonomous Robots, vol. 8, no. 3, pp. 325–344, 2000.

[84] S. Panzieri, F. Pascucci, and R. Setola, “Multirobot localisation using interlaced

extended kalman filter,” in IEEE International Conference on Intelligent Robots
and Systems, 2006, pp. 2816–2821.

[85] S. Roumeliotis and G. A. Bekey, “Distributed multirobot localization,” IEEE

Transactions on Robotics and Automation, vol. 18, no. 5, pp. 781–795, 2002.

[86] S. S. Kia, S. F. Rounds, and S. Martinez, “A centralized-equivalent decentralized

implementation of extended kalman filters for cooperative localization,” in IEEE
International Conference on Intelligent Robots and Systems, 2014, pp. 3761–3766.

[87] A. Prorok, A. Bahr, and A. Martinoli, “Low-cost collaborative localization for

large-scale multi-robot systems,” in IEEE International Conference on Robotics
and Automation. Ieee, 2012, pp. 4236–4241.

[88] B. Shirmohammadi and C. Taylor, “Self localizing smart camera networks,”

ACM Transactions on Sensor Networks, 2010.

[89] T. Eren, “Graph invariants for unique localizability in cooperative localization of

wireless sensor networks: rigidity index and redundancy index,” Ad Hoc Networks,
vol. 44, pp. 32–45, 2016.

[90] R. K. Williams, A. Gasparri, A. Priolo, and G. S. Sukhatme, “Evaluating network

rigidity in realistic systems: Decentralization, asynchronicity, and parallelization,”
IEEE Transactions on Automatic Control, vol. 4, no. 30, pp. 950–965, 2014.

146
 Bibliography

[91] I. Shames, A. N. Bishop, and B. D. O. Anderson, “Analysis of Noisy Bearing-

Only Network Localization,” IEEE Transactions on Automatic Control, vol. 58,
no. 1, pp. 247–252, Jan. 2013.

[92] M. Brand, M. Antone, and S. Teller, “Spectral solution of large-scale extrinsic

camera calibration as a graph embedding problem,” in IEEE European Conference
on Computer Vision, 2004, pp. 262–273.

[93] H. Li, “Multi-view structure computation without explicitly estimating motion,”

in IEEE Conference on Computer Vision and Pattern Recognition, 2010, pp.
2777–2784.

[94] B. Servatius and W. Whiteley, “Constraining plane configurations in computer-

aided design: Combinatorics of directions and lengths,” SIAM Journal on Discrete
Mathematics, vol. 12, no. 1, pp. 136–153, 1999.

[95] A. Franchi, G. Oriolo, and P. Stegagno, “Probabilistic mutual localization in

multi-agent systems from anonymous position measures,” in IEEE International
Conference on Decision and Control. IEEE, 2010, pp. 6534–6540.

[96] M. Cognetti, P. Stegagno, A. Franchi, G. Oriolo, and H. H. Bülthoff, “3-D mutual

localization with anonymous bearing measurements,” in IEEE International
Conference on Robotics and Automation, St. Paul, MN, May 2012, pp. 791–798.

[97] X. S. Zhou and S. I. Roumeliotis, “Determining 3-d relative transformations for

any combination of range and bearing measurements,” IEEE Transactions on
Robotics, vol. 29, no. 2, pp. 458–474, 2013.

[98] A. Martinelli, “Closed-form solutions for attitude, speed, absolute scale and
bias determination by fusing vision and inertial measurements,” INRIA Grenoble
Rhone-Alpes, Tech. Rep., 2011.

[99] G. C. Calafiore, L. Carlone, and M. Wei, “A distributed gradient method for

localization of formations using relative range measurements,” in IEEE Int. Symp.
on Computer-Aided Control System Design, Yokohama, Japan, Sep. 2010, pp.
1146–1151.

[100] T. Eren, “Using angle of arrival (bearing) information for localization in robot
networks,” Turkish J. of Elec. Eng. & Comput. Sci., vol. 15, no. 2, pp. 169–186,
2007.

[101] M. Ye, B. D. Anderson, and C. Yu, “Bearing-only measurement self-localization,

velocity consensus and formation control,” IEEE Transactions on Aerospace and
Electronic Systems, vol. 53, no. 2, pp. 575–586, 2017.

147
Bearing-based localization and control for multiple quadrotors

[102] A. Martinelli and R. Siegwart, “Observability analysis for mobile robot local-
ization,” in IEEE International Conference on Intelligent Robots and Systems.
IEEE, 2005, pp. 1471–1476.

[103] X. S. Zhou and S. Roumeliotis, “Determining the robot-to-robot 3d relative pose

using combinations of range and bearing measurements: 14 minimal problems
and closed-form solutions to three of them,” in IEEE International Conference
on Intelligent Robots and Systems, 2010, pp. 2983–2990.

[104] P. Stegagno, “Mutual localization from anonymous measurements in multi-

robot systems,” Ph.D. dissertation, Università di Roma La Sapienza, 2012.

[105] A. Franchi, G. Oriolo, and P. Stegagno, “Mutual localization in a multi-robot

system with anonymous relative position measures,” in IEEE International
Conference on Intelligent Robots and Systems, 2009, pp. 3974–3980.

[106] P. Stegagno, M. Cognetti, A. Franchi, and G. Oriolo, “Mutual localization

using anonymous bearing measurements,” in IEEE International Conference on
Intelligent Robots and Systems, 2011, pp. 469–474.

[107] P. Stegagno, M. Cognetti, G. Oriolo, H. H. Bülthoff, and A. Franchi, “Ground

and aerial mutual localization using anonymous relative-bearing measurements,”
IEEE Transactions on Robotics, vol. 32, no. 5, pp. 1133–1151, 2016.

[108] N. Biggs, E. K. Lloyd, and R. J. Wilson, Graph Theory, 1736-1936. Oxford

University Press, 1976.

[109] A.-L. Cauchy, “Recherche sur les polyèdres-premier mémoire,” Journal de

l’Ecole Polytechnique. v9, pp. 66–86, 1813.

[110] G. L’Huilier, “Recherche sur les polyèdres-premier mémoire,” Annales de

Mathématiques pures et appliquées, tome 3, pp. 169–189, 1812-1813.

[111] M. Mesbahi and M. Egerstedt, Graph theoretic methods in multiagent networks.

Princeton University Press, 2010.

[112] A. Franchi and P. Robuffo Giordano, “Decentralized control of parallel rigid

formations with direction constraints and bearing measurements,” in IEEE
International Conference on Decision and Control, Maui, HI, Dec. 2012, pp.
5310–5317.

[113] F. Bullo, J. Cortes, and S. Martinez, Distributed control of robotic networks: a

mathematical approach to motion coordination algorithms. Princeton University
Press, 2009.

148
 Bibliography

[114] M. Fiedler, “Algebraic connectivity of graphs,” Czechoslovak mathematical

journal, vol. 23, no. 2, pp. 298–305, 1973.

[115] ——, “Laplacian of graphs and algebraic connectivity,” Banach Center Publi-
cations, vol. 25, no. 1, pp. 57–70, 1989.

[116] L. Krick, M. E. Broucke, and B. A. Francis, “Stabilisation of infinitesimally

rigid formations of multi-robot networks,” International Journal of Control,
vol. 82, no. 3, pp. 423–439, Mar. 2009.

[117] B. D. Anderson, I. Shames, G. Mao, and B. Fidan, “Formal theory of noisy

sensor network localization,” SIAM Journal on Discrete Mathematics, vol. 24,
no. 2, pp. 684–698, 2010.

[118] H. Garcia de Marina, M. Cao, and B. Jayawardhana, “Controlling rigid forma-

tions of mobile agents under inconsistent measurements,” IEEE Transactions on
Robotics, vol. 31, no. 1, pp. 31–39, Feb 2015.

[119] D. Zelazo, A. Franchi, H. H. Bülthoff, and P. Robuffo Giordano, “Decentralized

Rigidity Maintenance Control with Range-only Measurements for Multi-Robot
Systems,” The Int. J. of Robotics Research, vol. 34, no. 1, pp. 105–128, 2015.

[120] T. Eren, “Formation shape control based on bearing rigidity,” International

Journal of Control, vol. 9, no. 85, pp. 1361–1379, 2012.

[121] A. Cornejo, A. J. Lynch, E. Fudge, S. Bilstein, M. Khabbazian, and

J. McLurkin, “Scale-free coordinates for multi-robot systems with bearing-only
sensors,” The Int. J. of Robotics Research, vol. 12, no. 32, pp. 1459–1474, 2013.

[122] 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, 2016.

[123] ——, “Translational and scaling formation maneuver control via a bearing-
based approach,” IEEE Transactions on Control of Network Systems, vol. 4,
no. 3, pp. 429–438, 2017.

[124] J. Aspnes, W. Whiteley, and Y. R. Yang, “A theory of network localization,”

IEEE Transactions on Mobile Computing, vol. 5, no. 12, pp. 1663–1678, 2006.

[125] I. Shames, A. N. Bishop, and B. D. O. Anderson, “Analysis of noisy bearing-

only network localization,” IEEE Trans. on Automatic Control, vol. 1, no. 58,
pp. 247–252, 2013.

149
Bearing-based localization and control for multiple quadrotors

[126] R. Tron, L. Carlone, F. Dellaert, and K. Daniilidis, “Rigid components iden-

tification and rigidity enforcement in bearing-only localization using the graph
cycle basis,” in IEEE American Control Conference, 2015.

[127] D. Zelazo, A. Franchi, F. Allgöwer, H. H. Bülthoff, and P. Robuffo Giordano,

“Rigidity maintenance control for multi-robot systems,” in 2012 Robotics: Science
and Systems, Sydney, Australia, Jul. 2012.

[128] G. Laman, “On graphs and rigidity of plane skeletal structures,” Journal of
Engineering Mathematics, vol. 4, no. 4, pp. 331–340, 1970.

[129] T. Tay and W. Whiteley, “Generating isostatic frameworks,” Structural Topol-

ogy, vol. 11, no. 1, pp. 21–69, 1985.

[130] W. Whiteley, “Some matroids from discrete applied geometry,” Contemporary

Mathematics, vol. 197, pp. 171–312, 1996.

[131] R. K. Williams, A. Gasparri, M. Soffietti, and G. S. Sukhatme, “Redundantly

rigid topologies in decentralized multi-agent networks,” in IEEE International
Conference on Decision and Control. IEEE, 2015, pp. 6101–6108.

[132] T. Eren, “Combinatorial measures of rigidity in wireless sensor and robot

networks,” in IEEE International Conference on Decision and Control. IEEE,
2015, pp. 6109–6114.

[133] L. Henneberg, Die graphische Statik der starren Systeme. BG Teubner, 1911,
vol. 31.

[134] S. Bereg, “Certifying and constructing minimally rigid graphs in the plane,” in
Proceedings of the twenty-first annual symposium on Computational geometry.
ACM, 2005, pp. 73–80.

[135] L. Asimow and B. Roth, “The Rigidity of Graphs, II,” Journal of Mathematical
Analysis and Applications, vol. 68, pp. 171–190, 1979.

[136] B. D. O. Anderson, B. Fidan, C. Yu, and D. van der Walle, “UAV formation
control: Theory and application,” in Recent Advances in Learning and Control,
ser. Lecture Notes in Control and Information Sciences, V. D. Blondel, S. P.
Boyd, and H. Kimura, Eds. Springer, 2008, vol. 371, pp. 15–34.

[137] T. Eren, W. Whiteley, and P. N. Belhumeur, “Using angle of arrival (bear-

ing) information in network localization,” in IEEE International Conference on
Decision and Control. IEEE, 2006, pp. 4676–4681.

150
 Bibliography

[138] T. Eren, “Formation shape control based on bearing rigidity,” International

Journal of Control, vol. 85, no. 9, pp. 1361–1379, Sep. 2012.

[139] J. N. Twigg, J. R. Fink, L. Y. Paul, and B. M. Sadler, “Rss gradient-assisted

frontier exploration and radio source localization,” in IEEE International Con-
ference on Robotics and Automation. IEEE, 2012, pp. 889–895.

[140] R. Tron, J. Thomas, G. Loianno, K. Daniilidis, and V. Kumar, “A distributed

optimization framework for localization and formation control,” IEEE Control
Systems Magazine, vol. 36, no. 4, pp. 22–44, 2016.

[141] S. Zhao and D. Zelazo, “Bearing-constrained formation control using bearing

measurements,” in Israel Annual Conference on Aerospace Sciences, 2015.

[142] Y. Ma, An invitation to 3-D vision: from images to geometric models. Springer,
2004.

[143] R. I. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision,

2nd ed. Cambridge University Press, 2004.

[144] E. S. Jones and S. Soatto, “Visual-inertial navigation, mapping and localization:

A scalable real-time causal approach,” International Journal of Computer Vision,
vol. 30, no. 4, pp. 407–430, 2011.

[145] D. Scaramuzza, M. C. Achtelik, L. Doitsidis, F. Fraundorfer, E. B. Kosmatopou-

los, A. Martinelli, M. W. Achtelik, M. Chli, S. A. Chatzichristofis, L. Kneip,
D. Gurdan, L. Heng, G. H. Lee, S. Lynen, L. Meier, M. Pollefeys, A. Renzaglia,
R. Siegwart, J. C. Stumpf, P. Tanskanen, C. Troiani, and S. Weiss, “Vision-
controlled micro flying robots: from system design to autonomous navigation and
mapping in GPS-denied environments,” IEEE Robotics & Automation Magazine,
vol. 21, no. 3, pp. 26–40, 2014.

[146] R. Spica and P. Robuffo Giordano, “Active decentralized scale estimation for
bearing-based localization,” in IEEE International Conference on Intelligent
Robots and Systems, 2016, pp. 5084–5091.

[147] J. Fink, N. Michael, S. Kim, and V. Kumar, “Planning and control for co-
operative manipulation and transportation with aerial robots,” The Int. J. of
Robotics Research, vol. 30, no. 3, pp. 324–334, 2010.

[148] T. Lee, M. Leokyand, and N. H. McClamroch, “Geometric tracking control of

a quadrotor UAV on SE(3),” in IEEE International Conference on Decision and
Control, 2010, pp. 5420–5425.

151
Bearing-based localization and control for multiple quadrotors

[149] C. D. Godsil and G. Royle, Algebraic Graph Theory. Springer, 2001.

[150] T. Eren, W. Whiteley, A. S. Morse, P. N. Belhumeur, and B. D. Anderson,

“Sensor and network topologies of formations with direction, bearing, and angle
information between agents,” in IEEE International Conference on Decision and
Control, vol. 3. IEEE, 2003, pp. 3064–3069.

[151] K.-K. Oh and H.-S. Ahn, “Formation control of mobile agents based on inter-
agent distance dynamics,” Automatica, vol. 47, no. 10, pp. 2306 – 2312, 2011.

[152] J. Cortés, “Global and robust formation-shape stabilization of relative sensing

networks,” Automatica, vol. 12, no. 45, pp. 2754–2762, 2009.

[153] E. Montijano, D. Zhou, M. Schwager, and C. Sagues, “Distributed formation

control without a global reference frame,” in IEEE American Control Conference,
Portland, OR, 2014, pp. 3862–3867.

[154] H. K. Khalil, Nonlinear Systems. Prentice-Hall, New Jersey, 1996.

[155] P. Yang, R. A. Freeman, G. J. Gordon, K. M. Lynch, S. S. Srinivasa, and

R. Sukthankar, “Decentralized estimation and control of graph connectivity for
mobile sensor networks,” Automatica, vol. 46, no. 2, pp. 390–396, 2010.

[156] A. De Luca, G. Oriolo, and P. Robuffo Giordano, “Feature depth observation

for image-based visual servoing: Theory and experiments,” The Int. J. of Robotics
Research, vol. 27, no. 10, pp. 1093–1116, 2008.

[157] R. Spica, P. Robuffo Giordano, and F. Chaumette, “Active Structure from

Motion: Application to Point, Sphere and Cylinder,” IEEE Transactions on
Automatic Control, vol. 30, no. 6, pp. 1499–1513, 2014.

[158] E. Rohmer, S. P. N. Singh, and M. Freese, “V-rep: a versatile and scalable

robot simulation framework,” in IEEE International Conference on Intelligent
Robots and Systems, 2013.

[159] V. Grabe, M. Riedel, H. H. Bulthoff, P. Robuffo Giordano, and A. Franchi,

“The TeleKyb framework for a modular and extendible ros-based quadrotor
control,” in IEEE European Conference on Mobile Robots, Barcelona, Spain,
2013, pp. 19–25.

[160] R. Spica, P. R. Giordano, M. Ryll, H. H. Bülthoff, and A. Franchi, “An open-

source hardware/software architecture for quadrotor uavs,” IFAC Proceedings
Volumes, vol. 46, no. 30, pp. 198–205, 2013.

152
 Bibliography

[161] L. Sabattini, C. Secchi, and N. Chopra, “Decentralized connectivity main-

tenance for networked lagrangian dynamical systems,” in IEEE International
Conference on Robotics and Automation. IEEE, 2012, pp. 2433–2438.

[162] S. Zhao and D. Zelazo, “Bearing-Based Distributed Control and Estimation of

Multi-Agent Systems,” in European Control Conference, Linz, Austria, 2015, pp.
2207–2212.

[163] A. R. Vidal, H. Rebecq, T. Horstschaefer, and D. Scaramuzza, “Hybrid, frame

and event based visual inertial odometry for robust, autonomous navigation of
quadrotors,” arXiv preprint arXiv:1709.06310, 2017.

[164] P. Lichtsteiner, C. Posch, and T. Delbruck, “A 128 × 128 120 db 15µs latency
asynchronous temporal contrast vision sensor,” IEEE Journal of solid-state
circuits, vol. 43, no. 2, pp. 566–576, 2008.

[165] M. I. Friswell, “The derivatives of repeated eigenvalues and their associated

eigenvectors,” Journal of Vibration and Acoustics, vol. 118, pp. 390–397, 1996.

[166] X. Chen, H. Qi, L. Qi, and K.-L. Teo, “Smooth convex approximation to the
maximum eigenvalue function,” Journal of Global Optimization, vol. 30, no. 2,
pp. 253–270, 2004.

[167] K. Sreenath, T. Lee, and V. Kumar, “Geometric control and differential

flatness of a quadrotor uav with a cable-suspended load,” in IEEE International
Conference on Decision and Control. IEEE, 2013, pp. 2269–2274.

[168] Z. Wang and M. Schwager, “Kinematic multi-robot manipulation with no

communication using force feedback,” in IEEE International Conference on
Robotics and Automation, 2016, pp. 427–432.

[169] T. Eren, W. Whiteley, and P. N. Belhumeur, “Using angle of arrival (bear-

ing) information in network localization,” in IEEE International Conference on
Decision and Control. IEEE, 2006, pp. 4676–4681.

[170] A. N. Bishop, I. Shames, and B. Anderson, “Stabilization of rigid formations

with direction-only constraints,” in IEEE International Conference on Decision
and Control, 2011, pp. 746–752.

[171] A. Franchi and P. R. Giordano, “Decentralized control of parallel rigid forma-

tions with direction constraints and bearing measurements,” in IEEE Interna-
tional Conference on Decision and Control, 2012, pp. 5310–5317.

153
Bearing-based localization and control for multiple quadrotors

[172] S. Zhao, F. Lin, K. Peng, B. M. Chen, and T. H. Lee, “Distributed control of

angle-constrained cyclic formations using bearing-only measurements,” Systems
& Control Letters, vol. 63, pp. 12–24, 2014.

[173] G. Michieletto, A. Cenedese, and A. Franchi, “Bearing rigidity theory in SE(3),”

in IEEE International Conference on Decision and Control. IEEE, 2016, pp.
5950–5955.

[174] A. Martinelli, “Vision and IMU data fusion: Closed-form solutions for attitude,
speed, absolute scale, and bias determination,” IEEE Transactions on Robotics,
vol. 28, no. 1, pp. 44–60, 2012.

[175] R. K. Williams and G. S. Sukhatme, “Observability in topology-constrained

multi-robot target tracking,” in IEEE International Conference on Robotics and
Automation. IEEE, 2015, pp. 1795–1801.

[176] R. Hermann and A. Krener, “Nonlinear controllability and observability,” IEEE

Transactions on automatic control, vol. 22, no. 5, pp. 728–740, 1977.

[177] A. Isidori, Nonlinear control systems. Springer Science & Business Media,
2013.

[178] F. Bullo and A. D. Lewis, Geometric control of mechanical systems: modeling,

analysis, and design for simple mechanical control systems. Springer Science &
Business Media, 2004, vol. 49.

[179] F. L. Markley and J. L. Crassidis, Fundamentals of spacecraft attitude deter-

mination and control. Springer, 2014, vol. 33.

[180] T. Lee, M. Leok, and N. H. McClamroch, “Nonlinear robust tracking control

of a quadrotor UAV on SE(3),” Asian Journal of Control, vol. 15, no. 2, pp.
391–408, 2013.

[181] G. Loianno, M. Watterson, and V. Kumar, “Visual inertial odometry for quadro-
tors on se(3),” in IEEE International Conference on Robotics and Automation.
IEEE, 2016, pp. 1544–1551.

[182] R. Tron and R. Vidal, “Distributed 3-D localization of camera sensor networks
from 2-D image measurements,” IEEE Transactions on Automatic Control, 2014.

[183] S. I. Roumeliotis and I. M. Rekleitis, “Propagation of uncertainty in cooperative

multirobot localization: Analysis and experimental results,” Autonomous Robots,
vol. 17, no. 1, pp. 41–54, 2004.

154
 Bibliography

[184] L. Luft, T. Schubert, S. I. Roumeliotis, and W. Burgard, “Recursive Decentral-

ized Collaborative Localization for Sparsely Communicating Robots,” Proceedings
of Robotics: Science and Systems, 2016.

[185] T. Lee, “Geometric control of multiple quadrotor uavs transporting a cable-

suspended rigid body,” in IEEE International Conference on Decision and Control.
IEEE, 2014, pp. 6155–6160.

[186] A. Barrau and S. Bonnabel, “The invariant extended kalman filter as a stable
observer,” IEEE Transactions on Automatic Control, vol. 62, no. 4, pp. 1797–1812,
2017.

[187] M. P. do Carmo, Riemannian geometry. Boston, MA: Birkhäuser, 1992.

[188] R. A. Horn and C. R. Johnson, Matrix analysis. Cambridge university press,

2012.

[189] P.-A. Absil, R. Mahony, and R. Sepulchre, Optimization Algorithms on Matrix

Manifolds. Princeton, NJ: Princeton University Press, 2008.

[190] R. Tron, “Distributed optimization on manifolds for consensus algorithms and

camera network localization,” Ph.D. dissertation, The Johns Hopkins University,
2012.

[191] Z. Lendek, R. Babuska, and B. D. Schutter, “Distributed Kalman filtering for

multiagent systems,” IEEE European Control Conference, vol. 19, pp. 2193–2200,
2007.

[192] A. Martinelli, “Nonlinear Unknown Input Observability: Extension of the

Observability Rank Condition,” IEEE Transactions on Automatic Control, vol.
9286, pp. 1–16, 2018.

[193] T. Toksoz, J. Redding, M. Michini, B. Michini, J. P. How, M. Vavrina, and

J. Vian, “Automated battery swap and recharge to enable persistent uav missions,”
in AIAA Infotech@ Aerospace Conference, 2011.

[194] N. K. Ure, G. Chowdhary, T. Toksoz, J. P. How, M. A. Vavrina, and J. Vian,

“An automated battery management system to enable persistent missions with
multiple aerial vehicles,” IEEE/ASME Transactions on Mechatronics, vol. 20,
no. 1, pp. 275–286, 2015.

[195] F. Cocchioni, E. Frontoni, G. Ippoliti, S. Longhi, A. Mancini, and P. Zingaretti,

“Visual based landing for an unmanned quadrotor,” The Int. J. of Robotics
Research, vol. 84, no. 1-4, pp. 511–528, 2016.

155
Bearing-based localization and control for multiple quadrotors

[196] E. Olson, “AprilTag: A robust and flexible visual fiducial system,” in IEEE
International Conference on Robotics and Automation. IEEE, May 2011, pp.
3400–3407.

[197] J. Wang and E. Olson, “AprilTag 2: Efficient and robust fiducial detection,”
in IEEE International Conference on Intelligent Robots and Systems. IEEE,
2016, pp. 4193–4198.

[198] E. Olson, J. Strom, R. Goeddel, R. Morton, P. Ranganathan, and A. Richard-

son, “Exploration and mapping with autonomous robot teams,” Communications
of the ACM, vol. 56, no. 3, pp. 62–70, 2013.

[199] A. Richardson, J. Strom, and E. Olson, “Aprilcal: Assisted and repeatable

camera calibration,” in IEEE International Conference on Intelligent Robots and
Systems. IEEE, 2013, pp. 1814–1821.

[200] M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W.

Achtelik, and R. Siegwart, “The euroc micro aerial vehicle datasets,” The Int. J.
of Robotics Research, vol. 35, no. 10, pp. 1157–1163, 2016.

[201] S. Kyristsis, A. Antonopoulos, T. Chanialakis, E. Stefanakis, C. Linardos,

A. Tripolitsiotis, and P. Partsinevelos, “Towards autonomous modular uav mis-
sions: The detection, geo-location and landing paradigm,” Sensors, vol. 16, no. 11,
p. 1844, 2016.

[202] M. Saska, J. Chudoba, L. Preucil, J. Thomas, G. Loianno, A. Tresnak,

V. Vonasek, and V. Kumar, “Autonomous deployment of swarms of micro-
aerial vehicles in cooperative surveillance,” in IEEE International Conference on
Unmanned Aircraft Systems, 2014, pp. 584–595.

[203] M. Saska, T. Baca, J. Thomas, J. Chudoba, L. Preucil, T. Krajnik, J. Faigl,

G. Loianno, and V. Kumar, “System for deployment of groups of unmanned
micro aerial vehicles in gps-denied environments using onboard visual relative
localization,” Autonomous Robots, pp. 1–26, 2016.

[204] S. Zhao, “Control and navigation of multi-vehicle systems using visual mea-
surements,” Ph.D. dissertation, National University of Singapore, 2014.

[205] T. Nestmeyer, P. R. Giordano, H. H. Bülthoff, and A. Franchi, “Decentralized

simultaneous multi-target exploration using a connected network of multiple
robots,” Autonomous Robots, vol. 41, no. 4, pp. 989–1011, 2017.

[206] M. A. Fischler and R. C. Bolles, “Random sample consensus: a paradigm for

model fitting with applications to image analysis and automated cartography,”
Communications of the ACM, vol. 24, no. 6, pp. 381–395, 1981.

156
 Bibliography

[207] C. Nissler, B. Stefan, Z.-C. Marton, L. Beckmann, U. Thomasy et al., “Eval-

uation and improvement of global pose estimation with multiple apriltags for
industrial manipulators,” in Emerging Technologies and Factory Automation
(ETFA), 2016 IEEE 21st International Conference on. IEEE, 2016, pp. 1–8.

[208] R. Spica, G. Claudio, F. Spindler, and P. Robuffo Giordano, “Interfacing

Matlab/Simulink with V-REP for an easy development of sensor-based control
algorithms for robotic platforms,” in IEEE International Conference on Robotics
and Automation Workshop on MATLAB/Simulink for Robotics Education and
Research, 2014.

[209] A. Mallet, C. Pasteur, M. Herrb, S. Lemaignan, and F. Ingrand, “GenoM3:

Building middleware-independent robotic components,” in IEEE International
Conference on Robotics and Automation. IEEE, 2010, pp. 4627–4632.

[210] F. Schiano, J. Alonso-Mora, K. Rudin, P. Beardsley, R. Siegwart, and S. B,

“Towards estimation and correction of wind effects on a quadrotor UAV,” in
International Micro Air Vehicle Conference, 2014, pp. 134–141.

157
 Vita

Fabrizio Schiano received his BSc degree in 2010 and MSc degree (highest honors) in
2013 in Automation Engineering from the Università degli Studi di Napoli Federico
II in Naples, Italy. He spent 6 months as a visiting MSc student at the Swiss Federal
Institute of Technology in Zurich (ETH) and Disney Research Zurich (DRZ) [210].
He worked for 13 months as a Research Scientist at the Zentrum für Telematik
(ZFT) in Würzburg, Germany. He is currently a Ph.D. student at INRIA Rennes
Bretagne Atlantique and Université de Rennes 1 in Rennes, France. During his
Ph.D. he spent 6 months at Boston University as a Ph.D. visiting researcher in
Boston (MA), USA.

159
Résumé
Le but de cette thèse est d’étendre l’état de l’art par des contributions sur le
comportement collectif d’un groupe de robots volants, à savoir des quadrirotors UAV.
Afin de pouvoir sûrement naviguer dans un environnement, ces derniers peuvent se
reposer uniquement sur leurs capacités à bord et non sur des systèmes centralisés
(e.g., Vicon ou GNSS). Nous réalisons cet objectif en offrant une possible solution
aux problèmes de contrôle en formation et de localisation à partir de mesures à
bord et via une communication locale. Nous abordons ces problèmes exploitant
différents concepts provenant de la théorie des graphes algébriques et de la théorie de
la rigidité. Cela nous permet de résoudre ces problèmes de façon décentralisée et de
proposer des algorithmes décentralisés capables de prendre en compte également des
limites sensorielles classiques. Les capacités embarquées que nous avons mentionnées
plus tôt sont représentées par une caméra monoculaire et une centrale inertielle
(IMU) auxquelles s’ajoute la capacité de chaque robot à communiquer (par RF)
avec certains de ses voisins. Cela est dû au fait que l’IMU et la caméra représentent
une possible configuration économique et légère pour la navigation et la localisation
autonome d’un quadrotor UAV.

Abstract
The aim of this Thesis is to give contributions to the state of the art on the collective
behavior of a group of flying robots, specifically quadrotor UAVs, which can only rely
on their onboard capabilities and not on a centralized system (e.g., Vicon or GNSS) in
order to safely navigate in the environment. We achieve this goal by giving a possible
solution to the problems of formation control and localization from onboard sensing
and local communication. We tackle these problems exploiting mainly concepts from
algebraic graph theory and the so-called theory of rigidity. This allows us to solve
these problems in a decentralized fashion, and propose decentralized algorithms able
to also take into account some typical sensory limitations. The onboard capabilities
we referred to above are represented by an onboard monocular camera and an inertial
measurement unit (IMU) in addition to the capability of each robot to communicate
(through RF) with some of its neighbors. This is due to the fact that an IMU and a
camera represent a possible minimal, lightweight and inexpensive configuration for
the autonomous localization and navigation of a quadrotor UAV.
View publication stats