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)
Show older comments
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
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.
Answers (1)
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
For more reference, you can refer to the documentation: https://www.mathworks.com/help/nav/ref/montecarlolocalization-system-object.html#mw_4a356fb0-0d51-467b-a154-ead10b6ff64a:~:text=A%20true%20value,is%20not%20available.
Hope this solves your issue.
0 Comments
See Also
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!