*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 *<sup>k</sup>* <sup>−</sup> 1 be denoted respectively as **<sup>x</sup>**<sup>ˆ</sup> *<sup>a</sup> <sup>i</sup>*,*k*−<sup>1</sup> <sup>∈</sup> <sup>R</sup>*na* and **<sup>P</sup>***<sup>a</sup> <sup>i</sup>*,*k*−<sup>1</sup> <sup>∈</sup> <sup>R</sup>*na*×*na* , where *na* <sup>=</sup> *nv* <sup>+</sup> *nu*,

$$\mathbf{x}\_{i,k-1}^{a} \triangleq \begin{bmatrix} \mathfrak{x}\_{i,k-1} \\ \mathfrak{u}\_{i,k-1} \end{bmatrix}, \text{ and } \mathbf{P}\_{i,k-1}^{a} \triangleq \begin{bmatrix} \mathbf{P}\_{i,k-1} & \mathbf{0} \\ \mathbf{0} & \mathbf{Q}\_{i} \end{bmatrix}. \tag{7}$$

A set of 2*na* <sup>+</sup> 1 sigma points, denoted as <sup>X</sup> *<sup>a</sup>*, is selected as follows:

$$\begin{split} \mathcal{X}\_{i,k-1}^{a,0} &= \mathfrak{X}\_{i,k-1}^{a} \\ \mathcal{X}\_{i,k-1}^{a,r} &= \mathfrak{X}\_{i,k-1}^{a} + \left\{ \sqrt{(n\_a + \gamma) \mathfrak{P}\_{i,k-1}^{a}} \right\}\_{(:,r)'} \text{ if } r \in \{1, \cdots, n\_a\}, \\ \mathcal{X}\_{i,k-1}^{a,r} &= \mathfrak{X}\_{i,k-1}^{a} - \left\{ \sqrt{(n\_a + \gamma) \mathfrak{P}\_{i,k-1}^{a}} \right\}\_{(:,r-n\_a)'} \text{ otherwise.} \end{split}$$

Here, *<sup>γ</sup>* <sup>=</sup> *<sup>α</sup>*<sup>2</sup> (*na* <sup>+</sup> *<sup>κ</sup>*) <sup>−</sup> *na* is a scaling parameter, with 0 <sup>&</sup>lt; *<sup>α</sup>* <sup>≤</sup> 1 and *<sup>κ</sup>* <sup>∈</sup> <sup>R</sup> as tuning parameters to control the spread of the sigma points. The weights for propagating the mean and covariances, denoted respectively as *W<sup>r</sup> <sup>m</sup>* and *W<sup>r</sup> <sup>c</sup>* , are computed as

$$\begin{aligned} \mathcal{W}\_{\text{m}}^{0} &= \gamma / \left( n\_{\text{a}} + \gamma \right), \\ \mathcal{W}\_{\text{c}}^{0} &= \gamma / \left( n\_{\text{a}} + \gamma \right) + \left( 1 - \kappa^{2} + \beta \right), \\ \mathcal{W}\_{\text{m}}^{r} &= \mathcal{W}\_{\text{c}}^{r} = 1 / 2 \left( n\_{\text{a}} + \gamma \right), \quad r = 1, \dots, 2n\_{\text{a}}, \end{aligned}$$

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

$$\sum\_{r=0}^{2n\_a} (\mathcal{W}\_m^r) \mathcal{X}\_{i,k-1}^{a,r} = \mathcal{X}\_{i,k-1}^{a,0} = \mathfrak{X}\_{i,k-1}^a$$

or equivalently,

$$\sum\_{r=0}^{2u\_x} \mathcal{W}\_m^r \mathcal{X}\_{i,k-1}^r = \hat{\mathbf{x}}\_{i,k-1}, \quad \sum\_{r=0}^{2u\_x} \mathcal{W}\_m^r \mathcal{U}\_{k-1}^r = \ddot{\mathbf{u}}\_{i,k-1}.$$

where <sup>X</sup> *<sup>r</sup> <sup>i</sup>*,*k*−<sup>1</sup> and <sup>U</sup>*<sup>r</sup> <sup>i</sup>*,*k*−<sup>1</sup> collect the components of <sup>X</sup> *<sup>a</sup>*,*<sup>r</sup> <sup>i</sup>*,*k*−<sup>1</sup> corresponding to, respectively, **<sup>x</sup>***i*,*k*−<sup>1</sup> and **<sup>u</sup>***i*,*k*−1. The above unscented transform is summarized below:

$$\mathcal{X}\_{i,k-1}^{a,r} = \mathcal{U}T(\mathfrak{X}\_{i,k-1}^{a}, \mathbf{P}\_{i,k-1}^{a})\_{\prime} \qquad 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} = \mathcal{U}T(\mathfrak{X}\_{t\_i,k-1}^a, \mathbf{P}\_{t\_i,k-1}^a)\_\prime \qquad r = 0, \cdots, 2n\_a.$$

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

$$\mathfrak{X}\_{i,k} = \sum\_{r=0}^{2n\_a} \mathcal{W}\_m^r \mathcal{X}\_{i,k'}^r \tag{8}$$

$$\mathbf{P}\_{i,k} = \sum\_{r=0}^{2\mathfrak{u}\_{\sigma}} \mathcal{W}\_{\varepsilon}^{r} \left( \mathcal{X}\_{i,k}^{r} - \mathfrak{x}\_{i,k} \right) \left( \mathcal{X}\_{i,k}^{r} - \mathfrak{x}\_{i,k} \right)^{\top},\tag{9}$$

and

$$\mathfrak{X}\_{t,k} = \sum\_{r=0}^{2n\_x} \mathcal{W}\_m^r \mathcal{X}\_{t\_i,k}^r \tag{10}$$

$$\mathbf{P}\_{t\_{i},k} = \sum\_{r=0}^{2n\_{x}} \mathcal{W}\_{c}^{r} \left( \mathcal{X}\_{t\_{i},k}^{r} - \hat{\mathbf{x}}\_{t\_{i},k} \right) \left( \mathcal{X}\_{t\_{i},k}^{r} - \hat{\mathbf{x}}\_{t\_{i},k} \right)^{\top},\tag{11}$$

where

$$\mathcal{X}\_{i,k}^r = f\_{\mathbb{P}}\left(\mathcal{X}\_{i,k-1}^r, \mathcal{U}\_{i,k-1}^r\right), \quad r = 0, \cdots, 2n\_{a,r}$$

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), \quad r = 0, \cdots, 2n\_a.$$

X *r ti*,*k*−<sup>1</sup> and <sup>W</sup>*<sup>r</sup> ti*,*k*−<sup>1</sup> are the sigma points corresponding to **<sup>x</sup>***ti*,*k*−<sup>1</sup> and **<sup>w</sup>***t*,*k*−1, and

$$\sum\_{r=0}^{2u\_s} \mathcal{W}\_m^r \mathcal{X}\_{t\_i,k-1}^r = \mathfrak{x}\_{t\_i,k-1}, \sum\_{r=0}^{2u\_s} \mathcal{W}\_m^r \mathcal{W}\_{k-1}^r = \mathbf{0}.$$

The propagation of the correlation term **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 *σij* is instead propagated as

$$
\sigma\_{\vec{i}\vec{j},k} = \mathcal{F}\_{\vec{v}} \sigma\_{\vec{i}\vec{j},k-1}.\tag{12}
$$

where F *<sup>v</sup>* is the inferred Jacobian matrix with regard to the dynamic function in Equation (1) and, according to [33], is defined as

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

where **P***xx <sup>i</sup>*,*k*|*k*−<sup>1</sup> <sup>≈</sup> <sup>∑</sup>2*na <sup>r</sup>*=0(<sup>X</sup> *<sup>r</sup> <sup>i</sup>*,*<sup>k</sup>* <sup>−</sup> **<sup>x</sup>**ˆ*i*,*k*)(<sup>X</sup> *<sup>r</sup> <sup>i</sup>*,*k*−<sup>1</sup> <sup>−</sup> **<sup>x</sup>**ˆ*i*,*k*−1).

#### *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, **x**ˆ−, **P**− and **x**ˆ, **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 H*i*,*<sup>k</sup>* corresponding to the measurement function in Equation (3) is obtained as

$$\mathsf{M}\_{i,k} = \mathbf{P}\_{i,k}^{\times zr} \left(\mathbf{P}\_{i,k}^{-}\right)^{-1}{}\_{i}$$

where

$$\mathbf{P}\_{i,k}^{xxzr} = \sum\_{r=0}^{2n\_a} \mathcal{W}\_c^r \left( \mathcal{X}\_{i,k}^r - \hat{\mathbf{x}}\_{i,k}^- \right) \left( h^a(\mathcal{X}\_{i,k}^r) - \mathbf{z}\_{i,k}^a \right) \dots$$

Then, the state and covariance can be updated as

$$\mathfrak{K}\_{i,k} = \mathfrak{k}\_{i,k}^{-} + \mathbf{K}\_{i,k} \left( \mathbf{z}\_{i,k}^{a} - h\_i^a(\mathfrak{k}\_{i,k}^{-}) \right), \tag{13}$$

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

where

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

The correlation term within local robot *i* is updated as

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