Outdoor & Large-Scale Navigation

7.1What Breaks Outside

The indoor stack of Phases 2–5 rests on assumptions so convenient they were never written down. Outdoors cancels them in bulk:

7.2GNSS: From Satellites to Centimetres

A GNSS receiver measures pseudoranges — signal travel time × c to each visible satellite — and solves for position plus its own clock error. Standalone accuracy is metres, and the error budget explains why: ionospheric and tropospheric delays, satellite clock and orbit errors, and multipath (signals bouncing off buildings before arriving). The route to centimetres is differential: a nearby base station at a surveyed position observes the same errors, and broadcasting corrections cancels everything the two receivers share:

pseudorange:  ρᵢ = ‖ sᵢ − r ‖ + c·δt_rx + iono + tropo + multipath + ε

RTK (real-time kinematic): use the carrier PHASE (~19 cm wavelength, measured
to ~1%) instead of the code. Double-difference across two receivers and two
satellites cancels both clocks and the shared atmosphere — leaving geometry
plus an INTEGER number of unknown wavelengths (the ambiguity):

  fix   = ambiguities resolved to integers  → ~1–2 cm  (trust it)
  float = ambiguities still real-valued     → dm-level  (suspect it)

corrections arrive over internet (NTRIP casters) or radio; RTKLIB is the
standalone open-source engine for all of it.

Operationally, the receiver’s fix/float/single status is a first-class input to fusion, not a footnote: an RTK-fixed position deserves centimetre-grade covariance; a float solution deserves decimetres and suspicion; and in an urban canyon, a "fixed" solution that jumps three metres between epochs is multipath wearing a confident face — which is what the gating in 7.4 exists to catch.

7.3Frames for the Planet: WGS84 → ECEF → ENU

GNSS speaks latitude, longitude, height on the WGS84 ellipsoid; every controller and costmap in this book speaks metres in a flat local frame. The conversion is two exact steps, and doing them properly (not with degree-to-metre folklore) is a roadmap item in its own right:

step 1 — geodetic (φ, λ, h) → ECEF (Earth-Centered Earth-Fixed):
  a = 6378137.0,   f = 1/298.257223563,   e² = f(2−f)          (WGS84)
  N(φ) = a / √(1 − e²·sin²φ)                 prime-vertical radius
  X = (N + h)·cosφ·cosλ
  Y = (N + h)·cosφ·sinλ
  Z = (N·(1 − e²) + h)·sinφ

step 2 — ECEF → ENU about a reference point r₀ (the "map origin"):
  d = p_ecef − r₀_ecef
  [E]   [ −sinλ         cosλ        0    ]
  [N] = [ −sinφ·cosλ   −sinφ·sinλ   cosφ ] · d
  [U]   [  cosφ·cosλ    cosφ·sinλ   sinφ ]

pick ONE reference r₀ per site, convert everything, and never do arithmetic
in degrees. The local tangent plane is honest to ~10 km; beyond that,
re-anchor or account for curvature.

// wgs84.cc — geodetic → ECEF → ENU, exactly; then the spherical shortcut.
#include <cmath>
#include <iostream>

struct V3 { double x, y, z; };
const double A = 6378137.0, F = 1.0 / 298.257223563, E2 = F * (2.0 - F);
const double D2R = M_PI / 180.0;

V3 geodetic_to_ecef(double lat, double lon, double h) {
  const double s = std::sin(lat * D2R), c = std::cos(lat * D2R);
  const double N = A / std::sqrt(1.0 - E2 * s * s);
  return { (N + h) * c * std::cos(lon * D2R),
           (N + h) * c * std::sin(lon * D2R),
           (N * (1.0 - E2) + h) * s };
}
V3 ecef_to_enu(V3 p, double lat0, double lon0, double h0) {
  const V3 r = geodetic_to_ecef(lat0, lon0, h0);
  const double dx = p.x - r.x, dy = p.y - r.y, dz = p.z - r.z;
  const double sφ = std::sin(lat0 * D2R), cφ = std::cos(lat0 * D2R);
  const double sλ = std::sin(lon0 * D2R), cλ = std::cos(lon0 * D2R);
  return { -sλ * dx + cλ * dy,
           -sφ * cλ * dx - sφ * sλ * dy + cφ * dz,
            cφ * cλ * dx + cφ * sλ * dy + sφ * dz };
}

int main() {
  const double lat0 = 12.9716, lon0 = 77.5946, h0 = 920.0;   // site origin

  // a point 0.001° north and 0.001° east of the origin
  V3 enu = ecef_to_enu(geodetic_to_ecef(lat0 + 0.001, lon0 + 0.001, h0),
                       lat0, lon0, h0);
  std::cout << "exact ENU      : E = " << enu.x << "  N = " << enu.y
            << "  U = " << enu.z << "\n";

  // the folklore shortcut: metres-per-degree on a 6371 km sphere
  const double R = 6371000.0;
  const double n_sph = 0.001 * D2R * R;
  const double e_sph = 0.001 * D2R * R * std::cos(lat0 * D2R);
  std::cout << "spherical guess: E = " << e_sph << "  N = " << n_sph << "\n"
            << "shortcut error : "
            << std::hypot(enu.x - e_sph, enu.y - n_sph) * 100.0
            << " cm over ~150 m\n";
}

7.4Fusing GNSS Into the Graph

GNSS is the sensor Phase 3 wished for: an absolute measurement, immune to drift, arriving at 1–10 Hz. The modern fusion architecture is graph-based: LIO (Phase 2) provides high-rate relative factors between poses; each GNSS epoch adds a unary factor pinning one pose toward its ENU measurement, weighted by the receiver’s reported (and status-scaled) covariance; the Phase 1 optimizer reconciles them continuously. This is exactly LIO-SAM’s shape, and the tightly-coupled end of the spectrum (GVINS-style) goes deeper — raw pseudorange/carrier factors instead of finished fixes — buying accuracy in partial-sky conditions where fewer than four satellites still contribute information.

graph:  X₀ ── odom ── X₁ ── odom ── X₂ ── odom ── X₃ ⋯      (LIO, Phase 2)
                       │                  │
                    GNSS z₁            GNSS z₃                (unary anchors)

r_gnss(Xᵢ) = pos(Xᵢ) − z_enu,     Ω from receiver covariance × status
                                   (fix: cm² — float: dm² — single: m²)
gate before insert: chi-squared against current marginal — a 3 m jump
against 5 cm of plausible drift is multipath, not information.

Two integration details decide whether this works in practice. Yaw alignment: ENU and the LIO map frame differ by an unknown rotation about gravity; position fixes make it observable only once the robot has moved (the trajectory’s shape in both frames is compared), so initialization needs a few metres of travel — dual-antenna heading receivers shortcut this by measuring yaw directly. The lever arm: the antenna is not at the body origin; T_body_antenna is a Phase 6 extrinsic, and forgetting it costs exactly that offset, rotated by every turn — visible as position "breathing" during rotation.

7.5Elevation Mapping & Traversability (Grid Map)

Outdoor ground is neither free nor occupied — it is terrain, and the question becomes "can these wheels handle this patch?". The workhorse representation is the 2.5D elevation map: a grid where each cell stores height (and its variance) — vastly cheaper than full 3D, expressive enough for slopes, curbs, and ruts. ETH’s Grid Map is the standard C++ library: multi-layer float grids (elevation, variance, traversability, anything), a moving robot-centric window with efficient shifting, iterators, and the robot-centric probabilistic elevation mapping stack built on it, which fuses range measurements with pose uncertainty so the map degrades honestly as odometry does. From elevation, traversability is derived per cell from three ingredients:

slope(x,y)     = ‖∇h‖ via finite differences over the cell's neighborhood
step(x,y)      = max height discontinuity to any neighbor   (curbs!)
roughness(x,y) = local height variance after plane removal

traversable  ⇔  slope < slope_max  ∧  step < step_max  ∧  roughness < r_max
   (limits are per-platform: what a tracked robot shrugs at kills a caster)
→ the result feeds Phase 5's planners as a cost layer, exactly like a costmap

7.6The World Moves: Dynamic Objects & Map Maintenance

A parking lot mapped on Monday is a lie by Tuesday. Dynamic objects poison maps twice: ghosts (the parked truck baked into the map keeps blocking the planner after it drives away) and swiss cheese (points on moving objects scatter through free space, corrupting registration). The classical, sensor-agnostic cure is visibility-based removal: if today’s ray passes through a map point and hits something behind it, that point cannot still exist — erase it. Removert refines this with multi-resolution range-image comparison (remove aggressively, then revert what static structure was wrongly deleted); ERASOR works in per-cell height stacks in bird’s-eye view, exploiting that dynamic objects touch the ground. Upstream, modern LIO simply segments people and vehicles out of scans before integration (a Phase 8 network, run in the pipeline), and mapping layers add decay — NVBlox-style down-weighting of unobserved regions — so the map forgets gracefully. The strategic upgrade is lifelong mapping: stop treating the map as an artifact and treat it as a maintained estimate, with sessions merged, changes detected, and stale geometry demoted; the two-frame contract of Phase 3.8 already gave the runtime side of that story.

7.7Appearance: Lighting, Weather, Seasons

Everything Phase 4 said about place recognition gets stress-tested outdoors, where the same street is a different image at noon, dusk, and midnight in sleet. The sensor hierarchy of robustness: lidar geometry barely notices light (but rain attenuates, snow adds noise clusters near the sensor, fog scatters — filter by intensity and range consistency); radar shrugs at all of it, at coarse resolution; cameras suffer most and carry the most information. The camera-side toolkit: learned descriptors trained across conditions (NetVLAD/CosPlace, Phase 4.5 — trained on exactly this problem), sequence matching (SeqSLAM’s insight: a single frame may be ambiguous at night, but a 100 m sequence of frames rarely is), and multi-session experience maps — store the site’s appearance per condition (sunny-summer, wet-winter-night) and localize against the nearest experience rather than forcing one representation to span them all. The honest engineering answer for 2026 deployments remains: geometry (lidar + RTK) for the safety-critical pose, appearance for semantics and redundancy.

// elev.cc — a 2.5D elevation map with slope/step/roughness traversability.
#include <algorithm>
#include <cmath>
#include <iostream>
#include <random>
#include <vector>

const int W = 60, H = 24;            // 6 m × 2.4 m at 10 cm cells
double terrain(double x, double y) { // ground truth height field
  double z = 0.0;
  if (x > 2.0 && x < 3.2) z += (x - 2.0) * 0.25;        // 14° ramp up
  if (x >= 3.2)           z += 0.30;                     // plateau
  if (x > 4.4 && y > 1.2) z += 0.18;                     // a curb (step!)
  if (x > 0.6 && x < 1.6 && y < 0.9)                     // gravel patch
    z += 0.03 * std::sin(37.0 * x) * std::sin(41.0 * y);
  return z;
}

int main() {
  std::mt19937 rng(9);
  std::normal_distribution<double> noise(0.0, 0.005);   // 5 mm sensor noise

  // "scan" the terrain: 40k random hits, keep per-cell mean height
  std::vector<double> sum(W * H, 0.0);  std::vector<int> cnt(W * H, 0);
  std::uniform_real_distribution<double> ux(0.0, 6.0), uy(0.0, 2.4);
  for (int i = 0; i < 40000; ++i) {
    const double x = ux(rng), y = uy(rng);
    const int c = std::min(H - 1, int(y * 10)) * W + std::min(W - 1, int(x * 10));
    sum[c] += terrain(x, y) + noise(rng);  cnt[c]++;
  }
  std::vector<double> z(W * H, 0.0);
  for (int c = 0; c < W * H; ++c) if (cnt[c]) z[c] = sum[c] / cnt[c];

  // traversability: slope < 20°, step < 12 cm  (a modest wheeled base)
  std::cout << "traversability ('.' go, '#' no-go):\n";
  for (int gy = H - 1; gy >= 0; --gy) {
    std::cout << "  ";
    for (int gx = 0; gx < W; ++gx) {
      double step = 0.0, sx = 0.0, sy = 0.0;
      for (int dy = -1; dy <= 1; ++dy) for (int dx = -1; dx <= 1; ++dx) {
        const int nx = gx + dx, ny = gy + dy;
        if (nx < 0 || ny < 0 || nx >= W || ny >= H) continue;
        step = std::max(step, std::abs(z[ny * W + nx] - z[gy * W + gx]));
      }
      if (gx > 0 && gx < W - 1) sx = (z[gy * W + gx + 1] - z[gy * W + gx - 1]) / 0.2;
      if (gy > 0 && gy < H - 1) sy = (z[(gy + 1) * W + gx] - z[(gy - 1) * W + gx]) / 0.2;
      const bool ok = std::hypot(sx, sy) < std::tan(20.0 * M_PI / 180.0)
                      && step < 0.12;
      std::cout << (ok ? '.' : '#');
    }
    std::cout << "\n";
  }
}

7.8Reading List

7.9Interview Questions