CMetricMapBuilderRBPF with beacon observations

[adelizer]

Hi, I am using CMetricMapBuilderRBPF to perform FastSLAM with fiducial landmarks which I can separately get their 3D world coordinates. Since CObservationBearingRange is not supported / processed by any of the maps (correct me if I am wrong), I am using CObservationBeaconRanges to express the measurements. However, when passing the observations to the processActionObservation() the beacon is initialized in the same pose as the particles. Also during motion the beacon location is the one updated and not the other way around.

The configuration file currently used is

occupancyGrid_count=0
gasGrid_count=0
landmarksMap_count=0
pointsMap_count=0
beaconMap_count=1

likelihoodMapSelection=-1

enableInsertion_pointsMap=1
enableInsertion_landmarksMap=1
enableInsertion_gridMaps=1
enableInsertion_gasGridMaps=1
enableInsertion_beaconMap=1

So my questions are:

• How can a beacon observation be inserted using processActionObservation() ?
• Is there another entry point for inserting an observation of the range-bearing type into rbpf ?
• What are the values that should be in the config file to ensure that beacon location is partially stable once initialized ?

Thank you.