(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 2: Description of the Rao-Blackwellized particle filter.