State Estimation & Odometry

2.1The Odometry Contract

Odometry is dead reckoning: integrate what the sensors say about motion, step by step, starting from wherever you started. No map, no absolute reference — just accumulation. That definition contains its own doom: every step adds a little error, errors compound through the pose chain, and the estimate drifts without bound. Nothing in this phase eliminates drift. The entire craft is making drift slow, smooth, and honestly quantified — and the entire point of Phase 3 (SLAM) and Phase 4 (global localization) is to cancel what remains against a map.

The contract every consumer relies on: odometry must be high-rate (a controller at 50 Hz cannot wait for a 10 Hz camera), smooth (no jumps — jumps belong to loop closures, Phase 3), locally accurate, and shipped with a covariance that admits the drift. Four sensor families supply the raw material, with beautifully complementary failure modes:

Odometry / dead reckoning

Estimating pose by integrating motion measurements from a known start. Relative, drifting, high-rate.

Localization

Estimating pose against a prior map. Absolute, non-drifting, needs the map (Phase 4).

Twist

The velocity object of Phase 1: linear plus angular velocity, an element of se(2) or se(3). Odometry integrates twists into poses with ⊞.

Proprioceptive vs exteroceptive

Sensors that measure the robot itself (encoders, IMU) versus the world (camera, lidar). Only exteroceptive sensing can ever cancel drift.

2.2Wheel Odometry: Rolling, Integrated

For a differential-drive robot — two independently driven wheels, the dominant indoor platform — the encoder-to-motion story is two lines of algebra. Wheel angular rates times radius give rim speeds; their mean moves the robot, their difference turns it:

v = (v_R + v_L) / 2                  forward speed
ω = (v_R − v_L) / b                  yaw rate,  b = wheel separation

per step, the body twist is  u = (v·Δt, 0, ω·Δt) ∈ se(2), and the pose update
is Phase 1's boxplus — exactly the arc the robot physically drove:

    X ← X ⊞ u  =  X · exp(u^)

(driving with constant v and ω traces a circular arc of radius v/ω;
the exp of an se(2) twist IS that arc — the V matrix from Phase 1.3)

Wheel odometry errors come in two species that are fixed in two different places. Systematic errors — wrong wheel radius, wrong wheel separation, unequal tire pressure — bias every step the same way and are calibratable: drive a known square or circle, measure the closure error, correct the parameters (the classic UMBmark procedure; Phase 6 revisits this). Stochastic errors — slip, skid, carpet fibers, debris — are unpredictable per step and can only be modeled, as the process noise Q that widens the covariance each update. Getting Q roughly right matters more than getting it precisely right: too small and the fused estimate ignores better sensors; too large and it jitters after every correction.

2.3The IMU: Gravity, Biases, and t² Drift

An IMU is two instruments in one package. The gyroscope measures angular velocity — genuinely, directly, at hundreds of hertz. The accelerometer does not measure acceleration. It measures specific force: the sum of true acceleration and the force resisting gravity. Sitting still, it reads +9.81 m/s² upward; in free fall it reads zero. Every IMU pipeline begins by writing that fact down correctly, and both sensors add a slowly wandering bias plus white noise:

gyro:   ω̃ = ω + b_g + n_g                     b_g, b_a: biases, random walk
accel:  ã = Rᵀ(a_w − g) + b_a + n_a           ḃ = n_b  (they wander — track them)

naive integration ("strapdown"):

  R ← R · exp((ω̃ − b_g)·Δt)                  attitude: Phase 1 exp again
  v ← v + (R·(ã − b_a) + g)·Δt                velocity: rotate, restore gravity
  p ← p + v·Δt + ½·(R·(ã − b_a) + g)·Δt²      position: integrate once more

error growth: gyro bias → heading error ~ t → position error ~ t², and an
attitude error tilts g (≈9.81 m/s²) into the velocity integral. Minutes? No.
Seconds.

So why tolerate a sensor that diverges in seconds? Because between any two camera frames or lidar scans, the IMU is the only sensor fast enough to know what happened — it bridges the gaps, predicts where features will reappear, de-blurs fast motion, and makes gravity (hence roll and pitch) observable. Fusion is not optional garnish; it is the design. The two integration styles are the fork in the road for every system in 2.7 and 2.8: filter-style (propagate an EKF between corrections — next section) and preintegration (summarize the IMU stream between keyframes as a single relative-motion factor, computed once and cheaply re-linearized as estimates improve — the Forster et al. trick that made optimization-based VIO real-time, and Phase 3’s factor graphs eat these factors directly).

Specific force

What accelerometers measure: true acceleration minus gravity, expressed in the sensor frame. Static reading: +1 g up.

Bias / random walk

A slowly wandering offset added to each measurement. Estimated on-line as part of the state; its noise density comes from an Allan-variance calibration (Phase 6).

Strapdown integration

Direct numerical integration of IMU readings into pose — the equations above. Exact in form, divergent in practice.

Preintegration

Compressing hundreds of IMU samples between keyframes into one relative constraint (ΔR, Δv, Δp) with covariance and bias-Jacobians, on the manifold.

2.4Fusion: The EKF and the Error-State Trick

Phase 1 left the Bayes filter abstract. Make it concrete: restrict the belief to a Gaussian, linearize the models at the current estimate with Jacobians, and you get the extended Kalman filter — the workhorse that fuses the last three sections into one estimate:

predict (each odometry/IMU step):
  x̂⁻ = f(x̂, u)                        propagate the estimate
  P⁻  = F P Fᵀ + G Q Gᵀ                F = ∂f/∂δx,  G = ∂f/∂u

correct (when a slower/absolute measurement arrives):
  K  = P⁻ Hᵀ (H P⁻ Hᵀ + R)⁻¹           H = ∂h/∂δx
  x̂  = x̂⁻ ⊞ K·(z − h(x̂⁻))             innovation, weighted by trust
  P  = (I − K H) P⁻

The subtlety is the δx. Robot states contain rotations, and Phase 1 spent three sections proving you cannot put a Gaussian on a rotation matrix. The error-state EKF (ESKF) is the manifold contract executed inside a filter: keep the nominal state on the manifold (quaternion, SE(3)), define the filter’s Gaussian over the small error δx in the tangent space, propagate the nominal with full nonlinear integration, and fold the estimated error back in with ⊞ after each correction. The error stays near zero — the one place linearization is guaranteed honest — which is why every IMU filter that works, from consumer drones to FAST-LIO2, is an ESKF. Solà’s tutorial (reading list) derives every Jacobian; manif hands them to you as code.

The UKF, in One Honest Paragraph

The unscented Kalman filter replaces Jacobians with deterministic sampling: pick 2n+1 sigma points spread along the covariance axes, push each through the true nonlinear model, refit a Gaussian to the results. Second-order accuracy instead of first, no derivatives to derive, similar cost. Choose it when models are violently nonlinear or Jacobians are error-prone; skip it when an ESKF is already consistent — on mildly nonlinear problems the two agree to within noise, and the ESKF’s structure is easier to introspect. Both remain unimodal Gaussians; when the belief genuinely splits (kidnapped robot, symmetric corridors), Phase 4’s particle filter takes over.

git clone https://github.com/artivis/manif.git
cmake -S manif -B manif/build && sudo cmake --install manif/build

// se2_ekf.cc — dead reckoning vs on-manifold EKF with sparse position fixes.
#include <manif/manif.h>
#include <Eigen/Dense>
#include <iostream>
#include <random>

int main() {
  std::mt19937 rng(7);
  auto gauss = [&](double s) {
    return std::normal_distribution<double>(0.0, s)(rng); };

  const double dt = 0.02;                       // 50 Hz odometry
  const int    N  = 3000;                       // 60 seconds
  const double gyro_bias = 0.005;               // rad/s — unmodeled, systematic

  manif::SE2d X_true = manif::SE2d::Identity(); // truth
  manif::SE2d X_dr   = X_true;                  // dead reckoning only
  manif::SE2d X      = X_true;                  // EKF estimate
  Eigen::Matrix3d P  = Eigen::Matrix3d::Zero();

  const Eigen::Matrix3d Q =                     // per-step twist noise
      Eigen::Vector3d(4e-6, 4e-6, 1e-7).asDiagonal();
  const Eigen::Matrix2d R_fix = Eigen::Matrix2d::Identity() * 0.05 * 0.05;

  manif::SE2d::Jacobian F_x, F_u;               // 3×3, filled by manif
  for (int k = 1; k <= N; ++k) {
    // truth drives a circle: v = 1 m/s, ω = 0.3 rad/s
    manif::SE2Tangentd u_true; u_true << 1.0 * dt, 0.0, 0.3 * dt;
    X_true = X_true + u_true;

    // measured twist = truth + noise + gyro bias
    manif::SE2Tangentd u_meas;
    u_meas << u_true.x() + gauss(2e-3), gauss(2e-3),
              u_true.angle() + gyro_bias * dt + gauss(3e-4);

    X_dr = X_dr + u_meas;                       // dead reckoning

    X = X.plus(u_meas, F_x, F_u);               // EKF predict, Jacobians free
    P = F_x * P * F_x.transpose() + F_u * Q * F_u.transpose();

    if (k % 50 == 0) {                          // 1 Hz position fix (GNSS/UWB)
      Eigen::Vector2d z = X_true.translation()
                        + Eigen::Vector2d(gauss(0.05), gauss(0.05));
      Eigen::Matrix<double, 2, 3> H;            // t(X⊞δ) ≈ t + R·δρ  ⇒  H=[R 0]
      H << X.rotation(), Eigen::Vector2d::Zero();
      const Eigen::Vector2d r = z - X.translation();
      const Eigen::Matrix2d S = H * P * H.transpose() + R_fix;
      const Eigen::Matrix<double, 3, 2> K = P * H.transpose() * S.inverse();
      X = X + manif::SE2Tangentd(K * r);        // correct with ⊞
      P = (Eigen::Matrix3d::Identity() - K * H) * P;
    }
  }
  const auto err = [&](const manif::SE2d& A) {
    return (X_true.inverse() * A).log().coeffs().head<2>().norm(); };
  std::cout << "dead-reckoning error: " << err(X_dr) << " m\n"
            << "EKF error           : " << err(X)    << " m\n";
}

2.5Visual Odometry: Features, Directness, and Scale

A camera measures bearings to texture — no range, no motion, just a dense grid of brightness. Visual odometry (VO) recovers camera motion by watching that texture flow between frames, and every VO system is one of two philosophies about what to compare.

Feature-based (indirect) methods distill each image into a few hundred distinctive points — corners with descriptors (ORB, or Phase 8’s learned SuperPoint) — match them across frames, and do geometry on the matches: the essential matrix from 2D–2D matches for initialization, then PnP (perspective-n-point) against already-triangulated 3D points for tracking. The residual is reprojection error, and minimizing it is — say it with me — the Phase 1 loop:

reprojection residual (feature-based):
  rᵢ = uᵢ − π(T_cw · pᵢ)               π = camera projection (Phase 6 calibrates it)
  epipolar constraint:  u₂ᵀ E u₁ = 0,  E = t^ R    (the 5-point algorithm's target)

photometric residual (direct):
  rᵢ = I₂( π(T · π⁻¹(uᵢ, dᵢ)) ) − I₁(uᵢ)          compare brightness, not landmarks

monocular scale: if (R, t) explains the images, so does (R, s·t) for ANY s —
scale is unobservable. Fixes: stereo baseline, RGB-D, or an IMU (section 2.8).

Direct methods skip features entirely: warp one image toward another through the current motion-and-depth estimate and minimize the brightness difference itself. They exploit the whole image (edges, gradients — not just corners), shine where texture is weak but gradients exist, and run fast; the price is a brutal sensitivity to exposure changes, rolling shutter, and un-modeled photometrics. Semi-direct hybrids (SVO) track sparse points directly and optimize structure like a feature method — the pragmatic middle.

2.6Point Cloud Registration: ICP and NDT

A lidar hands you geometry directly: thousands of 3D points per scan. Two consecutive scans overlap heavily, so the transform aligning scan to scan (or scan to a local map) is the robot’s motion. The classical tool is ICP — iterative closest point — and it earns the "iterative": correspondences are unknown, so guess them from proximity, solve, and repeat with better proximity:

repeat:
  1. associate: for each source point pᵢ (moved by current T),
     find the nearest target point qᵢ                     ← the guessed matches
  2. solve:     T = argmin Σᵢ ‖ T·pᵢ − qᵢ ‖²              ← closed form! (below)
  3. loop until the alignment stops improving

closed form for fixed pairs (Kabsch/Umeyama): with centroids p̄, q̄ and
cross-covariance  W = Σᵢ (qᵢ − q̄)(pᵢ − p̄)ᵀ  =  U S Vᵀ (SVD):

  R = U · diag(1, 1, det(UVᵀ)) · Vᵀ          t = q̄ − R·p̄

point-to-plane variant (what production systems use):
  rᵢ = nᵢᵀ (T·pᵢ − qᵢ)      distance along the target's normal — lets
                             flat surfaces slide into place, converges far faster

NDT (normal distributions transform) trades points for statistics: voxelize the target cloud, fit a little Gaussian (μ, Σ) to the points in each voxel, then optimize the pose so source points fall on high-probability ground — a smooth, differentiable score with a wider convergence basin and no per-point correspondence search. GICP generalizes both, modeling each point as a local Gaussian (plane-to-plane). The engineering tiers: brute-force ICP for learning, kd-tree accelerated point-to-plane for products, NDT when maps are big and initial guesses rough, GICP when accuracy is worth the cost.

// icp.cc — point-to-point ICP: associate by proximity, solve in closed form.
#include <Eigen/Dense>
#include <sophus/se3.hpp>
#include <iostream>
#include <random>
#include <vector>

using V3 = Eigen::Vector3d;

// Kabsch/Umeyama: best rigid transform for KNOWN correspondences P[i] ↔ Q[i]
Sophus::SE3d solve_pairs(const std::vector<V3>& P, const std::vector<V3>& Q) {
  V3 cp = V3::Zero(), cq = V3::Zero();
  for (size_t i = 0; i < P.size(); ++i) { cp += P[i]; cq += Q[i]; }
  cp /= P.size();  cq /= Q.size();
  Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
  for (size_t i = 0; i < P.size(); ++i)
    W += (Q[i] - cq) * (P[i] - cp).transpose();
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d R = svd.matrixU() * svd.matrixV().transpose();
  if (R.determinant() < 0) {                       // guard against reflections
    Eigen::Matrix3d D = Eigen::Matrix3d::Identity(); D(2, 2) = -1.0;
    R = svd.matrixU() * D * svd.matrixV().transpose();
  }
  return Sophus::SE3d(Sophus::SO3d(R), cq - R * cp);
}

int main() {
  std::mt19937 rng(1);
  std::uniform_real_distribution<double> box(-2.0, 2.0);
  std::normal_distribution<double> noise(0.0, 0.01);

  const double yaw_deg = 25.0;                     // experiment: try 60, 90, 120
  const Sophus::SE3d T_true(Sophus::SO3d::rotZ(yaw_deg * M_PI / 180.0),
                            V3(0.6, -0.4, 0.3));
  std::vector<V3> model, scan;
  for (int i = 0; i < 500; ++i) {
    V3 p(box(rng), box(rng), box(rng));
    model.push_back(p);
    scan.push_back(T_true * p + V3(noise(rng), noise(rng), noise(rng)));
  }

  Sophus::SE3d T;                                  // initial guess: identity
  for (int it = 0; it < 30; ++it) {
    std::vector<V3> src, dst;                      // associate: brute-force NN
    double rms = 0.0;
    for (const V3& p : model) {
      const V3 y = T * p;
      double best = 1e18; V3 bq;
      for (const V3& q : scan) {
        const double d = (y - q).squaredNorm();
        if (d < best) { best = d; bq = q; }
      }
      src.push_back(p); dst.push_back(bq); rms += best;
    }
    rms = std::sqrt(rms / model.size());
    T = solve_pairs(src, dst);                     // solve on guessed matches
    std::cout << "iter " << it << "  rms " << rms << "\n";
    if (rms < 0.02) break;
  }
  const auto err = T_true.inverse() * T;
  std::cout << "rotation error   : "
            << err.so3().log().norm() * 180.0 / M_PI << " deg\n"
            << "translation error: " << err.translation().norm() << " m\n";
}

2.7LiDAR-Inertial Odometry: The 2026 Default

Put sections 2.3, 2.4, and 2.6 in one tight loop and you get the architecture that now dominates mobile-robot odometry. The IMU propagates an error-state filter at hundreds of hertz; each arriving lidar scan is first deskewed (a spinning lidar collects points over ~100 ms of motion — each point gets un-warped using the IMU trajectory), then registered point-to-plane against a local map; the registration residuals update the same filter the IMU propagates, biases and all. That "same filter" is the definition of tightly coupled — raw measurements meet raw measurements inside one estimator, instead of two finished pose streams being averaged after the fact (loosely coupled, simpler and strictly weaker in degenerate scenes).

Three systems define the state of the art, all from the same research lineage and all standalone C++:

Why this family displaced the LOAM lineage that preceded it: LOAM extracted sparse edge/plane features from each scan, which threw information away and broke on featureless-but-geometric scenes; FAST-LIO2 registers raw points against the map and lets the filter weigh them. The enabler is the ikd-tree — a kd-tree that supports incremental insertion and deletion, so "the map" is a living structure updated per scan rather than rebuilt. GLIM and the full SLAM systems of Phase 3 pick up exactly here, adding loop closure on top.

Deskew / motion compensation

Undoing the motion the robot made during one lidar sweep, using the IMU trajectory, so the scan looks like a snapshot.

Tightly / loosely coupled

Raw measurements fused in one estimator vs finished pose estimates averaged afterwards. Tight wins in degenerate scenes because information (not conclusions) gets weighed.

ikd-tree

Incremental kd-tree: nearest-neighbor structure supporting insert/delete/re-balance on the fly. FAST-LIO2’s map container.

Iterated update

Re-linearizing the measurement model a few times within one correction step — an EKF borrowing one Gauss-Newton trick from Phase 1.

2.8Visual-Inertial Odometry: Four Systems Worth Knowing

Cameras and IMUs are the great complementary pair: the IMU is blind but fast and metric; the camera is slow and scale-blind but anchors drift to the world. Fusing them makes monocular scale observable (accelerometer units are metres per second squared), makes gravity observable (so roll/pitch never drift), and bridges motion blur. The fusion currency on the optimization side is preintegration: compress the hundreds of IMU samples between two keyframes into a single relative factor (ΔR, Δv, Δp) with covariance and bias Jacobians, computed once on the manifold and cheaply corrected when bias estimates move. Four mature open systems cover the design space:

Reading the table as a decision: ORB-SLAM3 when accuracy on benchmarks matters and compute is available; VINS-Fusion when the rig is imperfect and you want something that initializes itself and forgives miscalibration; OKVIS2 for a serious stereo product; SVO for embedded speed. All four assume Phase 6’s homework — intrinsics, extrinsics, and time offset between camera and IMU — and the difference between "works in the demo" and "works" is usually that homework, not the system choice.

2.9Choosing a Stack, and Watching It Fail

Odometry selection is failure-mode engineering: pick the primary sensor whose degenerate case you will meet least, and a partner that covers it. A decision table compresses this chapter:

2.10Reading List

2.11Interview Questions