SLAM & Navigation

The landmark is slightly to the left of the robot's heading. If the robot were facing directly at the landmark, φ would be zero.

The range innovation is +0.3m (landmark is farther than expected), and the bearing innovation is -0.09 rad (landmark is more to the right than expected). These residuals will drive the EKF update to shift the state estimate.

The predicted measurement h(x) depends on the robot pose (x_r, y_r, θ_r) and the observed landmark's position (x₂, y₂). It does not depend on any other landmark's position. So H has nonzero entries only in columns 1-3 (pose) and columns 6-7 (landmark 2). The other 4 columns are all zeros.

This sparse structure is critical: it means each observation only directly updates the robot pose and the observed landmark. However, because the covariance Σ has off-diagonal correlations, the correction indirectly propagates to all other landmarks through the Kalman gain K = ΣH^TS^-1.

The negative sign makes sense: increasing x_r (moving the robot right) decreases Δx when the landmark is to the right, which decreases the range. Note that ∂r/∂x_j = +Δx/√q — same magnitude, opposite sign.

javascript
function observationModel(xr, yr, thr, xj, yj) {
  const dx = xj - xr;
  const dy = yj - yr;
  const range = Math.sqrt(dx * dx + dy * dy);
  const bearing = Math.atan2(dy, dx) - thr;
  return [range, bearing];
}

d² = 1.0 means the innovation is exactly 1 standard deviation. For a χ² distribution with 1 DOF, this corresponds to the 68th percentile — quite plausible. The typical gate threshold at 95% confidence is d² < 3.84.

Even though the bearing innovation (-0.09 rad) looks small, its contribution (0.81) is larger than the range innovation's contribution (0.36) because bearing has much lower variance (0.01 vs 0.25). Mahalanobis distance respects this — small innovations in precise dimensions count more.

Candidate A: d² = 1.17 < 5.99 — passes the gate. This is a statistically plausible match.

Candidate B: d² = 8.5 > 5.99 — fails the gate. At 95% confidence, a true match would produce d² this large only 1.4% of the time. Reject it.

If no candidates pass the gate, the observation is treated as a new landmark and initialized. Here, A passes, so we match to A.

A single wrong association is catastrophic in EKF-SLAM. When you match observation z to the wrong landmark j, the update step "corrects" the state as if j generated z — this shifts both the robot pose and landmark j to explain a measurement that came from somewhere else entirely. Worse, these false corrections propagate through the covariance to every other landmark.

This is why robust data association methods like Joint Compatibility Branch and Bound (JCBB) exist — they evaluate the consistency of the entire set of associations jointly, not each one independently.

javascript
function mahalanobis(nu0, nu1, s00, s11) {
  return (nu0 * nu0) / s00 + (nu1 * nu1) / s11;
}

Line 6 is the bug. It computes the Euclidean distance (nu[0]² + nu[1]²) instead of the Mahalanobis distance (nu^T S^-1 nu). Without scaling by S^-1, the function treats range and bearing innovations as equally weighted — but range is in meters and bearing is in radians. A 0.5m range error and a 0.5 rad bearing error are very different levels of surprise!

The fix: const d2 = nu[0]*nu[0]/S[0][0] + nu[1]*nu[1]/S[1][1]; (assuming diagonal S).

10 meters of drift after 100 steps! For a warehouse robot moving 0.5 m/step, that's only 50 meters of travel. Without loop closure or absolute positioning, the map is unusable after a few minutes.

Random walk drift grows as √k, not k — much slower than systematic drift. This is why wheel encoders with zero-mean noise accumulate error slower than sensors with a constant bias. But even √k diverges eventually; you always need loop closure or external corrections for long-duration navigation.

Forward drift per side: 0.02 × 100 = 2.0 m. But on a square loop, the drift directions are:

On a closed loop with constant forward bias, the drift cancels if the robot turns enough. This is a special case. In practice, heading drift (which rotates the direction of subsequent drift) prevents this cancellation and is usually the dominant error source. A 1° heading bias causes the entire trajectory to curve, producing meters of closure error.

Each step k has accumulated heading error δθ = 0.01k rad. The lateral displacement at step k is approximately 0.5 × sin(0.01k) ≈ 0.5 × 0.01k = 0.005k (small angle).

Using the exact sin: ∑ 0.5 sin(0.01k) for k=1..100 ≈ 24.5 m. The heading drift causes massive lateral error — nearly 50% of the total distance traveled (50m). This is why gyroscope calibration and heading correction are critical for odometry-based navigation.

The correct approach distributes the error across all poses proportionally to their uncertainty. In EKF-SLAM, this happens automatically through the covariance — the Kalman gain allocates corrections based on how uncertain each state element is. In graph SLAM (Chapter 4), this is done by pose graph optimization, which minimizes the total weighted error across all constraints.

Correcting only the final pose would create a discontinuity — landmarks mapped near the end would suddenly jump. Distributing smoothly preserves the local consistency of the map.

Each constraint between i and j adds entries at (i,i), (i,j), (j,i), (j,j):

Every entry in the 3×3 matrix gets at least one contribution. Ω[0,0] gets contributions from three sources (odo01 + loop02 + prior), Ω[1,1] gets two (odo01 + odo12), etc. All 9 entries are nonzero because the loop closure connects x₀ to x₂, filling in the corners. Without the loop closure, Ω[0,2] and Ω[2,0] would be zero (tridiagonal structure).

The full matrix:
Ω = [[12, -1, -1], [-1, 2, -1], [-1, -1, 2]]
The diagonal entries are the sum of all information connected to that pose. The off-diagonal entries are the negated information of the direct constraint. This structure makes Ω diagonally dominant — a property that ensures numerical stability when solving the linear system.

Solving Ωμ = ξ for [[12,-1,-1],[-1,2,-1],[-1,-1,2]]μ = [10,1,3] via Cramer's rule:

The exact solution is μ = [1.4, 2.0, 2.6]. The strong prior (ω=10) on x₀ pulls it toward ξ[0]/Ω[0,0] = 10/12 ≈ 0.83, but the odometry and loop closure constraints push it higher. The full solution balances all constraints simultaneously.

Only 0.3% of the matrix is nonzero! This extreme sparsity is what makes graph SLAM tractable at large scale. Sparse Cholesky solvers exploit this structure to solve the optimization in O(N) time for chain graphs, compared to O(N³) for dense systems.

Each loop closure adds exactly one edge to the graph, which means 2 additional nonzero off-diagonal blocks in Ω. Five loop closures add only 10 blocks to the existing 2998 — the matrix stays 99.7% sparse.

However, these long-range connections create fill-in during Cholesky factorization. A good variable ordering (like COLAMD or nested dissection) minimizes this fill-in, keeping the solve fast. This is why graph SLAM implementations spend significant effort on variable ordering — it determines whether the solve takes milliseconds or seconds.

javascript
function buildOmega(n, edges) {
  const O = new Array(n * n).fill(0);
  for (const e of edges) {
    O[e.i * n + e.i] += e.omega;
    O[e.j * n + e.j] += e.omega;
    O[e.i * n + e.j] -= e.omega;
    O[e.j * n + e.i] -= e.omega;
  }
  return O;
}

E = [t]_×R has 6 underlying parameters (3 rotation + 3 translation). But scaling E by any constant c gives cE, which satisfies the same epipolar constraints (x'^T(cE)x = 0 iff x'^TEx = 0). This removes 1 DOF, leaving 5.

Algebraically, E has two equal nonzero singular values and one zero singular value. This imposes constraints on the 9 entries: the essential matrix manifold is a 5-dimensional surface in ℝ⁹.

This is the 5-point algorithm (Nister, 2004). Each correspondence provides one linear equation in the entries of E. Five correspondences give 5 equations for 5 unknowns (after fixing the scale). In practice, the 5-point algorithm uses polynomial solvers and can return up to 10 solutions, which are disambiguated using additional correspondences or geometric checks.

The result is -0.09, not zero. This means the correspondence either has noise or doesn't perfectly satisfy the epipolar constraint for this E. In RANSAC, you'd compare |x'^TEx| against a threshold (typically derived from the Sampson distance). A residual of 0.09 is moderate — whether it's an inlier depends on the noise level and the threshold you set.

Normalized coordinates: x = K^-1p. Substituting into x'^TEx = 0: (K'^-1p')^T E (K^-1p) = p'^T K'^-T E K^-1 p = 0. So F = K'^-TEK^-1, or equivalently E = K'^TFK.

When both cameras have the same intrinsics (K' = K), this simplifies to E = K^TFK. The essential matrix removes the camera-specific calibration, leaving only the pure geometric relationship. This is why calibrated cameras should use E (5 DOF, more constrained) while uncalibrated cameras must use F (7 DOF).

The monocular system gives the direction [0.8, 0, 0.6] but cannot determine the scale factor 2.5. The stereo baseline (or IMU, or known object size) provides the missing scale. Without it, all distances are relative — the VO trajectory could be scaled by any positive constant and still be consistent with the observations.

Each iteration: sample 5 random correspondences, compute E, count inliers (points with |x'^TEx| < threshold). After 146 iterations, there's a 99% chance at least one sample was all-inlier. With 500 correspondences, each iteration takes microseconds — 146 iterations is fast.

With 80% inliers instead of 50%, the iteration count drops from 146 to just 12 — a 12× reduction. This is why feature matching quality matters more than RANSAC sophistication. Better descriptors (SuperPoint, DISK) with better matchers (SuperGlue, LightGlue) can push inlier rates above 90%, making RANSAC nearly instantaneous.

1177 vs 146 — the 8-point algorithm needs 8× more RANSAC iterations at 50% inliers. Each additional sample point in the minimal set multiplies the iteration count because it's exponentially harder to draw a clean sample. This is why the 5-point algorithm (despite being algebraically harder) is preferred in practice: fewer samples = fewer RANSAC iterations = faster runtime.

javascript
function ransacIterations(inlierRatio, sampleSize, confidence) {
  const w = Math.pow(inlierRatio, sampleSize);
  return Math.ceil(Math.log(1 - confidence) / Math.log(1 - w));
}

Line 7 is the bug. It counts inliers against the sample (the 5 sampled points) instead of against ALL points. The model is fitted to exactly the sample, so it will always have 5 "inliers" from the sample — even if the model is complete nonsense for the rest of the data. The fix: countInliers(model, points). RANSAC's power comes from evaluating each hypothesis against the full dataset, not just the minimal sample used to generate it.

The inlier threshold is a critical hyperparameter. Too large (τ = 10 pixels): gross outliers sneak in and the "best" model may fit a mixture of good and bad correspondences. Too small (τ = 0.1 pixels): even well-matched points get rejected due to noise, making the effective inlier ratio drop and requiring many more iterations.

A good rule of thumb: set τ = 2-3× the measurement noise standard deviation. For feature matching with sub-pixel accuracy, this is typically 1-3 pixels. For the Sampson distance (a better geometric error than x'^TEx), τ² follows a χ²(1) distribution — set τ² = 3.84 for 95% confidence.

The system is over-determined: 11 factors constraining 8 variables (each with multiple DOF). This redundancy is what makes the estimate robust — if one factor is noisy, the others compensate.

When you eliminate x₂, all its neighbors must become connected (this is the fill-in caused by elimination). x₂'s neighbors are x₁ and x₃, so a new edge x₁—x₃ is created. The chain becomes x₀-x₁-x₃-x₄ with the extra x₁—x₃ edge.

For a chain graph, the optimal elimination order is from one end to the other (e.g., x₀, x₁, x₂, x₃, x₄). This produces zero fill-in because each node has only one remaining neighbor when eliminated. Eliminating from the middle causes fill-in, making the computation more expensive.

Eliminating the central node first is the worst possible choice — it creates maximum fill-in, turning the sparse star into a dense clique. The optimal order eliminates leaves first (zero fill-in per leaf), then the center last. This principle generalizes: always eliminate nodes with few connections first. Variable ordering algorithms like COLAMD automate this heuristic.

The system is over-determined by 300 DOF — there are 1.75× more constraints than unknowns. This redundancy is what makes the least-squares solution robust to noise. Each additional observation factor adds 2 constraints, further improving the estimate. An under-determined system (fewer constraints than unknowns) would have infinitely many solutions — you'd need more observations or stronger priors.

The correct order: Prior → Odometry → Data association → Observation factors → Optimize

First, anchor the graph with a prior on the initial pose. Then add the odometry constraint for the new motion. Before adding observations, perform data association to determine which landmark each measurement corresponds to (or if it's a new landmark). Then add the observation factors with the correct landmark IDs. Finally, optimize the entire graph to get the MAP estimate.

Reference values: p=0.5 → l=0, p=0.73 → l=1, p=0.88 → l=2, p=0.95 → l=3, p=0.99 → l=4.6.

One occupied and one free observation leaves the cell more likely occupied (p ≈ 0.79) because the occupied sensor (p=0.9, contributing +2.20 log-odds) was more confident than the free sensor (p=0.3, contributing -0.85 log-odds). The occupied observation "won" by a margin of 1.35 log-odds. To bring the cell back to 50/50, you'd need about 2.6 more free observations at p=0.3 each.

With a relatively weak sensor (70% accuracy), you need 4 consistent observations to reach 95% confidence. A stronger sensor (p=0.9) would need only 2 observations (2 × 2.197 = 4.394, p = 0.988). This is why LiDAR-based mapping (very confident per-ray) converges faster than camera-based mapping (less certain per pixel).

4 MB is manageable. But at 1cm resolution: 10,000 × 10,000 = 100M cells = 100 MB. And in 3D at 1cm: 10,000³ = 10¹² cells = 1 TB — impossible. This is why 3D maps use octrees (only allocate cells near surfaces), point clouds, or signed distance fields instead of dense grids.

javascript
function logOddsUpdate(currentLogOdds, sensorProb) {
  const update = Math.log(sensorProb / (1 - sensorProb));
  const newL = currentLogOdds + update;
  const newP = 1 / (1 + Math.exp(-newL));
  return [newL, newP];
}

In a static world, unclamped log-odds are fine — cells converge to their true occupancy. But in the real world, objects move. A chair observed 50 times at location A has l=50×2.2 = 110 (p≈1). When the chair moves, clearing it requires 50+ free observations to bring l back below 0. With clamping at l=5, you need at most ~6 free observations to flip the cell — the map responds to changes in seconds instead of minutes.

In 2D, each pose has 3 DOF and each landmark 2 DOF, so the total DOF is 300×3 + 600×2 = 900 + 1200 = 2100. The information matrix is 2100×2100 but extremely sparse — easily solvable in real-time with modern sparse solvers like GTSAM's iSAM2.

These per-step errors seem tiny, but they accumulate. After 100 steps (50m): σ_pos ≈ 0.005 × √100 = 0.05m (random walk). The heading error is more dangerous: σ_θ ≈ 0.00436 × √100 = 0.0436 rad ≈ 2.5°. A 2.5° heading error over the next 50m of travel causes ~2.2m of lateral drift. This is why heading estimation is the bottleneck in odometry-based SLAM.

About 5 landmarks per scan is typical for indoor environments. This means your factor graph gets ~5 observation factors per keyframe. Over 300 keyframes, that's ~1500 observation factors — providing dense constraints that make the optimization well-conditioned.

The occupancy grid dominates memory by 50×. The factor graph itself is tiny. This is why SLAM research focuses on efficient map representations — the map is the bottleneck, not the pose graph. At 1cm resolution, the grid would be 25 MB; at 0.5cm, 100 MB.

Loop closure verification requires multiple independent checks because a false positive is catastrophic (Ch. 2):

(1) Place recognition: Scan descriptors (scan context, BoW) identify candidate locations that "look like" the current scan. Fast but noisy — gives candidates, not confirmations.

(2) Geometric verification: ICP or scan matching computes the precise relative transform. If the alignment error is high, reject.

(3) Mahalanobis gate: Is the relative transform consistent with what the current pose graph predicts? If the graph says we should be 10m away but the closure says 0m, something is wrong.

(4) Consistency check: Does adding this constraint make the overall graph residual increase dramatically? If so, either this is a false closure or the graph has other errors that need resolution first.

The correct order: Motion prediction → Feature extraction → Data association → Loop closure detection → Graph optimization → Update occupancy grid

(1) Predict the new pose from odometry. (2) Extract features/landmarks from the new scan. (3) Match features to known landmarks (data association). (4) Check if this location has been visited before (loop closure — uses the current scan and map). (5) Add all new factors and optimize the graph. (6) With the optimized poses, ray-trace through the scans to update the occupancy grid.

The map update comes LAST because it depends on the optimized poses — updating the map with pre-optimization poses would project scans to wrong locations.

It fits with 19.5ms of headroom — about 20% margin. In practice, graph optimization can spike during loop closures (full relinearization instead of incremental), and feature extraction varies with scene complexity. The 20% margin is tight but workable. If it overruns, common optimizations: (1) run loop closure in a separate thread, (2) use iSAM2 for incremental optimization, (3) update the map at a lower rate (every 5th frame).