Particle Weight update does not seem to work

Hello,

I have implemented a 2D occupancy map and a robot model. The robot will move through the map. It is equipped with a simulated Lidar sensor. If I let the Monte Carlo Localization function of Matlab run, my code works. The particles converge to the robot's position. However, if I check the particle weights during the process, they are all the same. If I have 2000 particles, all 2000 particles always have the same weights. Does someone know why this is or what the problem might be?

I am assuming that the code works correctly since it does converge. So the weights have to be changing in the background without my knowledge.

The main part of the code is this in a loop for each time step. At the end Vehiclepose will be updated for the next time step.

[ranges,angles] = lidar(Vehiclepose,map);

scan = lidarScan(ranges,angles);

[isUpdated,estimatedPose, estimatedCovariance] = mcl(Vehiclepose, scan);

[particles,weights] = getParticles(mcl);

Thank you.

Answer 1

[Gokul Nath S J]

Hi DarZim,

Based on my understanding, it seems that the particles weights are uniform throughout the simulation.

If all 2000 particles have the same weight throughout the Monte Carlo Localization process, then it's possible that there is an error in your implementation of the particle weights update step.

Note that the particle weights are updated based on the likelihood of the measurement given the particle's position. Some possible explanation for why the the particles are having same weights are listed below,

• The likelihood function might not correctly implemented or is returning the same value for all particles.
• You could also try plotting the likelihood of each particle given the measurement to see if there are any patterns or issues with the calculation of the likelihood.
• The measurement noise variance is set too low, which could result in all particles having similar likelihood values and hence similar weights.
• If the resampling is not correctly performed, it could lead to all particles having the same weight.

Thanks,

Gokul Nath S J

[DarZim]

Hi Gokul Nath S J,

Thank you very much for your response. I agree with what you said. That is my understanding as well. However, I am relatively confident that my code is working or if it is working incorrectly, it at least still converges. Which should not happen if something is wrong.

If I plot my robot's pose on a 2D occupancy map and the particles as well, I can see them spread out over the entire map and after some steps, the particles converge to the robots position. That's why I am so confused. I feel like the algorithm is working but if I look at my weights, they dont change.

This is my model initialiaztion before the loop:

%% Lidar
% Simulate lidar sensor and set the detection angles to [-pi/2 pi/2]
lidar = rangeSensor;
lidar.HorizontalAngle = [-pi/2 pi/2];
SensorModel.NumBeams = 181;

% Set min and max values of the detectable range of the sensor in meters
lowerLimit = 0;
upperLimit = 30;
lidar.Range = [lowerLimit upperLimit];

% Generate lidar readings
[ranges,angles] = lidar(Vehiclepose,map);

% Store and visualize 2-D lidar scan
scan = lidarScan(ranges,angles);

%% MonteCarloLocalization / Particle Filter
odometryModel = odometryMotionModel;
odometryModel.Noise = [0.4 0.4 0.4 0.4];            

rangeFinderModel = likelihoodFieldSensorModel;
rangeFinderModel.SensorLimits = [lowerLimit upperLimit]; 
rangeFinderModel.NumBeams = 129; 
rangeFinderModel.Map = map;
rangeFinderModel.MeasurementNoise = 0.2;

mcl = monteCarloLocalization;
mcl.UseLidarScan = true;
mcl.MotionModel = odometryModel;
mcl.SensorModel = rangeFinderModel;
mcl.UpdateThresholds = [0.4 0.4 0.4];             
mcl.ResamplingInterval = 1;
mcl.ParticleLimits = [500 5000];                  
mcl.GlobalLocalization = true; % if false: use Initial Pose, if true: uniformly distribute Particles over entire map
mcl.InitialPose = Vehiclepose;
mcl.InitialCovariance = eye(3)*0.5;

And my loop:

for k = 1:size(traj,1)
    clf
    show(map)
    Vehiclepose = traj(k,:);
    [ranges,angles] = lidar(Vehiclepose,map);
    scan = lidarScan(ranges,angles);

    [isUpdated,estimatedPose, estimatedCovariance] = mcl(Vehiclepose, scan);
    [particles,weights] = getParticles(mcl);
    % plot particles
    for i = 1:size(particles,1)
        helperPlotParticles(gca,particles(i,:), weights(i));
    end
    hold on
    %visualize lidar scans
    for i = 1:size(scan.Ranges,1)
        scan_map(i,1) = Vehiclepose(1) + scan.Ranges(i) * cos( scan.Angles(i) + Vehiclepose(3) );
        scan_map(i,2) = Vehiclepose(2) + scan.Ranges(i) * sin( scan.Angles(i) + Vehiclepose(3) );
    end
    scatter(scan_map(:,1),scan_map(:,2),'filled')
    helperPlotRobot(gca,Vehiclepose); % show Robot in plot
    pause(0.5)
    % save the plot as an image
    exportgraphics(gca, gifFile, Append=true);
end

But the lower part is just all visualization. So the important lines in the loop should only be:

    [ranges,angles] = lidar(Vehiclepose,map);
    scan = lidarScan(ranges,angles);
    [isUpdated,estimatedPose, estimatedCovariance] = mcl(Vehiclepose, scan);
    [particles,weights] = getParticles(mcl);

As far as I understand.

Are you able to see what the problem might be?

Thank you very much,

DarZim