A Robust Indoor Localization System Integrating Visual Localization Aided by CNN-Based Image Retrieval with Monte Carlo Localization

. 2019 Jan 10;19(2):249. doi: 10.3390/s19020249

Algorithm 1: Adaptive Monte Carlo localization

Input: observation information

z_{t}

, control information

u_{t}

;
Output:

S_{t}

;

1:
Initialization: $N_{r} = 0$ , $N_{m} = 0$ , $N_{k l d} = 0$ , $N_{b} = 0$ , $ω_{a v g} = 0$ ;
2:
for all $b \in H$ do $b = 0$ ;
3:
end for
4:
if $r a n d () < m a x {0, 1 - ω_{f a s t} / ω_{s l o w}}$ then
5:
add random pose to $S_{t - 1}$ ;
6:
$N_{r} = N_{r} + 1$ ;
7:
else
8:
draw i with probability $\propto ω_{t - 1}^{(i)}$ ;
9:
$N_{m} = N_{m} + 1$ ;
10:
$x_{t}^{(N_{m})}$ = motion_model ( $u_{t - 1}, x_{t}^{(i - 1)}$ );
11:
$ω_{t}^{(N_{m})}$ = observation_model ( $z_{t}, x_{t}^{(N_{m})}, m$ );
12:
$S_{t} = S_{t} \cup x_{t}^{(i)}, w_{t}^{(i)}$ ;
13:
$ω_{a v g} = ω_{a v g} + ω_{t}^{(i)}$ ;
14:
if b( $x_{t}^{(N_{m})}$ ) = 0 then
15:
b( $x_{t}^{(N_{m})}$ ) = 1;
16:
$N_{b} = N_{b} + 1$ ;
17:
if $N_{b} > 1$ then
18:
$N_{k l d} = \frac{N_{b} - 1}{2 ε} {\{1 - \frac{2}{9 (N_{b} - 1)} + \sqrt{\frac{2}{9 (N_{b} - 1)}} z_{1 - δ}\}}^{3}$ ;
19:
end if
20:
end if
21:
end if
22:
while ( $N_{m} < N_{k l d}$ && $N_{r} + N_{m} < N_{m a x}$ ) or $N_{r} + N_{m} < N_{m i n}$ do
23:
$ω_{s l o w}$ = $α_{s l o w} ω_{a v g} / N_{m}$ + ( $1 - α_{s l o w}$ ) $ω_{s l o w}$ ;
24:
$ω_{f a s t}$ = $α_{f a s t} ω_{a v g} / N_{m}$ + ( $1 - α_{f a s t}$ ) $ω_{f a s t}$ ;
25:
end while
26:
return $S_{t}$ ;