Filter löschen
Filter löschen

Particle Weight update does not seem to work

7 Ansichten (letzte 30 Tage)
DarZim am 8 Apr. 2023
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.

Antworten (1)

Gokul Nath S J
Gokul Nath S J am 19 Apr. 2023
Bearbeitet: Gokul Nath S J am 19 Apr. 2023
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.
Gokul Nath S J
  2 Kommentare
DarZim am 19 Apr. 2023
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)
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));
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) );
helperPlotRobot(gca,Vehiclepose); % show Robot in plot
% save the plot as an image
exportgraphics(gca, gifFile, Append=true);
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,
Albert Llufriu López
Albert Llufriu López am 18 Mai 2024
Hello, it's been a year and I see no one has been able to help you. Have you found the problem? I think I'm having the same issue, I have basically the same code as you and I have no idea why I'm unable to update the particles, it's just a few lines of code, I don't know where I'm wrong.

Melden Sie sich an, um zu kommentieren.


Mehr zu Navigation and Mapping finden Sie in Help Center und File Exchange




Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by