0
$\begingroup$

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 x, 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.

$\endgroup$
  • $\begingroup$ What papers or books are you using to develop your particle filter? I'm a little confused on your terminology - generally, for a 2D plane, you can have Cartesian coordinates: x,y; or you can have polar coordinates: $\alpha$, d. You have x,y,theta - what's theta? You mention the particle filter "works well for the x,y error but not at all for the theta error" - again, what's theta? I would think you'll either want to convert your map from Cartesian to polar coordinates, or you'd want to convert your lidar output from polar to Cartesian coordinates. $\endgroup$ – Chuck Mar 10 '17 at 14:25
  • $\begingroup$ I'm going to put the question on hold (not closing it) to allow you time to clarify what you're trying to do. I don't understand how you have three variables on a 2D plane. Again, please provide more information on how you developed your method and what all your inputs and variables are. $\endgroup$ – Chuck Mar 10 '17 at 14:26
  • $\begingroup$ @Chuck It sounds like he's trying to estimate the position and heading of his robot. That is the source of his x, y, theta $\endgroup$ – Jacob Panikulam Mar 12 '17 at 1:46
  • $\begingroup$ @JacobPanikulam Exactly, I edited my question, I hope it is a bit clearer; in fact I just need a function that gives me a score of how similar are two arrays of float, but it has to take the position of number in array into account ! $\endgroup$ – EngelOfChipolata Mar 12 '17 at 15:51
  • $\begingroup$ Define a multivariate gaussian with means equal to values of your Real measurement. Then having the map and state of each particle which in your case is [x y theta], you can calculate "measurement" of each particle and evaluate your gaussian on it and get the probability which describes how well this particle fits in your actual measurement. $\endgroup$ – Long Smith Mar 17 '17 at 21:21
1
$\begingroup$

Having your "real" measurement, particles' state and model of taking "virtual" measurements from particles, you can define multivariate Gaussian and exploit it in order to get your probability.

Multivariate normal distribution has density function:

$f(x) = \frac{1}{\sqrt{(2\pi)^{k} |\boldsymbol{\sum}|}}\exp(-\frac{1}{2}(x-u)^{T}\sum^{-1}(x-u))$ where $x$ and $u$ are measurements vectors of size [k x 1] and $\sum$ is a covariance matrix of size [k x k]. As $u$ you use your "real" measurements taken by robot, as for $x$, it contains "virtual" measurements taken from particles.

In your case you have 5 measurements so $x$ is going to have a size of [5 x 1] and $\sum$ --- [5 x 5]. Since measurements are independent, off-diagonal elements of covariance matrix are going to be zeros and diagonal elements define how uncertain your measurements are. So covariance matrix looks like this:

$\sum = \begin{bmatrix} q_{0} & 0 & 0 & 0 & 0\\ 0 & q_{1} & 0 & 0 & 0\\ 0 & 0 & q_{2} & 0 & 0\\ 0 & 0 & 0 & q_{3} & 0\\ 0 & 0 & 0 & 0 & q_{4}\\ \end{bmatrix}$

And because all your measurements are taken by the same LIDAR $q = q_{0} = ... =q_{4}$.

Now having a state of your particle you can obtain "virtual" measurement and feed it to your density function and as an output you will get the probability $p(z|x)$ which describes how likely this "virtual" measurement is your "real" measurement. Then based on that probability you can resample your particles.

$\endgroup$
  • $\begingroup$ With this method I have the same probability for the Particle 1 and Particle 2 described in question, however the Particle 1 is clearly "better" than the Particle 2. Is it important ? Will multivariate Gaussian still work ? $\endgroup$ – EngelOfChipolata Mar 21 '17 at 16:24
  • $\begingroup$ @EngelOfChipolata can not see why do you think that Particle 1 is clearly "better". For me they are equal and naturally you get the same probabilities. $\endgroup$ – Long Smith Mar 25 '17 at 5:45
  • $\begingroup$ Suppose I only have in my environment an obstacle standing at 144°, 350mm away from me (thus, the real measurements). Particle 1 states that the obstacle is standing at 216° while Particle 2 says 288°, so for me Particle 1 makes an error of 72° and Particle 2, 144°. This is why I think Particle 1 is better than Particle 2. Am I wrong ? $\endgroup$ – EngelOfChipolata Mar 27 '17 at 7:06
  • $\begingroup$ @EngelOfChipolata can not say this is a good idea. Firstly because it will make your measurements dependent: i.e. you claim that your fourth measurement have to be close to third measurement. But why this has to be truth? $\endgroup$ – Long Smith Mar 27 '17 at 8:06
  • 1
    $\begingroup$ @EngelOfChipolata Look, if your particle is in the same point as your robot your state does make sense: the closer measurements are the closer orientations are. But remember you can also shift your particle. It may have the same orientation as yours but also be on other side of an obstacle. Because angle is higher than 180 it can detect obstacle. There are infinite number of positions which can make your measurements look like this. So in general it has nothing to do with your orientation. $\endgroup$ – Long Smith Mar 27 '17 at 8:28

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.