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}$.

particle-filter-tutorial/core/particle_filters/extended_kalman_particle_filter.py

Lines 161 to 176 in 8d1304e

	# New particle state: sample from normal distribution EKF
	updated_state = np.random.multivariate_normal(propagated_state, cov_ekf)
	self.validate_state(updated_state)
	# Compute likelihood using propagated state
	likelihood = self.compute_likelihood(propagated_state, measurements, landmarks)
	# Compute prior (mean is zero vector by default)
	prior = multivariate_normal.pdf(updated_state-propagated_state, cov=self.Q)
	# Importance density
	importance_density = multivariate_normal.pdf(updated_state-propagated_state, cov=cov_ekf)
	# Compute current particle's weight
	weight = likelihood * prior / importance_density
	sum_weights += weight
	# Store updated particle
	new_particles.append([weight, propagated_state, cov_ekf])

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