**6. Experimental Results**

This section presents the results of the experiments. The extended work discuss information sharing in heterogeneous maps made with different sensors and tests the proposed method under pressure with dynamic obstacles in the vicinity of robots.

We used Pioneer-P3DX [44] and Kobuki Turtlebot [45] robot shown in Figure 9a. Both the robots are wheeled differential drive robots and the motion model is explained in our previous work [12]. Both the robots used ROS [46] on Ubuntu computer and were on the same network to communicate with each other.

**<sup>1</sup>** # *xt*: robot state, *vt*, *ωt*: translation and rotational velocity.

$$\begin{aligned} &\mathcal{I}\_{1} = \begin{bmatrix} x\_{1} & y\_{1} \end{bmatrix} \begin{bmatrix} 1 \\ 2 \end{bmatrix} \notin \begin{cases} \text{RHS} \text{ uses Jacobian to handle non-linearity.} \ G\_{1}: \text{ Jacobian of motion function w.r.t state} \\ \text{\$G\_{1}\$ \leftarrow \$} \begin{bmatrix} 1 & 0 & -\frac{\overline{\alpha\_{1}}}{\alpha\_{1}\beta\_{1}} \cos\theta + \frac{\overline{\alpha\_{2}}}{\alpha\_{2}} \cos(\theta + \omega\_{1}\Delta t) \\ 0 & 1 \end{bmatrix} \\ &\text{\$\ast\$ \$} V: \begin{bmatrix} \text{L} \text{-} \, \text{alcion} + \frac{\overline{\alpha\_{1}}}{\alpha\_{1}} \sin(\theta + \omega\_{1}\Delta t) \\ 0 & 1 \end{bmatrix} \\ &\text{\$\ast\$ \$} V\_{1}: \begin{bmatrix} \frac{-\overline{\alpha\_{1}}\beta + \sin(\theta + \omega\_{1}\Delta t)}{\omega\_{1}} & \frac{\overline{\alpha\_{1}}\beta - \sin(\theta + \omega\_{1}\Delta t)}{\omega\_{1}} + \frac{\overline{\alpha\_{1}}(\alpha\_{2}\beta + \omega\_{1}\Delta t)\Delta t}{\omega\_{1}} \\ \frac{\alpha\_{2}}{\alpha\_{1}} & \frac{\omega\_{2}}{\alpha\_{2}} \end{bmatrix} \\ &\text{\$\ast\$ \$ M\_{1}\$: \$\text{Creatinine}\$ of noise in \$\text{no total}\$ space. \$\alpha\_{1}, \cdots, \alpha\_{4}\$: \$\text{Error-specific parameters.} \\ &\text{\$\ast\$ \$ M\_{1}\$: \$\text{Cartesian}\$ of noise in \$\text{no total}\$ space. \$\alpha\_{1}, \cdots, \alpha\_{4}\$: \$\text{Error-specific parameters.} \\ &\text{\$\ast\$ \$ M\_{1}\$: \$\text{Cartesian}\$ in \$\text{topassian}\$.} \\ &\text{$$

*T*

$$\mu\_t = \mu\_{t-1} + \begin{bmatrix} \frac{-\upsilon\_t}{\omega\_l} \sin \theta + \frac{\upsilon\_l}{\omega\_l} \sin(\theta + \omega\_l \Delta t) \\ \frac{\upsilon\_t}{\omega\_l} \cos \theta - \frac{\upsilon\_t}{\omega\_l} \cos(\theta + \omega\_l \Delta t) \\ \omega\_l \Delta t \end{bmatrix}$$


$$\mathbf{Q}\_{t} = \begin{bmatrix} \sigma\_{r}^{2} & 0 & 0\\ 0 & \sigma\_{\Phi}^{2} & 0\\ 0 & 0 & \sigma\_{s}^{2} \end{bmatrix}$$

**<sup>8</sup>** # [*mix miy*] *<sup>T</sup>*: coordinates of the *i*th landmark. *z<sup>i</sup> <sup>t</sup>*: measurement. *q*: squared distance.

$$\begin{aligned} \eta &= (m\_{k, \mathbf{x}} - \bar{\mu}\_{l, \mathbf{x}})^2 + (m\_{k, \mathbf{y}} - \bar{\mu}\_{l, \mathbf{y}})^2 \\ \mathcal{L}\_t^{\;k} &= \begin{bmatrix} \sqrt{\eta} \\ \operatorname{atan2}(m\_{k, \mathbf{y}} - \bar{\mu}\_{l, \mathbf{y}}, m\_{k, \mathbf{x}} - \bar{\mu}\_{l, \mathbf{x}}) - \bar{\mu}\_{l, \theta} \\ m\_{k, \mathbf{s}} & \end{bmatrix} \end{aligned}$$

**<sup>9</sup>** # *Ht*: Jacobian of measurement with respect to state.

$$H\_t^k = \begin{bmatrix} -\frac{m\_{k,x} - \underline{\rho}\_{t,x}}{\sqrt{\eta}} & -\frac{m\_{k,y} - \underline{\rho}\_{t,y}}{\sqrt{\eta}} & 0\\ \frac{m\_{k,y} - \underline{\rho}\_{t,y}}{\eta} & -\frac{m\_{k,x} - \underline{\rho}\_{t,x}}{\eta} & -1\\ 0 & 0 & 0 \end{bmatrix}$$

**<sup>10</sup>** # *St*: Measurement covariance matrix.

*Sk <sup>t</sup>* = *<sup>H</sup><sup>k</sup> t* Σ¯ *<sup>t</sup>*[*H<sup>k</sup> t* ] *<sup>T</sup>* + *Qt*.

**<sup>11</sup>** # *j*(*i*): likely correspondence after applying maximum likelihood estimate.

 $j(i) = \texttt{arg}\,\texttt{παx}$   $\frac{1}{\sqrt{\texttt{dst}(2\,\texttt{τ}S\_t^k)}}$   $e^{-\frac{1}{2}(z\_t^i - z\_t^k)^T \{S\_t^k\}^{-1} (z\_t^i - z\_t^k)}$   $\&\text{  $K$ :  $\texttt{\&laman cain.  $\mu\_t$ :  $\texttt{\&ta.  $\Sigma\_t$ :  $\texttt{\&v\&arīanos.}$ .}$ $ $ 

**<sup>12</sup>** # *Kt*: Kalman gain, *μt*: state, Σ*t*: covariance.

$$\begin{aligned} K\_t^\dagger &= \Sigma\_t [H\_t^{\dagger \langle i \rangle}]^T [S\_t^{\dagger \langle i \rangle}]^{-1}, \\ \mu\_t &= \mu\_t + K\_t^\dagger (z\_t^\dagger - 2\_t^{\dagger \langle i \rangle}), \\ \Sigma\_t &= (I - K\_t^\dagger H\_t^{\dagger \langle i \rangle}) \Sigma\_t, \\ \cdots, \cdots, \cdots & \cdots \end{aligned}$$

**<sup>13</sup>** # Apply Singular Value Decomposition and get Eigen-values *λi*:

$$
\lambda\_{1\prime} \cdots \lambda\_{\ell\prime} = \text{svd}(\Sigma\_{\ell}) = \text{svd}\left( \begin{bmatrix} \sigma\_x^2 & \sigma\_{\lambda\prime} & \sigma\_{x\theta} \\ \sigma\_{\lambda\theta} & \sigma\_y^2 & \sigma\_{y\theta} \\ \sigma\_{\lambda\theta} & \sigma\_{y\theta} & \sigma\_\theta^2 \\ \dots & \dots & \dots \end{bmatrix} \right),
$$

**<sup>14</sup>** # *n*: degree of decay curve, *tth*: threshold time, *cth*: threshold confidence, *tz*: time to decay to zero. *<sup>n</sup>* <sup>=</sup> log(1 <sup>−</sup> *cth*)

$$\epsilon = \frac{\log\left(\frac{l\_{\rm th}}{l\_{\rm z}}\right)}{\log\left(\frac{l\_{\rm th}}{l\_{\rm z}}\right)} \cdot \epsilon$$

**<sup>15</sup>** # *n* : degree of decay curve with uncertainty integrated, Ψ: decay control factor.

$$n' = \frac{\log\left(1 - t\_z^{-n} \left(\lambda\_1^2 + \lambda\_2^2\right)^{-\frac{n}{2}} \left[t\_z^n \left(\lambda\_1^2 + \lambda\_2^2\right)^{\frac{n}{2}} - \left\{t\_{th} \left(\lambda\_1^2 + \lambda\_2^2\right)^{\frac{1}{2}} - \Psi\right\}^n\right]\right)}{\log\left(t\_{th} \left(\lambda\_1^2 + \lambda\_2^2\right)^{\frac{1}{2}} - \Psi\right) - \log\left(t\_z \left(\lambda\_1^2 + \lambda\_2^2\right)^{\frac{1}{2}}\right)}$$

(**b**) (**c**)

**Figure 9.** Experiment setup. (**a**) Differential drive robots Kobuki-Turtlebot2 and Pioneer-P3Dx. (**b**) Environment with initial position of robots. (**c**) Another view of the environment. (**d**) Environment dimensions. (**e**) Node-map of the environment where S and G are the start and goal points.
