Algorithm 1 Improved probabilistic algorithm

Input:  previous state and sensor observations (Initialization) Output:  threshold of particle number [10]; 1:$x_t^{\left[\mathrm{i}\right]}$ = sample motion model($u_t,x_t^{\left[\mathrm{i}\right]}$); 2:${\omega }_t^{\left[\mathrm{i}\right]}$ = likelihood field($z_t,x_t^{\left[\mathrm{i}\right]},m$), (Update); 3:${\chi }_{temp}$ = ${\chi }_{temp}$ +<$x_t^{\left[\mathrm{i}\right]},{\omega }_t^{\left[\mathrm{i}\right]}$>, (Resampling); 4:if d>$\sigma$  then 5:    generate particle set $C_t$ center on $x_c$, (Incorporate visual observation) 6:    ${\chi }_c$= <$C_t$,${\omega }_t^{\left[\mathrm{i}\right]}$> 7:else${\chi }_c$= random particles 8:end if 9:add ${\chi }_c$ to ${\chi }_t$ with probability ($0,1-\frac{{\omega }_{fast}}{{\omega }_{slow}}$)