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])

end

[Manikanta Aditya]

"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]

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

Answer 1

[Kaustab Pal]

Hi @Albert Llufriu López,

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.