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."
- Map creates a set of features -
map.map = dim * (2*rand(2, nfeatures)-1);.
- 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:
- Estimate where the features are, so you can
- Use the scan data to measure where the features are, so you can
- Supply the particle filter with measurements, so you can
- 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.