I don’t know why my Monte Carlo Localization simulation doesn’t work, I’m using the Monte Carlo Localization toolbox
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])
endI 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 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 monte carlo localization, particle filter, lidar MATLAB Answers — New Questions