Hi,

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:

1. GPS

2. Wheel Encoder

3. Compass

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.

Thanks

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:

1. GPS

2. Wheel Encoder

3. Compass

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.

Thanks

Jaxam269

Comments

This content, along with any associated source code and files, is licensed under The Code Project Open License (CPOL)

CodeProject,
503-250 Ferrand Drive Toronto Ontario, M3C 3G8 Canada
+1 416-849-8900 x 100

"Generate N particles around the GPS value"? What does that mean? In this context what does "particle" correspond to physically? An obstacle?

Rather than focusing on "particle filter" as some neat mathematical algorithm that you might somehow be able to apply to your problem, figure out what you need to solve the problem.

I might suggest that you take a look at how old-time navigators navigate the ocean.

They know where the are when they start. They estimate their current position by dead-reckoning (continually adding their estimate of direction, speed and elapsed time.)

They cross-check their dead-reckoning position against a gross estimate of their current position by using a sextant and chronometer -- the precision of that estimate relative to the size of the ocean is probably similar to the precision of your GPS reading relative to the size of the space your navigating (in other words, extremely rough.)

After doing all that, they know they have no idea where they really are, so they post a watch to keep them from running into obstacles.

They also post a watch to look for landmarks on the shoreline. And the only time they have any idea where they really are is when they see a landmark on the shoreline -- or perferably two landmarks so they can triangulate their position.

The moral of the story: Unless you have a map it's impossible to know where you are, or if you got where you wanted to go.

NOTE: Even Google's self-driving car requires that you drive the route once before hand so it can map the objects along the way and then use them to know where it is.

If they are all based on the same source, then I don't see that it buys you anything.

Isn't GPS error mostly systematic, rather than random?

In other words, if you take five GPS reading in a row from basically the same position in a very short space of time, won't you get the same answer?

In that case, multiple readings aren't all that valuable.

You need more than one way to estimate position, GPS give you one, and it has an error. Dead reckoning gives you another, and it's error accumulates (but combined with the GPS you can correct for the accumulated error). If you had cell-tower triangulation, or strength of WiFi signals you'd have another way to estimate position -- each with it's own different error, and the combination could give you a better idea of position.

What you are talking about the dead-reckoning is similar to the idea described in step 3, ins't it? And combining them is again, very similar to step 6 and 7 combined?

Do you think an INS/IMU would works better for this?

You have only two ways to determine your position: comparing sonar readings with a map, and using your GPS position. The only way to reduce the error from either source is combining the two methods.