Algorithm 1 AGM-AMCL

Input:$S_{t-1}=\left\{{\langle x_{t-1}^{[i]},e_{t-1}^{[i]},{\omega }_{t-1}^{[i]}\rangle }_{i=1}^{N_{t-1}}\right\},u_t,z_t,m_g,m_a$

Output:$S_t=\left\{{\langle x_t^{[i]},e_t^{[i]},{\omega }_t^{[i]}\rangle }_{i=1}^{N_t}\right\}$

1:  $S_t\leftarrow \varnothing ,N_x\leftarrow 0,n\leftarrow 0,\beta \leftarrow 0$   // Initialization

2:  $C_t=\left\{{\langle x_{hit}^{[i]},d_{hit}^{[i]},{\mathrm{tag}}^{[i]}\rangle }_{i=1}^{N_{t-1}}\right\}$   //For storing RAYCAST result

3:  for $i\leftarrow 1,\dots ,N_{t-1}$ do   //Initialize $C_t$

4:   ${\mathrm{tag}}^{[i]}\leftarrow 0$

5:  end for

6:  do

7:   $j\leftarrow \mathrm{RESAMPLING} \left(S_{t-1}\right)$   //Resampling according to particle weight

8:   $d_{\mathrm{range}}=e_{t-1}^{[j]}$

9:   if ${\mathrm{tag}}^{[j]}==0$ then

10:    $\left({\mathsf{\mu }}_{hit},d_{hit}\right)\leftarrow \mathrm{RAYCAST}\left(x_{t-1}^{[j]},d_{\mathrm{range}},m_a\right)$

11:    $C_t[j]\leftarrow \left(x_{hit},d_{hit},1\right)$

12:   else

13:    $\left({\mathsf{\mu }}_{hit},d_{hit}\right)\leftarrow C_t[j]$

14:   end if

15:   $\alpha \leftarrow \mathrm{COMPUTEALPHA} \left(d_{hit},d_{\mathrm{range}}\right)$

16:   sample $c$ according to Bernoulli_Distribution( $\alpha$)

17:   if $c==1$

18:    ${\mathsf{\Sigma }}_{hit}\leftarrow \mathrm{COMPUTESIGMAHITT} \left(d_{\mathrm{range}},d_{hit},x_{t-1}^{[j]}\right)$

19:    Create $P_{\mathrm{portal}}\left(x_t|x_{t-1}^{[j]},e_{t-1}^{[i]},m_a\right)$ according to $N\left({\mathsf{\mu }}_{hit},{\mathsf{\Sigma }}_{hit}\right)$

20:    Sample    $x_t^{[n]}$ according to $P_{\mathrm{portal}}\left(x_t|x_{t-1}^{[j]},e_{t-1}^{[i]},m_a\right)$

21:   else

22:    Sample $x_t^{[n]}$ according to $P_{odo}\left(x_t|x_{t-1}^{[j]},u_t\right)$

23:   end if

24:   $e_t^{[n]}\leftarrow \mathrm{PREDICTAMBIGUITYERROR} \left(x_t^{[n]},e_{t-1}^{[j]},m_a\right)$

25:   ${\omega }_t^{[n]}\leftarrow P\left(z_t|x_t^{[n]},m_a\right)$ //Compute weight according to observation

26:   $\beta \leftarrow \beta +{\omega }_t$

27:   $S_t\leftarrow S_t\cup \left\{\langle x_t^{[n]},e_t^{[n]},{\omega }_t\rangle \right\}$

28:   $N_x\leftarrow \mathrm{ADPTINGSAMPLESIZE}\left(x_t^{[n]}\right)$

29:   $n\leftarrow n+1$

30:  while $n<N_x$ and $n<N_{\min }$

31:  for $i\leftarrow 1,\dots ,n$ do   //Normalize particle weight

32:   ${\omega }_t^{[i]}\leftarrow {\omega }_t^{[i]}/\beta$

33:  end for

34:  return $S_t$