Particle filter weight function

Question

I am trying to implement a particle filter in MATLAB to filter a robot's movement in 2D but I'm stuck at the weight function. My robot is detected by a camera via two points, so a single measure is a quadruple (xp1, yp1, xp2, yp2) and states are the usual (x,y,alpha) to detect its pose in 2D. As far as my understanding goes I should assign a weight to each particle based on its likelihood to be in that particle position with regards to the current measurement.

I also have the measure function to calculate an expected measurement for a particle, so basically I have, for each instant, the actual measurement and the measurement that a single particle would have generated if it were at the actual state.

Assuming all noises are Gaussian, how can I implement the weight function? I kind of noticed the mvnpdf function in MATLAB, but I can't actually find a way to apply it to my problem.

Answer 1

A little background.

You need to weight each particle by the liklihood of the particle being correct. The probability the particle is correct is given by the probability that it is correct given the measurements.

• Note that the "weight" (which is a terrible term) is simply the probability of the particle being correct. Therefore, each particle is really an estimate, and each estimate has some probability of being correct.
• Let $x$ be the state. There are a bunch of (say $N$) state estimates ($x_i$ for $i=1...N$), i.e., particles
• Let $z$ be your measurement

By Bayes rule:

$$p(x|z) = \frac{p(z|x)\cdot p(x)}{p(z)}$$

Which means, the probability of the particle being "correct" given the measurement is given by the probability that the measurement "makes sense" given the particle's estimate, times the probability that the particle was correct to begin with.

The denominator is commonly written using the law of total probability so the above becomes:

$$p(x_i|z) = \frac{p(z|x_i)\cdot p(x_i)}{\sum_{j=1}^N p(z|x_j)p(x_j)}$$

You can re-weight each particle using the above. Explicitly,

$$w_i\gets \frac{p(z|x_i)\cdot w_i}{\sum_{j=1}^N p(z|x_j)\cdot w_j}$$

Now, let's go over your problem.

You said you have a tuple $(x_1,y_1,x_2,y_2)$, but that's not correct, you have two measurements of the $(x,y)$ value for the robot's position. You need to do two updates of each particle. (EDIT: It seems the three-dimensional state of the robot was derived from these two points. I misunderstood so ignore that)

So, first calculate $p(z|x_i)$ for each particle $x_i$. In a two dimensional state, this is done as follows:

• for all $x_i$, calculate $p_i \gets p(z|x_i)$

E.G., In matlab: pi=chi2pdf( norm(z-xi), 2)). See: chi2pdf and norm. You may want to see this question if you don't understand this step.

• Now, let $D\gets\sum_{i=1}^N p(z|x_i)\cdot w_i$ using the above values of $p_i$ and previous values of $w_i$ for each particle.
• Then, let the new value of each particle be: $w_i\gets\frac{p_i\cdot w_i}{D}$.

For a complex discussion of why this works, what about it isn't so great, and so on, you'll want to read some literature. Thrun's Probablistic Robotics is a great resource.

EDIT: I should mention that commonly $p(z|x_i)$ is calculated not using CHI2PDF but instead by the EKF linearization of $z=h(x_i)$. That's beyond scope. Furthermore, the uncertainty in the robot position is not included. Using an EKF for each particle will address this but that's a different framework (sort of).

Answer 2

I suggest you to read the book "Probabilistic Robotics" by Sebastian Thrun et al. A simple test you can do is writing a weighting function as a product of Gaussians centered in the x,y of the particle and evaluated in the measurement points. You can use monodimensional Gaussians and multiply all together.

E.g.,

weight_particle_i = exp( (x_measure - x_particle_i)^2/sigma_x^2)*exp( (y_measure - y_particle_i)^2/sigma_y^2)

where the sigma represents the uncertainty in your measures

Answer 3

Thanks for all the replies (I'm the op with a new account so I can't comment your single replies).

@Mallo: thanks for the input, I was actually basing my work on that book ;) I ended up doing something very similar to what you have described.

@Josh:

"You said you have a tuple (x1,y1,x2,y2), but that's not correct..."

well, no. I was correct, let me explain better: the robot is identified by camera reading TWO points on it, therefore the measurement consist of FOUR coordinates and to generate the particle's weight I have the particle to examine (which is a (x,y,orientation)) and a measurement. I then apply the measure equations to the particle and confront the "should-be" measure generated by the particle with the actual measurement.

Also, weight is actually the appropriate term because they are just proportional to likelihood but they still need to be normalized (but it isn't necessary).

For the future readers that is what I ended up doing:

• generate particle measure
• calculate euclidean distance between particle's supposed measure and actual measure, which is accomplished in MATLAB with the pdist function.
• calculate the pdf of the distance for a standard gaussian (mean = 0 and sigma = 1), which is accomplished in MATLAB with normpdf(distance, 0, 1).