Cooperative Visual-SLAM System for UAV-Based Target Tracking in GPS-Denied Environments: A Target-Centric Approach

Abstract

Autonomous tracking of dynamic targets by the use of Unmanned Aerial Vehicles (UAVs) is a challenging problem that has practical applications in many scenarios. In this context, a fundamental aspect that must be addressed has to do with the position estimation of aerial robots and a target to control the flight formation. For non-cooperative targets, their position must be estimated using the on-board sensors. Moreover, for estimating the position of UAVs, global position information may not always be available (GPS-denied environments). This work presents a cooperative visual-based SLAM (Simultaneous Localization and Mapping) system that allows a team of aerial robots to autonomously follow a non-cooperative target moving freely in a GPS-denied environment. One of the contributions of this work is to propose and investigate the use of a target-centric SLAM configuration to solve the estimation problem that differs from the well-known World-centric and Robot-centric SLAM configurations. In this sense, the proposed approach is supported by theoretical results obtained from an extensive nonlinear observability analysis. Additionally, a control system is proposed for maintaining a stable UAV flight formation with respect to the target as well. In this case, the stability of control laws is proved using the Lyapunov theory. Employing an extensive set of computer simulations, the proposed system demonstrated potentially to outperform other related approaches.

1. Introduction

Nowadays, Unmanned Aerial Vehicles (UAVs), computer vision techniques, and flight control systems have received great attention from the research community in robotics. This interest has as a consequence the development of systems with a high degree of autonomy. UAVs are very versatile platforms. In particular, aerial robots with rotary wings, as the quadcopters, allow great flexibility of movements, which makes them very useful for several tasks and applications [1,2]. Multi-robot systems have also received great attention from the robotics research community. This attention is motivated by the inherent versatility that those systems have to perform tasks that could present difficulties to be realized by a single robot. The use of several robots can have advantages such as an increase of robustness, better performance, and efficiency [3,4].

In this context, one important research problem is the control and coordination of a team of UAVs flying in formation with respect to a non-cooperative moving target. In this context, a fundamental task that must be addressed to control the flight formation is the estimation of the positions of UAVs and the moving target. For most applications, GPS (Global Positioning System) still represents the main alternative for addressing the localization problem of UAVs. Nevertheless, the use of GPS presents some drawbacks, for instance, in scenarios where GPS signals are jammed intentionally [5] or when the precision error is substantial and they provide poor operability due to multipath propagation (natural and urban canyons [6,7]). In addition, there exist scenarios (e.g., indoor) where GPS is completely unavailable. Moreover, for non-cooperative targets, their position has to be estimated using on-board sensors.

In such scenarios, when considering a team of aerial robots flying in formation with respect to a particular target, the absolute pose (e.g., world-centric configuration) is not so important compared to the relative position information between the aerial vehicles and the target (e.g., Robot-centric configuration). In this case, sensors can help to estimate the required position information such as range sensors (laser or sonar) and radio-frequency (RF) tag-like-sensors (see [8,9,10,11]). However, these kinds of sensors are expensive and sometimes heavy, and their use in outdoor environments is limited somehow. Moreover, some of these sensors need the cooperation of the moving target. Active laser systems (e.g., LiDAR [12]) represent a very interesting sensing technology; they can operate under any visibility condition (i.e., both day and night, unlike cameras) and can directly provide 3D measurements about the surrounding environment. On the other hand, LiDAR is generally expensive overweighting the system for certain applications presenting moving parts which can induce some errors. Stereo systems [13] and depth cameras [14] also can obtain 3D information about the target and the environment; however, this kind of system requires that the objects to be measured are near the sensor, thus considerably limiting their range of application.

In the above context, more works have appeared focusing on the use of cameras to develop navigation systems based on visual information that can operate in periods or circumstances where the GPS is unavailable. Cameras are well suited for their use in embedded systems, and also can be used for estimating the target relative position.

1.1. Related Work

In this work, the use of a cooperative visual-based SLAM scheme is proposed, for addressing the problem of estimating the relative position of the aerial robots with respect to one target, to control the flight formation with respect to the same target.

In the literature, different approaches can be found that use visual information to carry out the control of UAVs (Visual-Servoing): [15,16,17,18,19]. In addition, Refs. [20,21,22,23] are examples of approaches where the position of a target is estimated in a probabilistic manner by means of the fusion of monocular measurements from two or more cameras. These works are only focused on the estimation of the target position and assume that there exists an ideal control scheme that lead the robots towards the moving target. In [24,25,26,27,28], different control schemes are presented for addressing the problem of maintaining a desired flight formation with respect to a moving target. A compilation of different techniques for addressing the flight formation control problem is presented in [29]. One of the underlying assumptions in these works is that the position of vehicles is available, either globally referenced or locally referenced.

In [30,31,32], methods for the tracking, control, and estimation of a target using a UAV with a monocular camera on board are presented. In these works, it is assumed that the geometry of the target is known or its movement is restricted to a plane. In [33,34], tracking control schemes using multi-robot and single-robot configurations are presented. These works make the strong assumption that the relative distances of the vehicles with respect to the target are known. In [35], a scheme for tracking control and estimation of a target using a small UAVs with monocular cameras on-board is presented. In this work, it is assumed that there are available measurements of the altitude of the vehicles with respect to the target. Another single-robot scheme is presented in [36]. This work relies on stereo-vision; however, as it will be shown later, the use of this sensor is unreliable in the present problem that authors state. In [37], a multi-robot system based on an LiDAR is presented. In [38], a method to control an space robot that follows an uncooperative target using a vision system is presented. The estimation of the position of the target is carried out using an adaptive unscented Kalman filter based on measurements obtained from an homography. In this case, to obtain the homography, the coplanar visual landmarks need to be extracted from the target.

In [39], a single-robot SLAM scheme, where the state of the dynamic target is included in the system state, is presented. In this case, the position of the robot, the map, and the target are estimated using a constrained local submap filter (CLSF) based on an EKF (Extended Kalman Filter) configuration. In [40], the problem of cooperative localization and target tracking with a team of moving robots is addressed. The problem is modeled as a least-squares minimization problem and solved using sparse optimization. Static known landmarks are used to define a common reference frame for the robots and the targets. However, in many applications, it is complicated or even impossible to have prior knowledge about the position of landmarks. In [41], an approach is presented for estimating the position of objects tracked by a team of mobile robots and the use of such objects for a better self-localization. In this case, relationship among objects is used to estimate objects’ position given a map. However, the geometric knowledge of objects is required in order to carry out the location of the robots. In [42], a control scheme for commanding the formation with respect to a target is presented. In this case, the local sensor information is used to estimate the relative position and orientation of the robots with respect to the target (target-centric configuration).

1.2. Objectives and Contributions

Recently, some works, such as [43], present the development of observers and controllers for relative estimation and circumnavigation of a target using bearing-only measurements or range with bearing measurements. In that work, the movement of the target is restricted to a plane. In addition, one of the assumptions is that range measurements with respect to the target are available, which is very difficult to obtain for non-cooperative targets. In [44], a Gaussian sum FIR filter (GSFF) to estimate the position of a target is presented. In this work, both estimation and tracking of the target are carried out only in a two-dimensional space. In [45], a UAV-based target tracking and its recognition system are presented. In this work, a geographic information system (GIS) is used to provide geo-location, environmental, and contextual information. The work in [46] presents a distributed control strategy applied to a team of agents to create a prescribed formation with its centroid tracking a moving target. In this work, one of the assumptions is that some relative position measurements between agents and the target are available.

To try avoiding the restrictions given in the previous approaches, in this present work, the use of a cooperative visual-based SLAM scheme is studied for addressing the problem of estimating the relative position of the aerial robots with respect to a non-cooperative dynamic target in GPS-denied environments. The general idea is to use a priori unknown static natural landmarks, randomly distributed in the environment, as reference points for locating UAVs with respect to a dynamic target moving freely in 3D space. The above objective is achieved using only monocular measurements of the target and landmarks, as well as measurements of altitude differential among UAVs. The proposed scheme by authors does not need any geometric knowledge of the moving target nor the landmarks. In addition, there are no assumptions about some cooperative behavior of the target.

The configuration of the proposed cooperative visual-based SLAM system is based on the standard EKF-SLAM (Extended Kalman Filter SLAM) methodology. In this context, it is extremely important to provide the proposed system with properties such as observability and robustness to ill-constrained initial conditions. The above properties have a fundamental role in the convergence of the filter, as shown in [47,48]. Therefore, an extensive nonlinear observability test is carried out in order to analyze the system. In this case, novel, contributive and important theoretical results are presented from this analysis.

In this work, two innovations are proposed to improve the observability properties of the system, and thus its performance. Firstly, the use of a target-centric SLAM configuration is proposed instead of the use of a more common World-centric or Robot-centric SLAM configuration. In the target-centric SLAM configuration, the system state is parameterized with respect to the target position. Secondly, measurements of altitude differential between pairs of UAVs obtained from altimeters are integrated into the system.

Another key difference from most related works is presented; typically, those works focus only on addressing one side of the dual nature of the estimation-control problem. In this work, additionally to the proposed SLAM estimation technique, a flight formation control scheme based on a back-stepping approach [49] is also proposed, to allow carrying out the formation of quadcopters with respect to the target. In this case, the stability of control laws is proved using the Lyapunov theory. In simulations, the state estimated by the SLAM system is used as a feedback to the proposed control scheme to test the closed-loop performance of both estimator and control.

1.3. Paper Outline

The document is organized in the following manner: Section 2 presents the general system specifications and mathematical models. Section 3 presents the nonlinear observability analysis. In Section 4, the proposed SLAM method is described. Section 5 presents the proposed control system. Section 6 shows the numerical simulations results, and finally, in Section 7, the conclusions and final remarks of this work are given.

2. System Specification

In this section, the mathematical models used in this work are introduced. Firstly, the model used for representing the dynamics of the relative position of the j-th UAV-camera system with respect to the target is described. Then, the model representation of the relative position of the i-th landmark with respect to the target is described. In addition, the measurement models used in this work are presented: (i) the camera projection model and (ii) the relative altitude model. Finally, the dynamic model of a quadcopter is presented as well.

In applications like aerial vehicles, the attitude and heading (roll, pitch, and yaw) estimation is well handled by available systems (e.g., [50,51]). In particular, in this work, it is assumed that the orientation of the camera is always pointing toward the ground. Considering the above assumption, the system state can be simplified by removing the variables related to attitude and heading (which are provided by the AHRS). In addition, therefore, the problem can be focused on relative position estimation. In addition, it is important to note that, with this assumption, the mathematical formulation is kept simple enough to make the observability analysis of Section 3 a tractable problem. In practice, the looking-downward camera assumption can be easily addressed, for instance, with the use of a servo-controlled camera gimbal.

Regarding the visual sensors required for implementing the proposed system, the only assumption is that a set of visual features of the environment, and one visual feature of the target, can be detected and tracked consistently using any available computer vision algorithm.

2.1. Dynamics of the System

Let consider the following continuous-time model describing the dynamics of the proposed system (see Figure 1):

$$\dot{\mathbf{x}}=\left[\begin{array}{c}{}^{\mathbf{t}}{\dot{\mathbf{x}}}_{\mathbf{c}}^{\mathit{1}}\hfill \\ {}^{\mathbf{t}}{\dot{\mathbf{v}}}_{\mathbf{c}}^{\mathit{1}}\hfill \\ :\hfill \\ {}^{\mathbf{t}}{\dot{\mathbf{x}}}_{\mathbf{c}}^j\hfill \\ {}^{\mathbf{t}}{\dot{\mathbf{v}}}_{\mathbf{c}}^j\hfill \\ {\dot{\mathbf{v}}}_{\mathbf{t}}\hfill \\ {}^{\mathbf{t}}{\dot{\mathbf{x}}}_{\mathbf{a}}^{\mathit{1}}\hfill \\ :\hfill \\ {}^{\mathbf{t}}{\dot{\mathbf{x}}}_{\mathbf{a}}^i\hfill \end{array}\right]=\left[\begin{array}{c}{\dot{\mathbf{x}}}_{\mathbf{c}}^{\mathit{1}}-{\dot{\mathbf{x}}}_{\mathbf{t}}\hfill \\ {\dot{\mathbf{v}}}_{\mathbf{c}}^{\mathit{1}}-{\dot{\mathbf{v}}}_{\mathbf{t}}\hfill \\ :\hfill \\ {\dot{\mathbf{x}}}_{\mathbf{c}}^j-{\dot{\mathbf{x}}}_{\mathbf{t}}\hfill \\ {\dot{\mathbf{v}}}_{\mathbf{c}}^j-{\dot{\mathbf{v}}}_{\mathbf{t}}\hfill \\ {\mathbf{0}}_{3\times 1}\hfill \\ {\dot{\mathbf{x}}}_{\mathbf{a}}^{\mathit{1}}-{\dot{\mathbf{x}}}_{\mathbf{t}}\hfill \\ :\hfill \\ {\dot{\mathbf{x}}}_{\mathbf{a}}^i-{\dot{\mathbf{x}}}_{\mathbf{t}}\hfill \end{array}\right]=\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{c}}^{\mathit{1}}-{\mathbf{v}}_{\mathbf{t}}\hfill \\ {\mathbf{0}}_{3\times 1}\hfill \\ :\hfill \\ {\mathbf{v}}_{\mathbf{c}}^j-{\mathbf{v}}_{\mathbf{t}}\hfill \\ {\mathbf{0}}_{3\times 1}\hfill \\ {\mathbf{0}}_{3\times 1}\hfill \\ {\mathbf{v}}_{\mathbf{a}}^{\mathit{1}}-{\mathbf{v}}_{\mathbf{t}}\hfill \\ :\hfill \\ {\mathbf{v}}_{\mathbf{a}}^i-{\mathbf{v}}_{\mathbf{t}}\hfill \end{array}\right]=\left[\begin{array}{c}{}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^{\mathit{1}}\hfill \\ {\mathbf{0}}_{3\times 1}\hfill \\ :\hfill \\ {}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^j\hfill \\ {\mathbf{0}}_{3\times 1}\hfill \\ {\mathbf{0}}_{3\times 1}\hfill \\ -{\mathbf{v}}_{\mathbf{t}}\hfill \\ :\hfill \\ -{\mathbf{v}}_{\mathbf{t}}\hfill \end{array}\right]$$

(1)

where the state vector $\mathbf{x}$ is defined by:

$$\mathbf{x}={\left[\begin{array}{ccccccccc}{}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^{\mathit{1}}\hfill & {}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^{\mathit{1}}& \cdots & {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j& {}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^j& {\mathbf{v}}_{\mathbf{t}}& {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^{\mathit{1}}& \cdots & {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^i\hfill \end{array}\right]}^{\mathrm{T}}$$

(2)

with $i=1,...,n_1$ and $j=1,...,n_2$, where $n_1$ and $n_2$ are respectively the number of landmarks included into the map and the number of UAV-camera systems.

Additionally, let ${\mathbf{x}}_{\mathbf{t}}={\left[\begin{array}{ccc}{x}_t\hfill & y_t& z_t\end{array}\right]}^{\mathrm{T}}$ represent the position (in meters) of the target, with respect to the reference system W. Let ${\mathbf{x}}_{\mathbf{c}}^j={\left[\begin{array}{ccc}{x}_c^j\hfill & y_c^j& z_c^j\end{array}\right]}^{\mathrm{T}}$ represent the position (in meters) of the reference system C of the j-th camera, with respect to the reference system W. Let ${\mathbf{v}}_{\mathbf{t}}={\left[\begin{array}{ccc}{\dot{x}}_t\hfill & {\dot{v}}_t& {\dot{z}}_t\end{array}\right]}^{\mathrm{T}}$ represent the linear velocity (in $\frac{m}{s}$) of the target, with respect to the reference system W. Let ${\mathbf{v}}_{\mathbf{c}}^j={\left[\begin{array}{ccc}{\dot{x}}_c^j\hfill & {\dot{y}}_c^j& {\dot{z}}_c^j\end{array}\right]}^{\mathrm{T}}$ represent the linear velocity (in $\frac{m}{s}$) of the j-th camera, with respect to the reference system W. Let ${\mathbf{x}}_{\mathbf{a}}^i={\left[\begin{array}{ccc}{x}_a^i\hfill & y_a^i& z_a^i\end{array}\right]}^{\mathrm{T}}$ be the position (in meters) of the i-th landmark with respect to the reference system W, defined by its Euclidean parameterization. Let ${\mathbf{v}}_{\mathbf{a}}^i={\left[\begin{array}{ccc}{\dot{x}}_a^i\hfill & {\dot{y}}_a^i& {\dot{z}}_a^i\end{array}\right]}^{\mathrm{T}}$ represent the linear velocity (in $\frac{m}{s}$) of the i-th landmark, with respect to the reference system W. Let ${}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j={\left[\begin{array}{ccc}{}^t{x}_c^j\hfill & {}^t{y}_c^j& {}^t{z}_c^j\end{array}\right]}^{\mathrm{T}}$ represent the relative position (in meters) of the j-th camera with respect to the reference system T. Let ${}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^i={\left[\begin{array}{ccc}{}^t{x}_a^i\hfill & {}^t{y}_a^i& {}^t{z}_a^i\end{array}\right]}^{\mathrm{T}}$ represent the relative position (in meters) of the i-th landmark with respect to the reference system T. Finally, let ${}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^j={\left[\begin{array}{ccc}{}^t{\dot{x}}_c^j\hfill & {}^t{\dot{y}}_c^j& {}^t{\dot{z}}_c^j\end{array}\right]}^{\mathrm{T}}$ represent the relative linear velocity (in $\frac{m}{s}$) of the j-th camera with respect to the reference system T. Figure 1 illustrates the coordinate reference systems used in this work. In Equation (1), each UAV-camera, as well as the target, is assumed to move freely in the three-dimensional space. Let note that a non-acceleration kinematic model is assumed for the UAV-camera systems and the target. In addition, note that the landmarks are assumed to remain static. Given the above ${\mathbf{v}}_{\mathbf{a}}^i=\mathbf{0}$.

2.2. Camera Measurement Model for Landmarks

Let us consider the projection of a single landmark over the image plane of a camera. Using the pinhole model [52], the following expression is defined:

$$\begin{array}{c}\hfill {}^i{\mathbf{z}}_{\mathbf{c}}^j={}^i{\mathbf{h}}_{\mathbf{c}}^j=\left[\begin{array}{c}{}^i{u}_c^j\hfill \\ {}^i{v}_c^j\hfill \end{array}\right]=\frac{1}{{}^i{z}_d^j}\left[\begin{array}{cc}\frac{f_c^j}{d_u^j}\hfill & 0\\ 0\hfill & \frac{f_c^j}{d_v^j}\end{array}\right]\left[\begin{array}{c}{}^i{x}_d^j\hfill \\ {}^i{y}_d^j\hfill \end{array}\right]+\left[\begin{array}{c}{c}_u^j+d_{ur}^j+d_{ut}^j\hfill \\ c_v^j+d_{vr}^j+d_{vt}^j\hfill \end{array}\right]\end{array}$$

(3)

Let ${}^i{u}_c^j$ and ${}^i{v}_c^j$ define the coordinates (in pixels) of the projection of the i-th landmark over the image of the j-th camera. Let $f_c^j$ be the focal length (in meters) of the j-th camera. Let $d_u^j$ and $d_v^j$ be the conversion parameters (in $m/pixel$) for the j-th camera. Let $c_u^j$ and $c_v^j$ be the coordinates (in pixels) of the image central point of the j-th camera. Let $d_{ur}^j$ and $d_{vr}^j$ be components (in pixels) accounting for the radial distortion of the j-th camera. Let $d_{ut}^j$ and $d_{vt}^j$ be components (in pixels) accounting for the tangential distortion of the j-th camera. All the intrinsic parameters of the j-th camera are assumed to be known by means of some calibration method. Let ${}^i{\mathbf{p}}_{\mathbf{d}}^j={\left[\begin{array}{ccc}{}^i{x}_d^j\hfill & {}^i{y}_d^j& {}^i{z}_d^j\hfill \end{array}\right]}^{\mathrm{T}}$ represent the position (in meters) of the i-th landmark with respect to the coordinate reference system C of the j-th camera.

Additionally,

$${}^i{\mathbf{p}}_{\mathbf{d}}^j={}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j\left({}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^i{-}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j\right)$$

(4)

where ${}^W{\mathbf{R}}_{\mathbf{c}}^j\in SO3$ is the rotation matrix that transforms from the world coordinate reference system W to the coordinate reference system C of the j-th camera. Recall that the rotation matrix ${}^W{\mathbf{R}}_{\mathbf{c}}^j$ is known and constant assuming the use of the servo-controlled camera gimbal.

2.3. Camera Measurement Model for the Target

Let consider the projection of the target over the image plane of a camera. In this case, it is assumed that some visual feature points can be extracted from the target by means of some available computer vision algorithm like [53,54,55,56,57,58]. Using the pinhole model, the following expression is defined:

$$\begin{array}{c}\hfill {}^{\mathbf{t}}{\mathbf{z}}_{\mathbf{c}}^j{=}^{\mathbf{t}}{\mathbf{h}}_{\mathbf{c}}^j=\left[\begin{array}{c}{}^t{u}_c^j\hfill \\ {}^t{v}_c^j\hfill \end{array}\right]=\frac{1}{{}^t{z}_d^j}\left[\begin{array}{cc}\frac{f_c^j}{d_u^j}\hfill & 0\\ 0\hfill & \frac{f_c^j}{d_v^j}\end{array}\right]\left[\begin{array}{c}{}^t{x}_d^j\hfill \\ {}^t{y}_d^j\hfill \end{array}\right]+\left[\begin{array}{c}{c}_u^j+d_{ur}^j+d_{ut}^j\hfill \\ c_v^j+d_{vr}^j+d_{vt}^j\hfill \end{array}\right]\end{array}$$

(5)

Let ${}^{\mathbf{t}}{\mathbf{p}}_{\mathbf{d}}^j={\left[\begin{array}{ccc}{}^t{x}_d^j\hfill & {}^t{y}_d^j& {}^t{z}_d^j\hfill \end{array}\right]}^{\mathrm{T}}$ represent the position (in meters) of the target with respect to the coordinate reference system C of the j-th camera. Additionally,

$${}^{\mathbf{t}}{\mathbf{p}}_{\mathbf{d}}^j={}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j\left({-}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j\right)$$

(6)

2.4. Altitude Differential Measurement Model

In this work, the altitude differential between a pair of UAVs will be used as a filter update measurement. In this case, the altitude differential between the j-th UAV and the n-th UAV, given by a pair of on-board altimeters in each vehicle, is defined by:

$${}^n{z}_a^j={}^n{h}_a^j={}^t{z}_c^n-{}^t{z}_c^j$$

(7)

2.5. Dynamic Model of the Quadcopter

This section presents the dynamic model of a quadcopter which is composed of a rigid structure and four rotors. The plant has six degrees of freedom: three for the translation and three for the rotation (see Figure 1). Using the Euler–Lagrange formalism and the parametrization with respect to the Tait–Bryan angles, the model can be defined similarly as [59]:

$${\ddot{\mathbf{x}}}_{\mathbf{q}}^j=\left[\begin{array}{c}{\ddot{x}}_q^j\hfill \\ {\ddot{y}}_q^j\hfill \\ {\ddot{z}}_q^j\hfill \end{array}\right]=\frac{{\mu }^j}{m^j}\left[\begin{array}{c}sin\left({\psi }^j\right)sin\left({\varphi }^j\right)+cos\left({\psi }^j\right)sin\left({\theta }^j\right)cos\left({\varphi }^j\right)\hfill \\ sin\left({\psi }^j\right)sin\left({\theta }^j\right)cos\left({\varphi }^j\right)-cos\left({\psi }^j\right)sin\left({\varphi }^j\right)\hfill \\ cos\left({\theta }^j\right)cos\left({\varphi }^j\right)-\frac{g·m^j}{u_q^j·{\mu }^j}\hfill \end{array}\right]u_q^j$$

(8)

$$\begin{array}{c}\hfill {\ddot{\sigma }}^j=\left[\begin{array}{c}{\ddot{\varphi }}^j\hfill \\ {\ddot{\theta }}^j\hfill \\ {\ddot{\psi }}^j\hfill \end{array}\right]={\mathbf{g}}^j+{\mathbf{B}}^j{\mathbf{\tau }}^j=\left[\begin{array}{c}\left(\frac{I_y^j-I_z^j}{I_x^j}\right){\dot{\theta }}^j{\dot{\psi }}^j+\left(\frac{J_p^j}{I_x^j}\right){\dot{\theta }}^j{\Omega }^j\hfill \\ \left(\frac{I_x^j-I_z^j}{I_y^j}\right){\dot{\varphi }}^j{\dot{\psi }}^j+\left(\frac{J_p^j}{I_y^j}\right){\dot{\varphi }}^j{\Omega }^j\hfill \\ \left(\frac{I_x^j-I_y^j}{I_z^j}\right){\dot{\varphi }}^j{\dot{\theta }}^j\hfill \end{array}\right]+\left[\begin{array}{ccc}\frac{{\mu }^j}{I_x^j}\hfill & 0& 0\\ 0\hfill & \frac{{\mu }^j}{I_y^j}& 0\\ 0\hfill & 0& \frac{d^j}{I_z^j}\end{array}\right]\left[\begin{array}{c}{\tau }_{\varphi }^j\hfill \\ {\tau }_{\theta }^j\hfill \\ {\tau }_{\psi }^j\hfill \end{array}\right]\end{array}$$

(9)

Let ${\mathbf{x}}_{\mathbf{q}}^j={[x_q^j,y_q^j,z_q^j]}^{\mathrm{T}}$ be the position (in meters) of the j-th quadcopter, with respect to the reference system W. Let ${\mathbf{\sigma }}^j={[{\varphi }^j,{\theta }^j,{\psi }^j]}^{\mathrm{T}}$ be the Tait–Bryan angles (in radians) of the j-th quadcopter, with respect to the reference system W. Let $m^j$ be the mass (in kg) of the j-th quadcopter. Let ${\mu }^j$ be the thrust factor (in N· s${}^2$) of the j-th quadcopter. Let g be the constant of gravity (in m/s${}^2$). Let $u_q^j$ be the total force (in N) of the j-th quadcopter, supplied by the four rotors over the axis z with respect to the coordinate reference system Q. Let $J_p^j$ be total inertia moment (in N· m · s${}^2$) of the j-th quadcopter, due to the rotors. Let $d^j$ be the drag factor (in N· m · s${}^2$) of the j-th quadcopter. Let $I_x^j$, $I_y^j$, and $I_z^j$ be the inertia moment over each axis (in N· m · s${}^2$) of the j-th quadcopter. Let ${\tau }_{\varphi }^j$, ${\tau }_{\theta }^j$, and ${\tau }_{\psi }^j$ be the torque (in $N·m$) of the j-th quadcopter, supplied by the rotors over each axis. Finally, let ${\Omega }^j$ be the overall propeller rotational speed (in rad/s) of the j-th quadcopter.

The inputs are defined by:

$$\left[\begin{array}{c}{u}_q^j\hfill \\ {\tau }_{\varphi }^j\hfill \\ {\tau }_{\theta }^j\hfill \\ {\tau }_{\psi }^j\hfill \end{array}\right]=\left[\begin{array}{cccc}1\hfill & 1& 1& 1\\ -l^j\hfill & 0& l^j& 0\\ 0\hfill & l^j& 0& -l^j\\ 1\hfill & -1& 1& -1\end{array}\right]\left[\begin{array}{c}{f}_1^j\hfill \\ f_2^j\hfill \\ f_3^j\hfill \\ f_4^j\hfill \end{array}\right]$$

(10)

where

$$f_m^j={\left(w_m^j\right)}^2$$

(11)

$${\Omega }^j=w_1^j-w_2^j+w_3^j-w_4^j$$

(12)

In addition, $m=\{1,...,4\}$. Let $l^j$ be the distance (in m) from the center of mass of the j-th quadcopter to the center of one rotor. Let $f_m^j$ be the thrust force (in N), supplied for the m rotor of the j-th quadcopter. In addition, let $w_m^j$ be the angular velocity (in rad/s) of the m rotor of the j-th quadcopter.

3. Observability Analysis

In this section, the nonlinear observability properties of the proposed system are studied. Observability is an inherent property of a dynamic system and has an important role in the accuracy and stability of its estimation process; moreover, this fact has important consequences in the context of SLAM and the convergence of the EKF.

A system is defined as observable if the initial state ${\mathbf{x}}_{\mathbf{0}}$, at any initial time $t_0$, can be determined given the state transition $\dot{\mathbf{x}}=\mathbf{f}\left(\mathbf{x}\right)$, the observation model $\mathbf{y}=\mathbf{h}\left(\mathbf{x}\right)$ of the system, and observations $\mathbf{z}[t_0,t]$ from time $t_0$ to a finite time t. In Hermann and Krener [60], it is demonstrated that a nonlinear system is locally weakly observable if the observability rank condition $rank\left(\mathcal{O}\right)=dim\left(\mathbf{x}\right)$ is verified, where $\mathcal{O}$ is the observability matrix.

3.1. Observability Matrix

The observability matrix $\mathcal{O}$ can be computed from:

$$\begin{array}{c}\hfill \mathcal{O}={\left[\begin{array}{ccccccccc}\frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^0\left({}^i{\mathbf{h}}_{\mathbf{c}}^j\right)\right)}{\partial \mathbf{x}}\hfill & \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^1\left({}^i{\mathbf{h}}_{\mathbf{c}}^j\right)\right)}{\partial \mathbf{x}}& \cdots & \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^0\left({}^{\mathbf{t}}{\mathbf{h}}_{\mathbf{c}}^j\right)\right)}{\partial \mathbf{x}}& \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^1\left({}^{\mathbf{t}}{\mathbf{h}}_{\mathbf{c}}^j\right)\right)}{\partial \mathbf{x}}& \cdots & \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^0\left({}^n{h}_a^j\right)\right)}{\partial \mathbf{x}}& \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^1\left({}^n{h}_a^j\right)\right)}{\partial \mathbf{x}}& \cdots \end{array}\right]}^{\mathrm{T}}\end{array}$$

(13)

where ${\mathcal{L}}_{\mathbf{f}}^s\mathbf{h}$ is the s-th-order Lie Derivative [61], of the scalar field $\mathbf{h}$ with respect to the vector field $\mathbf{f}$. In this work, for computing Equation (13), zero-order and first-order Lie derivatives will be used for each kind of measurement.

In case of the measurements given by a monocular camera, according to Equations (3) and (1), the following zero-order Lie derivative is defined for the projections of landmarks:

$$\begin{array}{c}\hfill \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^0\left({}^i{\mathbf{h}}_{\mathbf{c}}^j\right)\right)}{\partial \mathbf{x}}=\left[\begin{array}{cccccccccc}{\mathbf{0}}_{2\times 6(j-1)}\hfill & {-}^i{\mathbf{H}}_{\mathbf{c}}^j·{}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j& {\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 6(n_2-j)}& \mid & {\mathbf{0}}_{2\times 3}& \mid & {\mathbf{0}}_{2\times 3(i-1)}& {}^i{\mathbf{H}}_{\mathbf{c}}^j·{}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j& {\mathbf{0}}_{2\times 3(n_1-i)}\end{array}\right]\end{array}$$

(14)

where

$${}^i{\mathbf{H}}_{\mathbf{c}}^j=\frac{f_c^j}{{\left({}^i{z}_d^j\right)}^2}\left[\begin{array}{ccc}\frac{{}^i{z}_d^j}{d_u^j}\hfill & 0& -\frac{{}^i{x}_d^j}{d_u^j}\\ \hfill & \\ 0\hfill & \frac{{}^i{z}_d^j}{d_v^j}& -\frac{{}^i{y}_d^j}{d_v^j}\end{array}\right]$$

(15)

For the same kind of measurement, the following first-order Lie derivative can be defined:

$$\begin{array}{c}\hfill \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^1\left({}^i{\mathbf{h}}_{\mathbf{c}}^j\right)\right)}{\partial \mathbf{x}}=\left[\begin{array}{cccccccccc}{\mathbf{0}}_{2\times 6(j-1)}\hfill & {}^i{\mathbf{H}}_{\mathbf{dc}}^j& {-}^i{\mathbf{H}}_{\mathbf{c}}^j·{}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j& {\mathbf{0}}_{2\times 6(n_2-j)}& \mid & {}^i{\mathbf{H}}_{\mathbf{c}}^j·{}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j& \mid & {\mathbf{0}}_{2\times 3(i-1)}& {-}^i{\mathbf{H}}_{\mathbf{dc}}^j& {\mathbf{0}}_{2\times 3(n_1-i)}\end{array}\right]\end{array}$$

(16)

where

$${}^i{\mathbf{H}}_{\mathbf{dc}}^j=\left[\begin{array}{ccc}{}^i{\mathbf{H}}_{\mathbf{1}}^j\hfill & {}^i{\mathbf{H}}_{\mathbf{2}}^j& {}^i{\mathbf{H}}_{\mathbf{3}}^j\end{array}\right]{\left({}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j\right)}^2\left({}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^j+{\mathbf{v}}_{\mathbf{t}}\right)$$

(17)

and

$$\begin{array}{c}\hfill {}^i{\mathbf{H}}_{\mathbf{1}}^j=\frac{f_c^j}{d_u^j{\left({}^i{z}_d^j\right)}^2}\left[\begin{array}{ccc}0\hfill & 0& -1\\ 0\hfill & 0& 0\end{array}\right]{}^i{\mathbf{H}}_{\mathbf{2}}^j=\frac{f_c^j}{d_v^j{\left({}^i{z}_d^j\right)}^2}\left[\begin{array}{ccc}0\hfill & 0& 0\\ 0\hfill & 0& -1\end{array}\right]{}^i{\mathbf{H}}_{\mathbf{3}}^j=\frac{f_c^j}{{\left({}^i{z}_d^j\right)}^3}\left[\begin{array}{ccc}-\frac{{}^i{z}_d^j}{d_u^j}\hfill & 0& \frac{2\left({}^i{x}_d^j\right)}{d_u^j}\\ 0\hfill & -\frac{{}^i{z}_d^j}{d_v^j}& \frac{2\left({}^i{y}_d^j\right)}{d_v^j}\end{array}\right]\end{array}$$

(18)

In case of the measurement given by a monocular camera, according to Equatons (5) and (1), the following zero-order Lie derivative is defined for the projections of the target:

$$\begin{array}{c}\hfill \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^0\left({}^{\mathbf{t}}{\mathbf{h}}_{\mathbf{c}}^j\right)\right)}{\partial \mathbf{x}}=\left[\begin{array}{cccccccc}{\mathbf{0}}_{2\times 6(j-1)}\hfill & {-}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{c}}^j·{}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j& {\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 6(n_2-j)}& \mid & {\mathbf{0}}_{2\times 3}& \mid & {\mathbf{0}}_{2\times 3n_1}\end{array}\right]\end{array}$$

(19)

where

$${}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{c}}^j=\frac{f_c^j}{{\left({}^t{z}_d^j\right)}^2}\left[\begin{array}{ccc}\frac{{}^t{z}_d^j}{d_u^j}\hfill & 0& -\frac{{}^t{x}_d^j}{d_u^j}\\ \hfill & \\ 0\hfill & \frac{{}^t{z}_d^j}{d_v^j}& -\frac{{}^t{y}_d^j}{d_v^j}\end{array}\right]$$

(20)

For the same kind of measurement, the following first-order Lie derivative can be defined:

$$\begin{array}{c}\hfill \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^1\left({}^{\mathbf{t}}{\mathbf{h}}_{\mathbf{c}}^j\right)\right)}{\partial \mathbf{x}}=\left[\begin{array}{cccccccc}{\mathbf{0}}_{2\times 6(j-1)}\hfill & {}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{dc}}^j& {-}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{c}}^j·{}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j& {\mathbf{0}}_{2\times 6(n_2-j)}& \mid & {\mathbf{0}}_{2\times 3}& \mid & {\mathbf{0}}_{2\times 3n_1}\end{array}\right]\end{array}$$

(21)

where

$${}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{dc}}^j=\left[\begin{array}{ccc}{}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{1}}^j\hfill & {}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{2}}^j& {}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{3}}^j\end{array}\right]{\left({}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j\right)}^2\left({}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^j\right)$$

(22)

and

$$\begin{array}{c}\hfill {}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{1}}^j=\frac{f_c^j}{d_u^j{\left({}^t{z}_d^j\right)}^2}\left[\begin{array}{ccc}0\hfill & 0& -1\\ 0\hfill & 0& 0\end{array}\right]{}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{2}}^j=\frac{f_c^j}{d_v^j{\left({}^t{z}_d^j\right)}^2}\left[\begin{array}{ccc}0\hfill & 0& 0\\ 0\hfill & 0& -1\end{array}\right]{}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{3}}^j=\frac{f_c^j}{{\left({}^t{z}_d^j\right)}^3}\left[\begin{array}{ccc}-\frac{{}^t{z}_d^j}{d_u^j}\hfill & 0& \frac{2\left({}^t{x}_d^j\right)}{d_u^j}\\ 0\hfill & -\frac{{}^t{z}_d^j}{d_v^j}& \frac{2\left({}^t{y}_d^j\right)}{d_v^j}\end{array}\right]\end{array}$$

(23)

In case of the measurement of the altitude differential, according to Equations (7) and (1), for the zero-order Lie derivative, if $j<n$ (the index of the j-th camera is lesser than the index of the n-th camera)

$$\begin{array}{c}\hfill \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^0\left({}^n{h}_a^j\right)\right)}{\partial \mathbf{x}}=\left[\begin{array}{cccccccc}{\mathbf{0}}_{1\times 6(j-1)}\hfill & -{\mathbf{M}}_{\mathbf{x}}& {\mathbf{0}}_{1\times 6(n-j-1)}& {\mathbf{M}}_{\mathbf{x}}& {\mathbf{0}}_{1\times 6(n_2-n)}& \mid & {\mathbf{0}}_{1\times 3}& \mid {\mathbf{0}}_{1\times 3n_1}\end{array}\right]\end{array}$$

(24)

On the other hand, if $j>n$ (the index of the j-th camera is higher than the index of the n-th camera), then

$$\begin{array}{c}\hfill \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^0\left({}^n{h}_a^j\right)\right)}{\partial \mathbf{x}}=\left[\begin{array}{ccccccccc}{\mathbf{0}}_{1\times 6(n-1)}\hfill & {\mathbf{M}}_{\mathbf{x}}& {\mathbf{0}}_{1\times 6(j-n-1)}& -{\mathbf{M}}_{\mathbf{x}}& {\mathbf{0}}_{1\times 6(n_2-j)}& \mid & {\mathbf{0}}_{1\times 3}& \mid & {\mathbf{0}}_{1\times 3n_1}\end{array}\right]\end{array}$$

(25)

and for both cases

$${\mathbf{M}}_{\mathbf{x}}=\left[\begin{array}{ccc}{\mathbf{0}}_{1\times 2}\hfill & 1& {\mathbf{0}}_{1\times 3}\end{array}\right]$$

(26)

For the first-order Lie derivative, if $j<n$:

$$\begin{array}{c}\hfill \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^1\left({}^n{h}_a^j\right)\right)}{\partial \mathbf{x}}=\left[\begin{array}{ccccccccc}{\mathbf{0}}_{1\times 6(j-1)}\hfill & -{\mathbf{M}}_{\mathbf{dx}}& {\mathbf{0}}_{1\times 6(n-j-1)}& {\mathbf{M}}_{\mathbf{dx}}& {\mathbf{0}}_{1\times 6(n_2-n)}& \mid & {\mathbf{0}}_{1\times 3}& \mid & {\mathbf{0}}_{1\times 3n_1}\end{array}\right]\end{array}$$

(27)

In addition, if $j>n$:

$$\begin{array}{c}\hfill \frac{\partial \left({\mathcal{L}}_{\mathbf{f}}^1\left({}^n{h}_a^j\right)\right)}{\partial \mathbf{x}}=\left[\begin{array}{ccccccccc}{\mathbf{0}}_{1\times 6(n-1)}\hfill & {\mathbf{M}}_{\mathbf{dx}}& {\mathbf{0}}_{1\times 6(j-n-1)}& -{\mathbf{M}}_{\mathbf{dx}}& {\mathbf{0}}_{1\times 6(n_2-j)}& \mid & {\mathbf{0}}_{1\times 3}& \mid & {\mathbf{0}}_{1\times 3n_1}\end{array}\right]\end{array}$$

(28)

with

$${\mathbf{M}}_{\mathbf{dx}}=\left[\begin{array}{cc}{\mathbf{0}}_{1\times 5}\hfill & 1\end{array}\right]$$

(29)

Using the Lie derivatives described above, the observability matrix for the proposed system Equation (1) can be defined as follows:

$$\mathcal{O}=\left[\begin{array}{lllllllll}{\mathbf{0}}_{2\times 6(j-1)}& {-}^i{\mathbf{H}}_{\mathbf{c}}^j·{}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j{\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 6(n_2-j)}& \mid & {\mathbf{0}}_{2\times 3}& \mid & {\mathbf{0}}_{2\times 3(i-1)}& {}^i{\mathbf{H}}_{\mathbf{c}}^j·{}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j& {\mathbf{0}}_{2\times 3(n_1-i)}\\ {\mathbf{0}}_{2\times 6(j-1)}& {}^i{\mathbf{H}}_{\mathbf{dc}}^j{-}^i{\mathbf{H}}_{\mathbf{c}}^j·{}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j& {\mathbf{0}}_{2\times 6(n_2-j)}& \mid & {}^i{\mathbf{H}}_{\mathbf{c}}^j·{}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j& \mid & {\mathbf{0}}_{2\times 3(i-1)}& {-}^i{\mathbf{H}}_{\mathbf{dc}}^j& {\mathbf{0}}_{2\times 3(n_1-i)}\\ & ⋮& & & ⋮& & & ⋮& \\ {\mathbf{0}}_{2\times 6(j-1)}& {-}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{c}}^j·{}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j{\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 6(n_2-j)}& \mid & {\mathbf{0}}_{2\times 3}& \mid & & {\mathbf{0}}_{2\times 3n_1}& \\ {\mathbf{0}}_{2\times 6(j-1)}& {}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{dc}}^j{-}^{\mathbf{t}}{\mathbf{H}}_{\mathbf{c}}^j·{}^{\mathbf{W}}{\mathbf{R}}_{\mathbf{c}}^j& {\mathbf{0}}_{2\times 6(n_2-j)}& \mid & {\mathbf{0}}_{2\times 3}& \mid & & {\mathbf{0}}_{2\times 3n_1}& \\ & ⋮& & & ⋮& & & ⋮& \\ {\mathbf{0}}_{1\times 6(j-1)}& -{\mathbf{M}}_{\mathbf{x}}{\mathbf{0}}_{1\times 6(n-j-1)}{\mathbf{M}}_{\mathbf{x}}& {\mathbf{0}}_{1\times 6(n_2-n)}& \mid & {\mathbf{0}}_{1\times 3}& \mid & & {\mathbf{0}}_{1\times 3n_1}& \\ {\mathbf{0}}_{1\times 6(j-1)}& -{\mathbf{M}}_{\mathbf{dx}}{\mathbf{0}}_{1\times 6(n-j-1)}{\mathbf{M}}_{\mathbf{dx}}& {\mathbf{0}}_{1\times 6(n_2-n)}& \mid & {\mathbf{0}}_{1\times 3}& \mid & & {\mathbf{0}}_{1\times 3n_1}& \\ & ⋮& & & ⋮& & & ⋮& \\ {\mathbf{0}}_{1\times 6(n-1)}& {\mathbf{M}}_{\mathbf{x}}{\mathbf{0}}_{1\times 6(j-n-1)}-{\mathbf{M}}_{\mathbf{x}}& {\mathbf{0}}_{1\times 6(n_2-j)}& \mid & {\mathbf{0}}_{1\times 3}& \mid & & {\mathbf{0}}_{1\times 3n_1}& \\ {\mathbf{0}}_{1\times 6(n-1)}& {\mathbf{M}}_{\mathbf{dx}}{\mathbf{0}}_{1\times 6(j-n-1)}-{\mathbf{M}}_{\mathbf{dx}}& {\mathbf{0}}_{1\times 6(n_2-j)}& \mid & {\mathbf{0}}_{1\times 3}& \mid & & {\mathbf{0}}_{1\times 3n_1}& \\ & ⋮& & & ⋮& & & ⋮& \end{array}\right]$$

(30)

3.2. Theoretical Results

Three different cases of system configurations were analyzed. The idea is to study how the observability of the system is affected due to the availability (or unavailability) of the different types of measurements. The cases to be considered are:

• Case 1: The following measurements are available: (i) monocular measurements of the projection of landmarks over each UAV-camera system, (ii) multiple (two or more) monocular measurements of the projection of the target over each UAV-camera system.
• Case 2: The following measurements are available: (i) monocular measurements of projection of landmarks over each UAV-camera system, (ii) a single monocular measurement of the projection of the target over a UAV-camera system, and (iii) the altitude differential measurements between UAV-camera systems.
• Case 3: The following measurements are available: (i) monocular measurements of the projection of landmarks over each UAV-camera system, (ii) multiple (two or more) monocular measurements of the projection of the target over each UAV-camera system, and (iii) the altitude differential measurements between UAV-camera systems.

3.2.1. Case 1

For the first case, considering only the respective derivatives on the observability matrix, Equation (30), the maximum rank of the observability matrix $\mathcal{O}$ is $rank\left(\mathcal{O}\right)=(3n_1+6n_2+3)-1$, where $n_1$ is the number of landmarks being measured, $n_2$ is the number of robots, and 3 is the number of states of the linear velocity of the target. In this case, $n_1$ is multiplied by 3, since this is the number of states per landmark, and $n_2$ is multiplied by 6, since this is the number of states per robot. Therefore, $\mathcal{O}$ will be rank deficient ($rank\left(\mathcal{O}\right)<dim\left(\mathbf{x}\right)$). The unobservable modes are spanned by the right nullspace basis of the observability matrix $\mathcal{O}$:

$$\begin{array}{c}\hfill {\mathbf{N}}_{\mathbf{1}}=\mathit{null}\left(\mathcal{O}\right)={\left[\begin{array}{ccccccccccc}{}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^{\mathit{1}}\hfill & {}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^{\mathit{1}}& \cdots & {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j& {}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^j& \mid & {\mathbf{v}}_{\mathbf{t}}& \mid & {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^{\mathit{1}}& \cdots & {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^i\end{array}\right]}^{\mathrm{T}}=\mathbf{x}\end{array}$$

(31)

It is straightforward to verify that the right nullspace basis of $\mathcal{O}$ spans for ${\mathbf{N}}_{\mathbf{1}}$, (i.e., $\mathcal{O}{\mathbf{N}}_{\mathbf{1}}=\mathbf{0}$). From Equation (31), it can be seen that the unobservable modes cross through all states; therefore, all states are unobservable. It should be noted that, adding Lie derivatives of higher-order to the observability matrix, the previous result does not improve.

3.2.2. Case 2

For the second case, the maximum rank of the observability matrix $\mathcal{O}$ is $rank\left(\mathcal{O}\right)=(3n_1+6n_2+3)-2$. Therefore, $\mathcal{O}$ will be rank deficient ($rank\left(\mathcal{O}\right)<dim\left(\mathbf{x}\right)$). In this case, the unobservable modes are spanned by the following right nullspace basis of the observability matrix $\mathcal{O}$:

$$\begin{array}{c}\hfill {\mathbf{N}}_{\mathbf{2}}=\mathit{null}\left(\mathcal{O}\right)={\left[\begin{array}{ccccccccccc}{\mathbf{0}}_{3\times 1}\hfill & {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j& \cdots & {\mathbf{0}}_{3\times 1}& {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j& \mid & {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j& \mid & {\mathbf{0}}_{3\times 1}& \cdots & {\mathbf{0}}_{3\times 1}\\ {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j\hfill & {\left[{}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j{\times }^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^j\right]}_{\times }\mathbf{z}& \cdots & {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j& {\left[{}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j{\times }^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^j\right]}_{\times }\mathbf{z}& \mid & {\left[{}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j{\times }^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^j\right]}_{\times }\mathbf{z}& \mid & {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j& \cdots & {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j\end{array}\right]}^{\mathrm{T}}\end{array}$$

(32)

where $\mathbf{z}={\left[\begin{array}{ccc}0\hfill & 0& \frac{1}{{}^t{z}_c^j}\end{array}\right]}^{\mathrm{T}}$. The states ${}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j$, ${}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^j$ involved in the right nullspace basis $\left({\mathbf{N}}_{\mathbf{2}}\right)$ belong to the j-th robot that observes the target. It is straightforward to verify that the right nullspace basis of $\mathcal{O}$ spans for ${\mathbf{N}}_{\mathbf{2}}$, (i.e., $\mathcal{O}{\mathbf{N}}_{\mathbf{2}}=\mathbf{0}$). From Equation (32) it can be seen that the unobservable modes cross through all states, therefore, all states are unobservable. It should be noted that adding Lie derivatives of higher-order to the observability matrix the previous result does not improve.

3.2.3. Case 3

For the third case, the maximum rank of the observability matrix $\mathcal{O}$ is $rank\left(\mathcal{O}\right)=(3n_1+6n_2+3)$. Given that $dim\left(\mathbf{x}\right)=3n_1+6n_2+3$, then, the system under the third case is locally weakly observable because $rank\left(\mathcal{O}\right)=dim\left(\mathbf{x}\right)$.

3.2.4. Remarks to the Theoretical Results

The results of the observability analysis are summarized in

Table 1. Results of the nonlinear observability analysis of the proposed system.

		Monocular	Monocular
	Monocular	Measurements	Measurements	Altitude
	Measurements	of the Target	of the Target	Differential	Unobservable	Unobservable	Observable
	of Landmarks	(Single)	(Multiple)	Measurements	Modes	States	States
Case 1	Yes	No	Yes	No	1	$\mathbf{x}$	-
Case 2	Yes	Yes	No	Yes	2	$\mathbf{x}$	-
Case 3	Yes	No	Yes	Yes	0	-	$\mathbf{x}$

.

• Having altitude differential measurements between two UAV-camera systems is a necessary condition to obtain the previous results (see Figure 2). In this case, results do not improve when adding more altitude differentials.
• It is necessary to have at least two monocular measurements of the target in order to obtain the previous results (see Figure 2).
• To obtain the previous results, it is necessary to link the members of the multi-UAV system through at least two measurements in common (see Figure 2). That means that: (i) a robot needs to share the observation of at least two landmarks with any other robot; or (ii) a robot needs to share the observation of at least one landmark and the target with any other robot; or (iii) a robot needs to share the observation of at least one landmark with any other robot having the measurement of its altitude differential with that same robot; or (iv) a robot needs to share the observation of the target with any other robot having the measurement of its altitude differential with that same robot.

4. EKF-Based SLAM

In this work, the system state in Equation (2) is estimated using the EKF-based SLAM methodology [62,63]. Figure 3 shows the architecture of the proposed system. In this case, it is assumed a ground-based centralized architecture, where all the most computing-demanding processes, like the visual processing and the main estimation process, are carried out on a ground-based computer. Algorithm 1 outlines the proposed EKF-SLAM based method.

Algorithm 1: EKF-SLAM target-Centric Multi-UAV

4.1. System Prediction

At every step k, the estimated system state $\widehat{\mathbf{x}}$ takes a step forward by the following discrete model:

$${\mathbf{x}}_k=\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{n}}_{k-1})=\left[\begin{array}{c}{}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^{\mathit{1}}k\hfill \\ {}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^{\mathit{1}}k\hfill \\ :\hfill \\ {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^{j}k\hfill \\ {}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^{j}k\hfill \\ {\mathbf{v}}_{\mathbf{t}}k\hfill \\ {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^{\mathit{1}}k\hfill \\ :\hfill \\ {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^{i}k\hfill \end{array}\right]=\left[\begin{array}{c}{}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^{\mathit{1}}k-1+{(}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^{\mathit{1}}k-1)\Delta t\hfill \\ {}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^{\mathit{1}}k-1{+}^{\mathrm{t}}{\eta }_{\mathrm{c}}^{\mathit{1}}k-1\hfill \\ :\hfill \\ {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^{j}k-1+{(}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^{j}k-1)\Delta t\hfill \\ {}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^{j}k-1{+}^{\mathrm{t}}{\eta }_{\mathrm{c}}^{j}k-1\hfill \\ {\mathbf{v}}_{\mathbf{t}}k-1+{\zeta }_{\mathrm{t}}k-1\hfill \\ {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}{k-1}^{\mathit{1}}-\left({\mathbf{v}}_{\mathbf{t}}k-1\right)\Delta t\hfill \\ :\hfill \\ {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}{k-1}^i-\left({\mathbf{v}}_{\mathbf{t}}k-1\right)\Delta t\hfill \end{array}\right]$$

(33)

$${\mathbf{n}}_k=\left[\begin{array}{c}{}^{\mathrm{t}}{\eta }_{\mathrm{c}}^{j}k\hfill \\ {\zeta }_{\mathrm{t}}k\hfill \end{array}\right]=\left[{}^{\mathbf{t}}{\mathbf{a}}_{\mathbf{c}}^j\Delta t{\mathbf{a}}_{\mathbf{t}}\Delta t\right]$$

(34)

where ${}^{\mathbf{t}}{\mathbf{a}}_{\mathbf{c}}^j$ and ${\mathbf{a}}_{\mathbf{t}}$ represent unknown linear accelerations that are assumed to have Gaussian distribution with zero mean. Moreover, let $\mathbf{n}\sim \mathcal{N}(\mathbf{0},\mathbf{Q})$ be the noise vector that affects the state, while $\Delta t$ is the differential of time and k the sample step. In this work, a Gaussian random process is used for propagating the velocity of the vehicle. The proposed scheme is independent of the kind of aircraft and therefore is not restricted by the use of an specific dynamic model.

An Extended Kalman Filter (EKF) propagates the system state $\widehat{\mathbf{x}}$ over time as follows:

$${\widehat{\mathbf{x}}}_k^{-}=\mathbf{f}({\widehat{\mathbf{x}}}_{k-1},\mathbf{0})$$

(35)

The state covariance matrix P takes a step forward using [47]:

$${\mathbf{P}}_k^{-}={\mathbf{A}}_k{\mathbf{P}}_{k-1}{\mathbf{A}}_k^T+{\mathbf{W}}_k{\mathbf{Q}}_{k-1}{\mathbf{W}}_k^T$$

(36)

with

$$\begin{array}{c}\hfill {\mathbf{A}}_k=\frac{\partial \mathbf{f}}{\partial \mathbf{x}}({\widehat{\mathbf{x}}}_{k-1},\mathbf{0}){\mathbf{W}}_k=\frac{\partial \mathbf{f}}{\partial \mathbf{n}}({\widehat{\mathbf{x}}}_{k-1},\mathbf{0})\end{array}$$

(37)

4.2. Measurement Updates

Assuming that, for the current sample step, a set of measurements is available, then the filter is updated with the Kalman update equations as follows [47]:

$${\widehat{\mathbf{x}}}_k={\widehat{\mathbf{x}}}_k^{-}+{\mathbf{K}}_k({\mathbf{z}}_k-\mathbf{h}({\widehat{\mathbf{x}}}_k^{-},\mathbf{0}))$$

(38)

$${\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{C}}_k){\mathbf{P}}_k^{-}$$

(39)

with

$${\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathbf{C}}_k^T{({\mathbf{C}}_k{\mathbf{P}}_k^{-}{\mathbf{C}}_k^T+{\mathbf{V}}_k{\mathbf{R}}_k{\mathbf{V}}_k^T)}^{-1}$$

(40)

and

$$\begin{array}{c}\hfill {\mathbf{C}}_k=\frac{\partial \mathbf{h}}{\partial \mathbf{x}}({\widehat{\mathbf{x}}}_k^{-},\mathbf{0}){\mathbf{V}}_k=\frac{\partial \mathbf{h}}{\partial \mathbf{r}}({\widehat{\mathbf{x}}}_k^{-},\mathbf{0})\end{array}$$

(41)

where $\mathbf{z}$ is the vector of the current measurements, $\mathbf{h}$ is the vector of the current prediction measurements, and $\mathbf{r}\sim \mathcal{N}(\mathbf{0},\mathbf{R})$ is the noise vector that affects the measurements.

4.2.1. Visual Updates (Landmarks)

When in the current sample step a set of visual measurements of the landmarks is available, the system is updated with this kind of measurements. In this case:

$$\begin{array}{c}\hfill \mathbf{h}={\left[\begin{array}{ccc}{}^{\mathit{1}}{\mathbf{h}}_{\mathbf{c}}^j\hfill & \cdots & {}^i{\mathbf{h}}_{\mathbf{c}}^j\end{array}\right]}^{\mathrm{T}}\mathbf{z}={\left[\begin{array}{ccc}{}^{\mathit{1}}{\mathbf{z}}_{\mathbf{c}}^j\hfill & \cdots & {}^i{\mathbf{z}}_{\mathbf{c}}^j\end{array}\right]}^{\mathrm{T}}\end{array}$$

(42)

4.2.2. Visual Updates (Target)

When in the current sample step a set of visual measurements of the target is available, the system is updated with this kind of measurements. In this case:

$$\begin{array}{c}\hfill \mathbf{h}={\left[\begin{array}{ccc}{}^{\mathbf{t}}{\mathbf{h}}_{\mathbf{c}}^{\mathit{1}}\hfill & \cdots & {}^{\mathbf{t}}{\mathbf{h}}_{\mathbf{c}}^j\end{array}\right]}^{\mathrm{T}}\mathbf{z}={\left[\begin{array}{ccc}{}^{\mathbf{t}}{\mathbf{z}}_{\mathbf{c}}^{\mathit{1}}\hfill & \cdots & {}^{\mathbf{t}}{\mathbf{z}}_{\mathbf{c}}^j\end{array}\right]}^{\mathrm{T}}\end{array}$$

(43)

4.2.3. Altitude Differential Updates

When in the current sample step a set of altitude differential measurements between pairs of UAVs is available, the system is updated with this kind of measurements. In this case:

$$\begin{array}{c}\hfill \mathbf{h}={\left[\begin{array}{cc}{}^n{h}_a^j\hfill & \cdots \end{array}\right]}^{\mathrm{T}}\mathbf{z}={\left[\begin{array}{cc}{}^n{z}_a^j\hfill & \cdots \end{array}\right]}^{\mathrm{T}}\end{array}$$

(44)

4.3. Map Features Initialization

The system state $\mathbf{x}$ is augmented with new map features using a cooperative strategy. The 3D relative position with respect to the target of the new map features is estimated using a pseudo-stereo system formed by the monocular cameras mounted on a pair of UAVs that observe common landmarks. In this case, when a new potential landmark is observed by two cameras, then it is initialized employing a linear triangulation.

The state of the new feature is computed using the a posteriori values obtained in the correction stage of the EKF. According to Equation (3), the following expression can be defined in homogeneous coordinates [52]:

$${}^i{\gamma }_c^j\left[{}^i{u}_c^{j}i{v}_c^{j}1\right.=\left[\begin{array}{cc}{\mathbf{T}}_{\mathbf{c}}^j\hfill & {\mathbf{0}}_{3\times 1}\end{array}\right]{\widehat{\mathbf{E}}}_{\mathbf{c}}^j\left[\begin{array}{c}{}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^i\hfill \\ 1\hfill \end{array}\right]$$

(45)

where ${}^i{\gamma }_c^j$ is a scale factor. Additionally, it is defined:

$$\begin{array}{c}\hfill {\widehat{\mathbf{E}}}_{\mathbf{c}}^j=\left[\begin{array}{cc}{}^W{\mathbf{R}}_{\mathbf{c}}^j\hfill & {}^{\mathbf{t}}{\widehat{\mathbf{x}}}_{\mathbf{c}}^j\\ {\mathbf{0}}_{1\times 3}\hfill & 1\end{array}\right]\\ \hfill {\mathbf{T}}_{\mathbf{c}}^j=\left[\begin{array}{ccc}\frac{f_c^j}{d_u^j}\hfill & 0& c_u^j+d_{ur}^j+d_{ut}^j\\ 0\hfill & \frac{f_c^j}{d_v^j}& c_v^j+d_{vr}^j+d_{vt}^j\\ 0\hfill & 0& 1\end{array}\right]\end{array}$$

(46)

Using Equation (45), and considering the projection onto two any UAV-cameras, a linear system can be formed in order to estimate ${}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^i$:

$${\mathbf{D}}^i{·}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^i={\mathbf{b}}^i{}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^i={{\mathbf{D}}^i}^{†}·{\mathbf{b}}^i$$

(47)

where ${{\mathbf{D}}^i}^{†}$ is the Moore Penrose right pseudo-inverse matrix of ${\mathbf{D}}^i$, and

$$\begin{array}{c}\hfill {\mathbf{D}}^i=\left[\begin{array}{ccc}{k}_{31}^j{}^i{u}_c^j-k_{11}^j\hfill & k_{32}^j{}^i{u}_c^j-k_{12}^j& k_{33}^j{}^i{u}_c^j-k_{13}^j\\ k_{31}^j{}^i{v}_c^j-k_{21}^j\hfill & k_{32}^j{}^i{v}_c^j-k_{22}^j& k_{33}^j{}^i{v}_c^j-k_{23}^j\end{array}\right]\\ \hfill {\mathbf{b}}^i=\left[\begin{array}{c}{k}_{14}^j-k_{34}^j{}^i{u}_c^j\hfill \\ k_{24}^j-k_{34}^j{}^i{v}_c^j\hfill \end{array}\right]\end{array}$$

(48)

with

$$\left[\begin{array}{cc}{\mathbf{T}}_{\mathbf{c}}^j\hfill & {\mathbf{0}}_{3\times 1}\end{array}\right]{\widehat{\mathbf{E}}}_{\mathbf{c}}^j=\left[\begin{array}{cccc}{k}_{11}^j& k_{12}^j& k_{13}^j& k_{14}^j\\ k_{21}^j& k_{22}^j& k_{23}^j& k_{24}^j\\ k_{31}^j& k_{32}^j& k_{33}^j& k_{34}^j\end{array}\right]$$

(49)

When a new landmark is initialized, the system state $\mathbf{x}$ is augmented by:

$$\begin{array}{c}\hfill \mathbf{x}={\left[\begin{array}{cccccccccc}{}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^{\mathit{1}}\hfill & {}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^{\mathit{1}}& \cdots & {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{c}}^j& {}^{\mathbf{t}}{\mathbf{v}}_{\mathbf{c}}^j& {\mathbf{v}}_{\mathbf{t}}& {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^{\mathit{1}}& \cdots & {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^i& {}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{a}}^{new}\hfill \end{array}\right]}^{\mathrm{T}}\end{array}$$

(50)

In addition, the new covariance matrix ${\mathbf{P}}_{new}$ is computed by:

$${\mathbf{P}}_{new}=\Delta \mathbf{J}\left[\begin{array}{cc}\mathbf{P}\hfill & \mathbf{0}\\ \mathbf{0}\hfill & {}^i{\mathbf{R}}^j\end{array}\right.=\Delta {\mathbf{J}}^{\mathrm{T}}$$

(51)

where $\Delta \mathbf{J}$ is the Jacobian for the initialization function, and ${}^i{\mathbf{R}}^j$ is the measurement noise covariance matrix for ${(}^i{u}_c^j{,}^i{v}_c^j)$.

4.4. Map Management

It is well known that due to the nature of the Kalman Filter, in SLAM, the system state can always reach a size that will make it impossible to maintain a real-time performance for a given hardware configuration. In this sense, the present work is mainly intended to address the problem of navigation with respect to a target (local navigation). Therefore, features that are left behind by the movement of the cameras will be removed from the system state and covariance matrix. This strategy will prevent that the system state size reaches an unmanageable amount that seriously affects the computational performance.

5. Control System

This section presents the control scheme that allows for carrying out the flight formation of the UAVs with respect to the target. While the proposed control scheme is presented assuming that UAVs are quadcopters, the methodology can be easily extended for being used with any other aerial configurations.

5.1. Dynamic Model Flight Formation

The dynamic model used for representing the flight formation is based on the leader–follower scheme [64]. In this case, it is desired to maintain the j-th UAV to a distance ${}^t{x}_q^j$, ${}^t{y}_q^j$ (in the $x-y$ plane) from the target (See Figure 4). In addition, it is also desired to maintain the j-th UAV at an altitude differential ${}^t{z}_q^j$ from the target (See Figure 4). Given the above considerations, the following expression can be defined:

$${}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{q}}^j=\left[\begin{array}{c}{}^t{x}_q^j\hfill \\ {}^t{y}_q^j\hfill \\ {}^t{z}_q^j\hfill \end{array}\right]={\mathbf{x}}_{\mathbf{q}}^j-{\mathbf{x}}_{\mathbf{t}}$$

(52)

Differentiating twice Equation (52) with respect to time and using Equation (8), the dynamics of the formation ${}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{q}}^j$ can be obtained:

$$\begin{array}{c}\hfill {}^{\mathbf{t}}{\ddot{\mathbf{x}}}_{\mathbf{q}}^j={\ddot{\mathbf{x}}}_{\mathbf{q}}^j-{\ddot{\mathbf{x}}}_{\mathbf{t}}=\frac{{\mu }^j}{m^j}\left[\begin{array}{c}sin\left({\psi }^j\right)sin\left({\varphi }^j\right)+cos\left({\psi }^j\right)sin\left({\theta }^j\right)cos\left({\varphi }^j\right)\hfill \\ sin\left({\psi }^j\right)sin\left({\theta }^j\right)cos\left({\varphi }^j\right)-cos\left({\psi }^j\right)sin\left({\varphi }^j\right)\hfill \\ cos\left({\theta }^j\right)cos\left({\varphi }^j\right)-\frac{g·m^j}{u_q^j·{\mu }^j}\hfill \end{array}\right]u_q^j-{\ddot{\mathbf{x}}}_{\mathbf{t}}\end{array}$$

(53)

5.2. Control Scheme

The control scheme is designed to allow that the flight formation converges to the desired values. The control system is divided into two subsystems (see Figure 5): (i) the flight formation control, which involves the translational dynamics of the quadcopter and the target; (ii) the rotational control. Because the reference signal of the rotational subsystem is obtained from the flight formation control subsystem, it is assumed that the dynamics of the former is faster than the dynamics of the latter.

A master–slave system is defined by:

$$\sum =\left\{\begin{array}{c}{}^{\mathbf{t}}{\ddot{\mathbf{x}}}_{\mathbf{q}}^j={\ddot{\mathbf{x}}}_{\mathbf{q}}^j-{\ddot{\mathbf{x}}}_{\mathbf{t}}\hfill \\ {\ddot{\sigma }}^j={\mathbf{g}}^j+{\mathbf{B}}^j{\tau }^j\hfill \end{array}\right.$$

(54)

The overall control scheme is based on the Backstepping control technique [49], and each control-loop subsystem is designed by the so-called technique exact linearization by state feedback [49]. The state values required by the flight formation control subsystem are obtained from the estimator described in Section 4. The attitude of the j-th UAV is obtained assuming the availability of an on-board AHRS. To obtain the control laws using analysis in continuous time, it is assumed that the estimated values are passed through a zero-order hold (ZOH) (see Figure 5).

5.2.1. Flight Formation Subsystem Control

Since the estimated state of the relative position of the j-th UAV with respect to the target is defined with respect to the reference system C (see Section 2 and Section 4), it is necessary to apply a transformation to the estimated state ${}^{\mathbf{t}}{\widehat{\mathbf{x}}}_{\mathbf{c}}^j$ for obtaining ${}^{\mathbf{t}}{\widehat{\mathbf{x}}}_{\mathbf{q}}^j$, which, in turn, is necessary to obtain the control laws. Therefore, the following equation is defined:

$${}^{\mathbf{t}}{\widehat{\mathbf{x}}}_{\mathbf{q}}^j{=}^{\mathbf{t}}{\widehat{\mathbf{x}}}_{\mathbf{c}}^j-{}^{\mathbf{q}}{\mathbf{d}}_{\mathbf{c}}^j$$

(55)

Let ${}^{\mathbf{q}}{\mathbf{d}}_{\mathbf{c}}^j$ be the translation vector (in meters) from the coordinate reference system Q to the coordinate reference system C. Note that ${}^{\mathbf{q}}{\mathbf{d}}_{\mathbf{c}}^j$ is assumed to be known and constant.

Firstly, a control law for the altitude differential ${}^t{z}_q^j$ of the flight formation subsystem is developed. Considering the dynamics of ${}^t{z}_q^j$ given in Equation (53), with control input $u_q^j$, and defining:

$$s_z^j={\dot{e}}_z^j+{\lambda }_z^j{e}_z^j+{\kappa }_z^j{\int }_0^t{e}_z^{j}dt$$

(56)

where the error signal is $e_z^j={}^t{\widehat{z}}_q^j-{{}^t{z}_q^j}_d$, and ${{}^t{z}_q^j}_d$ is the desired value.

Deriving Equation (56) and substituting in Equation (53), it is obtained:

$$\begin{array}{c}\hfill {\dot{s}}_z^j=\left(\frac{{\mu }^j}{m^j}\right)\left(cos\left({\widehat{\theta }}^j\right)cos\left({\widehat{\varphi }}^j\right)\right)u_q^j-g+{\lambda }_z^j{\dot{e}}_z^j+{\kappa }_z^j{e}_z^j-{\ddot{\widehat{z}}}_t-{{}^t{\ddot{z}}_q^j}_d\end{array}$$

(57)

The following control law is proposed:

$$u_q^j=\frac{m^j({{}^t{\ddot{z}}_q^j}_d+{\ddot{\widehat{z}}}_t-{\lambda }_z^j{\dot{e}}_z^j-{\kappa }_z^j{e}_z^j-k_z^j{s}_z^j+g)}{{\mu }^j(cos\left({\widehat{\theta }}^j\right)cos\left({\widehat{\varphi }}^j\right))}$$

(58)

where $k_z^j$, ${\lambda }_z^j$, and ${\kappa }_z^j$ are positive gains assuming that ${\widehat{\theta }}^j$, ${\widehat{\varphi }}^j\in [-\frac{\pi }{2},\frac{\pi }{2}]$.

Now, consider the longitudinal and lateral differential ${}^t{x}_q^j$ and ${}^t{y}_q^j$ of the flight formation subsystem. By substituting Equation (58) in Equation (53), then, for the two first states, it can be obtained:

$$\left[\begin{array}{c}{}^t{\ddot{\widehat{x}}}_q^j\hfill \\ {}^t{\ddot{\widehat{y}}}_q^j\hfill \end{array}\right]=u_z^j\left[\begin{array}{cc}sin\left({\widehat{\psi }}^j\right)\hfill & cos\left({\widehat{\psi }}^j\right)\\ -cos\left({\widehat{\psi }}^j\right)\hfill & sin\left({\widehat{\psi }}^j\right)\end{array}\right]\left[\begin{array}{c}\frac{tan\left({\varphi }^j\right)}{cos\left({\widehat{\theta }}^j\right)}\hfill \\ tan\left({\theta }^j\right)\hfill \end{array}\right]-\left[\begin{array}{c}{\ddot{\widehat{x}}}_t\hfill \\ {\ddot{\widehat{y}}}_t\hfill \end{array}\right]$$

(59)

Taking ${\varphi }^j$ and ${\theta }^j$ as control inputs in Equation (59), and defining:

$$\begin{array}{c}\hfill s_x^j={\dot{e}}_x^j+{\lambda }_x^j{e}_x^j+{\kappa }_x^j{\int }_0^t{e}_x^{j}dt\\ \hfill s_y^j={\dot{e}}_y^j+{\lambda }_y^j{e}_y^j+{\kappa }_y^j{\int }_0^t{e}_y^{j}dt\end{array}$$

(60)

with error signals $e_x^j={}^t{\widehat{x}}_q^j-{{}^t{x}_q^j}_d$ and $e_y^j={}^t{\widehat{y}}_q^j-{{}^t{y}_q^j}_d$, being ${{}^t{x}_q^j}_d$ and ${{}^t{y}_q^j}_d$ the desired values, it is possible to derive Equation (60) and substitute in Equation (59) in order to obtain:

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\dot{s}}_x^j\hfill \\ {\dot{s}}_y^j\hfill \end{array}\right]=u_z^j\left[\begin{array}{cc}sin\left({\widehat{\psi }}^j\right)\hfill & cos\left({\widehat{\psi }}^j\right)\\ -cos\left({\widehat{\psi }}^j\right)\hfill & sin\left({\widehat{\psi }}^j\right)\end{array}\right]\left[\begin{array}{c}\frac{tan\left({\varphi }^j\right)}{cos\left({\widehat{\theta }}^j\right)}\hfill \\ tan\left({\theta }^j\right)\hfill \end{array}\right]+\left[\begin{array}{c}{\lambda }_x^j{\dot{e}}_x^j+{\kappa }_x^j{e}_x^j-{{}^t{x}_q^j}_d-{\ddot{\widehat{x}}}_t\hfill \\ {\lambda }_y^j{\dot{e}}_y^j+{\kappa }_y^j{e}_y^j-{{}^t{y}_q^j}_d-{\ddot{\widehat{y}}}_t\hfill \end{array}\right]\end{array}$$

(61)

The following control laws are proposed:

$$\left[\begin{array}{c}{\theta }^j\hfill \\ {\varphi }^j\hfill \end{array}\right]=arctan\left(\frac{1}{u_z^j}\left[\begin{array}{cc}cos\left({\widehat{\psi }}^j\right)\hfill & sin\left({\widehat{\psi }}^j\right)\\ sin\left({\widehat{\psi }}^j\right)cos\left({\widehat{\theta }}^j\right)\hfill & -cos\left({\widehat{\psi }}^j\right)cos\left({\widehat{\theta }}^j\right)\end{array}\right]\left[\begin{array}{c}{u}_x^j\hfill \\ u_y^j\hfill \end{array}\right]\right)$$

(62)

with

$$\begin{array}{c}\hfill u_x^j={{}^t{\ddot{x}}_q^j}_d+{\ddot{\widehat{x}}}_t-{\lambda }_x^j{\dot{e}}_x^j-{\kappa }_x^j{e}_x^j-k_x^j{s}_x^j\\ \hfill u_y^j={{}^t{\ddot{y}}_q^j}_d+{\ddot{\widehat{y}}}_t-{\lambda }_y^j{\dot{e}}_y^j-{\kappa }_y^j{e}_y^j-k_y^j{s}_y^j\\ \hfill u_z^j={{}^t{\ddot{z}}_q^j}_d+{\ddot{\widehat{z}}}_t-{\lambda }_z^j{\dot{e}}_z^j-{\kappa }_z^j{e}_z^j-k_z^j{s}_z^j+g\end{array}$$

(63)

and where $k_x^j$, ${\lambda }_x^j$, ${\kappa }_x^j$, $k_y^j$, ${\lambda }_y^j$ and ${\kappa }_y^j$ are positive gains.

For the proposed control laws in Equations (58) and (62), it is necessary to know the accelerations of the target, which are estimated by deriving the estimated velocities of the target obtained from the EKF by using the following differentiator with the super-twisting algorithm [65] (see Figure 5):

$$\begin{array}{c}\hfill {\ddot{\widehat{\mathbf{x}}}}_{\mathbf{t}}=-{\mathbf{K}}_{\mathbf{1}}{\left|{\dot{\widehat{\mathbf{x}}}}_{\mathbf{t}}-{\widehat{\mathbf{v}}}_{\mathbf{t}}\right|}^{\frac{1}{2}}sign({\dot{\widehat{\mathbf{x}}}}_{\mathbf{t}}-{\widehat{\mathbf{v}}}_{\mathbf{t}})+{\widehat{\omega }}_{\mathbf{t}}\\ \hfill {\dot{\widehat{\omega }}}_{\mathbf{t}}=-{\mathbf{K}}_{\mathbf{2}}sign({\dot{\widehat{\mathbf{x}}}}_{\mathbf{t}}-{\widehat{\mathbf{v}}}_{\mathbf{t}})\end{array}$$

(64)

where ${\dot{\widehat{\mathbf{x}}}}_{\mathbf{t}}$ is the velocity of the target estimated by the differentiator, ${\widehat{\mathbf{v}}}_{\mathbf{t}}$ is the velocity of the target estimated by the EKF, ${\mathbf{K}}_{\mathbf{1}}$ and ${\mathbf{K}}_{\mathbf{2}}$ are positive definite diagonal matrices of adequate dimensions.

To prove the stability of the flight formation subsystem, the following theorem is used:

Theorem 1.

Consider the dynamic in Equations (57) and (61) in closed loop with Equations (58) and (62); then, the origin $s_x^j=s_y^j=s_z^j=0$ of Equations (56) and (60) is asymptotically stable.

Proof. 

Consider the following Lyapunov candidate function:

$$V_t=\frac{1}{2}{{\mathbf{s}}_{\mathbf{t}}^j}^{\mathrm{T}}{\mathbf{s}}_{\mathbf{t}}$$

(65)

with ${\mathbf{s}}_{\mathbf{t}}^j={\left[\begin{array}{ccc}{s}_x^j\hfill & s_y^j& s_z^j\hfill \end{array}\right]}^{\mathrm{T}}$. By deriving:

$$\begin{array}{c}\hfill \dot{V_t}={{\mathbf{s}}_{\mathbf{t}}^j}^{\mathrm{T}}{\dot{\mathbf{s}}}_{\mathbf{t}}^j={{\mathbf{s}}_{\mathbf{t}}^j}^{\mathrm{T}}\left[\begin{array}{ccc}{u}_z^{j}sin\left({\widehat{\psi }}^j\right)\hfill & u_z^{j}cos\left({\widehat{\psi }}^j\right)& 0\\ -u_z^{j}cos\left({\widehat{\psi }}^j\right)\hfill & u_z^{j}sin\left({\widehat{\psi }}^j\right)& 0\\ 0\hfill & 0& \left(\frac{{\mu }^j}{m^j}\right)cos\left({\widehat{\varphi }}^j\right)cos\left({\widehat{\theta }}^j\right)\end{array}\right]\left[\begin{array}{c}\frac{tan\left({\varphi }^j\right)}{cos\left({\widehat{\theta }}^j\right)}\hfill \\ tan\left({\theta }^j\right)\hfill \\ u_q^j\hfill \end{array}\right]\\ \hfill +{{\mathbf{s}}_{\mathbf{t}}^j}^{\mathrm{T}}\left[\begin{array}{c}{\lambda }_x^j{\dot{e}}_x^j+{\kappa }_x^j{e}_x^j-{{}^t{x}_q^j}_d-{\ddot{\widehat{x}}}_t\hfill \\ {\lambda }_y^j{\dot{e}}_y^j+{\kappa }_y^j{e}_y^j-{{}^t{y}_q^j}_d-{\ddot{\widehat{y}}}_t\hfill \\ -g+{\lambda }_z^j{\dot{e}}_z^j+{\kappa }_z^j{e}_z^j-{{}^t{\ddot{z}}_q^j}_d-{\ddot{\widehat{z}}}_t\hfill \end{array}\right]\end{array}$$

(66)

Substituting Equations (58) and (62) in Equation (66), it is obtained:

$$\dot{V_t}=-{{{\mathbf{s}}_{\mathbf{t}}}^j}^{\mathrm{T}}{\mathbf{K}}_{\mathbf{t}}^j{\mathbf{s}}_{\mathbf{t}}^j\le 0$$

(67)

with

$${\mathbf{K}}_{\mathbf{t}}^j=\left[\begin{array}{ccc}{k}_x^j\hfill & 0& 0\\ 0\hfill & k_y^j& 0\\ 0\hfill & 0& k_z^j\end{array}\right]$$

(68)

Therefore, the origin ${\mathbf{s}}_{\mathbf{t}}^j=\mathbf{0}$ of Equations (56) and (60) is asymptotically stable. □

5.2.2. Rotational Subsystem Control

For the rotational subsystem, with dynamics characterized by Equation (9), and control input ${\tau }^j$, it can be defined:

$${\mathbf{s}}_{\mathbf{r}}^j={\dot{\mathbf{e}}}_{\mathbf{r}}^j+{\lambda }_{\mathbf{r}}^j{\mathbf{e}}_{\mathbf{r}}^j+{\kappa }_{\mathbf{r}}^j{\int }_0^t{\mathbf{e}}_{\mathbf{r}}^{j}dt$$

(69)

where the error signal is ${\mathbf{e}}_{\mathbf{r}}^j={\sigma }^j-{\sigma }_d^j$ and ${\sigma }_d^j$ is the desired value. Deriving Equation (69) and substituting in Equation (9), it is obtained:

$${\dot{\mathbf{s}}}_{\mathbf{r}}={\mathbf{g}}^j+{\mathbf{B}}^j{\tau }^j+{\lambda }_{\mathbf{r}}^j{\dot{\mathbf{e}}}_{\mathbf{r}}^j+{\kappa }_{\mathbf{r}}^j{\mathbf{e}}_{\mathbf{r}}^j-{\sigma }_d^j$$

(70)

The following control law is proposed:

$${\tau }^j={{\mathbf{B}}^j}^{-1}\left(-{\mathbf{g}}^j+{\sigma }_d^j-{\lambda }_{\mathbf{r}}^j{\dot{\mathbf{e}}}_{\mathbf{r}}^j-{\kappa }_{\mathbf{r}}^j{\mathbf{e}}_{\mathbf{r}}^j-{\mathbf{K}}_{\mathbf{r}}^j{\mathbf{s}}_{\mathbf{r}}^j\right)$$

(71)

where ${\lambda }_{\mathbf{r}}^j$, ${\kappa }_{\mathbf{r}}^j$, and ${\mathbf{K}}_{\mathbf{r}}$ are positive definite diagonal matrices of adequate dimensions. It is straightforward to demonstrate that ${{\mathbf{B}}^j}^{-1}$ exists.

To prove the stability of the rotational subsystem, the following theorem is used:

Theorem 2.

Consider the dynamic in Equation (70) in closed loop with Equation (71); then the origin ${\mathbf{s}}_{\mathbf{r}}^j=\mathbf{0}$ of Equation (69) is asymptotically stable.

Proof. 

Consider the following Lyapunov candidate function:

$$V_r=\frac{1}{2}{{\mathbf{s}}_{\mathbf{r}}^j}^{\mathrm{T}}{\mathbf{s}}_{\mathbf{r}}$$

(72)

By deriving:

$$\dot{V_r}={{\mathbf{s}}_{\mathbf{r}}^j}^{\mathrm{T}}{\dot{\mathbf{s}}}_{\mathbf{r}}^j={{\mathbf{s}}_{\mathbf{r}}^j}^{\mathrm{T}}\left({\mathbf{g}}^j+{\mathbf{B}}^j{\tau }^j+{\lambda }_{\mathbf{r}}^j{\dot{\mathbf{e}}}_{\mathbf{r}}^j+{\kappa }_{\mathbf{r}}^j{\mathbf{e}}_{\mathbf{r}}^j-{\sigma }_d^j\right)$$

(73)

Substituting Equation (71) in Equation (73), it is obtained:

$$\dot{V_r}=-{{{\mathbf{s}}_{\mathbf{r}}}^j}^{\mathrm{T}}{\mathbf{K}}_{\mathbf{r}}^j{\mathbf{s}}_{\mathbf{r}}^j\le 0$$

(74)

Therefore, the origin ${\mathbf{s}}_{\mathbf{r}}^j=\mathbf{0}$ of Equation (69) is asymptotically stable. □

5.2.3. Closed Loop Stability

In order to prove the closed-loop stability of the whole system in Equation (54), the following theorem is used:

Theorem 3.

Given the system in Equation (54), in closed loop with the control laws in Equations (58) and (71). Moreover, if ${\theta }_d^j$ and ${\varphi }_d^j$ are chosen according to Equation (62) as control laws for the longitudinal and lateral differential of the flight formation subsystem, then the origin ${\mathbf{s}}_{\mathbf{t}}^j={\mathbf{s}}_{\mathbf{r}}^j=\mathbf{0}$ is asymptotically stable.

Proof. 

Consider the following Lyapunov candidate function:

$$V_T=V_t+V_r$$

(75)

with derivative

$${\dot{V}}_T={\dot{V}}_t+{\dot{V}}_r={{\mathbf{s}}_{\mathbf{t}}^j}^{\mathrm{T}}{\dot{\mathbf{s}}}_{\mathbf{t}}^j+{{\mathbf{s}}_{\mathbf{r}}^j}^{\mathrm{T}}{\dot{\mathbf{s}}}_{\mathbf{r}}^j$$

(76)

from Equations (67) and (74), it can be seen that

$${\dot{V}}_T=-{{{\mathbf{s}}_{\mathbf{t}}}^j}^{\mathrm{T}}{\mathbf{K}}_{\mathbf{t}}{\mathbf{s}}_{\mathbf{t}}-{{{\mathbf{s}}_{\mathbf{r}}}^j}^{\mathrm{T}}{\mathbf{K}}_{\mathbf{r}}{\mathbf{s}}_{\mathbf{r}}\le 0$$

(77)

Therefore, the origin ${\mathbf{s}}_{\mathbf{t}}^j={\mathbf{s}}_{\mathbf{r}}^j=\mathbf{0}$ of the closed loop system is Globally Asymptotically Stable (GAS). □

6. Simulation Results

6.1. Simulation Setup

In this section, the proposed cooperative visual-SLAM system for UAV-based target tracking is validated through computer simulations. A Matlab${}^{\circledR }$ implementation of the proposed scheme was used for this purpose. To this aim, a simulation environment has been developed. The environment is composed of $3D$ landmarks, randomly distributed over the ground (See Figure 6). To execute the tests, two quadcopters (Quad 1 and Quad 2), equipped with the set of sensors required by the proposed method are simulated. In simulations, it is assumed that there exist enough landmarks in the environment that allow for being observed in common by the cameras of the UAVs, at least a subset of them.

The measurements from the sensors are emulated to be taken with a frequency of 10 Hz. To emulate the system uncertainty, the following Gaussian noise is added to measurements: Gaussian noise with ${\sigma }_c=4$ pixels is added to the measurements given by the cameras. Gaussian noise with ${\sigma }_a=25$ cm is added to the measurements of altitude differential between two quadcopters obtained by the altimeters. It is important to note that the noise considered for emulating monocular measurements is bigger than the typical magnitude of the noise of real monocular measurement. In this way, the errors in camera orientation are considered caused by the gimbal, in addition to the imperfection of the sensor.

In simulations, the target was moved along a predefined trajectory (see Figure 6).

The parameter values used for the quadcopters and the cameras are shown in

Table 2. Quadcopter and camera parameters.

Parameter	Value	Parameter	Value
$m^j$	$0.468$	$l^j$	$0.225$
${\mu }^j$	$2.98\times {10}^{-6}$	g	$9.81$
$J_p^j$	$3.357\times {10}^{-5}$	$f^j/d_u^j$	$262.92$
$d^j$	$1.14\times {10}^{-7}$	$f^j/d_v^j$	$261.66$
$I_x^j$, $I_y^j$	$4.856\times {10}^{-3}$	$c_u^j$	$359.51$
$I_z^j$	$8.801\times {10}^{-3}$	$c_v^j$	$239.50$

. The quadcopters’ parameters are like those presented in [66]. The camera parameters are like those presented in [17].

Figure 6 shows the trajectories of the three elements composing the flight formation. To highlight them, the position of the elements of the formation is indicated at different instants of time.

6.2. Convergence and Stability Tests

In a series of tests, the convergence and stability of the proposed cooperative visual-SLAM system, in open-loop, were evaluated under the three different observability conditions described in Section 3.2. In this case, it is assumed that there is a control system able to maintain the aerial robots flying in formation with respect to the target.

Two different kinds of tests were performed:

(a)

For the first test, the robustness of the cooperative visual-SLAM system with respect to errors in the initialization of map features is tested. In this case, the initial conditions of ${}^{\mathbf{t}}{\widehat{\mathbf{x}}}_{\mathbf{c}}^1$, ${}^{\mathbf{t}}{\widehat{\mathbf{x}}}_{\mathbf{c}}^2$, ${}^{\mathbf{t}}{\widehat{\mathbf{v}}}_{\mathbf{c}}^1$, ${}^{\mathbf{t}}{\widehat{\mathbf{v}}}_{\mathbf{c}}^2$ and ${\widehat{\mathbf{v}}}_{\mathbf{t}}$ are assumed to be known exactly, but each i map feature is forced to be initialized into the system state with a determined error position. Three different conditions of initial error are considered: ${\sigma }_i=\{0.5,1.5,2.25\}$ meters.

(b)

For the second test, additional to the map features initial errors, the initial conditions of ${}^{\mathbf{t}}{\widehat{\mathbf{x}}}_{\mathbf{c}}^1$, ${}^{\mathbf{t}}{\widehat{\mathbf{x}}}_{\mathbf{c}}^2$, ${}^{\mathbf{t}}{\widehat{\mathbf{v}}}_{\mathbf{c}}^1$, ${}^{\mathbf{t}}{\widehat{\mathbf{v}}}_{\mathbf{c}}^2$, and ${\widehat{\mathbf{v}}}_{\mathbf{t}}$ are not known exactly and have a considerable error with respect to the real values. In this case, the robustness of the proposed estimation system is evaluated in function to the errors in the initial conditions of all the state variables.

Figure 7 and Figure 8 show respectively the results for tests (a) and (b). In this case, the estimated relative position of the Quad 1 with respect to the target is plotted for each reference axis (row plots). Note that, for the sake of clarity, only the estimated values for Quad 1 are presented. The results for Quad 2 are quite similar to those presented for Quad 1. In these figures, each column of plots shows the results obtained from an observability case.

In both figures, it can be observed that both the observability property and the initial conditions play a preponderant role in the convergence and stability of the EKF-SLAM. For several applications, at least the initial position of the UAVs is assumed to be known. However, in SLAM, the position of the map features must be estimated online. The above outcome confirms the importance of using good feature initialization techniques in visual-SLAM. Of course, as it can be expected, the performance of the EKF-SLAM is considerably better when the system is observable. Note that, for the observability case 3, and despite a noticeable error drift, the filter still exhibits certain stability for the worst case of features initialization, when the initial conditions of the UAVs are known.

6.3. Comparative Study

Using the same simulation setup, for this series of tests, the performance of the proposed system, for estimating the relative position of the UAVs with respect to the moving target, was evaluated through a comparative study.

Table 3. Characteristics of the methods used in the comparative study.

Method	System Configuration	Parametrization	Visual Measurements	Other Measurements
Proposed	Multi-UAV-SLAM	Target-centric	Monocular	Altitude differential
(1)	Single-UAV-SLAM	Target-centric	Monocular	-
(2)	Multi-UAV-SLAM	Robot-centric	Monocular	Altitude differential
(3)	Single-UAV	Robot-centric	Stereo-vision	-
(4)	Multi-UAV	Robot-centric	Pseudo-stereo	GPS
(5)	Single-UAV	Robot-centric	Monocular	Range

summarizes the characteristics of five UAV-based target-tracking methods used in this comparative study.

There are some remarks about the methods used in the comparison. Method (1) represents a purely standard monocular-SLAM approach. Features initialization is based on [67]. Because the metric scale cannot be retrieved using only monocular vision, it is assumed that the positions of landmarks seen in the first frame are perfectly known. Method (2) is similar to the proposed previous method, but the system is robot-centric parametrized, instead of target-centric. Method (3) represents an standard visual-stereo tracking approach. In this case, the moving target position is directly obtained by a stereo system, with a baseline of 15 cm. The method (4) is based on the approach presented in [68]. In this case, UAVs are equipped with GPS, and the moving target position is estimated through the pseudo-stereo vision system composed of the monocular cameras of each UAV. Gaussian noise with ${\sigma }_g=20$ cm is added to GPS measurements. Method (5) estimates the target position using monocular and range measurements to the target. In this case, it is assumed some cooperative-target scheme for obtaining range measurements. Technology for obtaining such kind of range measurements is presented in [69]. Gaussian noise with ${\sigma }_r=20$ cm is added to the range measurements. Note that methods (3), (4), and (5) are not SLAM methods. Methods (3) and (5) provide only the relative estimation of the target position with respect to the UAV. Method (4), due to the GPS, provides also global position estimations of the target and UAVs.

In case of SLAM methods and to carry out a more realistic evaluation, the data association problem was also accounted for. To emulate the failures of the visual data association process, $5\%$ of the total number of visual correspondences are forced to be outliers in a random manner.

Table 4. Number of outliers introduced into the SLAM methods.

Method	Visual Outliers	Visual Outliers	Visual Outliers
	(Quad 1)	(Quad 2)	(Cooperative)
Proposed	3798	3798	2338
(1)	3820	-	-
(2)	3798	3798	2338

shows the number of outliers introduced into the simulation due to the data association problem. Outliers for the visual data association in each camera as well as outliers for the cooperative visual data association are considered.

Figure 9 shows the results obtained from each method for estimating the relative position of Quad 1 with respect to the target. For the sake of clarity, the results are shown in two columns of plots. Each row of plots represents a reference axis.

Table 5. Total Mean Squared Error of the estimated relative position of Quad 1 with respect to the target.

Method	$\mathbf{MSEX}\left(\mathit{m}\right)$	$\mathbf{MSEY}\left(\mathit{m}\right)$	$\mathbf{MSEZ}\left(\mathit{m}\right)$
Proposed	$0.0029$	$0.0032$	$0.0512$
(1)	$0.0975$	$0.1053$	$6.0702$
(2)	$0.0083$	$0.0090$	$0.5238$
(3)	$0.0069$	$0.0048$	$0.0937$
(4)	$0.2829$	$0.3539$	$23.4294$
(5)	$0.0045$	$0.0036$	$0.0230$

summarizes the Mean Squared Error (MSE) for the estimated relative position of Quad 1 with respect to the target in the three axes.

Regarding the performance of SLAM methods for estimating the features map,

Table 6. Total Mean Squared Error of the estimated position of landmarks.

Method	MSEX (m)	MSEY (m)	MSEZ (m)
Proposed	0.0969	0.0687	0.1991
(1)	6.1459	6.2934	5.9953
(2)	0.4231	0.3471	0.7477

shows the total (sum of all) Mean Squared Errors for the estimated position of landmarks, while

Table 7. Total Mean Squared Error of the estimated initial position of landmarks.

Method	MSEX (m)	MSEY (m)	MSEZ (m)
Proposed	0.9993	0.8787	1.2064
(1)	4.3668	4.4917	3.8829
(2)	1.4241	1.2655	1.7639

shows the total Mean Squared Errors for the initial estimated position of the landmarks.

According to the above results, the relative position of the UAV with respect to the moving target was recovered fairly well with the following methods: authors’ proposed Target-Centric Multi-UAV SLAM approach, (2) Robot-Centric Multi-UAV SLAM, (4) Multi-UAV Pseudo-stereo, and (5) Single-UAV monocular-range.

It is important to note that method (4) relies on GPS and thus it is not suitable for GPS-denied environments. Method (5) relies on range measurements to the target which can be often difficult to obtain, or it requires some cooperative target scheme which is not even possible to accomplish for several kinds of applications. Regarding the other approaches, method (1) does not have direct three-dimensional information of the target (only monocular measurement) and thus the estimation is not good. In the case of method (3), it is well known that, with a fixed stereo system, the quality of measurements considerably degenerates as the distance with respect to the target increases.

Thus far, the above results suggest that both the proposed approach and the method (2) (both SLAM systems) exhibit good performance in challenging conditions: (i) GPS-denied environments, (ii) non-cooperative target, and (iii) long distance to the target. The only difference between these two methods is the parametrization of the system (Target-centric vs. Robot-centric). To investigate if there is an advantage in the use of one parametrization over the other, another test was carried out.

In this case, using the same simulation setup, the robustness of the methods was tested against the loss of visual contact to the moving target by one UAV during some period. Figure 10 shows the estimated relative positions of Quad 1 with respect to the target obtained with both systems. Note that, during the seconds 45 to 75 of simulation, there are no monocular measurements of the target done by Quad 1. Only the estimated values of Quad 1 are presented, but the results of Quad 2 are very similar.

Given the above results, it can be seen that, contrary to the method (2), the proposed system is robust to the loss of visual contact of the target by one of the Quads. This is a clear advantage of the proposed Target-centric parameterization.

6.4. Control Simulations’ Results

A set of simulations was also carried out to test the whole proposed system in an estimation-control closed-loop manner. In this case, to maintain a stable flight formation with respect to the moving target, the estimates obtained from the proposed visual-based estimation system are used as feedback to the control scheme described in Section 5).

In this test, the values of vectors ${}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{q}}{d}^i$ that define the flight formation are:

For Quad 1: ${}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{q}}{d}^1=\left[\begin{array}{c}-1.4142-0.35sin(t·0.1),\hfill \end{array}\right.$ ${\left.\begin{array}{cc}1.4142+0.35sin(t·0.1),\hfill & 12\end{array}\right]}^T$.

For Quad 2: ${}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{q}}{d}^2={\left[\begin{array}{ccc}2+0.5sin(t·0.1),\hfill & 0,& 14\end{array}\right]}^T$.

In addition, in this test, a period of visual contact loss of target by Quad 1 is simulated (seconds 35 to 50). A period of a visual contact loss of target by both Quads is simulated (seconds 90 to 95) as well.

Figure 11 shows the evolution of the error with respect to the desired values ${}^{\mathbf{t}}{\mathbf{x}}_{\mathbf{q}}{d}^i$. In all the cases, note that the errors are bounded after an initial transient period.

Figure 12 shows the real and estimated relative position of Quad 1 (left column) and Quad 2 (right column) with respect to the target.

Table 8. Total Mean Squared Error in the estimated relative position of Quad 1 and Quad 2 with respect to the target.

	$\mathbf{MSEX}\left(\mathit{m}\right)$	$\mathbf{MSEY}\left(\mathit{m}\right)$	$\mathbf{MSEZ}\left(\mathit{m}\right)$
Quad 1	$0.0137$	$0.0080$	$0.0738$
Quad 2	$0.0134$	$0.0068$	$0.0848$

summarizes the Mean Squared Error (MSE), in each axis, for the estimated relative position of Quad 1 and Quad 2 with respect to the target.

According to the above results, it can be seen that the control system was able to maintain a stable flight formation along with all the trajectory with respect to the target, using the proposed visual-based estimation system as feedback.

7. Conclusions

This research presents a cooperative visual-based SLAM system that allows a team of aerial robots autonomously following a non-cooperative target moving freely in a GPS-denied environment. The relative position of each UAV with respect to the target is estimated by fusing monocular measurements of the target and landmarks using the standard EKF-based SLAM methodology.

The observability property of the system was investigated using an extensive nonlinear observability analysis. In this case, new theoretical results were presented and the observability conditions were derived. To improve the observability properties of the system, and thus its performance, the use of a Target-centric SLAM configuration is proposed. In addition, measurements of altitude differential between pairs of UAVs, obtained from altimeters, are integrated into the system for the same purpose. Additionally to the proposed estimation system, a control scheme was proposed allowing the control of UAVs flight formation with respect to the moving target. The stability of control laws is proved using the Lyapunov theory. An extensive set of computer simulations was performed in order to validate the proposed scheme. According to the simulation results, the proposed system shows an excellent and robust performance to estimate the relative position of each UAV with respect to target. The results also suggest that the proposed approach has better performance when it is compared with other related methods. Moreover, with the proposed control laws, the contributed SLAM system demonstrates good performance in closed-loop. On the other hand, although computer simulations are useful for evaluating the full statistical consistency of the methods, they can still neglect important practical issues that appear when the methods are applied in real scenarios. Accordingly, it is important to note that future work will be focused on developing experiments with real data in order to fully evaluate the applicability of the proposed approach.