Introduction

Drop a robot into a building it has never seen, with no GPS, and ask it two things at once: where am I? and what does this place look like? Each question needs the other's answer. To know where it is, the robot needs a map of nearby landmarks. To build that map, it needs to know where it was standing when it saw each landmark.

This is the simultaneous localization and mapping problem, or SLAM. Wheels slip, gyroscopes drift, and every sensor reading is a little bit wrong. A robot that trusted only its wheel odometry would wander off by meters after a short hallway; a robot that trusted only a fixed map would have no map to trust in the first place.

One of the earliest and still most widely taught solutions tracks the robot's pose and every landmark's position together, inside a single, growing statistical estimate that gets sharper every time an old landmark is seen again. That is EKF-SLAM — SLAM built on the extended Kalman filter.

Try It

Below, a small robot (blue) drives forward and turns using noisy motion, while a filter (red) tries to track both the robot's pose and four landmarks (dots) from noisy bearing-and-range observations alone. Uncertainty is drawn as ellipses: big and loose at first, tightening every time a landmark is re-observed.

<p class="hint">The gray dot is the robot's <b>true</b> path and the real landmarks (unknown to the filter); the red dot and trail are the <b>filter's estimate</b>. Ellipses show uncertainty. Every time a landmark comes back into range, its ellipse — and the robot's own — visibly tighten.</p>
<canvas id="scene" width="560" height="320"></canvas>
<div class="status" id="status">Press Step or Run to start driving and mapping.</div>
<div class="btns">
  <button id="step" type="button">Step</button>
  <button id="run" type="button">Run</button>
  <button id="reset" type="button" class="ghost">Reset</button>
</div>

* { box-sizing: border-box; }
body { font-family: system-ui, sans-serif; color: #222; margin: 0; }
.hint { font-size: .9rem; color: #444; margin: 0 0 .6rem; line-height: 1.45; }
canvas { width: 100%; max-width: 560px; height: auto; background: #f4f6f8; border: 1px solid #cdd9e3;
         border-radius: 8px; display: block; }
.status { font-size: .95rem; font-weight: 600; margin: .5rem 0; min-height: 1.4em; color: #1d3557; }
.btns { display: flex; gap: .5rem; flex-wrap: wrap; }
button { font: 600 14px system-ui, sans-serif; padding: .45rem .9rem; border: 1px solid #1d3557;
         background: #1d3557; color: #fff; border-radius: 8px; cursor: pointer; }
button.ghost { background: #fff; color: #1d3557; }
button:disabled { opacity: .5; cursor: default; }

const i18nFmt = (s, v) => s.replace(/%(\w+)%/g, (_, k) => String(v[k] ?? ''));

// EKF-SLAM demo: the robot drives a rough loop past four landmarks it has never
// seen before. A single extended Kalman filter tracks the robot's pose AND all
// landmark positions in one joint state vector, sharing one covariance matrix.

const canvas = document.getElementById('scene');
const ctx = canvas.getContext('2d');
const W = canvas.width, H = canvas.height;
const SCALE = 22;                 // pixels per meter, for drawing only
const ORIGIN = { x: 172, y: H - 30 };
const toPx = (x, y) => ({ px: ORIGIN.x + x * SCALE, py: ORIGIN.y - y * SCALE });

// Ground-truth landmark positions (meters); hidden from the filter until observed
const TRUE_LANDMARKS = [[4.5, 5.5], [0, 10], [-4.5, 5.5], [0, 1]];
const N = TRUE_LANDMARKS.length;
const DIM = 3 + 2 * N;             // state size: 3 for the robot pose (x, y, heading) + 2 per landmark

// Motion and sensor noise standard deviations (meters / radians)
const MOTION_TRANS_STD = 0.03;
const MOTION_ROT_STD = 0.012;
const OBS_RANGE_STD = 0.18;
const OBS_BEARING_STD = 0.035;
const MAX_SENSE_RANGE = 6.5;       // landmarks farther than this are not observed this step

// Small dense linear-algebra helpers (state is tiny, so plain arrays are fine)
function zeros(r, c) { return Array.from({ length: r }, () => new Array(c).fill(0)); }
function matMul(A, B) {
  const r = A.length, k = A[0].length, c = B[0].length;
  const R = zeros(r, c);
  for (let i = 0; i < r; i++) for (let j = 0; j < c; j++) { let s = 0; for (let t = 0; t < k; t++) s += A[i][t] * B[t][j]; R[i][j] = s; }
  return R;
}
function transpose(A) { const r = A.length, c = A[0].length; const R = zeros(c, r); for (let i = 0; i < r; i++) for (let j = 0; j < c; j++) R[j][i] = A[i][j]; return R; }
function matAdd(A, B) { const r = A.length, c = A[0].length; const R = zeros(r, c); for (let i = 0; i < r; i++) for (let j = 0; j < c; j++) R[i][j] = A[i][j] + B[i][j]; return R; }
function matSub(A, B) { const r = A.length, c = A[0].length; const R = zeros(r, c); for (let i = 0; i < r; i++) for (let j = 0; j < c; j++) R[i][j] = A[i][j] - B[i][j]; return R; }
function identity(n) { const R = zeros(n, n); for (let i = 0; i < n; i++) R[i][i] = 1; return R; }
function inv2(A) {
  const det = A[0][0] * A[1][1] - A[0][1] * A[1][0];
  return [[A[1][1] / det, -A[0][1] / det], [-A[1][0] / det, A[0][0] / det]];
}

function gauss(std) {
  const u1 = Math.max(Math.random(), 1e-9), u2 = Math.random();
  return std * Math.sqrt(-2 * Math.log(u1)) * Math.cos(2 * Math.PI * u2);
}

// Mutable simulation state, (re)initialized in reset()
let trueX, trueY, trueTheta, mu, Sigma, seen, trueTrail, estTrail, tick, running;

function reset() {
  trueX = 0; trueY = 0; trueTheta = 0;
  mu = new Array(DIM).fill(0);
  Sigma = zeros(DIM, DIM);
  for (let i = 3; i < DIM; i++) Sigma[i][i] = 1e6;   // unseen landmarks start with near-infinite variance
  seen = new Array(N).fill(false);
  trueTrail = [[0, 0]];
  estTrail = [[0, 0]];
  tick = 0;
  running = false;
  document.getElementById('run').textContent = 'Run';
  setStatus('Press Step or Run to start driving and mapping.');
  draw();
}

// Prediction step: move the true robot with noise, and the filter's mean/covariance without it
function predict(dTrans, dRot) {
  const nTrans = dTrans + gauss(MOTION_TRANS_STD);
  const nRot = dRot + gauss(MOTION_ROT_STD);
  trueTheta += nRot;
  trueX += nTrans * Math.cos(trueTheta);
  trueY += nTrans * Math.sin(trueTheta);

  const theta = mu[2];
  mu[0] += dTrans * Math.cos(theta);
  mu[1] += dTrans * Math.sin(theta);
  mu[2] += dRot;

  const G = identity(DIM);
  G[0][2] = -dTrans * Math.sin(theta);
  G[1][2] = dTrans * Math.cos(theta);

  const Qr = zeros(DIM, DIM);
  Qr[0][0] = MOTION_TRANS_STD * MOTION_TRANS_STD;
  Qr[1][1] = MOTION_TRANS_STD * MOTION_TRANS_STD;
  Qr[2][2] = MOTION_ROT_STD * MOTION_ROT_STD;
  Sigma = matAdd(matMul(matMul(G, Sigma), transpose(G)), Qr);
}

// Update step: fuse a noisy range-bearing sighting of one landmark into the joint state
function observe(idx) {
  const [lx, ly] = TRUE_LANDMARKS[idx];
  const dx = lx - trueX, dy = ly - trueY;
  const trueRange = Math.hypot(dx, dy);
  if (trueRange > MAX_SENSE_RANGE) return false;   // landmark too far away to sense this step
  const trueBearing = Math.atan2(dy, dx) - trueTheta;
  const zRange = trueRange + gauss(OBS_RANGE_STD);
  const zBearing = trueBearing + gauss(OBS_BEARING_STD);

  const li = 3 + 2 * idx;
  if (!seen[idx]) {
    // first sighting just plants the landmark estimate, no correction yet
    mu[li] = mu[0] + zRange * Math.cos(zBearing + mu[2]);
    mu[li + 1] = mu[1] + zRange * Math.sin(zBearing + mu[2]);
    Sigma[li][li] = 4; Sigma[li + 1][li + 1] = 4;
    seen[idx] = true;
    return true;
  }

  const pdx = mu[li] - mu[0], pdy = mu[li + 1] - mu[1];
  const q = pdx * pdx + pdy * pdy;
  const sq = Math.sqrt(q);
  const predRange = sq;
  const predBearing = Math.atan2(pdy, pdx) - mu[2];

  const Hm = zeros(2, DIM);
  Hm[0][0] = -pdx / sq; Hm[0][1] = -pdy / sq; Hm[0][2] = 0;
  Hm[1][0] = pdy / q; Hm[1][1] = -pdx / q; Hm[1][2] = -1;
  Hm[0][li] = pdx / sq; Hm[0][li + 1] = pdy / sq;
  Hm[1][li] = -pdy / q; Hm[1][li + 1] = pdx / q;

  const Qm = [[OBS_RANGE_STD * OBS_RANGE_STD, 0], [0, OBS_BEARING_STD * OBS_BEARING_STD]];
  const S = matAdd(matMul(matMul(Hm, Sigma), transpose(Hm)), Qm);
  const Sinv = inv2(S);
  const K = matMul(matMul(Sigma, transpose(Hm)), Sinv);

  let innovRange = zRange - predRange;
  let innovBearing = zBearing - predBearing;
  while (innovBearing > Math.PI) innovBearing -= 2 * Math.PI;
  while (innovBearing < -Math.PI) innovBearing += 2 * Math.PI;

  for (let i = 0; i < DIM; i++) mu[i] += K[i][0] * innovRange + K[i][1] * innovBearing;
  const KH = matMul(K, Hm);
  Sigma = matMul(matSub(identity(DIM), KH), Sigma);
  return true;
}

// One full simulation tick — drive, sense every visible landmark, redraw
function step() {
  tick++;
  const dTrans = 0.5;
  const dRot = 0.09;               // fixed forward + turn command that traces a rough loop
  predict(dTrans, dRot);
  let sawAny = false;
  for (let i = 0; i < N; i++) if (observe(i)) sawAny = true;
  trueTrail.push([trueX, trueY]);
  estTrail.push([mu[0], mu[1]]);
  if (trueTrail.length > 400) { trueTrail.shift(); estTrail.shift(); }
  const poseErr = Math.hypot(mu[0] - trueX, mu[1] - trueY);
  setStatus(i18nFmt('Step %n% — pose error %err% — landmarks seen %seen%/%total%', { n: tick, err: poseErr.toFixed(2), seen: seen.filter(Boolean).length, total: N }));
  draw();
}

function setStatus(t) { document.getElementById('status').textContent = t; }

// Draw a 1.5-sigma uncertainty ellipse from a 2x2 covariance block
function drawCovEllipse(cx, cy, sxx, syy, sxy, color) {
  const tr = sxx + syy, det = sxx * syy - sxy * sxy;
  const l1 = tr / 2 + Math.sqrt(Math.max(tr * tr / 4 - det, 0));
  const l2 = tr / 2 - Math.sqrt(Math.max(tr * tr / 4 - det, 0));
  const angle = Math.abs(sxy) < 1e-9 ? 0 : Math.atan2(l1 - sxx, sxy);
  const rx = Math.sqrt(Math.max(l1, 0)) * SCALE * 1.5;
  const ry = Math.sqrt(Math.max(l2, 0)) * SCALE * 1.5;
  ctx.save();
  ctx.translate(cx, cy);
  ctx.rotate(angle);
  ctx.beginPath();
  ctx.ellipse(0, 0, Math.min(rx, 220), Math.min(ry, 220), 0, 0, 2 * Math.PI);
  ctx.strokeStyle = color;
  ctx.lineWidth = 1.5;
  ctx.stroke();
  ctx.restore();
}

function drawTrail(trail, color) {
  ctx.beginPath();
  ctx.strokeStyle = color;
  ctx.lineWidth = 1.5;
  trail.forEach(([x, y], i) => {
    const { px, py } = toPx(x, y);
    if (i === 0) ctx.moveTo(px, py); else ctx.lineTo(px, py);
  });
  ctx.stroke();
}

function draw() {
  ctx.clearRect(0, 0, W, H);

  // Draw the real (hidden-from-the-filter) landmark positions
  TRUE_LANDMARKS.forEach(([x, y]) => {
    const { px, py } = toPx(x, y);
    ctx.beginPath();
    ctx.arc(px, py, 4, 0, 2 * Math.PI);
    ctx.fillStyle = '#8892a0';
    ctx.fill();
  });

  drawTrail(trueTrail, '#8892a0');
  drawTrail(estTrail, '#e63946');

  // Draw the filter's landmark estimates and their shrinking ellipses
  for (let i = 0; i < N; i++) {
    if (!seen[i]) continue;
    const li = 3 + 2 * i;
    const { px, py } = toPx(mu[li], mu[li + 1]);
    drawCovEllipse(px, py, Sigma[li][li], Sigma[li + 1][li + 1], Sigma[li][li + 1], '#e63946');
    ctx.beginPath();
    ctx.arc(px, py, 3.5, 0, 2 * Math.PI);
    ctx.fillStyle = '#e63946';
    ctx.fill();
  }

  // Draw the robot's real position
  {
    const { px, py } = toPx(trueX, trueY);
    ctx.beginPath();
    ctx.arc(px, py, 6, 0, 2 * Math.PI);
    ctx.fillStyle = '#1d3557';
    ctx.fill();
  }

  // Draw the filter's estimated pose and its own uncertainty ellipse
  {
    const { px, py } = toPx(mu[0], mu[1]);
    drawCovEllipse(px, py, Sigma[0][0], Sigma[1][1], Sigma[0][1], '#e63946');
    ctx.beginPath();
    ctx.arc(px, py, 5, 0, 2 * Math.PI);
    ctx.fillStyle = '#fff';
    ctx.strokeStyle = '#e63946';
    ctx.lineWidth = 2;
    ctx.fill();
    ctx.stroke();
  }
}

document.getElementById('step').addEventListener('click', step);
document.getElementById('reset').addEventListener('click', reset);
document.getElementById('run').addEventListener('click', () => {
  running = !running;
  document.getElementById('run').textContent = running ? 'Pause' : 'Run';
  if (running) loop();
});
function loop() {
  if (!running) return;
  step();
  if (tick >= 200) { running = false; document.getElementById('run').textContent = 'Run'; return; }
  setTimeout(loop, 90);
}

reset();
Result
Console

Press Step repeatedly, or Run, and watch two things happen together: the robot's own uncertainty ellipse shrinks whenever it re-sees a landmark it already mapped, and that landmark's ellipse shrinks too — because the filter keeps a single joint covariance matrix that links every estimate to every other one. Improve your knowledge of one thing and, through their shared correlations, you improve your knowledge of everything else.

The Real Complexity

EKF-SLAM's elegance is also its cost. The filter's state is one long vector: the robot's pose stacked on top of every landmark's coordinates, x=(xr,yr,θr,x1,y1,…,xn,yn)x = (x_r, y_r, \theta_r, x_1, y_1, \dots, x_n, y_n). Its uncertainty is a single covariance matrix over that whole vector, of size (2n+3)×(2n+3)(2n+3) \times (2n+3) for nn landmarks.

  • Prediction (the robot moves) only touches the robot's own rows and columns, and is cheap.
  • Update (the robot observes a landmark) can, in the worst case, touch a full row and column of the covariance matrix — updating it costs O(n2)O(n^2) time, and simply storing it costs O(n2)O(n^2) memory. Map ten thousand landmarks and the matrix alone needs hundreds of millions of entries.
  • Linearization is an approximation, not a fact. The extended Kalman filter works by linearizing nonlinear motion and sensor models around the current estimate — a first-order Taylor expansion. When the true error is small this tracks reality well, but a bad linearization point (a big turn, a wrong data association) can make the filter converge confidently to the wrong answer, and unlike exact Bayesian inference, there is no automatic recovery.
  • Data association is its own hard problem. Before any of the math above helps, the robot must decide which previously-seen landmark a new observation matches — get that wrong and the joint covariance happily fuses two different landmarks into a false correlation, corrupting the whole map at once.

The quadratic blow-up is precisely why later methods — sparse information filters, particle-filter-based FastSLAM, and pose-graph optimization — exist: to keep SLAM's accuracy while dodging the O(n2)O(n^2) wall that a single dense covariance matrix runs into.

Where It Matters

Estimating a moving, uncertain state from noisy sensors is one of the most common shapes a real engineering problem can take, and EKF-SLAM was the model that showed a map could be part of that state too:

  • Mobile robotics: warehouse and indoor-service robots that use EKF-SLAM or its direct descendants to build a landmark map while navigating it.
  • Underwater and aerial vehicles: where GPS is unavailable or unreliable, EKF-style fusion of sonar, inertial and visual landmarks keeps a vehicle localized.
  • Autonomous driving research: early self-driving stacks used EKF-SLAM-style fusion of lidar landmarks and odometry before graph-based and learned methods took over at scale.
  • Any noisy tracking problem: the same Kalman filter core reappears in aircraft navigation, spacecraft attitude estimation, and even financial time-series filtering — anywhere a hidden state must be inferred from imperfect, sequential measurements. A close cousin, particle-filter localization, swaps the Gaussian assumption for a swarm of weighted samples.

Understand why EKF-SLAM's covariance matters and you've understood the core trade-off behind all state estimation: more correlations tracked means better estimates, at a computational price that grows with what you're willing to remember.

Conclusion

EKF-SLAM answers the chicken-and-egg problem of mapping and localization with a single trick: don't solve them separately, estimate them jointly, and let their shared covariance carry information from every landmark sighting back to the robot's own pose, and back out again to every other landmark.

That joint estimate is also exactly why the method strains at scale — the same matrix that links everything together grows as the square of how much you're tracking. The next time a delivery robot glides confidently down a hallway it has only seen once before, you're watching a covariance matrix quietly do the work of answering "where am I, and what's around me?" at the same time.

Share this article

Pick a channel — or use your device's native share sheet.

Comments

Loading comments...

https://www.kipuhub.com/en/article/ekf-slam/Content licensed under CC BY-NC 4.0.