I don't know why my Monte Carlo Localization simulation doesn't work, I'm using the Monte Carlo Localization toolbox

5 views (last 30 days)
I think it's a relatively simple tool and I can't find my error. Here's the code:
timeStep = 0.1; % Time step (s)
lin_vel = 1; % Linear velocity (m/s)
resolution = 1; % Map resolution (m)
initial_position = [2 10 90]; % [m m degrees]
robot_pose = initial_position;
lidar_res = 0.1; % Lidar resolution (m)
lidar_range = 5.5;
num_beams = 173;
angles = linspace(-pi/2,pi/2,num_beams);
map_matrix = [1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1;
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1;
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1;
1 0 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 1;
1 0 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 1;
1 0 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 1;
1 0 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 1;
1 0 0 0 1 1 1 1 1 0 0 0 0 0 0 0 0 0 1;
1 0 0 0 1 1 1 1 1 0 0 0 0 0 0 0 0 0 1;
1 0 0 0 0 0 0 0 0 0 1 1 1 1 1 0 0 0 1;
1 0 0 0 0 0 0 0 0 0 1 1 1 1 1 0 0 0 1;
1 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 0 1;
1 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 0 1;
1 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 0 1;
1 0 0 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 1;
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1;
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1];
% Binary occupancy map matrix
map = binaryOccupancyMap(map_matrix,resolution);
% In order to follow the red path, the robot has to perform the following
% movements:
% - Up for 5 seconds
% - Right for 8 seconds
% - Down for 6 seconds
% - Right for 6 seconds
% - Down for 7 seconds
% - Left for 7 seconds
% - Up for 5 seconds
% - Left for 7 seconds
% - Up for 2 seconds
% Total simulation time: 53 seconds, we'll simulate for 1 whole minute
totalTime = 60; % Total simulation time (s)
numParticles = 5000; % Initial number of particles
mcl = monteCarloLocalization; % Initialize MCL object
mcl.UseLidarScan = true;
mcl.GlobalLocalization = true;
mcl.ParticleLimits = [500 numParticles];
mcl.UpdateThresholds = [0.2 0.2 0.2];
mcl.ResamplingInterval = 1;
motionModel = odometryMotionModel; % Initialise motion model
motionModel.Noise = [0.05 0.05 0.05 0.05]; % Add motion error
mcl.MotionModel = motionModel;
sensorModel = likelihoodFieldSensorModel;
sensorModel.Map = map;
sensorModel.SensorLimits = [0 lidar_range];
sensorModel.NumBeams = num_beams;
mcl.SensorModel = sensorModel;
% Simulation loop
for t=0:timeStep:totalTime
if t < 5
robot_pose(2) = robot_pose(2) + lin_vel * timeStep;
robot_pose(3) = 90;
elseif (t>=5)&&(t<13)
robot_pose(1) = robot_pose(1) + lin_vel * timeStep;
robot_pose(3) = 0;
elseif (t>=13)&&(t<19)
robot_pose(2) = robot_pose(2) - lin_vel * timeStep;
robot_pose(3) = 270;
elseif (t>=19)&&(t<25)
robot_pose(1) = robot_pose(1) + lin_vel * timeStep;
robot_pose(3) = 0;
elseif (t>=25)&&(t<32)
robot_pose(2) = robot_pose(2) - lin_vel * timeStep;
robot_pose(3) = 270;
elseif (t>=32)&&(t<39)
robot_pose(1) = robot_pose(1) - lin_vel * timeStep;
robot_pose(3) = 180;
elseif (t>=39)&&(t<44)
robot_pose(2) = robot_pose(2) + lin_vel * timeStep;
robot_pose(3) = 90;
elseif (t>=44)&&(t<51)
robot_pose(1) = robot_pose(1) - lin_vel * timeStep;
robot_pose(3) = 180;
elseif (t>=51)&&(t<53)
robot_pose(2) = robot_pose(2) + lin_vel * timeStep;
robot_pose(3) = 90;
else
% Robot stopped
end
% rayIntersection func. works with radians
robot_pose_radians = robot_pose;
robot_pose_radians(1,3) = robot_pose_radians(1,3)*(pi/180);
intersectionPts = rayIntersection(map,robot_pose_radians,angles,lidar_range);
ranges = sqrt((intersectionPts(:,1)-robot_pose(1)).^2+(intersectionPts(:,2)-robot_pose(2)).^2);
scan = lidarScan(ranges,angles);
[isUpdated,estimatedPose,covariance] = mcl(robot_pose,scan);
particles = getParticles(mcl);
% Update particles plot every 2 "seconds"
if mod(t,2) == 0
particles_plot = particles;
end
figure(1)
show(map)
hold on
scatter(particles_plot(:,1),particles_plot(:,2),'b.')
hold on
scatter(estimatedPose(:,1),estimatedPose(:,2),'ro')
hold on
scatter(intersectionPts(:,1),intersectionPts(:,2),'r.')
hold on
scatter(robot_pose(:,1),robot_pose(:,2),'bo')
hold off
xlim([0 19])
ylim([0 17])
end
  2 Comments
Manikanta Aditya
Manikanta Aditya on 19 May 2024
"I can't find my error."
Without an error message or a description of what’s going wrong, it’s hard to pinpoint the exact issue.
Albert Llufriu López
Albert Llufriu López on 19 May 2024
It's not a matlab error with an error code, it simply doesn't localize the robot pose propperly, it doesn't converge to where it should

Sign in to comment.

Answers (1)

Kaustab Pal
Kaustab Pal on 18 Jul 2024
From what I gather, your objective is to use MonteCarloLocalization to estimate your robot's pose. In the code snippet you shared, I noticed that you have set the GlobalLocalization property of the MonteCarloLocalization object to true.
When GlobalLocalization is enabled, the Monte Carlo Localization (MCL) algorithm initially distributes particles uniformly across the entire map. This approach is beneficial when the robot's initial pose is completely unknown or highly uncertain. However, it can lead to a poor pose estimate because many particles will be placed far from the robot's actual position. Consequently, most particles will have low weights (probabilities), making it challenging for the algorithm to converge quickly to the correct pose.
Given that you already know the initial pose of the robot, you can achieve a more accurate estimate by setting GlobalLocalization to false. Instead, configure the InitialPose and InitialCovariance properties.
Below is a slightly modified version of your code that should provide more accurate pose estimates:
timeStep = 0.1; % Time step (s)
lin_vel = 1; % Linear velocity (m/s)
resolution = 1; % Map resolution (m)
initial_position = [2 10 90]; % [m m degrees]
robot_pose = initial_position;
robot_pose_radians = robot_pose;
robot_pose_radians(1,3) = robot_pose_radians(1,3)*(pi/180);
lidar_res = 0.1; % Lidar resolution (m)
lidar_range = 5.5;
num_beams = 173;
angles = linspace(-pi/2,pi/2,num_beams);
map_matrix = [1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1;
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1;
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1;
1 0 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 1;
1 0 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 1;
1 0 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 1;
1 0 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 1;
1 0 0 0 1 1 1 1 1 0 0 0 0 0 0 0 0 0 1;
1 0 0 0 1 1 1 1 1 0 0 0 0 0 0 0 0 0 1;
1 0 0 0 0 0 0 0 0 0 1 1 1 1 1 0 0 0 1;
1 0 0 0 0 0 0 0 0 0 1 1 1 1 1 0 0 0 1;
1 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 0 1;
1 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 0 1;
1 0 0 1 1 1 1 1 0 0 1 1 1 1 1 0 0 0 1;
1 0 0 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 1;
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1;
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1];
% Binary occupancy map matrix
map = binaryOccupancyMap(map_matrix,resolution);
% In order to follow the red path, the robot has to perform the following
% movements:
% - Up for 5 seconds
% - Right for 8 seconds
% - Down for 6 seconds
% - Right for 6 seconds
% - Down for 7 seconds
% - Left for 7 seconds
% - Up for 5 seconds
% - Left for 7 seconds
% - Up for 2 seconds
% Total simulation time: 53 seconds, we'll simulate for 1 whole minute
totalTime = 60; % Total simulation time (s)
numParticles = 5000; % Initial number of particles
mcl = monteCarloLocalization; % Initialize MCL object
mcl.UseLidarScan = true;
mcl.GlobalLocalization = false;
mcl.InitialPose = robot_pose_radians;
mcl.InitialCovariance = [0.1, 0.1, 0.1;]
mcl.ParticleLimits = [500 numParticles];
mcl.UpdateThresholds = [0.2 0.2 0.2];
mcl.ResamplingInterval = 1;
motionModel = odometryMotionModel; % Initialise motion model
motionModel.Noise = [0.05 0.05 0.05 0.05]; % Add motion error
mcl.MotionModel = motionModel;
sensorModel = likelihoodFieldSensorModel;
sensorModel.Map = map;
sensorModel.SensorLimits = [0 lidar_range];
sensorModel.NumBeams = num_beams;
mcl.SensorModel = sensorModel;
% Simulation loop
for t=0:timeStep:totalTime
if t < 5
robot_pose(2) = robot_pose(2) + lin_vel * timeStep;
robot_pose(3) = 90;
elseif (t>=5)&&(t<13)
robot_pose(1) = robot_pose(1) + lin_vel * timeStep;
robot_pose(3) = 0;
elseif (t>=13)&&(t<19)
robot_pose(2) = robot_pose(2) - lin_vel * timeStep;
robot_pose(3) = 270;
elseif (t>=19)&&(t<25)
robot_pose(1) = robot_pose(1) + lin_vel * timeStep;
robot_pose(3) = 0;
elseif (t>=25)&&(t<32)
robot_pose(2) = robot_pose(2) - lin_vel * timeStep;
robot_pose(3) = 270;
elseif (t>=32)&&(t<39)
robot_pose(1) = robot_pose(1) - lin_vel * timeStep;
robot_pose(3) = 180;
elseif (t>=39)&&(t<44)
robot_pose(2) = robot_pose(2) + lin_vel * timeStep;
robot_pose(3) = 90;
elseif (t>=44)&&(t<51)
robot_pose(1) = robot_pose(1) - lin_vel * timeStep;
robot_pose(3) = 180;
elseif (t>=51)&&(t<53)
robot_pose(2) = robot_pose(2) + lin_vel * timeStep;
robot_pose(3) = 90;
else
% Robot stopped
end
% rayIntersection func. works with radians
robot_pose_radians = robot_pose;
robot_pose_radians(1,3) = robot_pose_radians(1,3)*(pi/180);
intersectionPts = rayIntersection(map,robot_pose_radians,angles,lidar_range);
ranges = sqrt((intersectionPts(:,1)-robot_pose_radians(1)).^2+(intersectionPts(:,2)-robot_pose_radians(2)).^2);
scan = lidarScan(ranges,angles);
[isUpdated,estimatedPose,covariance] = mcl(robot_pose_radians,scan);
particles = getParticles(mcl);
% Update particles plot every 2 "seconds"
if mod(t,2) == 0
particles_plot = particles;
end
figure(1)
show(map)
hold on
scatter(particles_plot(:,1),particles_plot(:,2),'b.')
hold on
scatter(estimatedPose(:,1),estimatedPose(:,2),'ro')
hold on
scatter(intersectionPts(:,1),intersectionPts(:,2),'r.')
hold on
scatter(robot_pose(:,1),robot_pose(:,2),'bo')
hold off
xlim([0 19])
ylim([0 17])
drawnow
end
Hope this solves your issue.

Products


Release

R2023b

Community Treasure Hunt

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

Start Hunting!