The project has been scripted in MATLAB because of it's amazing matrix math functions and simply because I had it installed on my laptop.

What I eventually would like to do is develop a SLAM (Simultaneous Localization and Mapping) algorithm to generate maps with only the use of a LiDAR and no other navigation aid (incl. encoders).

This project however only addresses the issue of localization of a robot in a known map - like the arena our robot is going to be in for the Trinity Fire Fighting Robot competition.

The key to the algorithm is calculating best-match scores for every possible linear and angular offsets for the pixelized LiDAR map on a pixelized known map. The best score is then obtained and the x and y indexes of the best score is used as the map offset. The LiDAR map is first rotated to make all lines vertical or horizontal - this leaves only for angular possibilities for the map-matching algorithm which significantly reduces the load on the processor. The rotation of the best match is inverted and the H/V rotation is also inverted to get the robot's position and heading.

For those who wish to use the algorithm in your own LiDAR-based mapping robot - here's my code:

(Hackaday could definitely use MATLAB or Octave syntax highlighting btw)

clear all;
close all;
clc;

% Hardware Specifications
LiDARRange = [20 600]; % [cm] LiDAR Range
LiDARdtheta = 1; % [deg] LiDAR Angular Resolution
LiDARdr = 0.2; % [cm] LiDAR Linear Resolution

map = [];

% Simulation Parameters
run = 1; % run simulation flag
tth = 1; % [cm] Thickness tolerance threshold
dt = 0.1; % [s] simulation time step
dd = 10; % [cm] Discontinuity check threshold
dr = 0.1; % Linearity check range threshold
da = 10; % [deg] Angularity tolerance
mR = 4; % Minimum repetition required to be valid
bounds = [0 244 0 244]; % [cm] Maze bounds (MATLAB axis)
RobotPos = [80 10]; % [cm] Position of Robot (x,y)
RobotHdg = 80; % [deg] Heading of Robot, with 0 deg pointing E, CCW +
LiDARMap = [transpose(0:359), zeros(360,1)+600];
res = 16; % [px] Map resolution

% Define Arena Walls
walls = [  0   0   0 244; % W Boundary
0   0 244   0; % S Boundary
244   0 244 244; % E Boundary
0 244 244 244; % N Boundary
126   0 126  45; % Room 1 W Wall    [M]
126  91 198  91; % Room 1 N Wall
0 103  72 103; % Room 2 N Wall
72  56  72 103; % Room 2 E Wall
46 157  72 157; % Room 3 S Wall
72 157  72 244; % Room 3 E Wall
126 147 126 198; % Room 4 W Wall
126 147 196 147; % Room 4 S Wall    [M]
196 147 196 198; % Room 4 E Wall
126 198 150 198]; % Room 4 N Wall   [M]

% Localization STEP 1 - Pixelize Known map <40x40> [ONE TIME PROCESS]
pixMap_s = zeros(res); % Stored Pixel Map

ds = 244/res;

for w = 1:size(walls,1)
p1 = walls(w,1:2);
p2 = walls(w,3:4);
if p1(1) == p2(1) % V Line
length = p2(2) - p1(2);
for s = 0:round(length/ds);
pixMap_s(1+round(p1(1)/ds),1+round(p1(2)/ds) + s) = 1;
end
else % H Line (There are only H and V Lines on this map)
length = p2(1) - p1(1);
for s = 0:round(length/ds);
pixMap_s(1+round(p1(1)/ds) + s,1+round(p1(2)/ds)) = 1;
end
end
end

while run

% clc;

LiDARMap = [transpose(0:359), zeros(360,1)+600];

% LiDAR Generate Range Map
for theta = 0:359
% Line12 - line from robot to LiDAR range along theta
x1 = RobotPos(1);
y1 = RobotPos(2);
x2 = RobotPos(1) + LiDARRange(2)*cosd(RobotHdg + theta);
y2 = RobotPos(2) + LiDARRange(2)*sind(RobotHdg + theta);

for wall = 1:size(walls,1)
% Line34 - wall
x3 = walls(wall,1);
y3 = walls(wall,2);
x4 = walls(wall,3);
y4 = walls(wall,4);

% Find Intersection point
den = (x1 - x2)*(y3 - y4) - (y1 - y2)*(x3 - x4);
numTerm1 = x1*y2 - y1*x2;
numTerm2 = x3*y4 - y3*x4;
Px = (numTerm1*(x3 - x4) - numTerm2*(x1 - x2))/den;
Py = (numTerm1*(y3 - y4) - numTerm2*(y1 - y2))/den;

% Check if intersection point is between bounds
if ((Px >= x1-tth && Px <= x2+tth) || (Px >= x2-tth && Px <= x1+tth)) && ...
((Py >= y1-tth && Py <= y2+tth) || (Py >= y2-tth && Py <= y1+tth)) && ...
((Px >= x3-tth && Px <= x4+tth) || (Px >= x4-tth && Px...