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 **z***<sup>t</sup> it*, 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 **P***iti* , 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.

$$
\begin{bmatrix}
\frac{1}{w\_1} \mathbf{P}^-\_{i,k} & \mathbf{0} \\
\mathbf{0} & \frac{1}{1-w\_1} \mathbf{P}^-\_{t\_i,k}
\end{bmatrix} \succeq \begin{bmatrix}
\mathbf{P}^-\_{i,k} & \mathbf{P}^-\_{it\_i,k} \\
\mathbf{P}^-\_{t\_i,k} & \mathbf{P}^-\_{t\_i,k}
\end{bmatrix}.
\tag{16}
$$

The weight *w* is determined according to [34]. Let **P**¯ <sup>−</sup> *<sup>i</sup>*,*<sup>k</sup>* - 1 *<sup>w</sup>* **P**<sup>−</sup> *<sup>i</sup>*,*<sup>k</sup>* and **<sup>P</sup>**¯ <sup>−</sup> *ti*,*<sup>k</sup>* - 1 <sup>1</sup>−*<sup>w</sup>* **<sup>P</sup>**<sup>−</sup> *ti*,*k*. The augmented state can be defined as

$$\mathbf{x}\_{i,k}^{b-} = \begin{bmatrix} \mathbf{x}\_{i,k}^{-} \\ \mathbf{x}\_{t\_i,k}^{-} \\ \mathbf{0} \end{bmatrix} \text{ and } \mathbf{P}\_{i,k}^{b} = \begin{bmatrix} \mathbf{P}\_{i,k}^{-} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{P}\_{t,k}^{-} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{R}\_{i}^{t} \end{bmatrix}.$$

Then, the augmented sigma points are obtained as

$$\mathcal{X}\_{i,k}^{b} = \mathcal{U}T\left(\mathfrak{X}\_{i,k}^{b-}, \mathfrak{P}\_{i,k}^{b-}\right), \quad r = 0, \cdots, 2n\_{\mathfrak{a}}.\tag{17}$$

The inferred measurement Jacobian is

$$
\begin{bmatrix}
\mathfrak{A}\_{i,k} & \mathfrak{A}\_{t\_i,k} & \mathfrak{A}\_{v,k}
\end{bmatrix} = \mathbf{P}\_{i,k}^{\mathbf{x}\mathbf{z}b} (\mathbf{P}\_{i,k}^{b-})^{-1},\tag{18}
$$

where

$$\mathbf{P}\_{i,k}^{\chi zb} = \sum\_{r=0}^{2n\_d} \mathsf{W}\_c^r \left( \mathcal{X}\_{i,k}^b - \mathbf{x}\_{i,k}^{b-} \right) \left( h^t(\mathcal{X}\_{i,k}^b) - \mathbf{z}\_{i t, k}^t \right) \dots$$

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

$$\mathfrak{X}\_{i,k} = \mathfrak{X}\_{i,k}^{-} + \mathbb{K}\_{i,k} \left( \mathbf{z}\_{it,k}^{t} - h\_i^t(\mathbf{x}\_{i,k}^{b-}) \right), \tag{19}$$

$$\hat{\mathbf{x}}\_{t\_{i}k} = \hat{\mathbf{x}}\_{t\_{i}k}^{-} + \mathbf{K}\_{t\_{i}k} \left( \mathbf{z}\_{it,k}^{t} - h\_{i}^{t}(\mathbf{x}\_{i,k}^{b-}) \right), \tag{20}$$

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

$$\mathbf{P}\_{t\_i,k} = (\mathbf{I} - \mathbf{K}\_{t\_i,k}\mathbf{\mathcal{H}}\_{i,k})\mathbf{\hat{P}}\_{t\_i,k'}^{-} \tag{22}$$

where the innovation covariance and gain are calculated as

$$\begin{split} \mathbf{S}\_{i,k} &= \begin{bmatrix} \mathbf{\mathcal{H}}\_{i,k} & \mathbf{\mathcal{H}}\_{i,k} \end{bmatrix} \begin{bmatrix} \mathbf{P}^{-}\_{i,k} & \mathbf{0} \\ \mathbf{0} & \mathbf{P}^{-}\_{l,k} \end{bmatrix} \begin{bmatrix} \mathbf{\mathcal{H}}\_{i,k} & \mathbf{\mathcal{H}}\_{i,k} \end{bmatrix}^{\top} + \mathbf{R}^{\mu}\_{i}, \\\ \mathbf{\mathbf{K}}\_{i,k} &= \mathbf{P}^{-}\_{i,k}\mathbf{\mathcal{H}}\_{i,k}\mathbf{S}^{-1}\_{i,k}, \\\ \mathbf{K}\_{l\_{i},k} &= \mathbf{P}^{-}\_{l,k}\mathbf{\mathcal{H}}\_{l\_{i},k}\mathbf{S}^{-1}\_{i,k}. \end{split}$$

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

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

On the basis of the decomposition in Equation (6), the correlation term *σij* can be calculated as below without communication:

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