Algorithm 4. MCL Algorithm

Input:${\chi }_{t-1},u_t,z_t,m$

Output:${\chi }_t$

1: Initialization, ${\stackrel{‒}{\chi }}_t={\chi }_t=Ø$

2: For $j=1$ to M do

3: $x_t^{\left[j\right]}=$ sample_motion_model $\left(u_t,x_{t-1}^{\left[j\right]}\right)$

4: $w_t^{\left[j\right]}=$ measurement_model $\left(z_t,x_t^{\left[j\right]},m\right)$

5: ${\stackrel{‒}{\chi }}_t={\stackrel{‒}{\chi }}_t+<x_t^{\left[m\right]},w_t^{\left[m\right]}>$

6: End for

7: For $j=1$ to M do

8: Draw i with probability $\propto w_t^{\left[i\right]}$

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

10: End for

11: Return ${\chi }_t$