I'm interested in the simple algorithm for particles filter shown in the algorithm above. It seems very simple but I have no idea on how to do it practically.
Edit: After the example and explanation given by @smjohns , I've implemented the simple example of two states according to what he said, just to better understand how it works. Can you, please check this simple C++ implementation that I made from his example, then tell me if I understood it correctly. If it is the case, I will add some more questions about particle filters algorithm. here is the code: http://pastebin.com/M1q1HcN4
Note, the algorithm from page 11 of www.cs.ubc.ca/~arnaud/doucet_defreitas_gordon_smcbookintro.ps
OK let's extend our two state model:
We attempt to track the position of (let's say) a robot across two states using four particles.
States: A, B (so x_t^i = A means that particle i is at position A at time t)
P(A 0) = 0.5, P(B 0) = 0.5 (The robot starts out as equally likely to be in either state.)
P(A _t|B _t-1) = 1/3, P(B _t|A _t-1) = 2/3 (This is the motion model. Any object has probability of moving to or staying in B of 2/3; any object has probability of moving to or staying in A of 1/3.)
P(y _t = s|x _t = s) = 4/5 (This is the measurement model. If an object is in state s (A or B), the probability that our observation is accurate is 80%.)
We create four particles; each particle is equally likely to be in A or B.
For each particle:
P(A1) = P(A1|A0)P(A0) + P(A1|B0)P(B0) = (1/3)(1/2) + (1/3)(1/2) = 1/3. P(B1) = 2/3.
In our example: let's say we end up with 2 particles in A, two particles in B.
In our example: Assume that we observe A.
The two particles in A each have unnormalized weight 4/5 (P(y1 = A|x1 = A)). The two particles in B have unnormalized weight 1/5 (P(y1 = A|x1 = B) = 1/5).
So our particles have weights A: 4/5, 4/5 B: 1/5, 1/5. These weights add up to two, so we normalize by dividing through by 2: A: 2/5, 2/5 B: 1/10, 1/10.
Now we resample our four particles. Every time, there's an 80% chance it will be A (2/5 + 2/5) and a 20% chance it will be B (1/10 + 1/10).
In our example: let's say we end up with 3 particles in A, 1 particle in B.
Repeat steps 2 and 3 until you have the robot localized to your satisfaction (all particles more or less track the robot, within error.)
answered 07 May '12, 11:39
Two-state system: in state A, there are 3 particles, each with weight 0.1. In state B, there is 1 particle, with weight 0.5). So if I resample my particles (choose a new distribution of my four particles based on existing weighting), then I repeat the following four times:
Choose a new particle. I choose a particle in state B with probability 5/8, and a particle in state A with probability 3/8. Where do these numbers come from? P(sample in state s) = (# particles in S * weight of particles in S) / sum over states S' (# particles in S' * weight of particles in S'). So Prob(A) = (3*0.1)/(3 * 0.1 + 1 * 0.5) = 0.3/0.8 = 3/8; Prob(B) = 0.5/0.8 = 5/8.
So you are likely to have more particles in B than A, after resampling. You are unlikely to have all particles in state A after resampling; you expect this to happen with probability 0.3 * 0.3 * 0.3 * 0.3 = 0.081 = 8.1% of the time.
Not sure about the paper but I coded a Kalman filter on jsfiddle for cs 373 that has a pretty little implementation. The actual kalman update is only about 7 lines. The rest is setup and interface code. If you are going to use this example, be careful, I add a decay step that makes the Kalman filter forget the past. This allows it to do fun things like change directions, but it is not part of the kalman definition.
answered 07 May '12, 11:06
You may find this post useful:
answered 08 May '12, 08:33