Unscented Transformation-Based Multi-Robot Collaborative Self-Localization and Distributed Target Tracking

Abstract

The problem of multi-robot collaborative self-localization and distributed target tracking in practical scenarios is studied in this work. The major challenge in solving the problem in a distributed fashion is properly dealing with inter-robot and robot–target correlations in order to realize consistent state estimates of the local robots and the target simultaneously. In this paper, an unscented transformation-based collaborative self-localization and target tracking algorithm is proposed. Inter-robot correlations are approximated in a distributed fashion, and robot–target correlations are safely discarded with a conservative covariance intersection method. Furthermore, the state update is realized in an asynchronous manner with different kinds of measurements while accounting for measurement and communication limitations. Finally, to deal with nonlinearity in the processes and measurement models, the unscented transformation approach is adopted. Unscented transformation is better able to characterize nonlinearity than the extended Kalman filter-based method and does not require computation of the Jacobian matrix. Simulations are extensively studied to show that the proposed method can realize stable state estimates of both local robots and targets, and results show that it outperforms the EKF-based method. Moreover, the effectiveness of the proposed method is verified on experimental quadrotor platforms carrying off-the-shelf onboard sensors.

1. Introduction

Multi-robot systems (MRSs) have garnered tremendous research interest in recent years [1]. Compared with a single robot, an MRS usually has greater efficiency and operational capability in accomplishing complex tasks, such as transportation [2], search and rescue [3], and mapping [4]. Among these MRS applications are the fundamental tasks of obtaining reliable localization information for the local robot and the uncooperative target using various measurements; these two processes are often referred to as collaborative self-localization (CL) [5,6,7,8,9] and distributed target tracking (DT) [10,11,12,13], respectively. In the CL process, each robot measures the relative quantities with regard to neighboring cooperative robots. By cooperating with other robots, each robot is able to refine its own positioning information. In the DT process, each robot performs a measurement function on the uncooperative targets to be tracked. Then, the states of the target can be estimated cooperatively through interactions with other robots. Although the problems of CL and DT are usually solved by two separate techniques, such as in [5,6,7,8,9,10,11,13,14,15,16,17], they are correlated in most practical scenarios. In the DT process, the target tracking accuracy is dependent on the localization information of the corresponding robots, as well as the relative measurements between the robot and target. The target tracking results obtained by each local robot, in turn, can be implemented to improve the localization performance of the robots. To realize MRS self-localization and target tracking simultaneously, a combined collaborative self-localization and distributed target tracking (CLAT) framework is studied in this paper.

The problem of multi-robot collaborative localization has drawn significant attention in recent years. In [18], the state of the art in collaborative localization is surveyed, and the theoretical limits, algorithms, and practical challenges are discussed. As one of the fundamental challenges in CL, the application of a proper data fusion strategy to deal with the correlations between robots was studied in [5,6,7,8,9,14,15]. A direct approach involves the local robot treating the states of neighboring robots as fully confident variables that will lead to zero correlations between robots [5]. However, this impractical assumption of neighboring positions can lead to overconfident estimates. A more practical method to fuse the relative measurements when the correlation is unknown is the implementation of conservative correlation approximation methods, such as covariance intersection (CI) [6] or split covariance intersection (SCI) [7]. A CL approach using CI was proposed in [14]. This method is provably consistent and can handle asynchronous communication and measurement. The SCI-based approach, as studied in [7,8], further separates the covariance into correlated and uncorrelated parts, and the latter is fused using the CI method. Despite the ability of CI-based methods to preserve the consistency of the estimates, they often have overly conservative results. Making a trade-off between estimation accuracy and the corresponding cost during the CI-based collaborative localization process was investigated using the optimal scheduling problem in [19]. Another popular method to deal with the CL problem is based on factor graphs, which are formulated on the basis of entire trajectories, such as in [15]. The correlation can be explicitly tracked in the factor-graph-based method. However, storing all of the measurements resulting from this method requires significantly more storage space than the recursive method. To address the above drawbacks, a recursive extended Kalman filter (EKF)-based CL method was proposed in [20], in which the correlation was accurately tracked in a decentralized manner. In [21], the processing and storage costs were further reduced by introducing a server that broadcasts an update message when an inter-robot relative measurement is taken. However, in this method, when a relative measurement is taken between two robots, communication involves all robots rather than just the two in the relative measurement, and this significantly increases the communication burden. Another recursive EKF-based CL method was proposed in [9]. This method efficiently approximates the correlation and only stores the current measurement. When the relative measurement is taken, only the communication between the two robots is required.

The distributed tracking problem has also been extensively studied [22]. Early-stage algorithms that have been proposed to solve this problem can be roughly split into two categories: consensus-based algorithms [16] and diffusion-based algorithms [10]. The former category, in general, requires multiple communication iterations during each sampling time interval and hence could lead to a heavy communication burden. To reduce the communication bandwidth, a distributed Kalman filter with event-triggered communication was proposed in [23], and the stability is guaranteed. The latter category does not have such drawbacks, but it may require local joint detectabilities at every single agent, and such a requirement might not be satisfied in a general multi-robot target tracking scenario. A more practical DT approach called distributed hybrid information fusion (DHIF) [11] is able to guarantee stability and is asymptotically unbiased with very mild sufficient conditions. To further solve the distributed tracking problem with a nonlinear process and sensing models, an EKF-based paradigm was proposed in [24], and the stability was analyzed in [25]. Also, the unscented transformation-based approach, which has been regarded as a superior alternative to the EKF when the systems are highly nonlinear, was integrated with the DT process in [26]. However, both the EKF and unscented Kalman filter (UKF) mentioned above are consensus-based and hence may generally result in a heavy communication burden. Recently, the aforementioned DHIF was extended to a nonlinear scenario using the DT approach in [12], and the stochastic stability was analytically studied. Besides the methodology research and theoretical analyses above, the performance of different fusion strategies in terms of their communication rate, information type, and memory size were compared in [27].

The CLAT framework has gained attention in recent years. There are two main kinds of methods: batch and recursive methods. The batch method estimates the entire state trajectory on the basis of all measurements and motion information up to the present, and the recursive method uses only the current measurement and control information. The batch-based method is supposed to outperform the recursive method but at the cost of significantly larger computation and storage requirements and, if in a distributed fashion, communication requirements. One batch-based method was proposed in [28]. By introducing a factor graph that contained robot and target nodes and relative measurements, the problem was formulated as a least-square minimization problem and was solved with sparse optimization methods. Another batch-based method was presented in [29], where the CLAT problem was formulated as a maximum a posteriori estimation problem, and the unscented transformation (UT) technique was implemented to better characterize the nonlinear process. Furthermore, the observability condition was extensively studied. For an MRS with limited computation and storage capacity, the recursive method is often preferred. A recursive-filter-based CLAT was studied in [30], and the error bounds are theoretically provided. Nevertheless, the results in [30] are based on a specially designed measurement graph so that the correlation can be tracked properly. A recursive Bayesian method was proposed in [31] to perform the CLAT in a distributed sequential fashion; however, this method needs synchronous communication at each time instance and will therefore add a significant communication burden. Further, an error propagation analysis was carried out, and the convergence conditions are given in [32], which showed that the localization and tracking accuracy only depends on the expectation of the measurement precision.

In this paper, the multi-robot localization and target tracking problem with a general nonlinear process and various measurement models is studied, and a UT-based CLAT scheme is proposed with consideration of the communication and memory limitations. The main contributions of this paper are summarized as follows: First, the proposed UT-based CLAT is recursive, and it does not store measurements; each robot only keeps the latest estimates of its own and the target, so the storage requirement is significantly reduced. Furthermore, communication is limited to the two robots to obtain a cooperative relative measurement, and no communication with other robots is needed. Meanwhile, to guarantee estimation consistency, inter-robot correlations are approximated in a distributed fashion on the basis of the covariance split method, and the robot–target correlation is discarded using the conservative CI method. Finally, the overall system is modeled on the basis of general nonlinear models and is characterized on the basis of the UT approach rather than the EKF method. Thus, the computation of a Jacobian is avoided. Simulations were carried out, and they indicate that the proposed UT-CLAT method is able to realize stable state estimates of both local robots and targets. More importantly, a hardware platform containing three quadrotors was implemented to verify the effectiveness of the proposed UT-CLAT method. Specifically, three types of measurements (absolute measurement, relative cooperative, and uncooperative measurement) from, respectively, the navigation system, ultra-wide bandwidth (UWB) transmitters, and onboard cameras were utilized to effectively estimate the states of the local robots and targets.

The rest of this paper is organized as follows: Section 2 formulates the CLAT problem, and Section 3 describes the proposed CLAT method. Section 4 and Section 5, respectively, provide the simulation results, which are based on synthetic data, and experimental results, which are based on hardware platforms. Section 6 concludes the paper.

2. Problem Formulation

2.1. Models

Consider N homogeneous cooperative robots, denoted as $i\in \mathcal{V}$, performing collaborative self-localization and distributed tracking of a target of interest, denoted as t. The dynamics of the cooperative robots and the target are expressed respectively with the following nonlinear process models:

$$\begin{array}{cc}\hfill {\mathbf{x}}_{i,k+1}& =f_v({\mathbf{x}}_{i,k},{\mathbf{u}}_{i,k}),\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill {\mathbf{x}}_{t,k+1}& =f_t({\mathbf{x}}_{t,k},{\mathbf{w}}_{t,k}),\hfill \end{array}$$

(2)

where ${\mathbf{x}}_{i,k}\in {\mathbb{R}}^{n_v}$ and ${\mathbf{x}}_{t,k}\in {\mathbb{R}}^{n_t}$ respectively denote the state of robot i and target t at time k. ${\mathbf{u}}_{i,k}\in {\mathbb{R}}^{n_u}$ is the control input of robot i, which is assumed to be subject to a Gaussian distribution ${\mathbf{u}}_{i,k}\sim \mathcal{N}\left({\overline{\mathbf{u}}}_{i,k},{\mathbf{Q}}_i\right)$. ${\overline{\mathbf{u}}}_i$ denotes the control command, and ${\mathbf{Q}}_i$ is the control input covariance. ${\mathbf{w}}_{t,k}\in {\mathbb{R}}^{n_w}$ is the process noise of the target and is assumed to be drawn from a zero-mean Gaussian distribution ${\mathbf{w}}_{t,k}\sim \mathcal{N}\left(\mathbf{0},{\mathbf{Q}}_t\right)$. It is assumed that all robots $i\in \mathcal{V}$ share the same nonlinear transformation $f_v:{\mathbb{R}}^{n_v}\times {\mathbb{R}}^{n_u}$, and the moving target nonlinear transformation $f_t:{\mathbb{R}}^{n_t}\times {\mathbb{R}}^{n_w}$ is known to all robots.

In the cooperative localization and target tracking scenario, each robot i is able to measure three pieces of information: its absolute state and the relative pairwise measurements to neighboring robots and to the target. The measurements at time instance k are denoted respectively as ${\mathbf{z}}_{i,k}^a\in {\mathbb{R}}^{n_{za}}$, ${\mathbf{z}}_{ij,k}^c\in {\mathbb{R}}^{n_{zc}},j\in \mathcal{V}\backslash \left\{i\right\}$, and ${\mathbf{z}}_{it,k}^t\in {\mathbb{R}}^{n_{zt}}$. The corresponding measurement functions are listed below:

$$\begin{array}{cc}\hfill {\mathbf{z}}_{i,k}^a& =h^a({\mathbf{x}}_{i,k},{\mathbf{v}}_{i,k}^a),\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill {\mathbf{z}}_{ij,k}^c& =h^c({\mathbf{x}}_{i,k},{\mathbf{x}}_{j,k},{\mathbf{v}}_{i,k}^c),\hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill {\mathbf{z}}_{it,k}^t& =h^t({\mathbf{x}}_{i,k},{\mathbf{x}}_{t,k},{\mathbf{v}}_{i,k}^t),\hfill \end{array}$$

(5)

where ${\mathbf{v}}_{i,k}^a,{\mathbf{v}}_{i,k}^c$ and ${\mathbf{v}}_{i,k}^t$ are the measurement noise of the above three measurement processes and assumed to be drawn from zero-mean Gaussian distributions, i.e., ${\mathbf{v}}_{i,k}^a\sim \mathcal{N}(\mathbf{0},{\mathbf{R}}_i^a)$, ${\mathbf{v}}_{i,k}^c\sim \mathcal{N}(\mathbf{0},{\mathbf{R}}_i^c)$, and ${\mathbf{v}}_{i,k}^t\sim \mathcal{N}(\mathbf{0},{\mathbf{R}}_i^t)$.

Defining the CLAT as a graph $G(V,E)$, the node set $V=\mathcal{V}\cup \left\{t\right\}\cup \left\{0\right\}$, and the special node 0 denotes the absolute position origin. The edge set is denoted as $E\subseteq \mathcal{V}\times V$. For a robot $i\in \mathcal{V}$, an edge $(i,0)$ denotes a robot that can access its own absolute position. In this paper, it is assumed that a subset of the robots $\mathcal{V}$ can obtain the absolute measurement ${\mathbf{z}}_k^a$. An edge $(i,t)$ indicates that a robot i is able to detect a target t. Since the sensing range of an uncooperative sensor is limited, the availability of an uncooperative target measurement depends on the relative positions of the robot and the target. An edge $(i,j),j\in \mathcal{V}\backslash i$ denotes that a pairwise measurement between robots i and j is obtained. Similarly, a cooperative relative measurement is available when two robots are within the cooperative sensing range. Moreover, in this paper, it is assumed that whenever a relative measurement ${\mathbf{z}}_{ij,k}^c$ is taken, a communication link is established simultaneously between robots i and j so that they can share information.

2.2. Motivation and Objective

Although the CL and DT problems have been extensively studied, their combination still draws limited attention, especially when considering practical multi-robot operation conditions such as nonlinear models or limited sensing and communication capabilities. One answer to the above challenge was provided in [9] by implementing the EKF scheme and asynchronized measurement update. However, this only covered the CL task, and the uncooperative target was not considered. On the other hand, it is also well known that the computation of Jacobian matrices is required by EKF-based algorithms. This may cause difficulties during implementation. Moreover, the estimation performance may deteriorate if the assumption of local linearity is not valid (e.g., bearing sensors). An alternative approach to extending the algorithm while avoiding the aforementioned potential drawbacks is to use the UT.

Motivated by the above observations, in this paper, a UT-based CLAT scheme (UT-CLAT) is proposed that can realize self-localization and target tracking simultaneously in practical multi-robot operation scenarios. The correlations are properly addressed by implementing split covariance methods, similar to the method in [9], and the covariance intersection method. The UT approach was adopted to approximate the statistics of random variables in nonlinear models. In the end, the effectiveness of the proposed UT-CLAT algorithm is illustrated using not only simulations with synthetic data but also experiments with a networked quadrotor system and off-the-shelf sensors (cameras and UWB transmitters).

3. UT-Based CLAT

In this section, the proposed UT-CLAT is described. The states of local robots and targets are estimated using a recursive UT-based Kalman filter, with the aforementioned three types of measurements updated in an asynchronous fashion. For each robot i, the local states, covariance, and the correlation between it and other robots $j\in \mathcal{V}$ are tracked. Specifically, the correlation term is approximately tracked in a distributed fashion, similar to [9]. As a matter of fact, the target may be detected by different robots at different times. It is difficult to track the robot–target correlation in a local robot when there are inter-robot correlations. To realize consistent state estimation under unknown robot–target correlations, a conservative CI method is introduced to safely remove the robot–target correlation terms and the correlation between target estimates from different robots. The above algorithm consists of state propagation (Section 3.1) and three types of measurement update processes (Section 3.2). In particular, the communication link is supposed to be established only during the cooperative relative measurement update process and the data from different robots are fused.

Suppose that at time k, each robot i has a posterior estimated state and its error covariance at a previous time instance, denoted as ${\widehat{\mathbf{x}}}_{i,k-1}$ and ${\mathbf{P}}_{i,k-1}$, respectively. If a relative measurement between robots i and j is taken before time instance k, then the correlated term ${\mathbf{P}}_{ij,k-1}$ is arbitrarily decomposed as

$${\mathbf{P}}_{ij,k-1}={\sigma }_{ij,k-1}{\sigma }_{ji,k-1}^{\top }$$

(6)

and respectively stored in robots i and j. Robot i also holds an estimation of the target t locally, denoted as ${\widehat{\mathbf{x}}}_{t_i,k-1}$ and ${\mathbf{P}}_{t_i,k-1}$.

3.1. Propagation

The propagation process involves the local robots as well as the target. According to the dynamics of Equations (1) and (2), each robot propagates its own state estimates and the local estimate of the target.

Let the augmented state vector and the corresponding augmented covariance matrix for each robot’s local state at time $k-1$ be denoted respectively as ${\widehat{\mathbf{x}}}_{i,k-1}^a\in {\mathbb{R}}^{n_a}$ and ${\mathbf{P}}_{i,k-1}^a\in {\mathbb{R}}^{n_a\times n_a}$, where $n_a=n_v+n_u$,

$${\widehat{\mathbf{x}}}_{i,k-1}^a\triangleq \left[\begin{array}{c}{\widehat{\mathbf{x}}}_{i,k-1}\\ {\overline{\mathbf{u}}}_{i,k-1}\end{array}\right],\mathrm{and}{\mathbf{P}}_{i,k-1}^a\triangleq \left[\begin{array}{cc}{\mathbf{P}}_{i,k-1}& \mathbf{0}\\ \mathbf{0}& {\mathbf{Q}}_i\end{array}\right].$$

(12)

A set of $2n_a+1$ sigma points, denoted as ${\mathcal{X}}^a$, is selected as follows:

$$\begin{array}{cc}\hfill {\mathcal{X}}_{i,k-1}^{a,0}& ={\widehat{\mathbf{x}}}_{i,k-1}^a,\hfill \\ \hfill {\mathcal{X}}_{i,k-1}^{a,r}& ={\widehat{\mathbf{x}}}_{i,k-1}^a+{\left\{\sqrt{\left(n_a+\gamma \right){\mathbf{P}}_{i,k-1}^a}\right\}}_{\left(:,r\right)},\mathrm{if}r\in \{1,\cdots ,n_a\},\hfill \\ \hfill {\mathcal{X}}_{i,k-1}^{a,r}& ={\widehat{\mathbf{x}}}_{i,k-1}^a-{\left\{\sqrt{\left(n_a+\gamma \right){\mathbf{P}}_{i,k-1}^a}\right\}}_{\left(:,r-n_a\right)},\mathrm{otherwise}.\hfill \end{array}$$

Here, $\gamma ={\alpha }^2\left(n_a+\kappa \right)-n_a$ is a scaling parameter, with $0<\alpha \le 1$ and $\kappa \in \mathbb{R}$ as tuning parameters to control the spread of the sigma points. The weights for propagating the mean and covariances, denoted respectively as $W_m^r$ and $W_c^r$, are computed as

$$\begin{array}{cc}\hfill W_m^0& =\gamma /\left(n_a+\gamma \right),\hfill \\ \hfill W_c^0& =\gamma /\left(n_a+\gamma \right)+\left(1-{\alpha }^2+\beta \right),\hfill \\ \hfill W_m^r& =W_c^r=1/2\left(n_a+\gamma \right),r=1,\cdots ,2n_a,\hfill \end{array}$$

where $\beta$ is used to incorporate extra higher-order effects. Note that the definition of the sigma points directly implies that

$${\sum }_{r=0}^{2n_a}\left(W_m^r\right){\mathcal{X}}_{i,k-1}^{a,r}={\mathcal{X}}_{i,k-1}^{a,0}={\widehat{\mathbf{x}}}_{i,k-1}^a,$$

or equivalently,

$${\sum }_{r=0}^{2n_a}{W}_m^r{\mathcal{X}}_{i,k-1}^r={\widehat{\mathbf{x}}}_{i,k-1},{\sum }_{r=0}^{2n_a}{W}_m^r{\mathcal{U}}_{k-1}^r={\overline{\mathbf{u}}}_{i,k-1},$$

where ${\mathcal{X}}_{i,k-1}^r$ and ${\mathcal{U}}_{i,k-1}^r$ collect the components of ${\mathcal{X}}_{i,k-1}^{a,r}$ corresponding to, respectively, ${\mathbf{x}}_{i,k-1}$ and ${\mathbf{u}}_{i,k-1}$.

The above unscented transform is summarized below:

$${\mathcal{X}}_{i,k-1}^{a,r}=UT({\widehat{\mathbf{x}}}_{i,k-1}^a,{\mathbf{P}}_{i,k-1}^a),r=0,\cdots ,2n_a.$$

By defining the augmented state vector and the covariance with regard to the local estimates of the target t within robot i similar to Equation (7), the UT of the target t can be summarized as

$${\mathcal{X}}_{t_i,k-1}^{a,r}=UT({\widehat{\mathbf{x}}}_{t_i,k-1}^a,{\mathbf{P}}_{t_i,k-1}^a),r=0,\cdots ,2n_a.$$

Then, the prior local estimates and corresponding error covariance of the current state and target are computed respectively as

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{i,k}& ={\sum }_{r=0}^{2n_a}{W}_m^r{\mathcal{X}}_{i,k}^r,\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{i,k}& ={\sum }_{r=0}^{2n_a}{W}_c^r\left({\mathcal{X}}_{i,k}^r-{\widehat{\mathbf{x}}}_{i,k}\right){\left({\mathcal{X}}_{i,k}^r-{\widehat{\mathbf{x}}}_{i,k}\right)}^{\top },\hfill \end{array}$$

(9)

and

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{t_i,k}& ={\sum }_{r=0}^{2n_a}{W}_m^r{\mathcal{X}}_{t_i,k}^r,\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{t_i,k}& ={\sum }_{r=0}^{2n_a}{W}_c^r\left({\mathcal{X}}_{t_i,k}^r-{\widehat{\mathbf{x}}}_{t_i,k}\right){\left({\mathcal{X}}_{t_i,k}^r-{\widehat{\mathbf{x}}}_{t_i,k}\right)}^{\top },\hfill \end{array}$$

(11)

where

$${\mathcal{X}}_{i,k}^r=f_v\left({\mathcal{X}}_{i,k-1}^r,{\mathcal{U}}_{i,k-1}^r\right),r=0,\cdots ,2n_a,$$

and

$${\mathcal{X}}_{t_i,k}^r=f_t\left({\mathcal{X}}_{t_i,k-1}^r,{\mathcal{W}}_{t_i,k-1}^r\right),r=0,\cdots ,2n_a.$$

${\mathcal{X}}_{t_i,k-1}^r$ and ${\mathcal{W}}_{t_i,k-1}^r$ are the sigma points corresponding to ${\mathbf{x}}_{t_i,k-1}$ and ${\mathbf{w}}_{t,k-1}$, and

$${\sum }_{r=0}^{2n_a}{W}_m^r{\mathcal{X}}_{t_i,k-1}^r={\widehat{\mathbf{x}}}_{t_i,k-1},{\sum }_{r=0}^{2n_a}{W}_m^r{\mathcal{W}}_{k-1}^r=\mathbf{0}.$$

The propagation of the correlation term ${\mathbf{P}}_{ij}$ involves the pose and control inputs of both i and j, and therefore cannot be propagated locally by robot i. To avoid communication, the local correlation term ${\sigma }_{ij}$ is instead propagated as

$${\sigma }_{ij,k}={\mathcal{F}}_v{\sigma }_{ij,k-1},$$

(12)

where ${\mathcal{F}}_v$ is the inferred Jacobian matrix with regard to the dynamic function in Equation (1) and, according to [33], is defined as

$${\mathcal{F}}_v={\mathbf{P}}_{i,k|k-1}^{xx}{\left({\mathbf{P}}_{i,k-1}\right)}^{-1},$$

where ${\mathbf{P}}_{i,k|k-1}^{xx}\approx {\sum }_{r=0}^{2n_a}({\mathcal{X}}_{i,k}^r-{\widehat{\mathbf{x}}}_{i,k}){({\mathcal{X}}_{i,k-1}^r-{\widehat{\mathbf{x}}}_{i,k-1})}^{\top }$.

3.2. Update

In the update stage, three types of measurement (Equations (3)–(5)) are considered. When a private measurement or a target measurement is taken by robot i, the information is updated locally to avoid communication. When two robots i and j are within the relative range, a relative measurement is taken, and local beliefs of both robot and target and the inter-robot correlation term are exchanged to update the estimates of the local robots and target. For clarity, ${\widehat{\mathbf{x}}}^{-}$, ${\mathbf{P}}^{-}$ and $\widehat{\mathbf{x}}$, $\mathbf{P}$ are used respectively to denote the state estimate and covariance prior to and after a certain measurement update process.

3.2.1. Private Update

During each private update process, the local robot measures its local pose through, for example, a GPS receiver and magnetometer, to refine its local estimation. Only the local pose participates in the private update process.

First, the inferred Jacobian ${\mathcal{H}}_{i,k}$ corresponding to the measurement function in Equation (3) is obtained as

$${\mathcal{H}}_{i,k}={\mathbf{P}}_{i,k}^{xzr}{\left({\mathbf{P}}_{i,k}^{-}\right)}^{-1},$$

where

$${\mathbf{P}}_{i,k}^{xzr}=\sum _{r=0}^{2n_a}{W}_c^r\left({\mathcal{X}}_{i,k}^r-{\widehat{\mathbf{x}}}_{i,k}^{-}\right)\left(h^a\left({\mathcal{X}}_{i,k}^r\right)-{\mathbf{z}}_{i,k}^a\right).$$

Then, the state and covariance can be updated as

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{i,k}& ={\widehat{\mathbf{x}}}_{i,k}^{-}+{\mathbf{K}}_{i,k}\left({\mathbf{z}}_{i,k}^a-h_i^a\left({\widehat{\mathbf{x}}}_{i,k}^{-}\right)\right),\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{i,k}& =(\mathbf{I}-{\mathbf{K}}_{i,k}{\mathcal{H}}_{i,k}){\mathbf{P}}_{i,k}^{-},\hfill \end{array}$$

(14)

where

$$\begin{array}{cc}\hfill {\mathbf{S}}_{i,k}& ={\left({\mathcal{H}}_{i,k}\right)}^{\top }{\mathbf{P}}_{i,k}^{-}{\mathcal{H}}_{i,k}+{\mathbf{R}}_i^a,\hfill \\ \hfill {\mathbf{K}}_{i,k}& ={\mathbf{P}}_{i,k}^{-}{\mathcal{H}}_{i,k}^{\top }{\mathbf{S}}_{i,k}^{-1}.\hfill \end{array}$$

The correlation term within local robot i is updated as

$$\begin{array}{cc}\hfill {\sigma }_{ij,k}& =(\mathbf{I}-{\mathbf{K}}_{i,k}{\mathcal{H}}_{i,k}){\sigma }_{ij,k}^{-},j\in \mathcal{V}\backslash i.\hfill \end{array}$$

(15)

3.2.2. Target Measurement Update

When a target is detected by robot i, a relative measurement related to the pose of both robot i and target t, denoted as ${\mathbf{z}}_{it}^t$, is obtained. The measurement update involves the estimates of the robot and the target, as well as their correlation term. As a matter of fact, the correlation term, denoted as ${\mathbf{P}}_{i{t}_i}$, is difficult to track in a distributed fashion owing to the existence of the inter-robot correlation term. Therefore, in this part, a conservative CI-based method [34] is used to remove the robot–target correlations and guarantee consistency at the same time.

$$\left[\begin{array}{cc}\frac{1}{w_1}{\mathbf{P}}_{i,k}^{-}& \mathbf{0}\\ \mathbf{0}& \frac{1}{1-w_1}{\mathbf{P}}_{t_i,k}^{-}\end{array}\right]\succcurlyeq \left[\begin{array}{cc}{\mathbf{P}}_{i,k}^{-}& {\mathbf{P}}_{i{t}_i,k}^{-}\\ {\mathbf{p}}_{t_{i}i,k}^{-}& {\mathbf{P}}_{t_i,k}^{-}\end{array}\right].$$

(16)

The weight w is determined according to [34]. Let ${\overline{\mathbf{P}}}_{i,k}^{-}\triangleq \frac{1}{w}{\mathbf{P}}_{i,k}^{-}$ and ${\overline{\mathbf{P}}}_{t_i,k}^{-}\triangleq \frac{1}{1-w}{\mathbf{P}}_{t_i,k}^{-}$. The augmented state can be defined as

$${\widehat{\mathbf{x}}}_{i,k}^{b-}=\left[\begin{array}{c}{\mathbf{x}}_{i,k}^{-}\\ {\mathbf{x}}_{t_i,k}^{-}\\ \mathbf{0}\end{array}\right]\mathrm{and}{\mathbf{P}}_{i,k}^b=\left[\begin{array}{ccc}{\overline{\mathbf{P}}}_{i,k}^{-}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\overline{\mathbf{P}}}_{t,k}^{-}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& {\mathbf{R}}_i^t\end{array}\right].$$

Then, the augmented sigma points are obtained as

$${\mathcal{X}}_{i,k}^b=UT\left({\widehat{\mathbf{x}}}_{i,k}^{b-},{\mathbf{P}}_{i,k}^{b-}\right),r=0,\cdots ,2n_a.$$

(17)

The inferred measurement Jacobian is

$$\left[\begin{array}{ccc}{\mathcal{H}}_{i,k}& {\mathcal{H}}_{t_i,k}& {\mathcal{H}}_{v,k}\end{array}\right]={\mathbf{P}}_{i,k}^{xzb}{\left({\mathbf{P}}_{i,k}^{b-}\right)}^{-1},$$

(18)

where

$${\mathbf{P}}_{i,k}^{xzb}=\sum _{r=0}^{2n_a}{W}_c^r\left({\mathcal{X}}_{i,k}^b-{\mathbf{x}}_{i,k}^{b-}\right)\left(h^t\left({\mathcal{X}}_{i,k}^b\right)-{\mathbf{z}}_{it,k}^t\right).$$

The target measurement update process is finally summarized as Equations (19)–(22):

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{i,k}& ={\widehat{\mathbf{x}}}_{i,k}^{-}+{\mathbf{K}}_{i,k}\left({\mathbf{z}}_{it,k}^t-h_i^t\left({\mathbf{x}}_{i,k}^{b-}\right)\right),\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{t_i,k}& ={\widehat{\mathbf{x}}}_{t,k}^{-}+{\mathbf{K}}_{t_i,k}\left({\mathbf{z}}_{it,k}^t-h_i^t\left({\mathbf{x}}_{i,k}^{b-}\right)\right),\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{i,k}& =(\mathbf{I}-{\mathbf{K}}_{i,k}{\mathcal{H}}_{i,k}){\overline{\mathbf{P}}}_{i,k}^{-},\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{t_i,k}& =(\mathbf{I}-{\mathbf{K}}_{t_i,k}{\mathcal{H}}_{i,k}){\overline{\mathbf{P}}}_{t_i,k}^{-},\hfill \end{array}$$

(22)

where the innovation covariance and gain are calculated as

$$\begin{array}{cc}\hfill {\mathbf{S}}_{i,k}& =\left[\begin{array}{cc}{\mathcal{H}}_{i,k}& {\mathcal{H}}_{i_t,k}\end{array}\right]\left[\begin{array}{cc}{\overline{\mathbf{P}}}_{i,k}^{-}& \mathbf{0}\\ \mathbf{0}& {\overline{\mathbf{P}}}_{t,k}^{-}\end{array}\right]{\left[\begin{array}{cc}{\mathcal{H}}_{i,k}& {\mathcal{H}}_{i_t,k}\end{array}\right]}^{\top }+{\mathbf{R}}_i^u,\hfill \\ \hfill {\mathbf{K}}_{i,k}& ={\overline{\mathbf{P}}}_{i,k}^{-}{\mathcal{H}}_{i,k}{\mathbf{S}}_{i,k}^{-1},\hfill \\ \hfill {\mathbf{K}}_{t_i,k}& ={\overline{\mathbf{P}}}_{t,k}^{-}{\mathcal{H}}_{t_i,k}{\mathbf{S}}_{i,k}^{-1}.\hfill \end{array}$$

Formally, the correlation between robots i and j should be updated as

$$\begin{array}{cc}\hfill {\mathbf{P}}_{ij,k}& =(\mathbf{I}-{\mathbf{K}}_{i,k}{\mathcal{H}}_{i,k}){\mathbf{P}}_{ij,k}^{-}.\hfill \end{array}$$

On the basis of the decomposition in Equation (6), the correlation term ${\sigma }_{ij}$ can be calculated as below without communication:

$${\sigma }_{ij,k}=(\mathbf{I}-{\mathbf{K}}_{i,k}{\mathcal{H}}_i){\sigma }_{ij,k}^{-},j\in \mathcal{V}\backslash i.$$

3.3. Neighbor Measurement Update and Target Information Fusion

When two robots i and j are within a given range, a relative measurement is taken, denoted as ${\mathbf{z}}_{ij,k}$, and a communication link between the two robots is established. The target update process is as follows. First, the covariance between two robots ${\mathbf{P}}_{ij}^{-}$ is recovered according to Equation (6). Similar to the target measurement update process, we define the augmented state prior to the measurement update as

$${\widehat{\mathbf{x}}}_k^{c-}=\left[\begin{array}{c}{\widehat{\mathbf{x}}}_{i,k}^{-}\\ {\widehat{\mathbf{x}}}_{j,k}^{-}\\ \mathbf{0}\end{array}\right]\mathrm{and}{\mathbf{P}}_k^{c-}=\left[\begin{array}{ccc}{\mathbf{P}}_i^{-}& {\mathbf{P}}_{ij}^{-}& \mathbf{0}\\ {{\mathbf{P}}_{ij}^{-}}^{\top }& {\mathbf{P}}_j^{-}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& {\mathbf{R}}_i^c\end{array}\right].$$

Then, the augmented sigma points are obtained as

$${\mathcal{X}}_k^c=UT\left({\widehat{\mathbf{x}}}_k^{c-},{\mathbf{P}}_{i,k}^{c-}\right),r=0,\cdots ,2n_a.$$

The inferred measurement Jacobian is

$$\left[\begin{array}{ccc}{\mathcal{H}}_{i,k}& {\mathcal{H}}_{j,k}& {\mathcal{H}}_{v,k}\end{array}\right]={\mathbf{P}}^{xzc}{\left({\mathbf{P}}_{i,k}^{c-}\right)}^{-1}$$

where

$${\mathbf{P}}_{i,k}^{xzc}=\sum _{r=0}^{2n_a}{W}_c^r\left({\mathcal{X}}_{i,k}^c-{\widehat{\mathbf{x}}}_{,k}^{c-}\right)\left(h^c\left({\mathcal{X}}_{i,k}^c\right)-{\mathbf{z}}_{ij,k}^c\right).$$

Consequently, the update process for the relative measurement between robots i and j is as below:

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{i,k}& ={\widehat{\mathbf{x}}}_{i,k}^{-}+{\mathbf{K}}_{i,k}\left({\mathbf{z}}_{ij,k}-h_i^c\left({\widehat{\mathbf{x}}}_{i,k}^{c-}\right)\right),\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{j,k}& ={\widehat{\mathbf{x}}}_{t,k}^{-}+{\mathbf{K}}_{t,k}\left({\mathbf{z}}_{it,k}-h_i^c\left({\widehat{\mathbf{x}}}_{i,k}^{c-}\right)\right),\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{i,k}& =(\mathbf{I}-{\mathbf{K}}_{i,k}{\mathcal{H}}_{i,k}){\mathbf{P}}_{i,k}^{-}-{\mathbf{K}}_{i,k}{\mathcal{H}}_{j,k}{\mathbf{P}}_{ji}^{-},\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{j,k}& =(\mathbf{I}-{\mathbf{K}}_{j,k}{\mathcal{H}}_{j,k}){\mathbf{P}}_{j,k}^{-}-{\mathbf{K}}_{j,k}{\mathcal{H}}_{i,k}{\mathbf{P}}_{ij}^{-},\hfill \end{array}$$

(26)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{ij,k}& =(\mathbf{I}-{\mathbf{K}}_{i,k}{\mathcal{H}}_{i,k}){\mathbf{P}}_{ij,k}^{-}-{\mathbf{K}}_{i,k}{\mathcal{H}}_{j,k}{\mathbf{P}}_j^{-},\hfill \end{array}$$

(27)

where

$$\begin{array}{cc}\hfill {\mathbf{S}}_{i,k}& =\left[\begin{array}{cc}{\mathcal{H}}_{i,k}& {\mathcal{H}}_{j,k}\end{array}\right]\left[\begin{array}{cc}{\mathbf{P}}_{i,k}& {\mathbf{P}}_{ij,k},\\ {\mathbf{P}}_{ij,k}^{\top }& {\mathbf{P}}_{j,k}\end{array}\right]{\left[\begin{array}{cc}{\mathcal{H}}_{i,k}& {\mathcal{H}}_{j,k}\end{array}\right]}^{\top }+{\mathbf{R}}^c,\hfill \\ \hfill {\mathbf{K}}_{i,k}& =({\mathbf{P}}_{i,k}{\mathcal{H}}_{i,k}+{\mathbf{P}}_{ij,k}{\mathcal{H}}_{j,k}){\mathbf{S}}_k^{-1},\hfill \\ \hfill {\mathbf{K}}_{j,k}& =({\mathbf{P}}_{ji,k}{\mathcal{H}}_{i,k}+{\mathbf{P}}_{j,k}{\mathcal{H}}_{j,k}){\mathbf{S}}_k^{-1}.\hfill \end{array}$$

After the relative measurement update, the correlation ${\mathbf{P}}_{ij,k}$ is decomposed again as two multiplicative parts ${\sigma }_{ij,k}$ and ${\sigma }_{ji,k}$ according to Equation (22). Then, ${\sigma }_{ij,k}$ and ${\sigma }_{ji,k}$ are stored in i and j, respectively.

The relative measurement update process also involves the correlation term ${\mathbf{P}}_{il,k},l\in \mathcal{V},l\ne i,j$. Formally, the ${\mathbf{P}}_{il,k}$ should be updated as

$${\mathbf{P}}_{il,k}=(\mathbf{I}-{\mathbf{K}}_{i,k}{\mathcal{H}}_{i,k}){\mathbf{P}}_{il,k}-{\mathbf{K}}_{i,k}{\mathcal{H}}_{j,k}{\mathbf{P}}_{jl,k}.$$

The correlation term ${\mathbf{P}}_{jl,k}$ is not available to robot i. To reduce the overall communication and avoid communication with l, the split term ${\sigma }_{il}^{-}$ in robot i is instead updated in an approximate form, similar to the process in [9], as below:

$${\sigma }_{il,k}={\mathbf{P}}_{i,k}{\left({\mathbf{P}}_{i,k}^{-}\right)}^{-1}{\sigma }_{il,k}^{-}.$$

(28)

In addition to the measurement update, the target beliefs $\{{\widehat{\mathbf{x}}}_{t_i,k},{\mathbf{P}}_{t_i,k}\}$ and $\{{\widehat{\mathbf{x}}}_{t_j,k},{\mathbf{P}}_{t_j,k}\}$ are fused simultaneously. As a matter of fact, the correlation between the two estimates is unknown owing to the unknown target–robot correlation. Again, the conservative CI algorithm can be used as below:

$$\begin{array}{cc}\hfill {\mathbf{P}}_{t,k}=& {\left(w_2{\left({\mathbf{P}}_{t_i,k}\right)}^{-1}+(1-w_2){\left({\mathbf{P}}_{t_j,k}\right)}^{-1}\right)}^{-1},\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{t,k}=& {\mathbf{P}}_{t,k}\left(w_2{\mathbf{P}}_{t_i,k}^{-1}{\widehat{\mathbf{x}}}_{t_i,k}+(1-w_2){\mathbf{P}}_{t_j,k}^{-1}{\widehat{\mathbf{x}}}_{t_j,k}\right).\hfill \end{array}$$

(30)

The weight $w_2$ can be determined according to [11]. The fused results are then stored in both robots i and j.

4. Simulation

In this section, the proposed UT-CLAT method is validated using synthetic data. Without loss of generality, the scenario contains four cooperative robots, labeled 1–4, tracking an uncooperative target in 2D space (as shown in Figure 1). The robots and the target are assumed to be subject to similar nonlinear unicycle models, as below:

$${\mathbf{x}}_{k+1}\triangleq \left[\begin{array}{c}{x}_{k+1}\\ y_{k+1}\\ {\theta }_{k+1}\end{array}\right]=\left[\begin{array}{c}{x}_k+{\Delta }_T(v_c+w_k^v)cos\left({\theta }_k\right)\\ y_k+{\Delta }_T(v_c+w_k^v)sin\left({\theta }_k\right)\\ {\theta }_k+{\Delta }_T({\omega }_c+w_k^{\omega })\end{array}\right],$$

A subscriber i or t is used to distinguish the robots or the target. The state vector ${\mathbf{x}}_k$ to be estimated contains three entries—$x_k$, $y_k$, and ${\theta }_k$—which represent the 2D position and the orientation of the robots and the target with respect to the global frame. It is assumed that at the initial time, the robots are randomly placed on different circles centered at $\left[-10,10\right],\left[10,10\right],\left[-10,-10\right],\left[10,-10\right]$. The same control command $\mathbf{u}={\left[v_c,{\omega }_c\right]}^{\top }={\left[0.3,0.0375\right]}^{\top }$ is applied to each robot to form four approximated circles with radii 8. The velocity and angular velocity noise are assumed to be subject to Gaussian distributions with the covariance ${\mathbf{Q}}_i=\mathrm{diag}\left([0.1{}^2,{(0.5\pi /180)}^2]\right)$. The target is initialized at ${\left[-15,-15\right]}^{\top }$ in the global frame, and the control input is set as ${\mathbf{u}}_t={[0.05,0]}^{\top }$. Similarly, the target control is subject to zero-mean Gaussian noise with ${\mathbf{Q}}_t=\mathrm{diag}\left([0.02{}^2,{(2\pi /180)}^2]\right)$.

In the simulation, robot 1 is assumed to be accessible to the global position and orientation in the global frame with the following measurement model:

$${\mathbf{z}}_{i,k}^a={\mathbf{x}}_{i,k}+{\mathbf{v}}_{i,k}^a,$$

where ${\mathbf{v}}_{i,k}^a\sim \mathcal{N}(\mathbf{0},\mathrm{diag}\left[{(0.5{}^2,0.5{}^2,0.5\pi /180)}^2\right])$ is the control noise.

Both the cooperative robots and uncooperative measurement are subject to a relative range measurement model as follows:

$$\begin{array}{c}\hfill z_{ij,k}^c={\parallel x_j-x_i\parallel }_d+v_{i,k}^c,\end{array}$$

(31)

$$\begin{array}{c}\hfill z_{it,k}^t={\parallel x_t-x_i\parallel }_d+v_{i,k}^t,\end{array}$$

(32)

${\parallel ·\parallel }_d$ is the operator that calculates the relative range between two robots or a robot and the target. The sensing range for the target is set as $r_t=20$, and the sensing range for cooperative robots, as well as the communication range, is set as $r_c=10$. The measurement noises are $v_i^c\sim \mathcal{N}(0,0.05{}^2)$ and $v_i^t\sim \mathcal{N}(0,0.05{}^2)$, respectively.

4.1. Scenario 1

One trial of the simulation described above was carried out. In this scenario, the target is jointly observed by the four robots intermittently. The observation measurement availability for both cooperative measurement and target measurement is based on the sensing ranges $r_c$ and $r_t$, respectively, and is shown in Figure 2 and Figure 3.

Although, for each robot, the observability of the local state and the target’s state cannot be guaranteed owing to the discontinuous range-only measurement, the joint observability for the entire system over a period of time can still be guaranteed through communication with neighbors according to [35].

The estimated trajectories of both robots and the target are plotted in Figure 4 in different colors. Each robot’s self-localization result and local target tracking result are drawn with solid lines of the same color. As observed in Figure 4, the estimated trajectories indicate that each robot is able to localize its true pose and track the true trajectory of the target. The four robots’ self-localization errors and covariances ($\pm 3\sigma$ bounds) are plotted in Figure 5, with solid lines in color and dashed lines in the same color, respectively. It shows that the self-localization errors by each robot are bounded by the $\pm 3\sigma$ envelopes in the steady state. Robot 1 has the lowest tracking error as it can access its own absolute measurement. The target tracking results from the four robots are plotted in Figure 6, where, for each robot, the target tracking errors (solid line in colors) are bounded by the corresponding $\pm 3\sigma$ envelopes (dashed line in the same colors) in the steady state. On the basis of Figure 5 and Figure 6, the min/max self-localization and target tracking errors for time instance $k\in [200,800]$ are listed in

Table 1. Self-localization and target tracking errors ($k\in [200,800]$).

	Absolute Self-Localization Error (min/max)			Absolute Target Tracking Error (min/max)
	x(m)	y(m)	$\mathit{\theta }$(rad)	x(m)	y(m)	$\mathit{\theta }$(rad)
robot 1	0.05/0.35	0.03/0.41	0/0.08	0.03/0.5	0.03/0.66	0.01/0.65
robot 2	0.04/0.94	0.1/1.13	0/0.11	0.04/0.54	0.05/0.54	0.01/0.71
robot 3	0.01/1.31	0.12/0.96	0/0.07	0.02/0.61	0.05/0.47	0/0.72
robot 4	0.02/1.08	0.04/1.24	0/0.12	0.04/0.44	0.03/0.55	0/0.75

.

4.2. Scenario 2

In this part, the performance results of the proposed UT-CLAT method are presented on the basis of 1000 Monte Carlo simulations. Specifically, the simulation in Scenario 1 was repeated 1000 times with $1\le k\le 1200$. For each robot, the position root-mean-square errors (PRMSEs) of the local posterior estimates and target posterior estimates were computed for all trails. Moreover, to demonstrate the effectiveness in a nonlinear scenario, the proposed UT-CLAT method is compared to the EKF-CLAT method by extending the CL algorithm in [9] to the CLAT scenario. In Figure 7, the averaged PRMSEs of the collaborative localization results of 1000 Monte Carlo simulations are plotted using both the UT-CLAT and EKF-CLAT methods.

As observed in Figure 7, both methods can realize stable self-localization in around 200 time instances. In general, the UT-CLAT method is able to achieve more accurate self-localization results. In Figure 8, the averaged PRMSEs of the target tracking results of different robots are plotted. Similar to the CL result in Figure 7, the UT-CLAT is able to realize stable target tracking, and it outperforms the EKF-CLAT method for each robot.

5. Experimental Validation of Quadrotors

In this section, the validation results of the proposed UT-CLAT method are presented. Validation was performed using hardware platforms that included three quadrotors tracking a ground robot. As shown in Figure 9, the system consists of three Intel Aero RTF quadrotors, referred to as $\mathtt{quad}1$–3, and one TurtleBot ground robot. Each quadrotor is equipped with UWB transmitters and a downward monochrome camera. The UWB sensors measure the relative distance and transmit information when two quadrotors are within the functional range of the UWB sensors. A camera is rigidly connected to the body frame of each quadrotor. A target is detected by the camera when the target is within the field-of-view (FOV). Furthermore, $\mathtt{quad}\mathtt{1}$ is assumed to have access to its position through the onboard navigation system. In addition to the above onboard devices, UWB ground anchors are used to record the ground truth states of both robots and the target.

5.1. Robot and Target Dynamics Model

For target monitoring, each quadrotor is controlled to follow a plenary circular trajectory in 3D space. The state ${\mathbf{x}}_i$ includes the position in 3D space and the heading angle, namely, ${\mathbf{x}}_t={[x_i,y_i,z_i,{\theta }_i]}^{\top }$. ${\mathbf{u}}_i\triangleq {[v_i,{\omega }_i]}^{\top }$ denotes the control input command, namely, the linear plenary velocity and angular velocity. The actual velocity and angular velocity are contaminated by zero-mean Gaussian noises, $w_{i,k}^v\sim \mathcal{N}\left(0,Q_i^v\right)$, $w_{i,k}^{\omega }\sim \mathcal{N}\left(0,Q_i^{\omega }\right)$. An extra altitude noise $w_{i,k}^z\sim \mathcal{N}\left(0,Q_i^z\right)$ is added to the process noise. The overall process noise covariance is denoted as ${\mathbf{Q}}_i=\mathrm{blkdiag}(Q_i^v,Q_i^{\omega },Q_i^z)$. On the basis of the above definition, the process model for each robot $f_i$ is defined as follows:

$${\mathbf{x}}_{i,k+1}\triangleq \left[\begin{array}{c}{x}_{i,k+1}\\ y_{i,k+1}\\ z_{i,k+1}\\ {\theta }_{i,k+1}\end{array}\right]=\left[\begin{array}{c}{x}_{i,k}+{\Delta }_T(v_i+w_{i,k}^v)cos\left({\theta }_{i,k}\right)\\ y_{i,k}+{\Delta }_T(v_i+w_{i,k}^v)sin\left({\theta }_{i,k}\right)\\ z_c+w_{i,k}^z\\ {\theta }_{i,k}+{\Delta }_T({\omega }_i+w_{i,k}^{\omega })\end{array}\right]$$

(33)

The target ground robot is modeled with a unicycle model $f_t$ similar to Equation (33). Correspondingly, the state, control input, and process noise are denoted as ${\mathbf{x}}_t$, ${\mathbf{u}}_t$, and ${\mathbf{Q}}_t=\mathrm{blkdiag}(Q_t^v,Q_t^{\omega },Q_t^z)$, respectively.

5.2. Measurement Model

The three types of measurement are utilized to realize the CLAT purpose in this system setup: private absolute measurement from the onboard navigation system, cooperative relative range measurement from the UWB sensors, and angle measurement relative to the target from the downward cameras.

Although the position information from the onboard navigation system is a fusion result from multiple sensors, in this part, it is treated as a private absolute measurement and is modeled as below:

$${\mathbf{z}}_{i,k}^a=H_i^a{\mathbf{x}}_{i,k}+{\mathbf{v}}_{i,k}^a$$

(34)

The measurement noise is assumed to be subject to a zero-mean Gaussian distribution, ${\mathbf{v}}_{i,k}^a\sim \mathcal{N}(\mathbf{0},{\mathbf{R}}^a)$. Obviously, the absolute measurement model is a linear model. Therefore, we can substitute the inferred Jacobian matrix (defined in Section 3.2.1) with $H_i^a$. In this paper, we assume that only a subset of all robots has access to the onboard navigation signal.

The cooperative relative measurement between robots i and j from UWB is a scalar distance modeled as the following nonlinear model:

$$z_{ij,k}^c={\parallel {\mathbf{x}}_{i,k}-{\mathbf{x}}_{j,k}\parallel }_d+v_{i,k}^c,$$

(35)

The measurement noise $v_{ij,k}^c$ is assumed to be subject to the zero-mean Gaussian noise $v_{ij,k}^c\sim \mathcal{N}(0,{\mathbf{R}}_i^c)$.

The target detection measurement is the position of the target on the captured image plane, and the measurement function is defined as

$${\mathbf{z}}_{it,k}^t=d_f\frac{{\overline{\mathbf{x}}}_{it,k}^{(1:2)}}{{\overline{\mathbf{x}}}_{it,k}^{\left(3\right)}}+{\mathbf{v}}_{i,k}^t$$

(36)

where $d_f$ denotes the focal length in pixels, ${\overline{\mathbf{x}}}_{it,k}$ denotes the position of the target in the camera coordinate, i.e., ${\overline{\mathbf{x}}}_{it,k}=\mathcal{R}\left({\theta }_{i,k}\right){\mathcal{R}}_{c,k}({\mathbf{x}}_{t,k}-{\mathbf{x}}_{i,k})$. $\mathcal{R}\left({\theta }_{i,k}\right)$ denotes the yaw angle rotation matrix, and

$$\mathcal{R}\left({\theta }_{i,k}\right)=\left[\begin{array}{ccc}cos\left({\theta }_{i,k}\right)& sin\left({\theta }_{i,k}\right)& 0\\ -sin\left({\theta }_{i,k}\right)& cos\left({\theta }_{i,k}\right)& 0\\ 0& 0& 1\end{array}\right].$$

${\mathcal{R}}_{c,k}$ denotes the roll and pitch rotation matrix and is assumed to be retrieved from the quadrotor navigation system, and therefore, it is treated as a known variable. The measurement noise is $v_{i,k}^t\sim \mathcal{N}(\mathbf{0},{\mathbf{R}}_i^t)$. In this paper, target detection is carried out using kernelized correlation filters (KCFs) [36] on the image plane.

5.3. Experiment Results and Analysis

According to the above model description, an experiment with three quadrotors tracking one ground robot on the basis of the UT-CLAT algorithm was carried out at 10 Hz. The experimental setup is shown in

Table 2. Experiment setup.

Item	Quantity
robot setup	circular center (in meters):
	$c_1={[-5,-5,15]}^{\top }$, $c_2={[5,-5,16.5]}^{\top }$
	$c_3={[-5,5,18]}^{\top }$
	control input:
	$v_i=0.35$ m/s, ${\omega }_i=0.035$ rad/s
	$Q_i^v=0.05{}^2$, $Q_i^w={(\pi /180)}^2$, $Q_i^z=0.1{}^2$
	initial state:
	${\mathbf{P}}_{i,0}=\mathrm{diag}\left([1^2,1^2,0.5{}^2,{(10\pi /180)}^2]\right)$
	${\mathbf{x}}_{1,0}={[-8.5,11.5,15,2.35]}^{\top }$,
	${\mathbf{x}}_{2,0}={[-10,-11,16.5,-0.53]}^{\top }$,
	${\mathbf{x}}_{3,0}={[-1,1,18,0.78]}^{\top }$
target setup	control input:
	$v_i=0.2$ m/s, ${\omega }_i=0$ rad/s
	$Q_t^v=0.02{}^2$, $Q_t^{\omega }={(0.5\pi /180)}^2$, $Q_t^z=0.05{}^2$
	initial state:
	${\mathbf{P}}_{t_i,0}=\mathrm{diag}\left([0.1{}^2,0.1{}^2,0.5{}^2,{(\pi /180)}^2]\right)$,
	${\mathbf{x}}_{t_i,0}={[-15,-15,0,0.785]}^{\top }$
measurement setup	${\mathbf{R}}^a=\mathrm{diag}\left([0.2{}^2,0.2{}^2,0.1{}^2]\right)$,
	${\mathbf{R}}^c=0.1{}^2$, ${\mathbf{R}}^t=\mathrm{diag}\left([5^2,5^2]\right)$

.

One experimental snapshot of the three quadrotors with the corresponding captured images is shown in Figure 10 when the ground robot is within the FOV of all three quadrotor cameras.

According to the CLAT algorithm, the trajectories of the three quadrotors’ self-localization results and target tracking results are plotted in Figure 11. As observed in Figure 11, the three quadrotors are able to localize themselves while stably tracking the target. It is obvious that the self-localization result from $\mathtt{quad}1$ is better than that of the other two quadrotors as it can obtain the navigation signal from the onboard navigation system. The errors of self-localization and target tracking are plotted in Figure 12 and Figure 13, respectively. The local estimation errors (solid line in colors) and the corresponding approximated $\pm 3\sigma$ envelopes (dashed lines in the same color) of the aforementioned three quadrotors are plotted. As observed in Figure 12 and Figure 13, the estimation errors by each quadrotor are bounded by the $\pm 3\sigma$ envelopes in the steady state.

6. Conclusions

A UT-based CLAT method is proposed to realize multi-robot self-localization and target tracking in a distributed fashion. The proposed method is recursive, and only the most recent estimation is stored within each local robot. The communication is limited to the two robots within the relative measurement, and estimation consistency is guaranteed with the covariance split and covariance intersection method. To deal with the nonlinearity in the dynamics models and measurement models, a UT was integrated into the CLAT framework. Both simulation and experimental results show that the proposed method can fulfill the self-localization and target tracking task in practical multi-robot operation scenarios. Future works will focus on the theoretical analysis of the error bounds of both self-localization and target tracking on the basis of different measurement setups.