Improved Rao-Blackwellized Particle Filter by Particle Swarm Optimization

Pseudocode 1

Description of the standard particle filter.

𝑠^𝑖_𝑘,𝑤^𝑖_𝑘^𝑁_𝑖=1=PF𝑠^𝑖_𝑘−1,𝑤^𝑖_𝑘−1^𝑁_𝑖=1,𝑧_𝑘

FOR =1 : N

 Draw 𝑠^𝑖_𝑘∼𝑝𝑠_𝑘∣𝑠^𝑖_𝑘−1

 Calculate 𝑤^𝑖_𝑘=𝑝𝑧_𝑘∣𝑠^𝑖_𝑘

END FOR

Calculate total weight: 𝑡=sum𝑤^𝑖_𝑘^𝑁_𝑖=1

For =1 : N

 Normalize: 𝑤^𝑖_𝑘=𝑡⁻¹𝑤^𝑖_𝑘

END FOR

Resample [17]

𝑠^𝑖_𝑘,𝑤^𝑖_𝑘,…^𝑁_𝑖=1 = RESAMPLE𝑠^𝑖_𝑘,𝑤^𝑖_𝑘^𝑁_𝑖=1