Clustered Exact Daum-Huang Particle Flow Filter

Algorithm 3

Clustered Exact Flow Daum-Huang Filter.

7  for to do

8   Set ;

9   for to do

10    Calculate by linearizing at ;

11   endfor

12   Cluster particles over the margin of error

13   for to do

14    Calculate and from (6) and (7) using and ;

15    Evaluate for each particle from (5)

16    Migrate particles:

17   endfor

18   Eliminate the largest cluster with the biggest error

19   Re-evaluate using the updated particles .

20  endfor

21  Apply UKF/EKF update: ;

22  Estimate from the particles using ;

23  Redraw particles ; (Optional)