(1) Initialization
  For , initialize the particles, and set
(2) FOR
 (2.1) PF measurement update
  Compute the particle weight using formula (18),
 (2.2) normalize the weight
 (2.3) Particle filter time update and Kalman filter:
  (a) Kalman filter measurement update using formula (15)
  (b) Particle filter time update:
   For predict new particles according to (19)
  (c) Kalman filter time update using (16)–(17)
 (2.4) Set and iterate from step (2.1)
Pseudocode 3: The PSO optimized Rao-Blackwellized particle filter.