Improved Rao-Blackwellized Particle Filter by Particle Swarm Optimization

Pseudocode 2

Description of the Rao-Blackwellized particle filter.

(1) Initialization

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

(2) FOR 𝑘>0

  (2.1) PF measurement update

   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)