Possible error(s) in extended Kalman particle filter update

[Paulkie99]

I think there may be some errors in the update step of the extended Kalman particle filter, but it is also possible that I am misunderstanding some things.

Below is a screenshot from page 56 of https://books.google.co.za/books?id=zABIY--qk2AC&pg=PA37&source=gbs_toc_r&cad=3#v=onepage&q&f=false

1. The likelihood seems to be based on the sampled state $x{k}^{i}$, and not the filtered EKF estimate $x̂{k}^{i}$. However in the code the likelihood is computed from the filtered EKF estimate propagated_state instead of the sample updated_state.

2. Should the prior not be computed as $\mathcal{N}(x{k}^{i}; x{k-1}^{i} + B_ku_k, Q)$ i.e. using the state propagated by the process model and not the filtered EKF estimate? In the code the mean used is propagated_state and not $x_{k-1}^{i} + B_ku_k$.

3. Finally, should the particle states not be set to updated_state instead of propagated_state in line 176? From the screenshot above the EKF filtering step makes use of the previous particle's sampled state $x{k-1}^{i}$ and not $x̂{k-1}^{i}$.

https://github.com/jelfring/particle-filter-tutorial/blob/8d1304ec23e634353d108a2795f6c492b330344b/core/particle_filters/extended_kalman_particle_filter.py#L161-L176

If the current implementation is correct, can an explanation be provided? I am just trying to learn :)

[jelfring]

1. The likelihood seems to be based on the sampled state xki, and not the filtered EKF estimate x̂ki. However in the code the likelihood is computed from the filtered EKF estimate propagated_state instead of the sample updated_state.
2. Should the prior not be computed as N(xki;xk−1i+Bkuk,Q) i.e. using the state propagated by the process model and not the filtered EKF estimate? In the code the mean used is propagated_state and not xk−1i+Bkuk.

Short answer: you are right. The naming of some of the variables is confusing, which makes it hard to understand the code and which, even worse, led to some mistakes that are not easy to spot. Thanks for pointing this out!

3. Finally, should the particle states not be set to updated_state instead of propagated_state in line 176? From the screenshot above the EKF filtering step makes use of the previous particle's sampled state xk−1i and not x̂k−1i.

Yes indeed.

@Paulkie99 , would you be willing to create a PR that fixes the problem? I just created this branch which I believe should solve the problem (if you prefer and agree I can merge that branch instead).