Table 3 Improved AMCL algorithm.

2:

\(\overline{X}_{t} = X_{t} = X_{tH} = X_{tL} = \phi\)

3:

for \(m=1 to {M}_{t-1} do\)

4:

for \(n=1 to B do\)

5:

\({x}_{t}^{[m, n]}=sample\_motion\_model({u}_{t}, {x}_{t-1}^{\left[m\right]})\)

6:

\(p({z}_{t}|{x}_{t}^{\left[m,n\right]})= measurement\_model({z}_{t}, {x}_{t}^{\left[m,n\right]},m)\)

7:

endfor

8:

\({x}_{t}^{[m]}={x}_{t}^{[m,j]} ;y\in \{1,\dots ,B\}\bigwedge argmax(p({z}_{t}|{x}_{t}^{\left[m,y\right]})) = \{ j \}\)

9:

\({w}_{t}^{[m]}=p({z}_{t}|{x}_{t}^{\left[m\right]})={\sum }_{n=1}^{n=B}p\left({z}_{t}|{x}_{t}^{\left[m,n\right]}\right)/B\)

10:

endfor

11:

for \(m=1 to {M}_{t-1} do\)

12:

if \({w}_{t}^{[m]}\ge {W}_{const}\) then

13:

\({X}_{tH}={X}_{tH}+\langle {x}_{t}^{[m]},{w}_{t}^{[m]}\rangle\)

14:

\(\overline{{X }_{t}}=\overline{{X }_{t}}+\langle {x}_{t}^{[m]},{w}_{t}^{[m]}\rangle\)

15:

else

16:

\({X}_{tL}={X}_{tL}+\langle {x}_{t}^{[m]}, {w}_{t}^{[m]}\rangle\)

17:

endfor

18:

for \(m=1 to {M}_{t}^{l}/3 do\)

19:

draw \({x}_{tH}^{[m]}\) from \({X}_{tH}\) with equal probility

20:

\({x}_{tS}^{[m]}=crossover\_model({x}_{tL}^{\left[m\right]},{x}_{tH}^{\left[m\right]})\)

21:

\({x}_{tM}^{[m]}=mutation\_model({x}_{tS}^{\left[m\right]},{x}_{tH}^{\left[m\right]})\)

22:

Recompute \({w}_{tM}^{[m]}\)

23:

\(\overline{{X }_{t}}=\overline{{X }_{t}}+\langle {x}_{tM}^{[m]},{w}_{tM}^{[m]}\rangle\)

24:

endfor

25:

compute \({w}_{slow}, {w}_{fast}\)

26:

\(SER=SER\_calculator(\widetilde{E},{z}_{t})\)

27:

for \(m=1 to {M}_{t} do\)

28:

With probability \(max(0.0, 1.0-{w}_{fast}/{w}_{slow})\) do

29:

draw \({x}_{t}^{[m]}\) from \(SER\) with equal probability

30:

else

31:

draw \({x}_{t}^{[m]}\) from \(\overline{{X }_{t}}\) with probability \(\propto {w}_{t}^{[m]}\)

32:

endwith

33:

add \({x}_{t}^{[m]}\) to \({X}_{t}\)

34:

if \({x}_{t}^{[m]}\) falls into an empty bin \(b\) then

35:

\(k:=k+1\)

36:

\(b:=non-empty\)

37:

\({M}_{t}=\frac{k-1}{2\epsilon }{\{1-\frac{2}{9\left(k-1\right)}+\sqrt{\frac{2}{9\left(k-1\right)}}{z}_{1-\delta }\}}^{3}\)

38:

endfor

39:

return \({X}_{t}\)