UAPF: A UWB Aided Particle Filter Localization For Scenarios with Few Features

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

Algorithm 1: UAPF(

x_{t - 1}

r_{i, t}

u_{t}

z_{t}

1:
$x_{r, t} = R a n g i n g_B a s e d_L o c a l i z a t i o n (r_{i, t})$
2:
$x_{p, t} = P F_b a s e d_L o c a l i z a t i o n (x_{t - 1}, u_{t}, z_{t}, Σ_{p})$
3:
Get $K N P$ according to distributions of $x_{r, t}$ and $x_{p, t}$
4:
if $K N P > t h r e s h o l d$ then
5:
$x_{t} = P o s e_F u s i o n (x_{p, t}, x_{r, t}, Σ_{r})$
6:
else
7:
if $Σ_{p} ≫ Σ_{r}$ then
8:
Re-localization: $x_{t} \sim N (μ_{r}, Σ_{p} + Σ_{r})$
9:
else
10:
$x_{t} = P o s e_F u s i o n (x_{p, t}, x_{r, t}, Σ_{r})$
11:
end if
12:
end if
13:
Update $Σ_{p}$ according to $K N P$ and pose difference
14:
return $x_{t}$