An Adaptive Unscented Particle Filter Algorithm through Relative Entropy for Mobile Robot Self-Localization

Algorithm 2

Inputs: 𝑆_𝑡−1=𝑥^𝑖_𝑡−1,𝑤^𝑖_𝑡−1𝑖=1,…,𝑛; the control measurement 𝑢_𝑡−1; the observation 𝑧_𝑡;

    the bound 𝜀 and the probability 𝜎; the bin size ℏ; 𝑆_𝑡=𝜙; 𝑛=0; 𝑘=0; 𝛼=0.

Generate the particles:

𝑖=1

do

 Sample a particle index 𝑗(𝑛) from the discrete distribution according to the weights in 𝑆_𝑡−1.

 Obtain the particles 𝑥^𝑛_𝑡 from the particles 𝑥^𝑗(𝑛)_𝑡−1 according to (1).

 Compute the importance weight:

 𝑤^𝑛_𝑡=𝑝(𝑧_𝑡∣𝑥^𝑛_𝑡)

 𝛼=𝛼+𝑤^𝑛_𝑡

 Insert particles into the particle set:

 𝑆_𝑡=𝑆_𝑡∪𝑥^𝑛_𝑡,𝑤^𝑛_𝑡

 if (𝑥^𝑛_𝑡 falls into empty bin)

    𝑘=𝑘+1

 end if

 𝑛=𝑛+1

while 𝑛<𝜎²_IS/2𝜀Var_𝑝(𝑥)𝜒²_{𝑘−1,1−𝛿}

Normalize the importance weight:

for 𝑖=1,…,𝑛

 𝑤^𝑖_𝑡=𝑤^𝑖_𝑡/𝛼

return 𝑆_𝑡