Particle Weight update does not seem to work

2 次查看（过去 30 天）
DarZim
DarZim 2023-4-8
评论： Albert Llufriu López 2024-5-18
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.

请先登录，再回答此问题。

回答（1 个）

Gokul Nath S J
Gokul Nath S J 2023-4-19
编辑：Gokul Nath S J 2023-4-19
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
  2 个评论
DarZim
DarZim 2023-4-19
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
Albert Llufriu López
Albert Llufriu López 2024-5-18
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.

请先登录，再进行评论。

类别

在 Help Center 和 File Exchange 中查找有关 Mapping and Localization 的更多信息

标签

产品


版本

R2022a

Community Treasure Hunt

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

Start Hunting!

Translated by