Particle Filter & Self-Driving Car - I need verification

Question

1.00/5 (1 vote)

See more:

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

Posted 14-Feb-13 8:16am

Jaxam

Add a Solution

Comments

TRK3 14-Feb-13 18:04pm

It's unclear to me what you mean in step 2.

"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.

onethecode 14-Feb-13 18:15pm

N here means N number of particle. Each is on its own a rough guess of where the robot is currently at. Particle filter has better advantage than dead-reckoning, one of which is due to the error accumulative. though, GPS is still vauge

TRK3 14-Feb-13 18:39pm

What are the N rough guesses based on?

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.

onethecode 14-Feb-13 18:48pm

I believe the robot has to have a know starting position and goal position. with this known starting position, the N particle can be generated around this vicinity to begin first.
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?

Stefan_Lang 15-Feb-13 5:35am

You didn't explain what a 'particle' is, or what GPS should do about them. If these are just virtual points in space around you, then any information about them is not going to help you in any way at all, since it will be based on your own GPS position. You cannot correct an error with the help of values that are themselves based on the erroneous original value - that's like trying to pull yourself out of a swamp by dragging your own hair!

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.

Jaxam 20-Feb-13 14:28pm

If I have a particle that has (x,y,theta) and now that i want to calculate the weight of that particle do i calculate each term (x,y,theta) seperate and multiply them together to get the weight of that particle?

1 solution

Add a Solution

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

Jaxam · Answer 1 · 2013-02-20T08:26:00

Solution 1

If I have a particle that has (x,y,theta) and now that i want to calculate the weight of that particle do i calculate each term (x,y,theta) seperate and multiply them together to get the weight of that particle?

Posted 20-Feb-13 8:26am

Jaxam

Updated 20-Feb-13 8:28am

v2

Particle Filter & Self-Driving Car - I need verification

1 solution

Solution 1

Add your solution here

Preview 0

Particle Filter & Self-Driving Car - I need verification

1 solution

Solution 1

Add your solution here

Preview 0

Existing Members

...or Join us