Understanding one step in particle_filter_base.py
Closed this issue · 2 comments
Hi,
I looked at the code and found the following step in compute_likelihood method:
# Map difference true and expected distance measurement to probability
p_z_given_x_distance = \
np.exp(-(expected_distance-measurement[i][0]) * (expected_distance-measurement[i][0]) /
(2 * self.measurement_noise[0] * self.measurement_noise[0]))
It seem an implementetation of the very well known gauss normal distribution:
but to me the term:
1 / (sigma * sqrt(2.0 * pi))
is missing.
Why?
PS: Thank you very much for the tutorial... you are helping thousand and thousand of people, who are train to understand particle filters
Short answer: because multiplying all weights with the same constant will not change the estimates of the filter. The main reason therefore is computational efficiency.
Longer answer:
As you can see, the term is constant, in other words, it is independent of the state of a particular particle. For ease of writing we can define:
c = 1 / (sigma * sqrt(2.0 * pi)).
Now let's assume this term will be added. The particle weights will now change as follows:
weight_1 = weight_1_without_term * c
weight_2 = weight_2_without_term * c
...
weight_n = weight_n_without_term * c
where weight without term refers to the weight the way it is computed in the code. As you can see, the ratio between the non-normalized weights remains unchanged after adding c.
Now let's normalize the new weights, such that they sum up to one. The sum of all weights after adding the term c changes as follows:
sum_all_weights = weight_1_without_term * c + weight_2_without_term * c + ... + weight_n_without_term * c,
which can be rewritten as:
sum_all_weights = c * (weight_1_without_term + weight_2_without_term + ... + weight_n_without_term)
If we now normalize the weights, the term c cancels out:
normalized_w1 = (weight_1_without_term * c) / (c * (weight_1_without_term + weight_2_without_term + ... + weight_n_without_term)).
So, the normalized weights will be equal to the normalized weights without the term c.
I hope this helps.
Many many thanks!!!!!
Very clear and understandable!