Table - PMC

Skip to main content

View full-text article in PMC

. 2020 Nov 28;20(23):6814. doi: 10.3390/s20236814

Algorithm 2 Improved PF-based Localization (

x_{t - 1}

,

u_{t}

,

z_{t}

,

Σ_{p}

).

1:
$X_{t}$ , ${\bar{X}}_{t} = ϕ$
2:
$x_{o d o m}$ = $M o t i o n_m o d e l (u_{t}, x_{t - 1})$
3:
Set m as the number of particles should be sampled
4:
for i = 0 to m do
5:
$x_{t}^{i}$ = $S a m p l e_m o d e l (x_{o d o m}, Σ_{p})$
6:
$w_{t}^{i}$ = $S c a n m a c h_m o d e l (z_{t}, x_{t}^{i}, m a p)$
7:
$\bar{X_{t}}$ += $< x_{t}^{i}, w_{t}^{i} >$
8:
end for
9:
for i = 0 to m do
10:
Draw $x_{t}^{i}$ from $\bar{χ_{t}}$ with probability $\propto w_{t}^{i}$
11:
$X_{t}$ += $x_{t}^{i}$
12:
end for
13:
$x_{p, t}$ = $M e a n (X_{t})$
14:
return $x_{p, t}$