Laser Scanner for localization in particle filter

Question

I'm working on a particle filter implementation in Matlab and I found one very good in the Robotics Toolbox (available here https://github.com/petercorke/robotics-toolbox-matlab). My problem is that I really don't know how to modify it in order to use it with a Laser scanner, I know that with a know map the laser beam will give the distance and angle (kind o like RangeAndBearing) but I haven't understood well how the likelihood field of the map fits into this.

In the end my question is how can I get the RangeAndBearing Measurement from a Laser scanner in order to use it in the particle filter?

Thanks in advance

Answer 1

The RangeBearing "sensor" is used to "find" landmarks given a map of landmarks. I looked at the source code a bit and it doesn't seem to be an especially useful or realistic "sensor."

Consider:

1. Map creates a set of features - map.map = dim * (2*rand(2, nfeatures)-1);.
2. RangeBearingSensor gets those set of features - jf = 1:numcols(s.map.map); and then picks a random one to return - jf = jf(i);, or you can pick a particular feature - xf = s.map.map(:,jf); and get the range and heading to that feature - dx = xf(1,:) - xv(:,1); dy = xf(2,:) - xv(:,2); and z = [sqrt(dx.^2 + dy.^2) atan2(dy, dx)-xv(:,3) ];.

The point is, the code completely glosses over how you reconcile a feature on a map to the vehicle. You can say, "where is feature XX?" and the RangeBearingSensor simply returns a range and heading. There's no code in there that really explains how you determine which feature in a set of scan data is feature XX.

For your case, in order to create the same functionality as RangeBearingSensor, you need to know roughly where feature XX is in order to locate it in the scan data, and then you use the scan data for that feature to return where it is. Your scan data might include a bunch of features, or none, and it's up to you to:

1. Estimate where the features are, so you can
2. Use the scan data to measure where the features are, so you can
3. Supply the particle filter with measurements, so you can
4. Estimate where the features are.

My point is that you're not going to (easily) replicate the functionality of RangeBearingSensor with your own lidar sensor code because RangeBearingSensor uses knowledge that is not directly obtainable - i.e., where all the landmarks are.

You might have two landmarks that are (statistically) in the same location or otherwise close enough that they are indistinguishable from your sensor's point of view. Your sensor can't tell them apart, but RangeBearingSensor uses its knowledge to generate a reading anyways because it generates measurements by taking a perfect measurement to an exact landmark and then adding noise.

Your scenario is trying to take noisy data and first identify the landmark, then use the landmark plus the measurement to eliminate the noise.

Answer 2

As already stated, you can not do what you are asking.

The task you need to do is score each particle's fit to the map given the incoming lidar scan.

The approach i used is: for each particle do a simulated scan against the known map and compare it to the incoming lidar scan. This comparison is used to rank each particle.

I initially achieved this with ray tracing and comparing the length of the trace to the scan length.

It is much more efficient to build a map for the particle filter where each occupied point is added as a gausian distribution with some arbitary (not to big, not to small) radius. so rather than ray tracing you can just calculate where each lidar scan line would end for each particle and take the score directly from the map for each line.