Mapping an unknown space while tracking your own position inside it
SLAM, simultaneous localization and mapping, is the chicken-and-egg problem at the heart of every autonomous robot: to know where you are, you need a map; to build a map, you need to know where you are. The robot never gets ground truth, only noisy sensor readings, and has to fuse them into a single best guess of both at once.
Drive the robot with W, S, A, D, or the arrow keys. The left panel is ground truth: what actually exists, which the robot never sees. The right panel is the robot's belief, the map and position it has built purely from its own noisy sensors, shown with an uncertainty circle around its estimated pose.
Worth trying: turn every sensor off and drive a lap to see dead reckoning alone drift away from the truth; raise the LiDAR noise (σ) and watch the mapped walls thicken into fuzz; or drift in a straight line along a wall and watch the fused estimate snap back on track.
To build a map you need to know where you are; to know where you are you need a map. SLAM, simultaneous localization and mapping, solves both at once by keeping a running belief: a best-guess pose plus a best-guess map, each refined using the other. The left view is reality; the right view is everything the robot is allowed to know. The entire field exists because those two views disagree, and every technique below is a way of pulling them back together.
turn every sensor off, drive a lap, and compare the two views. That gap is the problem statement.
A 2D LiDAR spins and reports one number per angle: how far until the beam hits something. Here each of the 180 beams is a ray/segment intersection test: solve p + t·d = a + s·(b−a) for the nearest wall, plus Gaussian noise on the range. The key intuition: a scan is a fingerprint of a place. Corners and wall junctions make the fingerprint distinctive; long featureless corridors make scans ambiguous, which is exactly where scan matching struggles (a real effect called the corridor problem).
raise LiDAR σ to 4 and watch walls in the belief map thicken into fuzz, since each cell is hit by samples scattered around the true surface.
Odometry integrates wheel motion: "I moved 2 cm, turned 0.5°, repeat." Each step's error is tiny, but integration means errors never cancel, they compound. Worse, the killer term is heading: a small angle error θ becomes a position error of distance × θ, so position error grows faster than linearly with travel. This sim adds noise proportional to motion plus a small systematic turn bias, which is why the red path doesn't just wander. It curls, like walking blindfolded with one leg slightly stronger.
drive a straight line along a wall with drift at 3.0: the red path bends away while the green stays true. Standing still adds zero drift.
A motion model is the set of moves a vehicle is physically allowed to make. The drone is holonomic: it can translate in any direction at any moment. Position and heading are independent, so it can strafe sideways along a wall while facing it. The car is non-holonomic: wheels forbid sideways motion, so heading change comes bundled with rolling, the bicycle model dθ = (v/L)·tan(δ) with wheelbase L and steering angle δ. A car can still reach any pose, but only via maneuvers (parallel parking exists because of this constraint). The diff-drive point robot sits between them: no sideways motion, but it can rotate in place.
Constraints matter to SLAM twice. First, they shape odometry error: a car physically cannot have drifted sideways, so a good filter shouldn't entertain lateral error its encoders can't produce. Second, they shape which trajectories exist at all: any planner for a car must respect its minimum turning radius.
switch to the car and notice you can't spin in place to look around. You must roll to turn. Then take the drone and strafe sideways with A/D.
The map is a grid where each cell holds the odds it's occupied. Each beam is evidence: cells the beam passed through are probably free, the cell it stopped at is probably occupied. We store log-odds so fusing evidence is just addition: L += +0.9 on a hit, L += −0.35 along the ray (traced with Bresenham's line algorithm), clamped so the map can still change its mind. One noisy scan proves nothing; a hundred overlapping scans vote a crisp wall into existence. Mid-gray cells are honest ignorance, cells never observed.
park the robot and wait: repeated scans sharpen nearby walls as the votes accumulate.
ICP (iterative closest point) treats the current scan like a drawing on tracing paper laid over a previous reference scan, then slides and rotates it until the drawings line up. Each iteration: pair every point with its nearest reference point, then solve the best rigid fit in closed form. The rotation is just atan2(Σ cross, Σ dot) of the centered pairs. Re-pair, re-fit, repeat until it converges. Whatever shift was needed to align the scans is precisely the odometry error, so we apply it to the pose. It needs a decent initial guess (odometry provides one) and distinctive geometry to grip onto.
enable scan matching and drive the same loop: the blue estimate hugs the green truth, and the keyframe counter ticks as references are saved.
Sensor fusion is a weighted argument between sources that disagree. The weight is the Kalman gain: K = P / (P + R), where P is how uncertain you are and R is how noisy the sensor is. Hopelessly lost (P ≫ R)? K → 1: take the measurement. Confident with a junk sensor (P ≪ R)? K → 0: ignore it. Between measurements, motion grows P; each measurement shrinks it by P ← (1−K)·P. The circle around the robot in the belief view is exactly this P. Watch it breathe: inflating as you drive, snapping tight on every fix. Each sensor constrains only what it observes: GPS pins position, the compass pins heading, ICP pins both locally.
enable only GPS and drive: the circle inflates between the 1 Hz fixes and contracts at each yellow ×.
GPS is the opposite personality of odometry: each fix is coarse (meters of noise) but the error is bounded. It never compounds. Odometry is smooth but drifts forever. Fusion gets both virtues: GPS anchors the long term, odometry smooths the short term. Two honest limitations are simulated here: fixes arrive slowly (1 Hz), and GPS says nothing about which way you're facing, so with GPS alone, position stays bounded while heading still drifts, and the map smears rotationally. That's why the IMU compass exists as a separate toggle: absolute heading is a different observation than absolute position. (Indoors, real robots swap GPS for UWB beacons, same math.)
enable GPS + compass but not ICP: the map stays roughly anchored yet walls are coarser than with scan matching. Then add ICP and compare.
Everything above corrects locally: each fix only looks at the recent past. Drive a long loop and tiny residual errors still accumulate, so when you return to the start, the map's two ends don't meet. Loop closure is recognizing "I've been here before" (e.g. ICP against an old keyframe succeeds), which adds a constraint between now and long ago. A pose graph then treats every keyframe pose as a node, every odometry/ICP/loop constraint as a spring, and relaxes the whole network at once with Gauss-Newton over all poses, redistributing the accumulated error along the entire trajectory. Watching a sheared map snap straight is the most satisfying moment in SLAM, and it's the natural next phase of this project: the keyframes here already store everything a pose graph needs.