(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) 