*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 **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 **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

$$\begin{aligned} \hat{\mathbf{x}}\_{k}^{c-} &= \begin{bmatrix} \hat{\mathbf{x}}\_{i,k}^{-} \\ \hat{\mathbf{x}}\_{j,k}^{-} \\ \mathbf{0} \end{bmatrix} \text{ and } \mathbf{P}\_{k}^{c-} = \begin{bmatrix} \mathbf{P}\_{i}^{-} & \mathbf{P}\_{ij}^{-} & \mathbf{0} \\ \mathbf{P}\_{ij}^{-} & \mathbf{P}\_{j}^{-} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{R}\_{i}^{c} \end{bmatrix}. \end{aligned}$$

Then, the augmented sigma points are obtained as

$$\mathcal{A}\_k^c = \mathcal{U}T\left(\hat{\mathbf{x}}\_k^{c-}, \mathbf{P}\_{i,k}^{c-}\right), \quad r = 0, \cdots, 2n\_d.$$

The inferred measurement Jacobian is

$$\begin{bmatrix} \mathfrak{A}\_{i,k} & \mathfrak{A}\_{j,k} & \mathfrak{A}\_{v,k} \end{bmatrix} = \mathbf{P}^{\scriptscriptstyle \mathbf{x} \boldsymbol{z} \boldsymbol{c}} (\mathbf{P}^{\scriptscriptstyle \mathbf{c} \boldsymbol{\smile}}\_{i,k})^{-1}$$

where

$$\mathbf{P}\_{i,k}^{\mathrm{xyz}} = \sum\_{r=0}^{2n\_s} \mathcal{W}\_c^r \left( \mathcal{X}\_{i,k}^c - \hat{\mathbf{x}}\_{,k}^{c-} \right) \left( h^c(\mathcal{X}\_{i,k}^c) - \mathbf{z}\_{i\dagger,k}^c \right) \dots$$

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

$$\mathfrak{X}\_{i,k} = \mathfrak{X}\_{i,k}^{-} + \mathbf{K}\_{i,k} \left( \mathbf{z}\_{ij,k} - h\_i^{\varepsilon} (\mathfrak{X}\_{i,k}^{\varepsilon-}) \right), \tag{23}$$

$$\mathfrak{X}\_{j,k} = \mathfrak{X}\_{t,k}^{-} + \mathbf{K}\_{t,k} \left( \mathbf{z}\_{it,k} - h\_i^c(\mathfrak{X}\_{i,k}^{c-}) \right), \tag{24}$$

$$\mathbf{P}\_{i,k} = (\mathbf{I} - \mathbf{K}\_{i,k}\mathbf{\mathcal{H}}\_{i,k})\mathbf{P}\_{i,k}^{-} - \mathbf{K}\_{i,k}\mathbf{\mathcal{H}}\_{j,k}\mathbf{P}\_{ji}^{-},\tag{25}$$

$$\mathbf{P}\_{j,k} = (\mathbf{I} - \mathbf{K}\_{j,k}\mathbf{\mathcal{H}}\_{j,k})\mathbf{P}\_{j,k}^{-} - \mathbf{K}\_{j,k}\mathbf{\mathcal{H}}\_{i,k}\mathbf{P}\_{ij}^{-},\tag{26}$$

$$\mathbf{P}\_{ij,k} = (\mathbf{I} - \mathbf{K}\_{i,k}\mathbf{\mathcal{H}}\_{i,k})\mathbf{P}\_{ij,k}^{-} - \mathbf{K}\_{i,k}\mathbf{\mathcal{H}}\_{j,k}\mathbf{P}\_{j}^{-},\tag{27}$$

where

$$\begin{split} \mathbf{S}\_{i,k} &= \begin{bmatrix} \mathbf{\mathcal{H}}\_{i,k} & \mathbf{\mathcal{H}}\_{j,k} \end{bmatrix} \begin{bmatrix} \mathbf{P}\_{i,k} & \mathbf{P}\_{ij,k} \\ \mathbf{P}\_{i,k}^{\top} & \mathbf{P}\_{j,k} \end{bmatrix} \begin{bmatrix} \mathbf{\mathcal{H}}\_{i,k} & \mathbf{\mathcal{H}}\_{j,k} \end{bmatrix}^{\top} + \mathbf{R}^{c}, \\ \mathbf{\mathbf{K}}\_{i,k} &= (\mathbf{P}\_{i,k}\mathbf{\mathcal{H}}\_{i,k} + \mathbf{P}\_{ij,k}\mathbf{\mathcal{H}}\_{j,k})\mathbf{S}\_{k}^{-1}, \\ \mathbf{\mathbf{K}}\_{j,k} &= (\mathbf{P}\_{j i,k}\mathbf{\mathcal{H}}\_{i,k} + \mathbf{P}\_{j,k}\mathbf{\mathcal{H}}\_{j,k})\mathbf{S}\_{k}^{-1}. \end{split}$$

After the relative measurement update, the correlation **P***ij*,*<sup>k</sup>* is decomposed again as two multiplicative parts *σij*,*<sup>k</sup>* and *σji*,*<sup>k</sup>* according to Equation (22). Then, *σij*,*<sup>k</sup>* and *σji*,*<sup>k</sup>* are stored in *i* and *j*, respectively.

The relative measurement update process also involves the correlation term **P***il*,*k*, *l* ∈ V, *l* = *i*, *j*. Formally, the **P***il*,*<sup>k</sup>* should be updated as

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

The correlation term **P***jl*,*<sup>k</sup>* is not available to robot *i*. To reduce the overall communication and avoid communication with *l*, the split term *σ*− *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} (\mathbf{P}\_{i,k}^{-})^{-1} \sigma\_{il,k}^{-}.\tag{28}
$$

In addition to the measurement update, the target beliefs {**x**ˆ*ti*,*k*, **P***ti*,*k*} and {**x**ˆ*tj*,*k*, **P***tj*,*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:

$$\mathbf{P}\_{t,k} = \left( w\_2(\mathbf{P}\_{t\_j,k})^{-1} + (1 - w\_2)(\mathbf{P}\_{t\_j,k})^{-1} \right)^{-1},\tag{29}$$

$$\mathfrak{X}\_{t,k} = \mathbf{P}\_{t,k} \left( w\_2 \mathbf{P}\_{t\_i,k}^{-1} \mathfrak{X}\_{t\_i,k} + (1 - w\_2) \mathbf{P}\_{t\_j,k}^{-1} \mathfrak{X}\_{t\_j,k} \right). \tag{30}$$

The weight *w*<sup>2</sup> 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 \begin{bmatrix} \mathbf{x}\_{k+1} \\ \mathbf{y}\_{k+1} \\ \boldsymbol{\theta}\_{k+1} \end{bmatrix} = \begin{bmatrix} \mathbf{x}\_{k} + \boldsymbol{\Delta}\_{T}(\boldsymbol{\upsilon}\_{\mathcal{E}} + \boldsymbol{w}\_{k}^{\boldsymbol{\upsilon}})\cos(\boldsymbol{\theta}\_{k}) \\ \mathbf{y}\_{k} + \boldsymbol{\Delta}\_{T}(\boldsymbol{\upsilon}\_{\mathcal{E}} + \boldsymbol{w}\_{k}^{\boldsymbol{\upsilon}})\sin(\boldsymbol{\theta}\_{k}) \\ \boldsymbol{\theta}\_{k} + \boldsymbol{\Delta}\_{T}(\boldsymbol{\omega}\_{\mathcal{E}} + \boldsymbol{w}\_{k}^{\boldsymbol{\omega}}) \end{bmatrix}^{\prime}$$

A subscriber *i* or *t* is used to distinguish the robots or the target. The state vector **x***<sup>k</sup>* to be estimated contains three entries—*xk*, *yk*, and *θ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 [−10, 10] , [10, 10] , [−10, −10] , [10, −10]. The same control command **u** = [*vc*, *ωc*] = [0.3, 0.0375] 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 **<sup>Q</sup>***<sup>i</sup>* <sup>=</sup> diag([0.12,(0.5*π*/180)2]). The target is initialized at [−15, <sup>−</sup>15] in the global frame,

and the control input is set as **u***<sup>t</sup>* = [0.05, 0] . Similarly, the target control is subject to zero-mean Gaussian noise with **Q***<sup>t</sup>* = diag([0.022,(2*π*/180)2]).

**Figure 1.** Simulation with four local robots and one moving target.

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 **v***<sup>a</sup> <sup>i</sup>*,*<sup>k</sup>* ∼ N (**0**, diag (0.52, 0.52, 0.5*π*/180)<sup>2</sup> ) is the control noise.

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

$$\|z\_{ij,k}^{\epsilon} = \|\mathbf{x}\_j - \mathbf{x}\_i\|\_d + \upsilon\_{i,k}^{\epsilon} \tag{31}$$

$$z\_{it,k}^t = ||\mathbf{x}\_t - \mathbf{x}\_i||\_d + v\_{i,k^\*}^t \tag{32}$$

·*<sup>d</sup>* 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 *rt* = 20, and the sensing range for cooperative robots, as well as the communication range, is set as *rc* = 10. The measurement noises are *v<sup>c</sup> <sup>i</sup>* ∼ N (0, 0.052) and *vt <sup>i</sup>* ∼ N (0, 0.052), 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 *rc* and *rt*, respectively, and is shown in Figures 2 and 3.

**Figure 2.** Measurement and communication link availability between two robots with *rc* = 10.

**Figure 3.** Measurement availability of target to four robots with *rt* = 20.

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 (±3*σ* 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 ±3*σ* 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 ±3*σ* envelopes (dashed line in the same colors) in the steady state. On the basis of Figures 5 and 6, the min/max self-localization and target tracking errors for time instance *k* ∈ [200, 800] are listed in Table 1.

**Figure 4.** Estimated trajectories of robots and the target in different colors (black dashed lines indicate the ground truth).

**Figure 5.** Self-localization errors and covariances of robots 1–4.



**Figure 6.** Target tracking errors and covariance of robots 1–4.

#### *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 ≤ *k* ≤ 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.

**Figure 7.** Self-localization estimation position root-mean-square errors (PRMSEs) of the unscented transformation-based collaborative self-localization and target tracking scheme (UT-CLAT) vs. the extended Kalman filter-based CLAT (EKF-CLAT) for robots 1–4.

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.

**Figure 8.** Target distributed estimation PRMSEs of the UT-CLAT vs. the EKF-CLAT for robots 1–4.
