Algorithm 1 The improved adaptive Monte Carlo localization (AMCL) algorithm.

Input: control information $u_t$, observation information $z_t$, matched template libraries $\mathbf{M}$;

Output: new particles set ${\chi }_t$;

1: Initialization: $\mathbf{R}=\overline{\mathbf{M}}={\chi }_t=\varnothing$, ${\omega }_{t-new}=0$, new particle $x_{i-new}=\varnothing$, cache particle set $\overline{{\chi }_t}=\varnothing$;

2: $\mathbf{R}$,$\overline{\mathbf{M}}$ = LaserReflectorIdentification$(z_t,u_t,\mathbf{M})$;

3: if  $\overline{\mathbf{M}}=\varnothing$   then

4: for i = 0 $to$   N do

5:    $Sample$   $x_t^{\left[i\right]}$∼$p\left(x_t|x_{t-1}^{\left[i\right]},u_t\right)$;//Original AMCL

6:    ${\omega }_t^{\left[i\right]}$∼$measurement\_model(z_t,x_t^{\left[i\right]})$;//Original AMCL

7:    $\overline{{\chi }_t}=\overline{{\chi }_t}+〈x_t^{\left[i\right]},{\omega }_t^{\left[i\right]}〉$;

8:    $draw$   i   $withprobability$   ∝   ${\omega }_{t-1}^{\left[i\right]}$;

9:    $add$   $x_t^{\left[i\right]}$$to$${\chi }_t$;

10:  end for

11: else

12:  $x_t$ = motion_model($x_{t-1},u_t$);

13: for   i = 0 $to$   N do

14:   $d^{\left[i\right]}=PositionWeightCalculation(\overline{M},x_t^{\left[i\right]})$;

15:    $s^{\left[i\right]}=OrientationWeightCalculation\left(z_i\right)$;

16:   ${\omega }_{t-new}^{\left[i\right]}=\alpha ·s^{\left[i\right]}·{\omega }_t^{\left[i\right]}+\beta ·d^{\left[i\right]}$;

17:   $\overline{{\chi }_t}=\overline{{\chi }_t}+〈x_t^{\left[i\right]},{\omega }_{t-new}^{\left[i\right]}〉$;

18:   end for

19:   ${\omega }_{t-new}=normolaizeweight\left({\omega }_{t-new}\right)$;

20:   for   i = 0 $to$   N do

21:   $p\left(x_{t-new}|x_t,u_{t-1},z_t,\overline{\mathbf{M}}\right)=probofnewparticle(d^{\left[i\right]})$;//function (8)

22:   if   $rand\left(\right)\le p\left(x_{t-new}|x_t,u_{t-1},z_t,\overline{\mathbf{M}}\right)$   then

23:    $x_{i-new}=ParticleGeneration\left({\omega }_{t-new}\right)$;

24:      $add$   $x_{i-new}$   $to$   ${\chi }_t$;

25:   end if

26:   end for

27: end if

28:return${\chi }_t$