SLAM explained: simultaneous localisation and mapping
Why a map needs a position and a position needs a map, and how a robot solves both at once. Three demos build an occupancy grid while the robot drives: with a perfect pose, with dead reckoning alone so the map smears, and with a fix from two tags so it stays sharp. Change the numbers, press Run and watch.
SLAM is what a robot does when it has to draw a map of somewhere it has never been, and work out where it is on that map, both at once, with nothing but its own sensors. It is the reason a robot vacuum can show you a floor plan of your house, the reason a phone can stick a virtual sofa to your real carpet, and part of how a self-driving car knows which lane it is in. On this page a small robot on a 2 metre mat does it, and each demo below is a real program you can change and run.
The letters stand for simultaneous localisation and mapping. Localisation is working out where you are. Mapping is working out what is around you. Doing them at the same time is much harder than doing either one alone, and the next section says why.
The chicken and the egg
Suppose you are handed a map of a building and put down somewhere inside it. You can find yourself: look around, compare what you see with the map, and the place that matches is where you are. That is localisation, and a particle filter does it well.
Now suppose instead you know exactly where you are at every moment, from a tag on the ceiling or a camera watching from above. You can draw the map: every time the range sensor says "something 60 cm that way", you know which point on the floor that is, and you mark it. That is mapping, and an occupancy grid is the usual way to store the answer.
A real robot arriving somewhere new has neither.
to put a reading on the map you need to know where you are
to know where you are you need a map to compare readings with
That is the whole problem in two lines. The way out is that the robot does have something: odometry, its own guess at how it has moved since the last moment. It is not good enough on its own, because the errors add up, which is what dead reckoning is about. But it is good enough for one step. So a SLAM system goes round a loop:
- Predict. Move the pose estimate by what the odometry says the robot just did.
- Map. Put this moment's sensor readings into the map, at that predicted pose.
- Correct. Compare what the sensors see with what the map already says, or with a landmark whose position is known, and pull the pose back.
Step 3 is the one that stops the errors growing. Without it you are dead reckoning with a map attached, and the map goes wrong exactly as fast as the pose does.
A pose is where the robot is and which way it faces: three numbers, x, y and a heading. Every demo on this page keeps it in one function called pose(), and that function is the only thing that changes between them. The steering is deliberately left out of the experiment: the three programs count their turns with heading(), the simulator's own truth, so that all three drive exactly the same route whatever their mapping pose believes. The mat and the mapping code are identical as well, so anything different in the map came from the pose the mapper used.
The mapping half
The map here is an occupancy grid: the floor cut into 5 cm squares, each holding one number saying how sure the robot is that something is in that square. A beam that stops in a square is evidence it is occupied; every square the beam passed through on the way is evidence those are free. The numbers are added, not multiplied, because they are stored as log odds. The occupancy grid mapping guide works through all of that; this page uses it and concentrates on the pose.
In the overhead view of each demo, light blue squares are cells the map says are free, black squares are cells it says are occupied, and anything still the colour of the mat is unknown. The simulator draws the true walls underneath, so you can see where the map is right and where it is not.
The mat is 2 metres square with nothing in it but its own walls and two tags, so a correct map is a clean square outline. That makes a wrong one easy to spot.
A map with a perfect position
First, the map you are aiming for. This robot takes its pose from position() and heading(), which in the simulator are the truth, the way an overhead camera in a lab is the truth. It turns on the spot, drives about 60 cm, turns again, drives again and turns a third time, mapping all the way, and finishes after 49 seconds.
The chart has three lines: known %, how much of the grid the map has an opinion about; pose error cm, how far the pose the mapper used is from the truth; and walls out of place %, the share of black cells that are more than one cell away from a real wall or tag. The program only works out the last two so it can draw them. The map never sees them.
The program
from bugbot import *
import math
connect()
CELL, PAD = 5, 15 # 5 cm cells, 15 cm of margin round the mat
W = int((200 + 2 * PAD) / CELL) # 46 cells across
L_OCC, L_FREE, FAR = 0.85, -0.4, 170
START = (70, 50) # where on the mat the robot begins
TAGS = {11: (70, 195), 12: (150, 195)}
grid = [0.0] * (W * W) # log odds, 0 = unknown
def pose():
# THE ONLY PART THAT CHANGES: the lab's overhead camera
px, py = position()
return START[0] + px, START[1] + py, heading()
def add(x, y, amount):
c, r = int((x + PAD) / CELL), int((y + PAD) / CELL)
if 0 <= c < W and 0 <= r < W:
grid[r * W + c] = max(-8, min(8, grid[r * W + c] + amount))
def update(x, y, h):
# every beam: free along the way, occupied where it stopped
for a, d in scan():
th = math.radians(h + a)
sx, sy = math.sin(th), math.cos(th)
r = 0.0
while r < min(d, FAR) - CELL:
add(x + r * sx, y + r * sy, L_FREE)
r += CELL / 2
if d < FAR:
add(x + d * sx, y + d * sy, L_OCC)
def real(x, y):
# the truth, only for the chart: a mat wall or a tag card
if min(x, y, 200 - x, 200 - y) < CELL:
return True
for tx, ty in TAGS.values():
if abs(x - tx) < 9 and abs(y - ty) < 9:
return True
return False
def show():
free, occ = [], []
for i, l in enumerate(grid):
p = (i % W * CELL - PAD + CELL / 2,
i // W * CELL - PAD + CELL / 2)
if l < -1: free.append(p)
if l > 1: occ.append(p)
draw("free", free, "#9ecae1", "squares", CELL)
draw("occupied", occ, "black", "squares", CELL)
wrong = [p for p in occ if not real(*p)]
plot("known %", 100 * (len(free) + len(occ)) / len(grid))
plot("walls out of place %",
100 * len(wrong) / max(1, len(occ)))
return len(free), len(occ), len(wrong)
ticks = [0]
def step():
x, y, h = pose()
update(x, y, h)
ticks[0] += 1
if ticks[0] % 10 == 0:
show()
tx, ty = position()
plot("pose error cm",
math.hypot(x - START[0] - tx, y - START[1] - ty))
wait(0.1)
def spin():
# one turn on the spot, mapping all the way round
drive(0, 0, 25)
last, turned = heading(), 0.0
while turned < 360:
step()
h = heading()
turned += abs((h - last + 180) % 360 - 180)
last = h
def go(seconds):
drive(45, 0, 0)
for tick in range(int(seconds * 10)):
step()
spin()
go(7)
spin()
go(7)
spin()
stop()
free, occ, wrong = show()
print("free", free, " occupied", occ, " out of place", wrong)
The first turn paints the whole outline of the mat: the fan of 8 beams is only 45 degrees wide, so the robot has to turn to see all round it. The two drives and the later turns add the corners the first spot could not reach, and fill in the floor.
Dead reckoning alone: the map smears
Now change one function. pose() takes the robot's own odometry() instead: where it thinks it is, from adding up the optical flow sensor underneath and the gyro. Nothing else in the program moves, and the robot drives exactly the same path.
The program
from bugbot import *
import math
connect()
CELL, PAD = 5, 15 # 5 cm cells, 15 cm of margin round the mat
W = int((200 + 2 * PAD) / CELL) # 46 cells across
L_OCC, L_FREE, FAR = 0.85, -0.4, 170
START = (70, 50) # where on the mat the robot begins
TAGS = {11: (70, 195), 12: (150, 195)}
grid = [0.0] * (W * W) # log odds, 0 = unknown
def pose():
# THE ONLY PART THAT CHANGES: the robot's own dead reckoning
x, y, h = odometry()
return START[0] + x, START[1] + y, h
def add(x, y, amount):
c, r = int((x + PAD) / CELL), int((y + PAD) / CELL)
if 0 <= c < W and 0 <= r < W:
grid[r * W + c] = max(-8, min(8, grid[r * W + c] + amount))
def update(x, y, h):
for a, d in scan():
th = math.radians(h + a)
sx, sy = math.sin(th), math.cos(th)
r = 0.0
while r < min(d, FAR) - CELL:
add(x + r * sx, y + r * sy, L_FREE)
r += CELL / 2
if d < FAR:
add(x + d * sx, y + d * sy, L_OCC)
def real(x, y):
if min(x, y, 200 - x, 200 - y) < CELL:
return True
for tx, ty in TAGS.values():
if abs(x - tx) < 9 and abs(y - ty) < 9:
return True
return False
def show():
free, occ = [], []
for i, l in enumerate(grid):
p = (i % W * CELL - PAD + CELL / 2,
i // W * CELL - PAD + CELL / 2)
if l < -1: free.append(p)
if l > 1: occ.append(p)
draw("free", free, "#9ecae1", "squares", CELL)
draw("occupied", occ, "black", "squares", CELL)
wrong = [p for p in occ if not real(*p)]
plot("known %", 100 * (len(free) + len(occ)) / len(grid))
plot("walls out of place %",
100 * len(wrong) / max(1, len(occ)))
return len(free), len(occ), len(wrong)
ticks = [0]
def step():
x, y, h = pose()
update(x, y, h)
ticks[0] += 1
if ticks[0] % 10 == 0:
show()
tx, ty = position()
plot("pose error cm",
math.hypot(x - START[0] - tx, y - START[1] - ty))
plot("heading error deg", (h - heading() + 180) % 360 - 180)
wait(0.1)
def spin():
drive(0, 0, 25)
last, turned = heading(), 0.0
while turned < 360:
step()
h = heading()
turned += abs((h - last + 180) % 360 - 180)
last = h
def go(seconds):
drive(45, 0, 0)
for tick in range(int(seconds * 10)):
step()
spin()
go(7)
spin()
go(7)
spin()
stop()
free, occ, wrong = show()
print("free", free, " occupied", occ, " out of place", wrong)
print("odometry says", odometry())
print("truth is ", position(), round(heading(), 1))
Look at the mat rather than the chart. The first turn's outline is nearly right, because the robot has hardly moved yet and its estimate is still good. By the last turn the estimate is 10.2 cm out and the heading 9.1 degrees out, and the walls painted from there land somewhere else.
Three things go wrong, and all of them are measurable.
- The mat's left and right walls come out four cells thick instead of three, because the later turns drew them a cell further out than the first turn did.
- The far wall, which the robot only gets close to at the end, is drawn 5 cm past where the mat really stops.
- Nine black cells sit where there is nothing at all. Eight of them are in one line at x = 192.5, running up the last 40 cm of the right-hand wall: the wall is bent inwards by 7.5 cm, which is what a heading error looks like on a map.
The heading is the part that hurts. A position error of 10 cm moves every reading by 10 cm. A heading error turns the whole fan, so it moves a reading by an amount that grows with how far away the reading is.
This is why mapping and localisation cannot be separated. The map is drawn from the pose, so an error in the pose is written straight into the map, and once it is written the map is wrong for ever. Worse, if the robot then tried to localise against that map, it would be comparing its readings with its own mistakes.
A fix from a tag: the map stays sharp
Now put step 3 back. There are two AprilTags on this mat, at (70, 195) and (150, 195), both on the far wall and 80 cm apart, and the robot knows where they are. Whenever the camera can read both at once, the program works out the pose from them and resets the odometry to it.
Each tag the camera reads comes back as an id, a position across the picture in pixels, and a distance. The pixel position gives a bearing, the angle from the robot's nose:
bearing = atan((cx - 160) / 92.4)
160 is the middle of the 320 pixel wide picture and 92.4 is the camera's focal length in pixels, which comes from its 120 degree view. Lesson Intrinsics works out where that number comes from.
One tag gives a range and a bearing: two numbers, and the pose has three. Two tags give four, which is enough. The trick this program uses is that the line from one tag to the other is a fixed thing on the mat, and the robot can also work out that line from its own two readings. Comparing the two directions gives the heading, and once the heading is known either tag gives the position.
The program
from bugbot import *
import math
connect()
CELL, PAD = 5, 15 # 5 cm cells, 15 cm of margin round the mat
W = int((200 + 2 * PAD) / CELL) # 46 cells across
L_OCC, L_FREE, FAR = 0.85, -0.4, 170
START = (70, 50) # where on the mat the robot begins
TAGS = {11: (70, 195), 12: (150, 195)}
CAM_F = 92.4 # the camera's focal length, in pixels
grid = [0.0] * (W * W) # log odds, 0 = unknown
fixes = [0]
set_cv("apriltag")
def pose():
# THE ONLY PART THAT CHANGES: dead reckoning, corrected by the tags
seen = {}
for tag, cx, cy, dist in apriltags():
if tag in TAGS:
seen[tag] = (math.atan((cx - 160) / CAM_F), dist)
if len(seen) == 2:
(t1, (b1, d1)), (t2, (b2, d2)) = list(seen.items())
# the line from tag 1 to tag 2, as the robot sees it
ax = d2 * math.sin(b2) - d1 * math.sin(b1)
ay = d2 * math.cos(b2) - d1 * math.cos(b1)
# and as it really lies on the mat
dx = TAGS[t2][0] - TAGS[t1][0]
dy = TAGS[t2][1] - TAGS[t1][1]
# the angle between the two is the heading
h = math.degrees(math.atan2(dx, dy)
- math.atan2(ax, ay)) % 360
th = math.radians(h) + b1
reset_odometry(TAGS[t1][0] - d1 * math.sin(th) - START[0],
TAGS[t1][1] - d1 * math.cos(th) - START[1], h)
fixes[0] += 1
x, y, h = odometry()
return START[0] + x, START[1] + y, h
def add(x, y, amount):
c, r = int((x + PAD) / CELL), int((y + PAD) / CELL)
if 0 <= c < W and 0 <= r < W:
grid[r * W + c] = max(-8, min(8, grid[r * W + c] + amount))
def update(x, y, h):
for a, d in scan():
th = math.radians(h + a)
sx, sy = math.sin(th), math.cos(th)
r = 0.0
while r < min(d, FAR) - CELL:
add(x + r * sx, y + r * sy, L_FREE)
r += CELL / 2
if d < FAR:
add(x + d * sx, y + d * sy, L_OCC)
def real(x, y):
if min(x, y, 200 - x, 200 - y) < CELL:
return True
for tx, ty in TAGS.values():
if abs(x - tx) < 9 and abs(y - ty) < 9:
return True
return False
def show():
free, occ = [], []
for i, l in enumerate(grid):
p = (i % W * CELL - PAD + CELL / 2,
i // W * CELL - PAD + CELL / 2)
if l < -1: free.append(p)
if l > 1: occ.append(p)
draw("free", free, "#9ecae1", "squares", CELL)
draw("occupied", occ, "black", "squares", CELL)
wrong = [p for p in occ if not real(*p)]
plot("known %", 100 * (len(free) + len(occ)) / len(grid))
plot("walls out of place %",
100 * len(wrong) / max(1, len(occ)))
return len(free), len(occ), len(wrong)
ticks = [0]
def step():
x, y, h = pose()
update(x, y, h)
ticks[0] += 1
if ticks[0] % 10 == 0:
show()
tx, ty = position()
plot("pose error cm",
math.hypot(x - START[0] - tx, y - START[1] - ty))
plot("heading error deg", (h - heading() + 180) % 360 - 180)
wait(0.1)
def spin():
drive(0, 0, 25)
last, turned = heading(), 0.0
while turned < 360:
step()
h = heading()
turned += abs((h - last + 180) % 360 - 180)
last = h
def go(seconds):
drive(45, 0, 0)
for tick in range(int(seconds * 10)):
step()
spin()
go(7)
spin()
go(7)
spin()
stop()
free, occ, wrong = show()
print("free", free, " occupied", occ, " out of place", wrong)
print("fixes taken", fixes[0])
For the first 14 seconds nothing happens. The robot is still at its starting spot, and from there the further tag is 166 cm away, past the camera's 150 cm range, so only one tag is ever in view and the program keeps dead reckoning. The error curve for those first 14 seconds is identical to the one in the last demo.
Then the robot drives up the mat, both tags come into range together, and the pose error falls off a cliff. After that it is a sawtooth. The camera sees 120 degrees and the two tags can be 90 degrees apart once the robot is close to them, so for much of each turn only one of them is in view: the fix fired on 154 of the run's 486 ticks. Between fixes the error creeps up, and each fix knocks it back down. It never passes 1.4 cm, against 10.3 cm without the fix, and the heading stays inside 2.8 degrees.
That is enough. Every one of the 287 black cells is on a real wall or a tag, the walls are three cells thick as they were with a perfect pose, and the far wall is back where the mat really stops.
Try taking the tags away, by changing if len(seen) == 2: to if len(seen) == 3: so the fix never fires. The program becomes the second demo again.
What real SLAM does instead
The tags on this mat are a cheat, and a useful one: a tag is a tiny map, put there in advance, with a position somebody measured. Warehouses and film studios do work this way, and so does an indoor drone flying over a floor of markers. But a robot vacuum in a house nobody has surveyed does not have them. It has to build its landmarks out of what it finds, and that is where the real methods start.
Scan matching. The robot takes a scan, and slides and turns it against the map it already has until the two line up best. The shift it needed is the correction to the pose. This is the workhorse of 2D SLAM, and the reason grid SLAM works at all indoors, where walls and corners give a scan plenty to lock on to. In a long featureless corridor it has almost nothing to lock on to, and the robot slides along the corridor in its own map without noticing.
Loop closure. The robot drives round a building and comes back to a room it has already mapped. Its pose has drifted, so the new scan lands 40 cm from the old one. Recognising that this is the same place is loop closure, and it is the single most valuable piece of information SLAM ever gets, because it ties two far-apart moments together. Getting it wrong is also the worst thing that can happen: a map folded onto the wrong room is very hard to recover from.
Pose graph optimisation. Rather than fixing the pose now and living with whatever was already written into the map, modern SLAM keeps the whole path as a chain of poses with the measured links between them, adds the loop closure as one more link, and then moves every pose at once so that all the links fit as well as possible. The map is redrawn afterwards from the corrected path. This is what "graph SLAM" means, and it is why a vacuum's floor plan can suddenly snap straight after it finds its way back to the kitchen.
Filters. The older family keeps a running estimate with its uncertainty instead of the whole history: EKF-SLAM holds the robot's pose and every landmark in one big Kalman filter, and FastSLAM gives each particle of a particle filter its own map. They use less memory and cannot go back and fix the past.
Visual and lidar SLAM. The sensor changes the details, not the loop. A spinning lidar gives a 360 degree scan and suits scan matching. A camera gives thousands of tiny features to track, which is ORB-SLAM and the visual-inertial systems in phones and headsets. A depth camera gives both. All of them predict, map, and correct.
Where this is taught
- Where am I? and Monte Carlo localisation: finding yourself when you already have a map.
- A map of cells, The inverse sensor model and Log odds: what a grid map is and how a reading changes it.
- Building a grid and Project: map the mat: mapping while the robot drives.
- Write your own odometry and A fix from a landmark: the predict step and the correct step, on their own.
- A tag as a fix and Intrinsics: where a tag's range and bearing come from.
- Drift and correction: the same idea in the Robot club module, with no maths.
Questions
What is SLAM in robotics?
Simultaneous localisation and mapping: a robot building a map of an unknown place while working out its own position on that map, using only its own sensors. It matters because a robot arriving somewhere new has neither a map nor a position, and each one is normally needed to get the other.
Why is SLAM hard?
Because the two halves depend on each other. Putting a reading on the map needs a pose, and working out a pose needs a map to compare the reading with. The robot has to do both from a starting guess that is always a little wrong, and every error it makes gets written into the map, where it then corrupts the next position estimate.
How does SLAM work, step by step?
Predict the new pose from odometry. Add this moment's sensor readings to the map at that pose. Correct the pose by comparing the readings with the map, or with a landmark whose position is known. Repeat a few times a second. The third step is what stops the error growing, and it is the step this page's second demo leaves out.
What is the difference between SLAM and localisation?
Localisation assumes you already have a map and only asks where you are on it. SLAM has no map to start with, so it has to make one and locate itself on it at the same time. A robot vacuum does SLAM on its first run round a house and plain localisation on every run after that, using the map it kept.
What is odometry and why is it not enough?
Odometry is the robot measuring its own movement, usually from wheel encoders, an optical flow sensor or a gyro, and adding it up. Each measurement is a little wrong and nothing ever takes the errors back out, so the estimate drifts further from the truth the longer it runs. On this page it reached 10.2 cm and 9.1 degrees in 49 seconds, which was enough to bend one wall 7.5 cm inwards and draw the far one past the edge of the mat.
What is loop closure in SLAM?
Recognising that the place the robot is now is a place it has already mapped, after driving a loop. It gives the system a link between two moments that are far apart in time, which lets it correct all the poses in between. It is the most valuable measurement SLAM gets, and also the most dangerous one to get wrong.
What is a pose in SLAM?
Where the robot is and which way it faces. On a flat floor that is three numbers: x, y and a heading. A flying robot needs six: three for position and three for orientation. The whole of SLAM is about keeping those numbers right while the map is being drawn from them.
What sensors does SLAM need?
Something that measures range or sees features, and something that measures movement. Common pairings are a spinning lidar with wheel odometry, a depth camera with an IMU, or a plain camera with an IMU, which is visual-inertial SLAM. The robot on this page uses an 8 beam depth sensor, an optical flow sensor, a gyro and a camera that reads tags.
What is the difference between grid SLAM and landmark SLAM?
Grid SLAM stores the world as an occupancy grid of squares and corrects the pose by matching whole scans against it. Landmark SLAM stores a list of distinct things whose positions it is also estimating, such as tags, corners or visual features, and corrects the pose from the range and bearing to them. Grids are better for planning a path; landmarks are cheaper and suit cameras.
Can you write SLAM in Python?
Yes, and the demos on this page are about a hundred lines each, including the chart. A full system for a real building is a much bigger job, which is why most robots use an existing one such as GMapping, Cartographer or ORB-SLAM through ROS rather than writing their own.
Is SLAM on the A level syllabus?
No. None of the GCSE or A level computer science specifications mention SLAM or robot mapping. The parts it is built from are on them: two dimensional arrays, iteration and functions from the programming content, and graphs and weighted edges from the data structures content. The trigonometry and the probability are A level Maths. It makes a strong A level project or personal statement piece for anyone applying to study robotics.
Learn it step by step
These lessons build the same ideas one at a time, each with tasks the simulator marks.
- 2.5 Drift and correction Sensing, Robot club
- U3.2 Write your own odometry Odometry and drift, University
- U3.5 A fix from a landmark Odometry and drift, University
- U7.1 Where am I? Localisation, University
- U7.6 Monte Carlo localisation Localisation, University
- U8.1 A map of cells Mapping, University
- U8.2 The inverse sensor model Mapping, University
- U8.3 Log odds Mapping, University
- U8.4 Building a grid Mapping, University
- U8.5 Ray casting the map Mapping, University
- U8.6 Frontiers Mapping, University
- U8.7 Project: map the mat Mapping, University
- U11.2 Intrinsics and calibration Vision, University
- U11.4 A tag as a fix Vision, University