# Laser Scanner for localization in particle filter

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?

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.

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.

• What do you mean by length of the trace and scan length? Also, what comparision metric are you using between the two parameters (length of the trace and scan length) to rank the particles? Commented Aug 11, 2021 at 20:27
• Also, can you please elaborate the approach mentioned in the last paragraph? Has it got something to do with the NDT algorithm? Commented Aug 11, 2021 at 20:29
• 'length of the trace' is the expected distance that the LIDAR should return (for each individual return) given where the particle is supposedly located. 'scan length' is the actual distance that the LIDAR returned. The comparison metric is sum('length of trace' - 'scan length'), closer to zero is better Commented Aug 14, 2021 at 7:16
• I had to look up what NDT algorithm is, but yes it seems to be what I am doing. I use a 2D array and for each known occupied point in the map I place a Gaussian distribution into the 2D array around that location. When a point is received from the LIDAR, I can simply do a look up using that distance and angle for each particle in the filter in the 2D array, and I immediately without any further calculation get a score for that point.... score = map[x][y] rather than the whole ray trace thing. This approach saw my particle filter go from being able to run 500 particles to running 5000 particles Commented Aug 14, 2021 at 7:20
• What do you mean by 'placing a Gaussian distribution into the 2D array around that location'? Commented Aug 19, 2021 at 5:17