For outdoor AMR, a 2D occupancy grid is often insufficient: the ground is not planar, and mobility feasibility depends on terrain geometry (slope/steps) and surface variability (roughness), not only on free/occupied status. An elevation map stores a height field \( h(x,y) \) on a 2D grid; a traversability map converts local terrain geometry into a scalar cost used by planning and control.

We target a standard outdoor sensing pipeline: sensor returns (LiDAR/Depth) produce 3D points in a global or local frame, these points are fused into a probabilistic elevation field, and then local features are extracted to compute a traversability cost:

flowchart TD
  A["3D sensor points (x,y,z)"] --> B["Project to grid cell (i,j)"]
  B --> C["Fuse height into probabilistic elevation (mu,sigma2)"]
  C --> D["Compute local features: slope, roughness, step"]
  D --> E["Traversability cost in [0,1]"]
  E --> F["Navigation costmap / local planner"]
        

The key engineering question is: how do we fuse many noisy height measurements per cell while keeping uncertainty? The key mathematical answer is: use Bayesian conjugacy (Gaussian likelihood + Gaussian prior), yielding closed-form recursive updates.

Discretize the plane into cells indexed by \( (i,j) \) with resolution \( \Delta \). For each cell, we model the unknown terrain height as a random variable: \( H_{ij} \in \mathbb{R} \).

A common assumption (and the one used in this lesson) is a Gaussian belief per cell: \( H_{ij} \sim \mathcal{N}(\mu_{ij}, \sigma_{ij}^2) \). This is not the only model (mixtures and robust distributions exist), but it yields a strong baseline with efficient closed-form fusion.

A sensor measurement \( Z \) of height in that cell is modeled as:

\[ Z = H_{ij} + V,\quad V \sim \mathcal{N}(0, R) \]

where \( R \) is the measurement noise variance (can be sensor-dependent, range-dependent, or inflated to account for pose uncertainty).

Consider one fixed cell \( (i,j) \). Drop indices and write prior \( H \sim \mathcal{N}(\mu^-, (\sigma^-)^2) \). Observe a height measurement \( z \) with likelihood \( p(z|H)=\mathcal{N}(H, R) \).

The posterior is Gaussian and can be written in the standard 1D Kalman form with innovation \( \nu = z - \mu^- \) and innovation variance \( S = (\sigma^-)^2 + R \).

\[ K = \frac{(\sigma^-)^2}{(\sigma^-)^2 + R},\quad \mu^+ = \mu^- + K\,\nu,\quad (\sigma^+)^2 = (1-K)(\sigma^-)^2 \]

Robust gating (outdoor necessity). Outdoor point clouds may include vegetation, rocks, multipath artifacts, or dynamic objects. A simple and effective Bayesian consistency test is:

\[ \text{accept measurement} \iff |\nu| \; < \; k\sqrt{S} \]

where \( k \) is typically 2–4. If rejected, the measurement is treated as an outlier for the ground-elevation cell (often handled by separate obstacle layers).

Proof sketch (Gaussian conjugacy). Multiply the prior and likelihood:

\[ p(H|z) \propto \exp\!\left(-\frac{(H-\mu^-)^2}{2(\sigma^-)^2}\right)\, \exp\!\left(-\frac{(z-H)^2}{2R}\right) \]

Collect quadratic terms in \( H \) to obtain another Gaussian \( \mathcal{N}(\mu^+,(\sigma^+)^2) \). In information form:

\[ \underbrace{\frac{1}{(\sigma^+)^2}}_{\text{posterior precision}} = \underbrace{\frac{1}{(\sigma^-)^2}}_{\text{prior precision}} + \underbrace{\frac{1}{R}}_{\text{measurement precision}}, \quad \frac{\mu^+}{(\sigma^+)^2} = \frac{\mu^-}{(\sigma^-)^2} + \frac{z}{R} \]

Solving these two linear equations yields the Kalman gain form above. This equivalence is important: sequential fusion of many measurements is equivalent to adding precisions (when independence holds).

After fusion, the map stores \( \mu_{ij} \approx h(x_i,y_j) \). We extract features over a local neighborhood (typically 3x3 or 5x5 cells). Let \( \Delta \) be the grid resolution.

4.1 Slope. Approximate the gradient with central differences:

\[ \frac{\partial h}{\partial x}(i,j) \approx \frac{\mu_{i+1,j}-\mu_{i-1,j}}{2\Delta}, \quad \frac{\partial h}{\partial y}(i,j) \approx \frac{\mu_{i,j+1}-\mu_{i,j-1}}{2\Delta} \]

Define the slope angle (w.r.t. the horizontal plane) by:

\[ \theta_{ij} = \arctan\!\left(\sqrt{\left(\frac{\partial h}{\partial x}\right)^2 + \left(\frac{\partial h}{\partial y}\right)^2}\right) \]

Justification. The surface \( z=h(x,y) \) has normal proportional to:

\[ \mathbf{n} \propto \left(-\frac{\partial h}{\partial x},\; -\frac{\partial h}{\partial y},\; 1\right) \]

The tilt angle from vertical satisfies \( \tan(\theta)=\sqrt{(\partial h/\partial x)^2+(\partial h/\partial y)^2} \), hence the formula.

4.2 Roughness. For neighborhood heights \( \{\mu_k\} \) (e.g., 3x3 window), define roughness as standard deviation:

\[ \rho_{ij} = \sqrt{\frac{1}{N}\sum_{k=1}^{N}\left(\mu_k - \bar{\mu}\right)^2} \]

Roughness penalizes highly oscillatory terrain even if mean slope is small (e.g., gravel, roots).

4.3 Step height. A simple step metric is the maximum neighbor difference:

\[ s_{ij} = \max_{(p,q)\in \mathcal{N}(i,j)}\left|\mu_{pq}-\mu_{ij}\right| \]

flowchart TD
  A["Fused elevation mu(i,j)"] --> B["Compute dz/dx, dz/dy \nby finite differences"]
  A --> C["Neighborhood statistics \n(std-dev)"]
  A --> D["Max neighbor height difference"]
  B --> E["Slope theta"]
  C --> F["Roughness rho"]
  D --> G["Step s"]
  E --> H["Combine features → traversability cost"]
  F --> H
  G --> H
        

A traversability map should produce a scalar cost suitable for planners. Let \( \theta_{ij} \) be slope, \( \rho_{ij} \) roughness, and \( s_{ij} \) step. Normalize by reference limits: \( \theta_{\text{ref}}, \rho_{\text{ref}}, s_{\text{ref}} \) determined by vehicle capability (tire size, clearance, suspension).

A principled and differentiable model is logistic:

\[ c_{ij} = \sigma\!\left( w_\theta \frac{\theta_{ij}}{\theta_{\text{ref}}} + w_\rho \frac{\rho_{ij}}{\rho_{\text{ref}}} + w_s \frac{s_{ij}}{s_{\text{ref}}} + b \right), \quad \sigma(x)=\frac{1}{1+e^{-x}} \]

Then \( c_{ij}\in(0,1) \) can be interpreted as a soft “non-traversability probability” or a cost that can be scaled into navigation layers. The weights \( w_\theta, w_\rho, w_s \) and bias \( b \) are tuned from field trials or learned from labeled data (covered later in outdoor AMR chapters).

Resolution trade-off. Smaller \( \Delta \) captures narrow obstacles and steps, but increases memory and can amplify noise in finite differences. A common practice is a moderate elevation map resolution plus multi-resolution costmaps.

Uncertainty-aware fusion. If robot pose uncertainty projects into height error, you can inflate \( R \) per point. A minimal model is \( R_{\text{eff}} = R_z + \alpha\sigma_{\text{pose}}^2 \) for some calibration \( \alpha \). (Full probabilistic treatment is an active research area.)

Outdoor failure modes. Vegetation causes returns above ground; water and mud cause missing or biased returns; dynamic people/animals create transient spikes. The gating test in Section 3 plus temporal filtering can reduce these effects, but robust multi-layer mapping is often used in deployed systems.

The following script implements: (i) per-cell 1D Bayesian/Kalman fusion, (ii) slope/roughness/step extraction, and (iii) a logistic traversability cost. It uses only NumPy and Matplotlib.

File: Chapter9_Lesson4.py

#!/usr/bin/env python3
# Chapter9_Lesson4.py
# Elevation + traversability maps (outdoor AMR) — minimal, from-scratch implementation.

import math
import numpy as np
import matplotlib.pyplot as plt


def sigmoid(x: np.ndarray) -> np.ndarray:
    return 1.0 / (1.0 + np.exp(-x))


class ElevationTraversabilityMap:
    """
    Grid-based elevation map where each cell stores:
      - mu: posterior mean of terrain elevation (meters)
      - sigma2: posterior variance (m^2)

    Updates are done with a 1D Kalman filter per cell:
      z = h + v,  v ~ N(0, R)
    """

    def __init__(self, x_min, x_max, y_min, y_max, resolution,
                 sigma0=2.0,  # initial std dev in meters
                 meas_sigma=0.10,  # sensor std dev in meters
                 gate_k=3.0):
        self.x_min = float(x_min)
        self.x_max = float(x_max)
        self.y_min = float(y_min)
        self.y_max = float(y_max)
        self.res = float(resolution)

        self.nx = int(math.ceil((self.x_max - self.x_min) / self.res))
        self.ny = int(math.ceil((self.y_max - self.y_min) / self.res))

        # Mean and variance fields
        self.mu = np.zeros((self.nx, self.ny), dtype=np.float64)
        self.sigma2 = (sigma0 ** 2) * np.ones((self.nx, self.ny), dtype=np.float64)

        # Simple bookkeeping: number of fusions per cell
        self.count = np.zeros((self.nx, self.ny), dtype=np.int32)

        self.R = float(meas_sigma) ** 2
        self.gate_k = float(gate_k)

    def _to_index(self, x, y):
        ix = int((x - self.x_min) / self.res)
        iy = int((y - self.y_min) / self.res)
        if 0 <= ix < self.nx and 0 <= iy < self.ny:
            return ix, iy
        return None

    def update_point(self, x, y, z):
        idx = self._to_index(x, y)
        if idx is None:
            return False
        ix, iy = idx

        mu = self.mu[ix, iy]
        s2 = self.sigma2[ix, iy]

        # Innovation
        nu = z - mu
        S = s2 + self.R

        # Robust gate: if far from current belief, reject as obstacle/outlier
        if self.count[ix, iy] > 0 and abs(nu) > self.gate_k * math.sqrt(S):
            return False

        # Kalman gain
        K = s2 / S

        # Posterior
        self.mu[ix, iy] = mu + K * nu
        self.sigma2[ix, iy] = (1.0 - K) * s2
        self.count[ix, iy] += 1
        return True

    def update_point_cloud(self, pts_xyz):
        accepted = 0
        for x, y, z in pts_xyz:
            accepted += int(self.update_point(float(x), float(y), float(z)))
        return accepted

    def compute_features(self):
        """
        Compute local geometric features from elevation:
          - slope: arctan(||grad h||) (radians)
          - roughness: std-dev of neighborhood heights (meters)
          - step: max neighbor height difference (meters)
        """
        mu = self.mu
        nx, ny = mu.shape

        slope = np.zeros_like(mu)
        rough = np.zeros_like(mu)
        step = np.zeros_like(mu)

        # Use finite-difference gradients (skip borders)
        for i in range(1, nx - 1):
            for j in range(1, ny - 1):
                if self.count[i, j] == 0:
                    continue

                dzdx = (mu[i + 1, j] - mu[i - 1, j]) / (2.0 * self.res)
                dzdy = (mu[i, j + 1] - mu[i, j - 1]) / (2.0 * self.res)
                slope[i, j] = math.atan(math.sqrt(dzdx * dzdx + dzdy * dzdy))

                neigh = mu[i - 1:i + 2, j - 1:j + 2].reshape(-1)
                rough[i, j] = float(np.std(neigh))

                # Step: max absolute neighbor difference
                diffs = np.abs(neigh - mu[i, j])
                step[i, j] = float(np.max(diffs))

        return slope, rough, step

    def traversability_cost(self, slope, rough, step,
                            slope_ref=0.35,   # ~20 deg in rad
                            rough_ref=0.08,   # meters
                            step_ref=0.12,    # meters
                            w_s=3.0, w_r=2.0, w_d=2.5,
                            bias=-3.0):
        """
        Map (slope, roughness, step) -> [0,1] cost where 0 is easy and 1 is hard.
        """
        # Normalize features
        s = slope / max(1e-9, float(slope_ref))
        r = rough / max(1e-9, float(rough_ref))
        d = step / max(1e-9, float(step_ref))

        score = w_s * s + w_r * r + w_d * d + bias
        return sigmoid(score)

    def save_csv(self, path_prefix):
        np.savetxt(path_prefix + "_elevation_mu.csv", self.mu, delimiter=",")
        np.savetxt(path_prefix + "_elevation_sigma2.csv", self.sigma2, delimiter=",")
        np.savetxt(path_prefix + "_count.csv", self.count, delimiter=",")


def generate_synthetic_point_cloud(n_ground=50000, n_outliers=2000, seed=7):
    """
    Synthetic terrain z_true(x,y) + noisy measurements, plus some obstacle outliers.
    """
    rng = np.random.default_rng(seed)

    # Terrain domain
    x = rng.uniform(-10.0, 10.0, size=n_ground)
    y = rng.uniform(-10.0, 10.0, size=n_ground)

    # Smooth terrain
    z_true = (
        0.20 * np.sin(0.35 * x) +
        0.15 * np.cos(0.25 * y) +
        0.03 * x
    )

    # Add a hill/bump (still traversable)
    z_true += 0.25 * np.exp(-0.08 * ((x - 2.0) ** 2 + (y + 1.0) ** 2))

    # Sensor noise
    z_meas = z_true + rng.normal(0.0, 0.10, size=n_ground)

    ground = np.stack([x, y, z_meas], axis=1)

    # Outliers (e.g., vegetation/rocks) that should be rejected by gating
    xo = rng.uniform(-10.0, 10.0, size=n_outliers)
    yo = rng.uniform(-10.0, 10.0, size=n_outliers)
    zo = (
        0.20 * np.sin(0.35 * xo) +
        0.15 * np.cos(0.25 * yo) +
        0.03 * xo +
        0.8 + rng.normal(0.0, 0.05, size=n_outliers)
    )
    outliers = np.stack([xo, yo, zo], axis=1)

    pts = np.concatenate([ground, outliers], axis=0)
    rng.shuffle(pts, axis=0)
    return pts


def main():
    # Build map
    emap = ElevationTraversabilityMap(
        x_min=-10.0, x_max=10.0, y_min=-10.0, y_max=10.0,
        resolution=0.20,
        sigma0=2.0, meas_sigma=0.10, gate_k=3.0
    )

    pts = generate_synthetic_point_cloud()
    accepted = emap.update_point_cloud(pts)
    print("Points accepted:", accepted, "out of", pts.shape[0])

    slope, rough, step = emap.compute_features()
    cost = emap.traversability_cost(slope, rough, step)

    # Visualize
    plt.figure()
    plt.title("Elevation mean (m)")
    plt.imshow(emap.mu.T, origin="lower", aspect="equal")
    plt.colorbar()

    plt.figure()
    plt.title("Traversability cost (0 easy, 1 hard)")
    plt.imshow(cost.T, origin="lower", aspect="equal")
    plt.colorbar()

    plt.show()

    # Export
    emap.save_csv("Chapter9_Lesson4_output")


if __name__ == "__main__":
    main()
      

This C++ version avoids external dependencies and exports CSV maps that can be visualized in Python/MATLAB.

File: Chapter9_Lesson4.cpp

// Chapter9_Lesson4.cpp
// Elevation + traversability maps (outdoor AMR) — minimal C++ implementation (no external deps).
// Output: CSV files for elevation and traversability.

#include <cmath>
#include <cstdint>
#include <fstream>
#include <iostream>
#include <random>
#include <string>
#include <vector>
#include <algorithm>

struct Point3 {
  double x, y, z;
};

static double sigmoid(double x) {
  return 1.0 / (1.0 + std::exp(-x));
}

class ElevationTraversabilityMap {
 public:
  ElevationTraversabilityMap(double x_min, double x_max, double y_min, double y_max, double res,
                             double sigma0, double meas_sigma, double gate_k)
      : x_min_(x_min), x_max_(x_max), y_min_(y_min), y_max_(y_max), res_(res),
        R_(meas_sigma * meas_sigma), gate_k_(gate_k) {
    nx_ = static_cast<int>(std::ceil((x_max_ - x_min_) / res_));
    ny_ = static_cast<int>(std::ceil((y_max_ - y_min_) / res_));
    const int N = nx_ * ny_;
    mu_.assign(N, 0.0);
    sigma2_.assign(N, sigma0 * sigma0);
    count_.assign(N, 0);
  }

  bool updatePoint(double x, double y, double z) {
    int ix, iy;
    if (!toIndex(x, y, ix, iy)) return false;
    const int k = idx(ix, iy);

    const double mu = mu_[k];
    const double s2 = sigma2_[k];

    const double nu = z - mu;
    const double S = s2 + R_;

    if (count_[k] > 0 && std::abs(nu) > gate_k_ * std::sqrt(S)) {
      return false;
    }

    const double K = s2 / S;
    mu_[k] = mu + K * nu;
    sigma2_[k] = (1.0 - K) * s2;
    count_[k] += 1;
    return true;
  }

  int updatePointCloud(const std::vector<Point3>& pts) {
    int acc = 0;
    for (const auto& p : pts) acc += static_cast<int>(updatePoint(p.x, p.y, p.z));
    return acc;
  }

  // Compute slope/roughness/step using neighborhood finite differences.
  void computeFeatures(std::vector<double>& slope, std::vector<double>& rough, std::vector<double>& step) const {
    slope.assign(nx_ * ny_, 0.0);
    rough.assign(nx_ * ny_, 0.0);
    step.assign(nx_ * ny_, 0.0);

    for (int i = 1; i < nx_ - 1; ++i) {
      for (int j = 1; j < ny_ - 1; ++j) {
        const int k0 = idx(i, j);
        if (count_[k0] == 0) continue;

        const double dzdx = (mu_[idx(i + 1, j)] - mu_[idx(i - 1, j)]) / (2.0 * res_);
        const double dzdy = (mu_[idx(i, j + 1)] - mu_[idx(i, j - 1)]) / (2.0 * res_);
        slope[k0] = std::atan(std::sqrt(dzdx * dzdx + dzdy * dzdy));

        // neighborhood stats (3x3)
        double m = 0.0;
        double m2 = 0.0;
        int n = 0;
        double maxdiff = 0.0;
        for (int di = -1; di <= 1; ++di) {
          for (int dj = -1; dj <= 1; ++dj) {
            const double v = mu_[idx(i + di, j + dj)];
            m += v;
            m2 += v * v;
            n += 1;
            maxdiff = std::max(maxdiff, std::abs(v - mu_[k0]));
          }
        }
        m /= static_cast<double>(n);
        const double var = std::max(0.0, m2 / static_cast<double>(n) - m * m);
        rough[k0] = std::sqrt(var);
        step[k0] = maxdiff;
      }
    }
  }

  std::vector<double> traversabilityCost(const std::vector<double>& slope,
                                         const std::vector<double>& rough,
                                         const std::vector<double>& step,
                                         double slope_ref = 0.35,
                                         double rough_ref = 0.08,
                                         double step_ref  = 0.12,
                                         double w_s = 3.0, double w_r = 2.0, double w_d = 2.5,
                                         double bias = -3.0) const {
    std::vector<double> cost(nx_ * ny_, 0.0);
    const double inv_s = 1.0 / std::max(1e-9, slope_ref);
    const double inv_r = 1.0 / std::max(1e-9, rough_ref);
    const double inv_d = 1.0 / std::max(1e-9, step_ref);

    for (int k = 0; k < nx_ * ny_; ++k) {
      const double s = slope[k] * inv_s;
      const double r = rough[k] * inv_r;
      const double d = step[k] * inv_d;
      const double score = w_s * s + w_r * r + w_d * d + bias;
      cost[k] = sigmoid(score);
    }
    return cost;
  }

  void saveCSV(const std::string& path, const std::vector<double>& grid) const {
    std::ofstream f(path);
    if (!f) {
      std::cerr << "Failed to open " << path << "\n";
      return;
    }
    for (int j = 0; j < ny_; ++j) {
      for (int i = 0; i < nx_; ++i) {
        f << grid[idx(i, j)];
        if (i + 1 < nx_) f << ",";
      }
      f << "\n";
    }
  }

  void saveCSVInt(const std::string& path, const std::vector<int>& grid) const {
    std::ofstream f(path);
    if (!f) return;
    for (int j = 0; j < ny_; ++j) {
      for (int i = 0; i < nx_; ++i) {
        f << grid[idx(i, j)];
        if (i + 1 < nx_) f << ",";
      }
      f << "\n";
    }
  }

  const std::vector<double>& mu() const { return mu_; }
  const std::vector<double>& sigma2() const { return sigma2_; }
  const std::vector<int>& count() const { return count_; }

 private:
  bool toIndex(double x, double y, int& ix, int& iy) const {
    ix = static_cast<int>((x - x_min_) / res_);
    iy = static_cast<int>((y - y_min_) / res_);
    if (ix < 0 || ix >= nx_ || iy < 0 || iy >= ny_) return false;
    return true;
  }

  int idx(int ix, int iy) const { return ix + nx_ * iy; }

  double x_min_, x_max_, y_min_, y_max_, res_;
  int nx_, ny_;
  double R_, gate_k_;
  std::vector<double> mu_;
  std::vector<double> sigma2_;
  std::vector<int> count_;
};

static std::vector<Point3> syntheticPointCloud(int n_ground, int n_outliers, int seed) {
  std::mt19937 rng(seed);
  std::uniform_real_distribution<double> unif(-10.0, 10.0);
  std::normal_distribution<double> noise(0.0, 0.10);
  std::normal_distribution<double> noise_out(0.0, 0.05);

  std::vector<Point3> pts;
  pts.reserve(static_cast<size_t>(n_ground + n_outliers));

  auto z_true = [](double x, double y) {
    const double hill = 0.25 * std::exp(-0.08 * ((x - 2.0) * (x - 2.0) + (y + 1.0) * (y + 1.0)));
    return 0.20 * std::sin(0.35 * x) + 0.15 * std::cos(0.25 * y) + 0.03 * x + hill;
  };

  for (int i = 0; i < n_ground; ++i) {
    const double x = unif(rng);
    const double y = unif(rng);
    const double z = z_true(x, y) + noise(rng);
    pts.push_back({x, y, z});
  }

  for (int i = 0; i < n_outliers; ++i) {
    const double x = unif(rng);
    const double y = unif(rng);
    const double z = z_true(x, y) + 0.8 + noise_out(rng);
    pts.push_back({x, y, z});
  }

  std::shuffle(pts.begin(), pts.end(), rng);
  return pts;
}

int main() {
  ElevationTraversabilityMap emap(-10.0, 10.0, -10.0, 10.0, 0.20, 2.0, 0.10, 3.0);

  auto pts = syntheticPointCloud(50000, 2000, 7);
  const int accepted = emap.updatePointCloud(pts);
  std::cout << "Points accepted: " << accepted << " out of " << pts.size() << "\n";

  std::vector<double> slope, rough, step;
  emap.computeFeatures(slope, rough, step);
  auto cost = emap.traversabilityCost(slope, rough, step);

  emap.saveCSV("Chapter9_Lesson4_elevation_mu.csv", emap.mu());
  emap.saveCSV("Chapter9_Lesson4_traversability_cost.csv", cost);
  emap.saveCSVInt("Chapter9_Lesson4_fusion_count.csv", emap.count());

  std::cout << "Saved CSV files.\n";
  return 0;
}
      

This Java version mirrors the math exactly and exports CSV maps.

File: Chapter9_Lesson4.java

// Chapter9_Lesson4.java
// Elevation + traversability maps (outdoor AMR) — minimal Java implementation (no external deps).
// Output: CSV files for elevation and traversability.

import java.io.BufferedWriter;
import java.io.FileWriter;
import java.io.IOException;
import java.util.Random;

public class Chapter9_Lesson4 {

  static class ElevationTraversabilityMap {
    final double xMin, xMax, yMin, yMax, res;
    final int nx, ny;
    final double R;
    final double gateK;

    final double[][] mu;
    final double[][] sigma2;
    final int[][] count;

    ElevationTraversabilityMap(double xMin, double xMax, double yMin, double yMax,
                              double res, double sigma0, double measSigma, double gateK) {
      this.xMin = xMin; this.xMax = xMax;
      this.yMin = yMin; this.yMax = yMax;
      this.res = res;
      this.nx = (int) Math.ceil((xMax - xMin) / res);
      this.ny = (int) Math.ceil((yMax - yMin) / res);
      this.R = measSigma * measSigma;
      this.gateK = gateK;

      mu = new double[nx][ny];
      sigma2 = new double[nx][ny];
      count = new int[nx][ny];

      double s2 = sigma0 * sigma0;
      for (int i = 0; i < nx; i++) {
        for (int j = 0; j < ny; j++) {
          sigma2[i][j] = s2;
          mu[i][j] = 0.0;
          count[i][j] = 0;
        }
      }
    }

    boolean updatePoint(double x, double y, double z) {
      int ix = (int) ((x - xMin) / res);
      int iy = (int) ((y - yMin) / res);
      if (ix < 0 || ix >= nx || iy < 0 || iy >= ny) return false;

      double m = mu[ix][iy];
      double s2 = sigma2[ix][iy];

      double nu = z - m;
      double S = s2 + R;

      if (count[ix][iy] > 0 && Math.abs(nu) > gateK * Math.sqrt(S)) {
        return false;
      }

      double K = s2 / S;
      mu[ix][iy] = m + K * nu;
      sigma2[ix][iy] = (1.0 - K) * s2;
      count[ix][iy] += 1;
      return true;
    }

    int updatePointCloud(double[][] pts) {
      int acc = 0;
      for (int k = 0; k < pts.length; k++) {
        acc += updatePoint(pts[k][0], pts[k][1], pts[k][2]) ? 1 : 0;
      }
      return acc;
    }

    static double sigmoid(double x) {
      return 1.0 / (1.0 + Math.exp(-x));
    }

    void computeFeatures(double[][] slope, double[][] rough, double[][] step) {
      for (int i = 1; i < nx - 1; i++) {
        for (int j = 1; j < ny - 1; j++) {
          if (count[i][j] == 0) continue;

          double dzdx = (mu[i + 1][j] - mu[i - 1][j]) / (2.0 * res);
          double dzdy = (mu[i][j + 1] - mu[i][j - 1]) / (2.0 * res);
          slope[i][j] = Math.atan(Math.sqrt(dzdx * dzdx + dzdy * dzdy));

          double sum = 0.0;
          double sum2 = 0.0;
          int n = 0;
          double maxdiff = 0.0;

          for (int di = -1; di <= 1; di++) {
            for (int dj = -1; dj <= 1; dj++) {
              double v = mu[i + di][j + dj];
              sum += v;
              sum2 += v * v;
              n += 1;
              maxdiff = Math.max(maxdiff, Math.abs(v - mu[i][j]));
            }
          }
          double mean = sum / n;
          double var = Math.max(0.0, sum2 / n - mean * mean);
          rough[i][j] = Math.sqrt(var);
          step[i][j] = maxdiff;
        }
      }
    }

    double[][] traversabilityCost(double[][] slope, double[][] rough, double[][] step,
                                  double slopeRef, double roughRef, double stepRef,
                                  double wS, double wR, double wD, double bias) {
      double[][] cost = new double[nx][ny];
      double invS = 1.0 / Math.max(1e-9, slopeRef);
      double invR = 1.0 / Math.max(1e-9, roughRef);
      double invD = 1.0 / Math.max(1e-9, stepRef);

      for (int i = 0; i < nx; i++) {
        for (int j = 0; j < ny; j++) {
          double s = slope[i][j] * invS;
          double r = rough[i][j] * invR;
          double d = step[i][j] * invD;
          double score = wS * s + wR * r + wD * d + bias;
          cost[i][j] = sigmoid(score);
        }
      }
      return cost;
    }

    void saveCSV(String path, double[][] grid) throws IOException {
      try (BufferedWriter bw = new BufferedWriter(new FileWriter(path))) {
        for (int j = 0; j < ny; j++) {
          for (int i = 0; i < nx; i++) {
            bw.write(Double.toString(grid[i][j]));
            if (i + 1 < nx) bw.write(",");
          }
          bw.newLine();
        }
      }
    }

    void saveCSVInt(String path, int[][] grid) throws IOException {
      try (BufferedWriter bw = new BufferedWriter(new FileWriter(path))) {
        for (int j = 0; j < ny; j++) {
          for (int i = 0; i < nx; i++) {
            bw.write(Integer.toString(grid[i][j]));
            if (i + 1 < nx) bw.write(",");
          }
          bw.newLine();
        }
      }
    }
  }

  static double terrainTrue(double x, double y) {
    double hill = 0.25 * Math.exp(-0.08 * ((x - 2.0) * (x - 2.0) + (y + 1.0) * (y + 1.0)));
    return 0.20 * Math.sin(0.35 * x) + 0.15 * Math.cos(0.25 * y) + 0.03 * x + hill;
  }

  static double[][] generateSyntheticPointCloud(int nGround, int nOutliers, long seed) {
    Random rng = new Random(seed);
    double[][] pts = new double[nGround + nOutliers][3];

    for (int i = 0; i < nGround; i++) {
      double x = -10.0 + 20.0 * rng.nextDouble();
      double y = -10.0 + 20.0 * rng.nextDouble();
      double z = terrainTrue(x, y) + 0.10 * rng.nextGaussian();
      pts[i][0] = x; pts[i][1] = y; pts[i][2] = z;
    }
    for (int i = 0; i < nOutliers; i++) {
      double x = -10.0 + 20.0 * rng.nextDouble();
      double y = -10.0 + 20.0 * rng.nextDouble();
      double z = terrainTrue(x, y) + 0.8 + 0.05 * rng.nextGaussian();
      pts[nGround + i][0] = x; pts[nGround + i][1] = y; pts[nGround + i][2] = z;
    }

    for (int i = pts.length - 1; i > 0; i--) {
      int j = rng.nextInt(i + 1);
      double[] tmp = pts[i];
      pts[i] = pts[j];
      pts[j] = tmp;
    }
    return pts;
  }

  public static void main(String[] args) throws Exception {
    ElevationTraversabilityMap emap = new ElevationTraversabilityMap(
        -10.0, 10.0, -10.0, 10.0, 0.20,
        2.0, 0.10, 3.0
    );

    double[][] pts = generateSyntheticPointCloud(50000, 2000, 7);
    int accepted = emap.updatePointCloud(pts);
    System.out.println("Points accepted: " + accepted + " out of " + pts.length);

    double[][] slope = new double[emap.nx][emap.ny];
    double[][] rough = new double[emap.nx][emap.ny];
    double[][] step  = new double[emap.nx][emap.ny];
    emap.computeFeatures(slope, rough, step);

    double[][] cost = emap.traversabilityCost(
        slope, rough, step,
        0.35, 0.08, 0.12,
        3.0, 2.0, 2.5,
        -3.0
    );

    emap.saveCSV("Chapter9_Lesson4_elevation_mu.csv", emap.mu);
    emap.saveCSV("Chapter9_Lesson4_traversability_cost.csv", cost);
    emap.saveCSVInt("Chapter9_Lesson4_fusion_count.csv", emap.count);

    System.out.println("Saved CSV files.");
  }
}
      

MATLAB is convenient for matrix operations and visualization. A simple Simulink model can represent the per-cell discrete recursion \( (\mu,\sigma^2) \mapsto (\mu^+,\sigma^{2+}) \).

File: Chapter9_Lesson4.m

% Chapter9_Lesson4.m
% Elevation + traversability maps (outdoor AMR) — MATLAB + Simulink-friendly script.
% This script:
%   1) Generates a synthetic point cloud for terrain
%   2) Fuses points into a probabilistic elevation grid (per-cell 1D Kalman)
%   3) Computes slope/roughness/step and a traversability cost map
%   4) Optionally builds a simple Simulink model for the per-cell Kalman update

clear; clc; close all;

% -----------------------------
% Parameters
% -----------------------------
xMin = -10; xMax = 10;
yMin = -10; yMax = 10;
res = 0.20;

nx = ceil((xMax - xMin)/res);
ny = ceil((yMax - yMin)/res);

sigma0 = 2.0;       % initial std (m)
measSigma = 0.10;   % sensor std (m)
R = measSigma^2;
gateK = 3.0;

mu = zeros(nx, ny);
sigma2 = (sigma0^2) * ones(nx, ny);
count = zeros(nx, ny);

% -----------------------------
% Synthetic point cloud
% -----------------------------
rng(7);
nGround = 5e4;
nOut = 2e3;

x = (xMin + (xMax-xMin)*rand(nGround,1));
y = (yMin + (yMax-yMin)*rand(nGround,1));

zTrue = 0.20*sin(0.35*x) + 0.15*cos(0.25*y) + 0.03*x ...
        + 0.25*exp(-0.08*((x-2).^2 + (y+1).^2));

zMeas = zTrue + measSigma*randn(nGround,1);
ground = [x, y, zMeas];

xo = (xMin + (xMax-xMin)*rand(nOut,1));
yo = (yMin + (yMax-yMin)*rand(nOut,1));
zo = 0.20*sin(0.35*xo) + 0.15*cos(0.25*yo) + 0.03*xo ...
     + 0.25*exp(-0.08*((xo-2).^2 + (yo+1).^2)) ...
     + 0.8 + 0.05*randn(nOut,1);
outl = [xo, yo, zo];

pts = [ground; outl];
pts = pts(randperm(size(pts,1)), :);

% -----------------------------
% Per-cell Kalman fusion
% -----------------------------
accepted = 0;
for k = 1:size(pts,1)
    px = pts(k,1); py = pts(k,2); pz = pts(k,3);

    ix = floor((px - xMin)/res) + 1;
    iy = floor((py - yMin)/res) + 1;

    if ix < 1 || ix > nx || iy < 1 || iy > ny
        continue;
    end

    m = mu(ix,iy);
    s2 = sigma2(ix,iy);

    nu = pz - m;
    S = s2 + R;

    if count(ix,iy) > 0 && abs(nu) > gateK*sqrt(S)
        continue; % reject outlier/obstacle
    end

    K = s2 / S;
    mu(ix,iy) = m + K*nu;
    sigma2(ix,iy) = (1 - K)*s2;
    count(ix,iy) = count(ix,iy) + 1;
    accepted = accepted + 1;
end

fprintf('Points accepted: %d out of %d\n', accepted, size(pts,1));

% -----------------------------
% Features: slope, roughness, step
% -----------------------------
slope = zeros(nx,ny);
rough = zeros(nx,ny);
stepH = zeros(nx,ny);

for i = 2:nx-1
    for j = 2:ny-1
        if count(i,j) == 0
            continue;
        end

        dzdx = (mu(i+1,j) - mu(i-1,j)) / (2*res);
        dzdy = (mu(i,j+1) - mu(i,j-1)) / (2*res);
        slope(i,j) = atan(sqrt(dzdx^2 + dzdy^2));

        neigh = mu(i-1:i+1, j-1:j+1);
        rough(i,j) = std(neigh(:));

        stepH(i,j) = max(abs(neigh(:) - mu(i,j)));
    end
end

% -----------------------------
% Traversability cost
% -----------------------------
slopeRef = 0.35;  % rad
roughRef = 0.08;  % m
stepRef  = 0.12;  % m

wS = 3.0; wR = 2.0; wD = 2.5; bias = -3.0;

s = slope / max(1e-9, slopeRef);
r = rough / max(1e-9, roughRef);
d = stepH / max(1e-9, stepRef);
score = wS*s + wR*r + wD*d + bias;

cost = 1 ./ (1 + exp(-score));

% -----------------------------
% Plots
% -----------------------------
figure; imagesc(mu'); axis image; colorbar;
title('Elevation mean (m)'); set(gca,'YDir','normal');

figure; imagesc(cost'); axis image; colorbar;
title('Traversability cost (0 easy, 1 hard)'); set(gca,'YDir','normal');

% -----------------------------
% Export
% -----------------------------
writematrix(mu, 'Chapter9_Lesson4_elevation_mu.csv');
writematrix(cost, 'Chapter9_Lesson4_traversability_cost.csv');
writematrix(count, 'Chapter9_Lesson4_fusion_count.csv');

% -----------------------------
% Optional: build a Simulink model that implements ONE cell's 1D Kalman update.
% The idea is to show how the recursion (mu, sigma2) can be implemented as discrete blocks.
% Run: buildSimulinkKalmanCell();
% -----------------------------
% buildSimulinkKalmanCell();

function buildSimulinkKalmanCell()
    model = 'Chapter9_Lesson4_KalmanCell';
    if bdIsLoaded(model); close_system(model, 0); end
    new_system(model); open_system(model);

    add_block('simulink/Sources/In1', [model '/z_meas'], 'Position', [30 40 60 60]);
    add_block('simulink/Sources/In1', [model '/R'], 'Position', [30 90 60 110]);
    add_block('simulink/Discrete/Unit Delay', [model '/mu(z^-1)'], 'Position', [140 30 200 70]);
    add_block('simulink/Discrete/Unit Delay', [model '/sigma2(z^-1)'], 'Position', [140 90 200 130]);

    add_block('simulink/User-Defined Functions/MATLAB Function', [model '/KalmanUpdate'], ...
              'Position', [260 40 420 120]);
    set_param([model '/KalmanUpdate'], 'Script', ...
        ['function [mu_p, sigma2_p] = f(mu, sigma2, z, R)\n' ...
         '% 1D Kalman update: K = sigma2 / (sigma2 + R)\n' ...
         'S = sigma2 + R;\n' ...
         'K = sigma2 / S;\n' ...
         'mu_p = mu + K*(z - mu);\n' ...
         'sigma2_p = (1 - K)*sigma2;\n' ...
         'end\n']);

    add_block('simulink/Sinks/Out1', [model '/mu_plus'], 'Position', [470 45 500 65]);
    add_block('simulink/Sinks/Out1', [model '/sigma2_plus'], 'Position', [470 95 500 115]);

    add_line(model, 'z_meas/1', 'KalmanUpdate/3');
    add_line(model, 'R/1', 'KalmanUpdate/4');
    add_line(model, 'mu(z^-1)/1', 'KalmanUpdate/1');
    add_line(model, 'sigma2(z^-1)/1', 'KalmanUpdate/2');

    add_line(model, 'KalmanUpdate/1', 'mu_plus/1');
    add_line(model, 'KalmanUpdate/2', 'sigma2_plus/1');

    add_line(model, 'KalmanUpdate/1', 'mu(z^-1)/1', 'autorouting', 'on');
    add_line(model, 'KalmanUpdate/2', 'sigma2(z^-1)/1', 'autorouting', 'on');

    save_system(model);
    disp(['Saved Simulink model: ' model '.slx']);
end
      

The notebook below performs the same pipeline and visualizes maps via ArrayPlot and ListPlot3D.

File: Chapter9_Lesson4.nb

(* Chapter9_Lesson4.nb *)
Notebook[{
  Cell["Chapter 9 - Lesson 4: Elevation and Traversability Maps (outdoor AMR)", "Title"],
  Cell["This notebook implements a minimal elevation-map fusion (per-cell 1D Kalman update) and a traversability cost map based on slope, roughness, and step height.", "Text"],

  Cell["1. Synthetic Terrain and Point Cloud", "Section"],
  Cell[BoxData@ToBoxes[
    ClearAll["Global`*"];
    SeedRandom[7];

    xMin = -10; xMax = 10; yMin = -10; yMax = 10;
    res = 0.2;
    nx = Ceiling[(xMax - xMin)/res];
    ny = Ceiling[(yMax - yMin)/res];

    sigma0 = 2.0; measSigma = 0.10; R = measSigma^2; gateK = 3.0;

    mu = ConstantArray[0.0, {nx, ny}];
    sigma2 = ConstantArray[sigma0^2, {nx, ny}];
    count = ConstantArray[0, {nx, ny}];

    terrainTrue[x_, y_] := 0.20*Sin[0.35*x] + 0.15*Cos[0.25*y] + 0.03*x +
      0.25*Exp[-0.08*((x - 2)^2 + (y + 1)^2)];

    nGround = 50000; nOut = 2000;

    ground = Table[
      With[{x = RandomReal[{xMin, xMax}], y = RandomReal[{yMin, yMax}]},
        {x, y, terrainTrue[x, y] + RandomVariate[NormalDistribution[0, measSigma]]}
      ],
      {nGround}
    ];

    outliers = Table[
      With[{x = RandomReal[{xMin, xMax}], y = RandomReal[{yMin, yMax}]},
        {x, y, terrainTrue[x, y] + 0.8 + RandomVariate[NormalDistribution[0, 0.05]]}
      ],
      {nOut}
    ];

    pts = RandomSample[Join[ground, outliers]];
  ], "Input"],

  Cell["2. Per-cell 1D Kalman Fusion", "Section"],
  Cell[BoxData@ToBoxes[
    accepted = 0;
    Do[
      {px, py, pz} = pts[[k]];
      ix = Floor[(px - xMin)/res] + 1;
      iy = Floor[(py - yMin)/res] + 1;

      If[1 <= ix <= nx && 1 <= iy <= ny,
        m = mu[[ix, iy]];
        s2 = sigma2[[ix, iy]];

        nu = pz - m;
        S = s2 + R;

        If[count[[ix, iy]] == 0 || Abs[nu] <= gateK*Sqrt[S],
          K = s2/S;
          mu[[ix, iy]] = m + K*nu;
          sigma2[[ix, iy]] = (1 - K)*s2;
          count[[ix, iy]] = count[[ix, iy]] + 1;
          accepted++
        ];
      ],
      {k, Length[pts]}
    ];
    accepted
  ], "Input"],

  Cell["3. Features and Traversability Cost", "Section"],
  Cell[BoxData@ToBoxes[
    slope = ConstantArray[0.0, {nx, ny}];
    rough = ConstantArray[0.0, {nx, ny}];
    stepH = ConstantArray[0.0, {nx, ny}];

    Do[
      If[count[[i, j]] > 0,
        dzdx = (mu[[i + 1, j]] - mu[[i - 1, j]])/(2*res);
        dzdy = (mu[[i, j + 1]] - mu[[i, j - 1]])/(2*res);
        slope[[i, j]] = ArcTan[Sqrt[dzdx^2 + dzdy^2]];

        neigh = Flatten[mu[[i - 1 ;; i + 1, j - 1 ;; j + 1]]];
        rough[[i, j]] = StandardDeviation[neigh];
        stepH[[i, j]] = Max[Abs[neigh - mu[[i, j]]]];
      ],
      {i, 2, nx - 1}, {j, 2, ny - 1}
    ];

    slopeRef = 0.35; roughRef = 0.08; stepRef = 0.12;
    wS = 3.0; wR = 2.0; wD = 2.5; bias = -3.0;

    score = wS*(slope/slopeRef) + wR*(rough/roughRef) + wD*(stepH/stepRef) + bias;
    cost = 1/(1 + Exp[-score]);
  ], "Input"],

  Cell["4. Visualization", "Section"],
  Cell[BoxData@ToBoxes[
    Print["Elevation mean (ArrayPlot):"];
    ArrayPlot[Transpose[mu], PlotLegends -> Automatic, Frame -> True];

    Print["Traversability cost (ArrayPlot):"];
    ArrayPlot[Transpose[cost], PlotLegends -> Automatic, Frame -> True];

    Print["3D terrain (ListPlot3D) for elevation mean (subsampled):"];
    ListPlot3D[Transpose[mu[[;; ;; 2, ;; ;; 2]]], Mesh -> None, PlotRange -> All]
  ], "Input"]
},
WindowSize -> {1100, 800},
StyleDefinitions -> "Default.nb"
]
      

Problem 1 (Bayesian fusion derivation): Let \( H \sim \mathcal{N}(\mu^-,(\sigma^-)^2) \) and measurement model \( Z|H \sim \mathcal{N}(H,R) \). Derive the posterior mean and variance \( (\mu^+,(\sigma^+)^2) \).

Solution: Multiply prior and likelihood and complete the square. In information form:

\[ \frac{1}{(\sigma^+)^2} = \frac{1}{(\sigma^-)^2} + \frac{1}{R}, \quad \frac{\mu^+}{(\sigma^+)^2} = \frac{\mu^-}{(\sigma^-)^2} + \frac{z}{R} \]

Solving yields:

\[ (\sigma^+)^2 = \left(\frac{1}{(\sigma^-)^2} + \frac{1}{R}\right)^{-1}, \quad \mu^+ = (\sigma^+)^2\left(\frac{\mu^-}{(\sigma^-)^2} + \frac{z}{R}\right) \]

Rearranging gives the Kalman gain form \( K=\frac{(\sigma^-)^2}{(\sigma^-)^2+R} \), \( \mu^+=\mu^-+K(z-\mu^-) \), \( (\sigma^+)^2=(1-K)(\sigma^-)^2 \).

Problem 2 (Sequential fusion equivalence): Suppose you fuse two independent measurements \( z_1, z_2 \) with variances \( R_1, R_2 \) into the same cell, starting from prior \( \mathcal{N}(\mu_0,\sigma_0^2) \). Show that the final posterior precision equals \( \sigma^{-2} = \sigma_0^{-2} + R_1^{-1} + R_2^{-1} \).

Solution: Apply the information update twice: \( \sigma_1^{-2}=\sigma_0^{-2}+R_1^{-1} \) then \( \sigma_2^{-2}=\sigma_1^{-2}+R_2^{-1}=\sigma_0^{-2}+R_1^{-1}+R_2^{-1} \). This is exactly the additivity of independent measurement information.

Problem 3 (Slope formula): For a surface \( z=h(x,y) \), prove that the slope angle \( \theta \) satisfies \( \tan(\theta)=\sqrt{(\partial h/\partial x)^2+(\partial h/\partial y)^2} \).

Solution: A normal vector is \( \mathbf{n}=(-h_x,-h_y,1) \) where \( h_x=\partial h/\partial x \), \( h_y=\partial h/\partial y \). The angle from the vertical axis satisfies \( \tan(\theta)=\frac{\sqrt{n_x^2+n_y^2}}{n_z}=\sqrt{h_x^2+h_y^2} \). Taking \( \arctan \) yields the slope expression.

Problem 4 (Traversability thresholding): Consider the logistic traversability cost \( c=\sigma(w_\theta \theta/\theta_{\text{ref}} + w_\rho \rho/\rho_{\text{ref}} + w_s s/s_{\text{ref}} + b) \). Choose \( b \) so that when \( \theta=\theta_{\text{ref}}, \rho=\rho_{\text{ref}}, s=s_{\text{ref}} \), the cost is \( c=0.5 \).

Solution: \( c=0.5 \) implies the logistic argument is zero. Therefore: \( w_\theta + w_\rho + w_s + b = 0 \Rightarrow b = -(w_\theta+w_\rho+w_s) \).

Problem 5 (Gating probability): If the innovation is Gaussian under the inlier model, \( \nu \sim \mathcal{N}(0,S) \), compute the probability that a measurement passes the gate \( |\nu| < k\sqrt{S} \).

Solution: Let \( u=\nu/\sqrt{S} \sim \mathcal{N}(0,1) \). Then \( \mathbb{P}(|\nu| < k\sqrt{S})=\mathbb{P}(|u| < k)=2\Phi(k)-1 \), where \( \Phi \) is the standard normal CDF.

We modeled outdoor terrain as a probabilistic elevation grid, derived closed-form Bayesian (1D Kalman) fusion per cell, introduced robust gating for outliers, and constructed a traversability map by extracting slope, roughness, and step features from the fused height field. These representations form the geometric bridge between raw 3D sensing and navigation costmaps in outdoor AMR.