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

Algorithm 1

(1) Initialization: 𝑡=0

  𝑖=1

  do

     draw the particles 𝑥^𝑖₀ from the prior 𝑝(𝑥₀) and set

             𝑥^_₀^(𝑖)=𝐸[𝑥₀^(𝑖)]

            𝑃₀^(𝑖)=𝐸[(𝑥₀^(𝑖)−𝑥^_₀^(𝑖))(𝑥₀^(𝑖)−𝑥^_₀^(𝑖))^𝑇]

𝑥^_₀^(𝑖)𝑎=𝐸[𝑥^(𝑖)𝑎]=(𝑥^_₀^(𝑖))^𝑇00^𝑇

                  𝑃₀^(𝑖)𝑎=𝐸[(𝑥₀^(𝑖)𝑎−𝑥^_₀^(𝑖)𝑎)(𝑥₀^(𝑖)𝑎−𝑥^_₀^(𝑖)𝑎)^𝑇]

=𝑃₀^(𝑖)000𝑄000𝑅

     // 𝑁 is the number of particles in UPF. 𝑥^_₀^(𝑖) is the initial mean value of 𝑥₀^(𝑖).

     // 𝑃₀^(𝑖) is the initial covariance of 𝑥₀^(𝑖). 𝑥^_𝑎₀ is the initial mean value of 𝑥^𝑎₀.

     // 𝑄 is the covariance of 𝑤_𝑡. 𝑅 is the covariance of 𝑣_𝑡.

  While ( 𝑖++=<𝑁 )

(2) For 𝑡=1,2,…,𝑡_𝑓

  // 𝑡_𝑓 is the run time of UPF.

  (1) Importance sampling

  𝑖=1

  do

     Update the particles with UKF, obtain 𝑥^__𝑡^(𝑖) and 𝑃_𝑡^(𝑖).

          𝑥_𝑡^(𝑖)∼𝑞(𝑥_𝑡^(𝑖)∣𝑥^(𝑖)_0∶𝑡−1,𝑧_1∶𝑡)=𝑁(𝑥^__𝑡^(𝑖),𝑃_𝑡^(𝑖))

     // The particles 𝑥_𝑡^(𝑖) are sampled from 𝑁(𝑥^__𝑡^(𝑖),𝑃_𝑡^(𝑖)).

  while ( 𝑖++=<𝑁 )

  (2) Computing the importance weights

   𝑖=1

   do

            𝑤_𝑡^(𝑖)∝𝑝(𝑧_𝑡∣𝑥_𝑡^(𝑖))𝑝(𝑥_𝑡^(𝑖)∣𝑥^𝑖_𝑡−1)𝑞(𝑥_𝑡^(𝑖)∣𝑥^(𝑖)_0∶𝑡−1,𝑧_1∶𝑡)

   // 𝑤_𝑡^(𝑖) is the importance weight of the 𝑖th particle at the time step 𝑡.

   // The state transition probability 𝑝(𝑥_𝑡^(𝑖)∣𝑥^𝑖_𝑡−1) can be obtained from (1).

   // The observation model 𝑝(𝑧_𝑡∣𝑥_𝑡^(𝑖)) can be obtained from (2).

   // 𝑞(𝑥_𝑡^(𝑖)∣𝑥^(𝑖)_0∶𝑡−1,𝑧_1∶𝑡) is the proposal distribution.

   while ( 𝑖++=<𝑁 )

  (3) Normalize the importance weights

   𝑖=1

   do

               𝑤_𝑡^(𝑖)=𝑤_𝑡^(𝑖)∑^𝑁_𝐼=1𝑤_𝑡^(𝑖)

   while ( 𝑖++=<𝑁 )

  (4) Resampling step

   𝑖=1

   do

   Multiply/Suppress particles 𝑥_𝑡^(𝑖) according to the importance weights 𝑤_𝑡^(𝑖) to obtain 𝑁

   random particles 𝑥_𝑡^(𝑖).

   while( 𝑖++=<𝑁 )

  (5) Output

   The output 𝑥_𝑡 is the state estimation of 𝑥 at the time step 𝑡. It can be approximated by (3).