Improved Rao-Blackwellized Particle Filter by Particle Swarm Optimization

Pseudocode 3

The PSO optimized Rao-Blackwellized particle filter.

(1) Initialization

  For 𝑖=1,…,𝑁, initialize the particles, 𝑥₀^𝑛,(𝑖)∼𝑝𝑥^𝑛₀ and set 𝑥₀^𝑙,(𝑖),𝑃₀^(𝑖)=𝑥^𝑙₀,𝑃₀

(2) FOR 𝑘>0

 (2.1) PF measurement update

  𝑥_𝑘^𝑖∗𝑁_𝑖=1=PSO𝑥^𝑖_𝑘^𝑁_𝑖=1,𝑧_𝑘

  Compute the particle weight 𝑤^𝑖_𝑘 using formula (18),

 (2.2) normalize the weight

             𝑤^𝑖_𝑘=𝑤^𝑖_𝑘∑^𝑘_𝑖=1𝑤^𝑖_𝑘

 (2.3) Particle filter time update and Kalman filter:

  (a) Kalman filter measurement update using formula (15)

  (b) Particle filter time update:

   For 𝑖=1,…,𝑁 predict new particles according to (19)

  (c) Kalman filter time update using (16)–(17)

 (2.4) Set 𝐾∶=𝐾+1 and iterate from step (2.1)