Unknown Landmarks: an Introduction to SLAM
← Part 2 recap: full pose (x, y, θ) from known landmarks
Parts 1 and 2 assumed the landmarks' positions were known ahead of time. Real robots usually don't get that luxury — they have to build the map while figuring out where they are in it. That's SLAM (Simultaneous Localization and Mapping), and it turns out to need exactly one new idea layered onto the machinery you already have.
What's new: the map is unknown too
Recap
Before: unknown = robot pose (x, y, θ), landmarks L given. Now: unknown = robot pose and every landmark position. With M unknown landmarks, the state grows from 3 numbers to:
Every observation still produces the same range and bearing residuals from parts 1–2. The only change is that a residual can now depend on landmark unknowns as well as pose unknowns — so each observation contributes a Jacobian row with a couple of nonzero entries for the pose, a couple more for whichever landmark it's looking at, and zeros everywhere else. Bigger state, same update rule.
Gauge freedom: the whole map can float
A new kind of ambiguity
Drag the sliders below. They shift and rotate the entire configuration — robot and both landmarks — together, rigidly. Watch the cost: it stays at zero no matter where you drag it to, because every relative range and bearing is exactly preserved. There is no "correct" absolute position and orientation to converge to — only a correct shape, floating freely in the plane.
Anchoring: pinning the map down
The fix
Same sliders, same scene — but now the diamond landmark on the left is treated as a fixed, known anchor. Its true position is pinned on the canvas. As soon as you shift or rotate the scene, the anchor landmark visibly leaves its pin, and a second cost term — how far the anchor has drifted from where it's supposed to be — climbs away from zero. That term is what turns "any shift is equally good" back into an ordinary bowl with one correct answer.
Warm-up: locating one landmark from a known pose
Landmark-only unknowns
The robot visits two known positions (shown as gray triangles) and takes one noisy range+bearing reading of the same unknown landmark from each. A single reading alone would place the landmark exactly at the tip of its own measurement — no fitting required. But with two independent noisy readings disagreeing slightly, there's no single point that satisfies both exactly, so we're back to minimizing a sum of squared residuals — the same machinery as parts 1–2, just with a landmark's (Lx, Ly) as the only unknowns instead of the robot's pose.
Click the map to set a starting guess for the landmark.
Joint estimation: pose and map together
Gauss-Newton, now sparse
Two known anchors (diamonds) plus two unknown landmarks (circles) — the robot observes all four. Step through Gauss-Newton and watch the pose and both unknown landmark estimates converge together. To the right, the block structure of the Hessian JᵀJ for this exact problem: the pose interacts with every landmark it observes (dense blocks along the top row and left column), but two different landmarks never interact with each other directly — only through the shared pose. That empty block is sparsity, and it's the reason real SLAM systems with thousands of landmarks are solvable at all.
Click the map to set the robot's starting position.
JᵀJ block structure (7×7: pose, landmark 1, landmark 2)
Levenberg-Marquardt on the joint problem
Same damping trick, bigger state
λI just grows to match. Try a starting guess that's badly wrong about both the robot's pose and the landmarks at once.Same anchored scenario, but start from a rough, hand-wavy guess for everything — robot pose and both unknown landmarks placed more or less arbitrarily. Plain Gauss-Newton can thrash around before settling; adaptive λ tames the early, unreliable steps and speeds up once the estimate is closer to correct.
Click the map to set the robot's starting position.
Playground: build the map and localize at once
Put it all together
Two known anchors, three unknown landmarks, one robot pose — all estimated jointly. Race gradient descent, Gauss-Newton, and Levenberg-Marquardt (full Newton is skipped here on purpose: with landmarks in the mix too, hand-deriving every curvature term gets unwieldy fast — exactly why Gauss-Newton, not Newton, is the real-world default for SLAM-shaped problems).
Click the map to set the shared starting position.
| Method | Iters | Pos. err | Map err | Status |
|---|
Cheat sheet
Recap
| Concept | What changed from part 2 |
|---|---|
| Unknown | Pose (x, y, θ) → pose plus every unknown landmark's (Lx, Ly) |
| New ambiguity | Gauge freedom — the whole map can slide and spin with zero cost change |
| Fix | Anchor at least one landmark (or the first pose) as a known reference |
| Jacobian rows | Nonzero for the pose and whichever landmark that observation is looking at; zero elsewhere |
| Hessian shape | No longer dense — landmark-landmark blocks are zero unless a robot pose observed both; this sparsity is what makes large SLAM problems tractable |
| Update rules | Identical in form to parts 1–2, just over a bigger state vector |
This is genuinely how SLAM back-ends work, just at a scale of thousands of landmarks and thousands of poses instead of a handful: a giant, extremely sparse least-squares problem, solved with Gauss-Newton or Levenberg-Marquardt, using sparse linear algebra to exploit exactly the block structure shown above. From here, two directions extend this further — chaining many robot poses together over time with odometry between them (a full pose graph, rather than a single anchored moment), and moving from a flat 2D plane to full 3D rotations.