I want to implement a particle filter for robot localisation, however I am still unsure whether my understanding of particle filter is correct. So can someone take a look to see if I am understanding it right?
Here is the list of sensor available for use in this algorithm.
A small robot/ toy car that has the following sensors on board:
2. Wheel Encoder
4. Sonar/ Distance/range measurement sensor (for landmark or obstacle avoiding)
Here is my goal: I want the robot to move from location to location (eg, A -> B -> C) using all the sensors available
Here is what I am thinking of using particle filter, in these steps:
1. Get current GPS reading
2. Generate N number of particles around this GPS value with some small variance. Each particle is going to have (x, y, theta) ~ (x, y, angle heading)
3. Send control input to robot which make it move. After some time (500 ms?), get the estimate of the current robot state (location) using the
x' = x_old + v*t*sin(theta), y' = y_old + v*t*cos(theta), theta' = theta_old + w*t, where w is angular velocity, where theta is getting from compass
4. Propagate the N particle in the previous state into the current state using the same equation as explained in 3
5. Get the current GPS reading (we know that GPS could give up to 20 meter error)
6. Use this GPS value as a actual measurement value and calculate the weight of all those N particles all in the vicinity of GPS and some small variance
7. Resample, pick more of the particle that has higher weight and less of that lower weight
8. Repeat from step 3
So, please give me some feed back of what you think.