I am currently trying to implement a particle filter an a robot in a view to localize it on a 2D plane (i.e. to determine
y and its orientation
theta ). I am using a LIDAR which give me
(alpha, d) with alpha the angle of measurement and d the distance measured at this angle. For now, I can compute the theoretical measures for each of my particle. But I am struggling with the evaluation function (the function that will give me the probability (or weight) of a particle considering the real measures).
Suppose my LIDAR give me 5 values per rotation (0°, 72°, 144°, 216°, 288°), thus I store one rotation in an array (5000mm is my maximum value) :
- Real LIDAR value :
[5000, 5000, 350, 5000, 5000]
- Particle 1 :
[5000, 5000, 5000, 350, 5000]
- Particle 2 :
[5000, 5000, 5000, 5000, 350]
In this example, I want the function to return a higher probability (or weight) for Particle 1 than for Particle 2 (72° error vs 144°).
For now I am just doing the invert of the sum of the absolute difference between the two value at the same place in the array (e.g. for Particle 1 :
1 / (5000-5000 + 5000-5000 + 5000-350 + 5000-350 + 5000-5000)). The problem with this method is that, in this example, Particle 1 and 2 have the same probability.
So, what kind of function should I use to have the probability of a particle to be the right one with those kind of measurements ?
PS : I am trying to adapt what is in this course : https://classroom.udacity.com/courses/cs373/lessons/48704330/concepts/487500080923# to my problem.