A mobile robot operating in an unknown environment must maintain an internal model of free space and obstacles. An occupancy grid discretizes the plane into cells and assigns to each cell a probability of being occupied. The map is therefore a random field over a finite lattice.

Let the map have \( N \) cells. For each cell \( i \in \{1,\dots,N\} \), define a binary random variable \( m_i \in \{0,1\} \), where \( m_i=1 \) denotes “occupied” and \( m_i=0 \) denotes “free”. The occupancy grid stores (an estimate of) \( p(m_i=1) \) for all cells.

The key design parameters are: \( r \) (resolution, meters per cell), \( (o_x,o_y) \) (world origin of the grid), and the finite support \( [0,W)\times[0,H) \) in cell coordinates. A world point \( (x,y) \) is mapped to indices by

\[ g_x = \left\lfloor \frac{x-o_x}{r} \right\rfloor,\qquad g_y = \left\lfloor \frac{y-o_y}{r} \right\rfloor. \]

Occupancy grids are attractive because they: (i) integrate naturally with range sensors; (ii) support conservative planning via “occupied probability thresholds”; and (iii) yield an explicit representation for navigation layers (e.g., costmaps) later in the course.

flowchart TD
  S["Start"] --> P["Choose grid: resolution r, size W x H, origin (ox, oy)"]
  P --> I["Initialize prior occupancy p0 for all cells"]
  I --> T["For each time step t"]
  T --> X["Get robot pose x_t (from odometry/localization)"]
  X --> Z["Get range scan z_t (e.g., LiDAR rays)"]
  Z --> B["For each beam in the scan"]
  B --> R["Trace ray through grid cells (line traversal)"]
  R --> U["Update cells: free along ray, occupied at hit (if any)"]
  U --> B
  B --> T
  T --> O["Output occupancy grid map"]
        

Let \( x_{1:t} \) be the robot poses up to time \( t \) and \( z_{1:t} \) be the sensor measurements (e.g., range scans). The mapping problem seeks the posterior distribution \( p(m\mid x_{1:t}, z_{1:t}) \), where \( m \) denotes the entire grid map (the collection of all \( m_i \)).

By Bayes’ rule:

\[ p(m\mid x_{1:t}, z_{1:t}) = \eta\; p(z_{1:t} \mid m, x_{1:t})\; p(m), \]

where \( \eta \) is a normalizing constant. A standard modeling assumption is conditional independence of measurements over time given the map and poses:

\[ p(z_{1:t} \mid m, x_{1:t}) = \prod_{k=1}^{t} p(z_k \mid m, x_k). \]

Even with this factorization, the exact posterior over the full map is high-dimensional and intractable to update exactly. Occupancy grids therefore adopt an additional approximation: cellwise independence of the map posterior, i.e. approximate \( p(m\mid x_{1:t}, z_{1:t}) \approx \prod_{i=1}^{N} p(m_i\mid x_{1:t}, z_{1:t}) \). This approximation is not exact (sensor rays correlate cells), but it is the key to a tractable incremental update.

The core update target is each cell’s marginal posterior \( p(m_i\mid x_{1:t}, z_{1:t}) \). Under standard Bayesian filtering logic (conditioning on the newest measurement):

\[ p(m_i\mid x_{1:t}, z_{1:t}) = \eta\; p(z_t \mid m_i, x_t)\; p(m_i\mid x_{1:t-1}, z_{1:t-1}). \]

A common implementation path is to specify an inverse sensor model: \( p(m_i \mid z_t, x_t) \). Using Bayes’ rule on the likelihood term:

\[ p(z_t \mid m_i, x_t) = \frac{p(m_i \mid z_t, x_t)\, p(z_t\mid x_t)}{p(m_i)}. \]

Substituting into the filter update yields a multiplicative correction by the inverse model:

\[ p(m_i\mid x_{1:t}, z_{1:t}) = \eta\; \frac{p(m_i \mid z_t, x_t)}{p(m_i)}\; p(m_i\mid x_{1:t-1}, z_{1:t-1}). \]

This identity is the formal bridge from “sensor geometry” (inverse model) to “Bayesian update” (mapping). In practice, storing probabilities directly can be numerically fragile when probabilities approach 0 or 1. A numerically stable alternative is to work with odds and log-odds; the full log-odds development and efficient incremental formulas are the focus of Lesson 2, but we provide the essential algebra here to motivate the implementations.

Define the odds of occupancy for cell \( i \) at time \( t \): \( O_t(i) = \dfrac{p(m_i=1 \mid x_{1:t}, z_{1:t})}{p(m_i=0 \mid x_{1:t}, z_{1:t})} \). Also define the prior odds \( O_0(i) = \dfrac{p(m_i=1)}{p(m_i=0)} \) and the inverse-model odds \( O_{inv}(i) = \dfrac{p(m_i=1 \mid z_t, x_t)}{p(m_i=0 \mid z_t, x_t)} \).

Proposition (odds update). Under the per-cell update equation in Section 2, the posterior odds satisfy:

\[ O_t(i) = O_{t-1}(i)\, \frac{O_{inv}(i)}{O_0(i)}. \]

Proof sketch. Start from \( p(m_i\mid x_{1:t}, z_{1:t}) \propto \frac{p(m_i\mid z_t, x_t)}{p(m_i)}\, p(m_i\mid x_{1:t-1}, z_{1:t-1}) \). Write the same relation for \( m_i=1 \) and \( m_i=0 \), take their ratio, and note that the normalizer \( \eta \) and the factor \( p(z_t\mid x_t) \) cancel. The remaining ratio is exactly the odds update above.

Taking logs converts the multiplicative update into an additive update:

\[ \ell_t(i) = \log O_t(i) = \ell_{t-1}(i) + \log O_{inv}(i) - \log O_0(i). \]

where \( \ell_t(i) \) is the log-odds. This is the computational form used by most occupancy grid mappers (including the provided code), because it is stable and efficient.

Consider a planar range sensor producing beams indexed by \( k \). Each beam returns a range \( z_t^{(k)} \in [0, z_{max}] \) at bearing \( \phi^{(k)} \) relative to the robot heading. Let the robot pose be \( x_t = (x_t, y_t, \theta_t) \). The beam direction in the world is \( \alpha^{(k)} = \theta_t + \phi^{(k)} \).

A simple inverse model partitions cells near the ray into three classes: (i) cells before the hit are likely free; (ii) cells at the hit are likely occupied (if a hit occurs before max range); (iii) other cells are left at the prior. Let \( d_i \) denote the distance from the sensor origin to the center of cell \( i \) projected along the ray direction (for cells on the ray). Choose a “thickness” parameter \( \alpha > 0 \) (often on the order of one cell). Then a canonical piecewise inverse model is:

\[ p(m_i=1 \mid z_t^{(k)}, x_t) = \begin{cases} p_{free} & \text{if } d_i < z_t^{(k)} - \alpha/2 \\ p_{occ} & \text{if } \bigl|d_i - z_t^{(k)}\bigr| \le \alpha/2 \;\text{ and }\; z_t^{(k)} < z_{max} \\ p_0 & \text{otherwise} \end{cases} \]

with design choices \( p_{occ} > 0.5 \) and \( p_{free} < 0.5 \). The “otherwise” case covers cells not intersected by the ray, cells beyond the observed range, and max-range returns (no obstacle hit). In a discrete grid, the set of intersected cells is computed by a line traversal algorithm (e.g., Bresenham).

In log-odds form with prior \( p_0 \), the per-cell update implied by this inverse model is: if a cell is labeled “free”, add \( \operatorname{logit}(p_{free}) - \operatorname{logit}(p_0) \); if “occupied”, add \( \operatorname{logit}(p_{occ}) - \operatorname{logit}(p_0) \); otherwise add 0. This is exactly what the implementations do.

flowchart TD
  A["Inputs: pose (x,y,theta), beam angle phi, range z, max range zMax"] --> B["Compute world beam angle a = theta + phi"]
  B --> C["Compute endpoint (xe, ye) = (x + z*cos(a), y + z*sin(a))"]
  C --> D["Traverse grid cells along segment start->end"]
  D --> E["If z below zMax: \nmark last cell as occupied"]
  D --> F["Mark all preceding cells as free"]
  E --> G["Clamp logOdds to [Lmin, Lmax]"]
  F --> G
  G --> H["Continue with next beam"]
        

Resolution vs computational load. If the mapped rectangle has physical size \( L_x \times L_y \), then the grid has \( W = \lfloor L_x/r \rfloor \) columns and \( H = \lfloor L_y/r \rfloor \) rows, so the cell count is \( N = WH \). Memory and update time scale roughly linearly with \( N \) for storage, and with \( t \times K \times \bar{L} \) for updates, where \( t \) is time steps, \( K \) beams per scan, and \( \bar{L} \) average number of traversed cells per beam.

Clamping / saturation. In log-odds form, repeated evidence can drive \( \ell_t(i) \) to large magnitude. Practical mappers clamp log-odds: \( \ell_t(i) ← \min(\ell_{max}, \max(\ell_{min}, \ell_t(i))) \). This prevents numerical overflow and avoids overconfidence under model mismatch.

Static-world assumption. Classic occupancy grids assume the map is static while the robot moves. In real deployments, people and moving obstacles violate this assumption and create “ghost” artifacts. Later chapters address navigation stacks that mitigate dynamics via temporal filtering and layered costmaps.

The following implementations all follow the same core structure: (i) store an occupancy grid as log-odds; (ii) simulate range rays in a small synthetic environment; (iii) update free/occupied cells along each ray; and (iv) visualize or export the resulting occupancy probability map.

6.1 Python

Chapter9_Lesson1.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# Chapter9_Lesson1.py
#
# Occupancy Grid Mapping (2D) — reference implementation for a university-level AMR course.
# This script simulates a simple environment, a robot trajectory, synthetic 2D LiDAR scans,
# and builds an occupancy grid map using log-odds updates.

from __future__ import annotations
import math
from dataclasses import dataclass
from typing import List, Tuple

import numpy as np
import matplotlib.pyplot as plt


def logit(p: float) -> float:
    p = float(np.clip(p, 1e-9, 1 - 1e-9))
    return math.log(p / (1.0 - p))


def sigmoid(x: float) -> float:
    # numerically stable sigmoid for scalar
    if x >= 0:
        z = math.exp(-x)
        return 1.0 / (1.0 + z)
    z = math.exp(x)
    return z / (1.0 + z)


@dataclass
class GridSpec:
    width: int           # number of columns
    height: int          # number of rows
    resolution: float    # meters per cell
    origin_x: float      # world x of grid cell (0,0) lower-left
    origin_y: float      # world y of grid cell (0,0) lower-left


class OccupancyGrid:
    """
    Stores log-odds for each cell.
    Probability can be obtained via p = sigmoid(l).
    """

    def __init__(
        self,
        spec: GridSpec,
        p0: float = 0.5,
        p_occ: float = 0.7,
        p_free: float = 0.3,
        l_min: float = -10.0,
        l_max: float = 10.0,
    ):
        self.spec = spec
        self.l0 = logit(p0)
        self.l_occ = logit(p_occ)
        self.l_free = logit(p_free)
        self.l_min = l_min
        self.l_max = l_max
        self.log_odds = np.full((spec.height, spec.width), self.l0, dtype=np.float64)

    def world_to_grid(self, x: float, y: float) -> Tuple[int, int]:
        gx = int(math.floor((x - self.spec.origin_x) / self.spec.resolution))
        gy = int(math.floor((y - self.spec.origin_y) / self.spec.resolution))
        return gx, gy

    def grid_to_world_center(self, gx: int, gy: int) -> Tuple[float, float]:
        x = self.spec.origin_x + (gx + 0.5) * self.spec.resolution
        y = self.spec.origin_y + (gy + 0.5) * self.spec.resolution
        return x, y

    def in_bounds(self, gx: int, gy: int) -> bool:
        return (0 <= gx < self.spec.width) and (0 <= gy < self.spec.height)

    def clamp(self):
        np.clip(self.log_odds, self.l_min, self.l_max, out=self.log_odds)

    def update_cell_logodds(self, gx: int, gy: int, l_update: float):
        if not self.in_bounds(gx, gy):
            return
        self.log_odds[gy, gx] += (l_update - self.l0)  # standard log-odds increment
        # (equivalent to l_t = l_{t-1} + l_inv - l0)
        # clamp per-update for safety
        if self.log_odds[gy, gx] > self.l_max:
            self.log_odds[gy, gx] = self.l_max
        elif self.log_odds[gy, gx] < self.l_min:
            self.log_odds[gy, gx] = self.l_min

    def probability_grid(self) -> np.ndarray:
        # sigmoid applied elementwise
        return 1.0 / (1.0 + np.exp(-self.log_odds))


def bresenham(x0: int, y0: int, x1: int, y1: int) -> List[Tuple[int, int]]:
    """
    Bresenham line traversal returning grid indices from (x0,y0) to (x1,y1), inclusive.
    """
    points: List[Tuple[int, int]] = []
    dx = abs(x1 - x0)
    dy = abs(y1 - y0)
    sx = 1 if x0 < x1 else -1
    sy = 1 if y0 < y1 else -1
    err = dx - dy
    x, y = x0, y0
    while True:
        points.append((x, y))
        if x == x1 and y == y1:
            break
        e2 = 2 * err
        if e2 > -dy:
            err -= dy
            x += sx
        if e2 < dx:
            err += dx
            y += sy
    return points


# --- Simple environment and ray casting for synthetic LiDAR --- #

@dataclass
class Segment:
    x0: float
    y0: float
    x1: float
    y1: float


def ray_segment_intersection(
    rx: float, ry: float, rdx: float, rdy: float,
    s: Segment
) -> float | None:
    """
    Ray (rx,ry) + t*(rdx,rdy), t>=0 with segment s.
    Returns smallest positive t where intersection occurs, else None.
    """
    x1, y1, x2, y2 = s.x0, s.y0, s.x1, s.y1
    # Solve intersection between parametric ray and segment
    # Using 2D cross product approach:
    # ray: p + t r, segment: q + u s
    # t = (q - p) x s / (r x s), u = (q - p) x r / (r x s)
    rxs = rdx * (y2 - y1) - rdy * (x2 - x1)
    if abs(rxs) < 1e-12:
        return None  # parallel
    qpx = x1 - rx
    qpy = y1 - ry
    t = (qpx * (y2 - y1) - qpy * (x2 - x1)) / rxs
    u = (qpx * rdy - qpy * rdx) / rxs
    if t >= 0.0 and 0.0 <= u <= 1.0:
        return t
    return None


def simulate_lidar_scan(
    pose: Tuple[float, float, float],
    segments: List[Segment],
    angles: np.ndarray,
    z_max: float,
) -> np.ndarray:
    """
    Returns ranges for each beam angle relative to robot heading.
    """
    x, y, th = pose
    ranges = np.full_like(angles, z_max, dtype=np.float64)
    for i, a in enumerate(angles):
        wa = th + a
        rdx, rdy = math.cos(wa), math.sin(wa)
        t_min = None
        for seg in segments:
            t = ray_segment_intersection(x, y, rdx, rdy, seg)
            if t is not None and t <= z_max:
                if (t_min is None) or (t < t_min):
                    t_min = t
        if t_min is not None:
            ranges[i] = t_min
    return ranges


def build_box_world(xmin: float, ymin: float, xmax: float, ymax: float) -> List[Segment]:
    """
    Axis-aligned rectangular boundary as four segments.
    """
    return [
        Segment(xmin, ymin, xmax, ymin),
        Segment(xmax, ymin, xmax, ymax),
        Segment(xmax, ymax, xmin, ymax),
        Segment(xmin, ymax, xmin, ymin),
    ]


def add_vertical_wall(x: float, y0: float, y1: float) -> Segment:
    return Segment(x, y0, x, y1)


def add_horizontal_wall(y: float, x0: float, x1: float) -> Segment:
    return Segment(x0, y, x1, y)


def update_grid_from_scan(
    grid: OccupancyGrid,
    pose: Tuple[float, float, float],
    angles: np.ndarray,
    ranges: np.ndarray,
    z_max: float,
):
    """
    Inverse sensor model:
    - cells along the ray up to (hit cell exclusive) are updated as FREE
    - the end cell is updated as OCCUPIED if range < z_max (hit)
    """
    x, y, th = pose
    gx0, gy0 = grid.world_to_grid(x, y)

    for a, r in zip(angles, ranges):
        wa = th + float(a)
        ex = x + float(r) * math.cos(wa)
        ey = y + float(r) * math.sin(wa)

        gx1, gy1 = grid.world_to_grid(ex, ey)

        # traverse cells from robot to endpoint
        cells = bresenham(gx0, gy0, gx1, gy1)
        if len(cells) == 0:
            continue

        hit = (float(r) < float(z_max) - 1e-12)

        # free cells: all but last (or all if no hit, conservative: treat all traversed as free)
        free_cells = cells[:-1] if hit else cells
        for (gx, gy) in free_cells:
            grid.update_cell_logodds(gx, gy, grid.l_free)

        # occupied: last cell only if hit
        if hit:
            lx, ly = cells[-1]
            grid.update_cell_logodds(lx, ly, grid.l_occ)


def main():
    # --- Map and sensor settings ---
    spec = GridSpec(
        width=220,
        height=180,
        resolution=0.1,
        origin_x=-11.0,
        origin_y=-9.0
    )
    grid = OccupancyGrid(spec, p0=0.5, p_occ=0.75, p_free=0.30, l_min=-8.0, l_max=8.0)

    # LiDAR: 180 deg scan, 181 beams
    fov = math.radians(180.0)
    n_beams = 181
    angles = np.linspace(-fov / 2.0, fov / 2.0, n_beams)
    z_max = 10.0

    # --- Synthetic environment (walls) ---
    segments: List[Segment] = []
    segments += build_box_world(-10.0, -8.0, 10.0, 8.0)   # boundary
    segments.append(add_vertical_wall(0.0, -6.0, 6.0))    # internal wall
    segments.append(add_horizontal_wall(2.0, -8.0, -2.0)) # short wall

    # --- Robot trajectory (simple loop) ---
    poses: List[Tuple[float, float, float]] = []
    for t in range(120):
        # move on an ellipse, slowly rotating heading
        ang = 2.0 * math.pi * (t / 120.0)
        x = -6.0 + 6.0 * math.cos(ang)
        y =  0.0 + 4.0 * math.sin(ang)
        th = ang + math.pi / 2.0
        poses.append((x, y, th))

    # --- Mapping loop ---
    for pose in poses:
        ranges = simulate_lidar_scan(pose, segments, angles, z_max)
        # optional: add small noise
        ranges = np.clip(ranges + np.random.normal(0.0, 0.02, size=ranges.shape), 0.0, z_max)
        update_grid_from_scan(grid, pose, angles, ranges, z_max)

    # --- Visualization ---
    p = grid.probability_grid()
    plt.figure(figsize=(9, 6))
    plt.imshow(
        p,
        origin="lower",
        extent=[
            spec.origin_x,
            spec.origin_x + spec.width * spec.resolution,
            spec.origin_y,
            spec.origin_y + spec.height * spec.resolution,
        ],
        vmin=0.0, vmax=1.0
    )
    plt.colorbar(label="P(occupied)")
    plt.title("Occupancy Grid Map (probability)")
    plt.xlabel("x [m]")
    plt.ylabel("y [m]")
    plt.tight_layout()
    plt.show()


if __name__ == "__main__":
    main()
      

6.2 C++

Chapter9_Lesson1.cpp

// Chapter9_Lesson1.cpp
//
// Occupancy Grid Mapping (2D) — C++ reference implementation.
// Simulates a simple environment, generates synthetic 2D LiDAR scans,
// and builds an occupancy grid map using log-odds updates.
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter9_Lesson1.cpp -o Chapter9_Lesson1
// Run:
//   ./Chapter9_Lesson1

#include <cmath>
#include <cstdint>
#include <iostream>
#include <limits>
#include <random>
#include <string>
#include <tuple>
#include <utility>
#include <vector>

struct GridSpec {
  int width;
  int height;
  double resolution;
  double origin_x;
  double origin_y;
};

static inline double clamp(double x, double lo, double hi) {
  if (x < lo) return lo;
  if (x > hi) return hi;
  return x;
}

static inline double logit(double p) {
  p = clamp(p, 1e-9, 1.0 - 1e-9);
  return std::log(p / (1.0 - p));
}

static inline double sigmoid(double x) {
  // stable sigmoid
  if (x >= 0.0) {
    double z = std::exp(-x);
    return 1.0 / (1.0 + z);
  }
  double z = std::exp(x);
  return z / (1.0 + z);
}

struct OccupancyGrid {
  GridSpec spec;
  double l0;
  double l_occ;
  double l_free;
  double l_min;
  double l_max;
  std::vector<double> log_odds; // row-major [gy*W + gx]

  OccupancyGrid(const GridSpec& s,
                double p0 = 0.5,
                double p_occ = 0.75,
                double p_free = 0.30,
                double lmin = -8.0,
                double lmax = 8.0)
      : spec(s),
        l0(logit(p0)),
        l_occ(logit(p_occ)),
        l_free(logit(p_free)),
        l_min(lmin),
        l_max(lmax),
        log_odds(static_cast<size_t>(s.width * s.height), l0) {}

  inline bool in_bounds(int gx, int gy) const {
    return (0 <= gx && gx < spec.width && 0 <= gy && gy < spec.height);
  }

  inline std::pair<int,int> world_to_grid(double x, double y) const {
    int gx = static_cast<int>(std::floor((x - spec.origin_x) / spec.resolution));
    int gy = static_cast<int>(std::floor((y - spec.origin_y) / spec.resolution));
    return {gx, gy};
  }

  inline void update_cell(int gx, int gy, double l_update) {
    if (!in_bounds(gx, gy)) return;
    size_t idx = static_cast<size_t>(gy * spec.width + gx);
    log_odds[idx] += (l_update - l0);
    if (log_odds[idx] > l_max) log_odds[idx] = l_max;
    if (log_odds[idx] < l_min) log_odds[idx] = l_min;
  }

  inline double prob(int gx, int gy) const {
    if (!in_bounds(gx, gy)) return 0.5;
    size_t idx = static_cast<size_t>(gy * spec.width + gx);
    return sigmoid(log_odds[idx]);
  }
};

// Bresenham line traversal (inclusive endpoints)
static std::vector<std::pair<int,int>> bresenham(int x0, int y0, int x1, int y1) {
  std::vector<std::pair<int,int>> pts;
  int dx = std::abs(x1 - x0);
  int dy = std::abs(y1 - y0);
  int sx = (x0 < x1) ? 1 : -1;
  int sy = (y0 < y1) ? 1 : -1;
  int err = dx - dy;
  int x = x0, y = y0;
  for (;;) {
    pts.push_back({x, y});
    if (x == x1 && y == y1) break;
    int e2 = 2 * err;
    if (e2 > -dy) { err -= dy; x += sx; }
    if (e2 <  dx) { err += dx; y += sy; }
  }
  return pts;
}

struct Segment {
  double x0, y0, x1, y1;
};

// Ray-segment intersection: ray p + t*r, t>=0. Return smallest t if intersects segment.
static double ray_segment_intersection(double rx, double ry, double rdx, double rdy,
                                      const Segment& s, bool& hit) {
  // Cross product approach
  double x1 = s.x0, y1 = s.y0, x2 = s.x1, y2 = s.y1;
  double sx = x2 - x1;
  double sy = y2 - y1;
  double rxs = rdx * sy - rdy * sx;
  if (std::abs(rxs) < 1e-12) { hit = false; return 0.0; } // parallel
  double qpx = x1 - rx;
  double qpy = y1 - ry;
  double t = (qpx * sy - qpy * sx) / rxs;
  double u = (qpx * rdy - qpy * rdx) / rxs;
  if (t >= 0.0 && u >= 0.0 && u <= 1.0) { hit = true; return t; }
  hit = false; return 0.0;
}

static std::vector<Segment> build_box_world(double xmin, double ymin, double xmax, double ymax) {
  return {
    {xmin, ymin, xmax, ymin},
    {xmax, ymin, xmax, ymax},
    {xmax, ymax, xmin, ymax},
    {xmin, ymax, xmin, ymin}
  };
}

static Segment vertical_wall(double x, double y0, double y1) {
  return {x, y0, x, y1};
}

static Segment horizontal_wall(double y, double x0, double x1) {
  return {x0, y, x1, y};
}

static std::vector<double> simulate_lidar_scan(const std::tuple<double,double,double>& pose,
                                        const std::vector<Segment>& segs,
                                        const std::vector<double>& angles,
                                        double z_max) {
  double x = std::get<0>(pose);
  double y = std::get<1>(pose);
  double th = std::get<2>(pose);

  std::vector<double> ranges(angles.size(), z_max);
  for (size_t i = 0; i < angles.size(); ++i) {
    double wa = th + angles[i];
    double rdx = std::cos(wa);
    double rdy = std::sin(wa);

    double tmin = std::numeric_limits<double>::infinity();
    bool any = false;
    for (const auto& s : segs) {
      bool hit = false;
      double t = ray_segment_intersection(x, y, rdx, rdy, s, hit);
      if (hit && t <= z_max && t < tmin) {
        tmin = t;
        any = true;
      }
    }
    if (any) ranges[i] = tmin;
  }
  return ranges;
}

static void update_from_scan(OccupancyGrid& grid,
                             const std::tuple<double,double,double>& pose,
                             const std::vector<double>& angles,
                             const std::vector<double>& ranges,
                             double z_max) {
  double x = std::get<0>(pose);
  double y = std::get<1>(pose);
  double th = std::get<2>(pose);

  auto [gx0, gy0] = grid.world_to_grid(x, y);

  for (size_t i = 0; i < angles.size(); ++i) {
    double a = angles[i];
    double r = ranges[i];
    double wa = th + a;
    double ex = x + r * std::cos(wa);
    double ey = y + r * std::sin(wa);

    auto [gx1, gy1] = grid.world_to_grid(ex, ey);
    auto cells = bresenham(gx0, gy0, gx1, gy1);
    if (cells.empty()) continue;

    bool hit = (r < (z_max - 1e-12));
    // Free cells: all but last if hit; otherwise all
    size_t free_count = hit ? (cells.size() - 1) : cells.size();
    for (size_t k = 0; k < free_count; ++k) {
      grid.update_cell(cells[k].first, cells[k].second, grid.l_free);
    }
    // Occupied: last if hit
    if (hit) {
      auto [lx, ly] = cells.back();
      grid.update_cell(lx, ly, grid.l_occ);
    }
  }
}

int main() {
  GridSpec spec{220, 180, 0.1, -11.0, -9.0};
  OccupancyGrid grid(spec, 0.5, 0.75, 0.30, -8.0, 8.0);

  // LiDAR angles: 180 deg, 181 beams
  const double fov = M_PI; // 180 deg
  const int n_beams = 181;
  std::vector<double> angles;
  angles.reserve(static_cast<size_t>(n_beams));
  for (int i = 0; i < n_beams; ++i) {
    double a = -fov/2.0 + (fov * i) / (n_beams - 1);
    angles.push_back(a);
  }
  const double z_max = 10.0;

  // Environment
  std::vector<Segment> segs = build_box_world(-10.0, -8.0, 10.0, 8.0);
  segs.push_back(vertical_wall(0.0, -6.0, 6.0));
  segs.push_back(horizontal_wall(2.0, -8.0, -2.0));

  // Trajectory
  std::vector<std::tuple<double,double,double>> poses;
  const int T = 120;
  poses.reserve(static_cast<size_t>(T));
  for (int t = 0; t < T; ++t) {
    double ang = 2.0 * M_PI * (static_cast<double>(t) / T);
    double x = -6.0 + 6.0 * std::cos(ang);
    double y =  0.0 + 4.0 * std::sin(ang);
    double th = ang + M_PI/2.0;
    poses.emplace_back(x, y, th);
  }

  // Noise
  std::mt19937_64 rng(0);
  std::normal_distribution<double> noise(0.0, 0.02);

  // Mapping loop
  for (const auto& pose : poses) {
    auto ranges = simulate_lidar_scan(pose, segs, angles, z_max);
    for (auto& r : ranges) {
      r = clamp(r + noise(rng), 0.0, z_max);
    }
    update_from_scan(grid, pose, angles, ranges, z_max);
  }

  // Minimal output: write occupancy probabilities as ASCII PGM-like grayscale (0..255)
  // This keeps the example dependency-free.
  // Occupied high probability -> darker pixel (closer to 0).
  std::cout << "P2\n" << spec.width << " " << spec.height << "\n255\n";
  for (int gy = spec.height - 1; gy >= 0; --gy) { // print top-to-bottom
    for (int gx = 0; gx < spec.width; ++gx) {
      double p = grid.prob(gx, gy);
      int pix = static_cast<int>(std::round((1.0 - p) * 255.0));
      pix = static_cast<int>(clamp(pix, 0, 255));
      std::cout << pix << " ";
    }
    std::cout << "\n";
  }

  std::cerr << "Wrote ASCII PGM to stdout. Save as map.pgm and view with an image viewer.\n";
  std::cerr << "Example: ./Chapter9_Lesson1 > map.pgm\n";
  return 0;
}
      

6.3 Java

Chapter9_Lesson1.java

// Chapter9_Lesson1.java
//
// Occupancy Grid Mapping (2D) — Java reference implementation.
// Simulates a simple environment, generates synthetic 2D LiDAR scans,
// and builds an occupancy grid map using log-odds updates.
// Output: ASCII PGM written to stdout (portable, dependency-free).
//
// Compile:
//   javac Chapter9_Lesson1.java
// Run:
//   java Chapter9_Lesson1 > map.pgm

import java.util.ArrayList;
import java.util.List;
import java.util.Locale;
import java.util.Random;

public class Chapter9_Lesson1 {

    static class GridSpec {
        int width, height;
        double resolution;
        double originX, originY;

        GridSpec(int w, int h, double res, double ox, double oy) {
            width = w; height = h; resolution = res; originX = ox; originY = oy;
        }
    }

    static double clamp(double x, double lo, double hi) {
        return Math.max(lo, Math.min(hi, x));
    }

    static double logit(double p) {
        p = clamp(p, 1e-9, 1.0 - 1e-9);
        return Math.log(p / (1.0 - p));
    }

    static double sigmoid(double x) {
        if (x >= 0.0) {
            double z = Math.exp(-x);
            return 1.0 / (1.0 + z);
        } else {
            double z = Math.exp(x);
            return z / (1.0 + z);
        }
    }

    static class OccupancyGrid {
        GridSpec spec;
        double l0, lOcc, lFree, lMin, lMax;
        double[] logOdds; // row-major

        OccupancyGrid(GridSpec spec, double p0, double pOcc, double pFree, double lMin, double lMax) {
            this.spec = spec;
            this.l0 = logit(p0);
            this.lOcc = logit(pOcc);
            this.lFree = logit(pFree);
            this.lMin = lMin;
            this.lMax = lMax;
            this.logOdds = new double[spec.width * spec.height];
            for (int i = 0; i < logOdds.length; i++) logOdds[i] = l0;
        }

        boolean inBounds(int gx, int gy) {
            return (0 <= gx && gx < spec.width && 0 <= gy && gy < spec.height);
        }

        int[] worldToGrid(double x, double y) {
            int gx = (int)Math.floor((x - spec.originX) / spec.resolution);
            int gy = (int)Math.floor((y - spec.originY) / spec.resolution);
            return new int[]{gx, gy};
        }

        void updateCell(int gx, int gy, double lUpdate) {
            if (!inBounds(gx, gy)) return;
            int idx = gy * spec.width + gx;
            logOdds[idx] += (lUpdate - l0);
            if (logOdds[idx] > lMax) logOdds[idx] = lMax;
            if (logOdds[idx] < lMin) logOdds[idx] = lMin;
        }

        double prob(int gx, int gy) {
            if (!inBounds(gx, gy)) return 0.5;
            int idx = gy * spec.width + gx;
            return sigmoid(logOdds[idx]);
        }
    }

    static class Segment {
        double x0, y0, x1, y1;
        Segment(double x0, double y0, double x1, double y1) {
            this.x0=x0; this.y0=y0; this.x1=x1; this.y1=y1;
        }
    }

    // Ray/segment intersection. Returns t (distance along ray) if intersects, otherwise NaN.
    static double raySegmentIntersection(double rx, double ry, double rdx, double rdy, Segment s) {
        double sx = s.x1 - s.x0;
        double sy = s.y1 - s.y0;
        double rxs = rdx * sy - rdy * sx;
        if (Math.abs(rxs) < 1e-12) return Double.NaN; // parallel
        double qpx = s.x0 - rx;
        double qpy = s.y0 - ry;
        double t = (qpx * sy - qpy * sx) / rxs;
        double u = (qpx * rdy - qpy * rdx) / rxs;
        if (t >= 0.0 && u >= 0.0 && u <= 1.0) return t;
        return Double.NaN;
    }

    static List<Segment> buildBoxWorld(double xmin, double ymin, double xmax, double ymax) {
        List<Segment> segs = new ArrayList<>();
        segs.add(new Segment(xmin, ymin, xmax, ymin));
        segs.add(new Segment(xmax, ymin, xmax, ymax));
        segs.add(new Segment(xmax, ymax, xmin, ymax));
        segs.add(new Segment(xmin, ymax, xmin, ymin));
        return segs;
    }

    static Segment verticalWall(double x, double y0, double y1) { return new Segment(x, y0, x, y1); }
    static Segment horizontalWall(double y, double x0, double x1) { return new Segment(x0, y, x1, y); }

    static double[] simulateLidarScan(double x, double y, double th, List<Segment> segs, double[] angles, double zMax) {
        double[] ranges = new double[angles.length];
        for (int i = 0; i < ranges.length; i++) ranges[i] = zMax;

        for (int i = 0; i < angles.length; i++) {
            double wa = th + angles[i];
            double rdx = Math.cos(wa);
            double rdy = Math.sin(wa);

            double tmin = Double.POSITIVE_INFINITY;
            boolean any = false;
            for (Segment s : segs) {
                double t = raySegmentIntersection(x, y, rdx, rdy, s);
                if (!Double.isNaN(t) && t <= zMax && t < tmin) {
                    tmin = t;
                    any = true;
                }
            }
            if (any) ranges[i] = tmin;
        }
        return ranges;
    }

    // Bresenham inclusive
    static List<int[]> bresenham(int x0, int y0, int x1, int y1) {
        List<int[]> pts = new ArrayList<>();
        int dx = Math.abs(x1 - x0);
        int dy = Math.abs(y1 - y0);
        int sx = (x0 < x1) ? 1 : -1;
        int sy = (y0 < y1) ? 1 : -1;
        int err = dx - dy;
        int x = x0, y = y0;
        while (true) {
            pts.add(new int[]{x, y});
            if (x == x1 && y == y1) break;
            int e2 = 2 * err;
            if (e2 > -dy) { err -= dy; x += sx; }
            if (e2 <  dx) { err += dx; y += sy; }
        }
        return pts;
    }

    static void updateFromScan(OccupancyGrid grid, double x, double y, double th,
                               double[] angles, double[] ranges, double zMax) {
        int[] g0 = grid.worldToGrid(x, y);
        int gx0 = g0[0], gy0 = g0[1];

        for (int i = 0; i < angles.length; i++) {
            double a = angles[i];
            double r = ranges[i];
            double wa = th + a;
            double ex = x + r * Math.cos(wa);
            double ey = y + r * Math.sin(wa);

            int[] g1 = grid.worldToGrid(ex, ey);
            int gx1 = g1[0], gy1 = g1[1];

            List<int[]> cells = bresenham(gx0, gy0, gx1, gy1);
            if (cells.isEmpty()) continue;

            boolean hit = (r < (zMax - 1e-12));
            int freeCount = hit ? (cells.size() - 1) : cells.size();

            for (int k = 0; k < freeCount; k++) {
                int[] c = cells.get(k);
                grid.updateCell(c[0], c[1], grid.lFree);
            }
            if (hit) {
                int[] last = cells.get(cells.size() - 1);
                grid.updateCell(last[0], last[1], grid.lOcc);
            }
        }
    }

    public static void main(String[] args) {
        Locale.setDefault(Locale.US);

        GridSpec spec = new GridSpec(220, 180, 0.1, -11.0, -9.0);
        OccupancyGrid grid = new OccupancyGrid(spec, 0.5, 0.75, 0.30, -8.0, 8.0);

        // LiDAR angles: 180 deg, 181 beams
        int nBeams = 181;
        double fov = Math.PI;
        double[] angles = new double[nBeams];
        for (int i = 0; i < nBeams; i++) {
            angles[i] = -fov/2.0 + (fov * i) / (nBeams - 1);
        }
        double zMax = 10.0;

        // Environment
        List<Segment> segs = buildBoxWorld(-10.0, -8.0, 10.0, 8.0);
        segs.add(verticalWall(0.0, -6.0, 6.0));
        segs.add(horizontalWall(2.0, -8.0, -2.0));

        // Trajectory and noise
        Random rng = new Random(0);
        int T = 120;
        double noiseStd = 0.02;

        for (int t = 0; t < T; t++) {
            double ang = 2.0 * Math.PI * ((double)t / (double)T);
            double x = -6.0 + 6.0 * Math.cos(ang);
            double y =  0.0 + 4.0 * Math.sin(ang);
            double th = ang + Math.PI/2.0;

            double[] ranges = simulateLidarScan(x, y, th, segs, angles, zMax);
            for (int i = 0; i < ranges.length; i++) {
                // gaussian noise (Box-Muller)
                double u1 = Math.max(1e-12, rng.nextDouble());
                double u2 = rng.nextDouble();
                double z = Math.sqrt(-2.0 * Math.log(u1)) * Math.cos(2.0 * Math.PI * u2);
                ranges[i] = clamp(ranges[i] + noiseStd * z, 0.0, zMax);
            }
            updateFromScan(grid, x, y, th, angles, ranges, zMax);
        }

        // Output ASCII PGM to stdout (P2)
        System.out.println("P2");
        System.out.println(spec.width + " " + spec.height);
        System.out.println("255");
        for (int gy = spec.height - 1; gy >= 0; gy--) { // top to bottom
            StringBuilder sb = new StringBuilder();
            for (int gx = 0; gx < spec.width; gx++) {
                double p = grid.prob(gx, gy);
                int pix = (int)Math.round((1.0 - p) * 255.0);
                pix = (int)clamp(pix, 0, 255);
                sb.append(pix).append(' ');
            }
            System.out.println(sb.toString());
        }

        System.err.println("Wrote ASCII PGM to stdout. Save as map.pgm and view with an image viewer.");
        System.err.println("Example: java Chapter9_Lesson1 > map.pgm");
    }
}
      

6.4 MATLAB / Simulink

Chapter9_Lesson1.m

% Chapter9_Lesson1.m
%
% Occupancy Grid Mapping (2D) — MATLAB reference implementation.
% Simulates a simple environment, generates synthetic 2D LiDAR scans,
% and builds an occupancy grid map using log-odds updates.
%
% Run:
%   Chapter9_Lesson1

function Chapter9_Lesson1()

  % --- Map / sensor settings ---
  spec.width = 220;
  spec.height = 180;
  spec.res = 0.1;
  spec.ox = -11.0;
  spec.oy = -9.0;

  p0 = 0.5;
  pOcc = 0.75;
  pFree = 0.30;
  l0 = logit(p0);
  lOcc = logit(pOcc);
  lFree = logit(pFree);
  lMin = -8.0;
  lMax = 8.0;

  logOdds = l0 * ones(spec.height, spec.width);

  % LiDAR: 180 deg, 181 beams
  fov = pi;
  nBeams = 181;
  angles = linspace(-fov/2, fov/2, nBeams);
  zMax = 10.0;

  % --- Synthetic environment: boundary + internal walls (segments) ---
  segs = {};
  segs = [segs; buildBoxWorld(-10.0, -8.0, 10.0, 8.0)];
  segs{end+1} = struct('x0',0.0,'y0',-6.0,'x1',0.0,'y1',6.0);     % vertical wall
  segs{end+1} = struct('x0',-8.0,'y0',2.0,'x1',-2.0,'y1',2.0);    % horizontal wall

  % --- Trajectory ---
  T = 120;
  poses = zeros(T,3);
  for t = 1:T
    ang = 2*pi*(t-1)/T;
    poses(t,1) = -6.0 + 6.0*cos(ang);
    poses(t,2) =  0.0 + 4.0*sin(ang);
    poses(t,3) = ang + pi/2;
  end

  rng(0);

  % --- Mapping loop ---
  for t = 1:T
    x = poses(t,1); y = poses(t,2); th = poses(t,3);
    ranges = simulateLidarScan([x y th], segs, angles, zMax);
    ranges = min(max(ranges + 0.02*randn(size(ranges)), 0.0), zMax);
    logOdds = updateFromScan(logOdds, spec, l0, lOcc, lFree, lMin, lMax, [x y th], angles, ranges, zMax);
  end

  % --- Visualization ---
  p = 1.0 ./ (1.0 + exp(-logOdds));
  figure; imagesc( ...
    [spec.ox, spec.ox + spec.width*spec.res], ...
    [spec.oy, spec.oy + spec.height*spec.res], ...
    p ...
  );
  set(gca,'YDir','normal');
  colorbar; caxis([0 1]);
  title('Occupancy Grid Map (probability)');
  xlabel('x [m]'); ylabel('y [m]');

end

% ===== Helpers =====

function y = clamp(x, lo, hi)
  y = min(max(x, lo), hi);
end

function l = logit(p)
  p = clamp(p, 1e-9, 1 - 1e-9);
  l = log(p/(1-p));
end

function [gx,gy] = worldToGrid(spec, x, y)
  gx = floor((x - spec.ox) / spec.res);
  gy = floor((y - spec.oy) / spec.res);
end

function pts = bresenham(x0,y0,x1,y1)
  dx = abs(x1-x0);
  dy = abs(y1-y0);
  sx = 1; if x0 > x1, sx = -1; end
  sy = 1; if y0 > y1, sy = -1; end
  err = dx - dy;
  x = x0; y = y0;
  pts = [];
  while true
    pts = [pts; x y];
    if x==x1 && y==y1
      break;
    end
    e2 = 2*err;
    if e2 > -dy
      err = err - dy;
      x = x + sx;
    end
    if e2 < dx
      err = err + dx;
      y = y + sy;
    end
  end
end

function segs = buildBoxWorld(xmin, ymin, xmax, ymax)
  segs = {
    struct('x0',xmin,'y0',ymin,'x1',xmax,'y1',ymin);
    struct('x0',xmax,'y0',ymin,'x1',xmax,'y1',ymax);
    struct('x0',xmax,'y0',ymax,'x1',xmin,'y1',ymax);
    struct('x0',xmin,'y0',ymax,'x1',xmin,'y1',ymin);
  };
end

function t = raySegmentIntersection(rx, ry, rdx, rdy, s)
  sx = s.x1 - s.x0;
  sy = s.y1 - s.y0;
  rxs = rdx*sy - rdy*sx;
  if abs(rxs) < 1e-12
    t = NaN; return;
  end
  qpx = s.x0 - rx;
  qpy = s.y0 - ry;
  tCand = (qpx*sy - qpy*sx) / rxs;
  u = (qpx*rdy - qpy*rdx) / rxs;
  if tCand >= 0.0 && u >= 0.0 && u <= 1.0
    t = tCand;
  else
    t = NaN;
  end
end

function ranges = simulateLidarScan(pose, segs, angles, zMax)
  x = pose(1); y = pose(2); th = pose(3);
  ranges = zMax * ones(size(angles));
  for i = 1:length(angles)
    wa = th + angles(i);
    rdx = cos(wa); rdy = sin(wa);
    tmin = inf; any = false;
    for j = 1:length(segs)
      s = segs{j};
      t = raySegmentIntersection(x, y, rdx, rdy, s);
      if ~isnan(t) && t <= zMax && t < tmin
        tmin = t; any = true;
      end
    end
    if any
      ranges(i) = tmin;
    end
  end
end

function logOdds = updateFromScan(logOdds, spec, l0, lOcc, lFree, lMin, lMax, pose, angles, ranges, zMax)
  x = pose(1); y = pose(2); th = pose(3);
  [gx0, gy0] = worldToGrid(spec, x, y);

  for i = 1:length(angles)
    a = angles(i); r = ranges(i);
    wa = th + a;
    ex = x + r*cos(wa);
    ey = y + r*sin(wa);

    [gx1, gy1] = worldToGrid(spec, ex, ey);
    cells = bresenham(gx0, gy0, gx1, gy1);
    if isempty(cells), continue; end

    hit = (r < (zMax - 1e-12));
    if hit
      freeCells = cells(1:end-1,:);
    else
      freeCells = cells;
    end

    for k = 1:size(freeCells,1)
      gx = freeCells(k,1); gy = freeCells(k,2);
      if 0 <= gx && gx < spec.width && 0 <= gy && gy < spec.height
        logOdds(gy+1, gx+1) = clamp(logOdds(gy+1, gx+1) + (lFree - l0), lMin, lMax);
      end
    end

    if hit
      gx = cells(end,1); gy = cells(end,2);
      if 0 <= gx && gx < spec.width && 0 <= gy && gy < spec.height
        logOdds(gy+1, gx+1) = clamp(logOdds(gy+1, gx+1) + (lOcc - l0), lMin, lMax);
      end
    end
  end
end
      

6.5 Wolfram Mathematica

Chapter9_Lesson1.nb

(* Chapter9_Lesson1.nb (Wolfram Language script content)
   Occupancy Grid Mapping (2D) — Mathematica reference implementation.
   Generates a synthetic environment, ray-casts a simple LiDAR, and builds
   an occupancy grid with log-odds updates. Produces an ArrayPlot of P(occupied).
*)

ClearAll["Global`*"];

Clamp[x_, lo_, hi_] := Max[lo, Min[hi, x]];
Logit[p_] := Module[{pp = Clamp[p, 10^-9, 1 - 10^-9]}, Log[pp/(1 - pp)]];
Sigmoid[x_] := 1/(1 + Exp[-x]);

(* Grid specification *)
spec = <|
   "W" -> 220, "H" -> 180,
   "r" -> 0.1,
   "ox" -> -11.0, "oy" -> -9.0
|>;

p0 = 0.5; pOcc = 0.75; pFree = 0.30;
l0 = Logit[p0]; lOcc = Logit[pOcc]; lFree = Logit[pFree];
lMin = -8.0; lMax = 8.0;

logOdds = ConstantArray[l0, {spec["H"], spec["W"]}];

WorldToGrid[{x_, y_}] := Module[{gx, gy},
  gx = Floor[(x - spec["ox"])/spec["r"]];
  gy = Floor[(y - spec["oy"])/spec["r"]];
  {gx, gy}
];

InBoundsQ[{gx_, gy_}] := (0 <= gx < spec["W"] && 0 <= gy < spec["H"]);

UpdateCell[{gx_, gy_}, lUpdate_] := Module[{},
  If[!InBoundsQ[{gx, gy}], Return[]];
  logOdds[[gy + 1, gx + 1]] = Clamp[logOdds[[gy + 1, gx + 1]] + (lUpdate - l0), lMin, lMax];
];

(* Bresenham line traversal (inclusive endpoints) *)
Bresenham[{x0_, y0_}, {x1_, y1_}] := Module[
  {dx, dy, sx, sy, err, x = x0, y = y0, pts = {}, e2},
  dx = Abs[x1 - x0]; dy = Abs[y1 - y0];
  sx = If[x0 < x1, 1, -1];
  sy = If[y0 < y1, 1, -1];
  err = dx - dy;
  While[True,
    AppendTo[pts, {x, y}];
    If[x == x1 && y == y1, Break[]];
    e2 = 2 err;
    If[e2 > -dy, err = err - dy; x = x + sx;];
    If[e2 < dx, err = err + dx; y = y + sy;];
  ];
  pts
];

(* Environment as line segments: {x0,y0,x1,y1} *)
BuildBoxWorld[{xmin_, ymin_}, {xmax_, ymax_}] := {
  {xmin, ymin, xmax, ymin},
  {xmax, ymin, xmax, ymax},
  {xmax, ymax, xmin, ymax},
  {xmin, ymax, xmin, ymin}
};

segs = Join[
  BuildBoxWorld[{-10.0, -8.0}, {10.0, 8.0}],
  {
    {0.0, -6.0, 0.0, 6.0},
    {-8.0, 2.0, -2.0, 2.0}
  }
];

(* Ray/segment intersection using cross products. Return t or Missing[] *)
RaySegIntersect[{rx_, ry_}, {rdx_, rdy_}, {x0_, y0_, x1_, y1_}] := Module[
  {sx, sy, rxs, qpx, qpy, t, u},
  sx = x1 - x0; sy = y1 - y0;
  rxs = rdx sy - rdy sx;
  If[Abs[rxs] < 10^-12, Return[Missing["NoHit"]]];
  qpx = x0 - rx; qpy = y0 - ry;
  t = (qpx sy - qpy sx)/rxs;
  u = (qpx rdy - qpy rdx)/rxs;
  If[t >= 0 && 0 <= u <= 1, t, Missing["NoHit"]]
];

SimulateScan[{x_, y_, th_}, angles_List, zMax_] := Module[
  {ranges = ConstantArray[zMax, Length[angles]], wa, rdx, rdy, tlist, tmin},
  Do[
    wa = th + angles[[i]];
    rdx = Cos[wa]; rdy = Sin[wa];
    tlist = DeleteMissing[RaySegIntersect[{x, y}, {rdx, rdy}, #] & /@ segs];
    If[Length[tlist] > 0,
      tmin = Min[Select[tlist, # <= zMax &]];
      If[NumericQ[tmin], ranges[[i]] = tmin];
    ];
    ,
    {i, 1, Length[angles]}
  ];
  ranges
];

UpdateFromScan[{x_, y_, th_}, angles_List, ranges_List, zMax_] := Module[
  {g0, gx0, gy0, i, a, r, wa, ex, ey, g1, cells, hit, freeCells},
  g0 = WorldToGrid[{x, y}]; gx0 = g0[[1]]; gy0 = g0[[2]];
  Do[
    a = angles[[i]]; r = ranges[[i]];
    wa = th + a;
    ex = x + r Cos[wa];
    ey = y + r Sin[wa];
    g1 = WorldToGrid[{ex, ey}];
    cells = Bresenham[{gx0, gy0}, g1];
    If[Length[cells] == 0, Continue[]];
    hit = (r < (zMax - 10^-12));
    freeCells = If[hit, Most[cells], cells];
    (UpdateCell[#, lFree] & /@ freeCells);
    If[hit, UpdateCell[Last[cells], lOcc]];
    ,
    {i, 1, Length[angles]}
  ];
];

(* Trajectory *)
T = 120;
poses = Table[
  Module[{ang = 2 Pi (t - 1)/T},
    {-6.0 + 6.0 Cos[ang], 0.0 + 4.0 Sin[ang], ang + Pi/2}
  ],
  {t, 1, T}
];

fov = Pi; nBeams = 181;
angles = Table[-fov/2 + fov (i - 1)/(nBeams - 1), {i, 1, nBeams}];
zMax = 10.0;

SeedRandom[0];
Do[
  Module[{pose = poses[[t]], scan},
    scan = SimulateScan[pose, angles, zMax];
    scan = Clamp[scan + RandomVariate[NormalDistribution[0, 0.02], Length[scan]], 0.0, zMax];
    UpdateFromScan[pose, angles, scan, zMax];
  ],
  {t, 1, T}
];

pOccGrid = Sigmoid /@ logOdds;

ArrayPlot[
  pOccGrid,
  ColorFunction -> "GrayTones",
  PlotRange -> {0, 1},
  Frame -> True,
  FrameLabel -> {"gx", "gy"},
  PlotLabel -> "Occupancy Grid Map (P(occupied))"
]
      

Problem 1 (Grid sizing and memory): A robot must map a rectangular area of size \( L_x = 30\,\mathrm{m} \) by \( L_y = 20\,\mathrm{m} \). You choose resolution \( r = 0.05\,\mathrm{m} \) per cell. (a) Compute \( W, H, N \). (b) If you store one double-precision log-odds per cell (8 bytes), estimate the memory in MB.

Solution: (a) \( W = \lfloor L_x/r \rfloor = \lfloor 30/0.05 \rfloor = 600 \), \( H = \lfloor 20/0.05 \rfloor = 400 \), so \( N = WH = 240{,}000 \) cells. (b) Memory is \( 8N \) bytes \( = 1{,}920{,}000 \) bytes \( \approx 1.83\,\mathrm{MB} \).

Problem 2 (Derive the odds update): Starting from the per-cell Bayesian update \( p(m_i\mid x_{1:t}, z_{1:t}) = \eta\, \frac{p(m_i\mid z_t, x_t)}{p(m_i)}\, p(m_i\mid x_{1:t-1}, z_{1:t-1}) \), derive the odds update \( O_t(i) = O_{t-1}(i)\, \frac{O_{inv}(i)}{O_0(i)} \).

Solution: Write the update once for \( m_i=1 \) and once for \( m_i=0 \). Divide the two equations. The normalizer \( \eta \) cancels, leaving:

\[ \frac{p(m_i=1\mid x_{1:t}, z_{1:t})}{p(m_i=0\mid x_{1:t}, z_{1:t})} = \frac{p(m_i=1\mid x_{1:t-1}, z_{1:t-1})}{p(m_i=0\mid x_{1:t-1}, z_{1:t-1})} \cdot \frac{\frac{p(m_i=1\mid z_t, x_t)}{p(m_i=1)}}{\frac{p(m_i=0\mid z_t, x_t)}{p(m_i=0)}}. \]

Recognize the left side as \( O_t(i) \), the first ratio on the right as \( O_{t-1}(i) \), the numerator odds as \( O_{inv}(i) \), and the denominator odds as \( O_0(i) \).

Problem 3 (Max-range returns): In the inverse sensor model, why do we typically avoid marking an “occupied cell” when the measurement equals the max range \( z_t^{(k)} = z_{max} \)?

Solution: A max-range return often indicates “no obstacle detected within sensor range.” Marking an occupied endpoint would create false obstacles at the sensor horizon, biasing the map toward spurious walls. In the piecewise model of Section 4, the occupied case requires \( z_t^{(k)} < z_{max} \).

Problem 4 (Closed-form probability after repeated identical evidence): Suppose a cell starts with prior \( p_0 \). At each time step the inverse model returns a constant \( p_{inv} \) for that cell (e.g., repeated “free” evidence with \( p_{inv} < 0.5 \)). Assuming independence and using the odds update repeatedly, derive \( p_t \) after \( t \) updates.

Solution: Let \( O_0 = \frac{p_0}{1-p_0} \) and \( O_{inv} = \frac{p_{inv}}{1-p_{inv}} \). Repeating \( O_t = O_{t-1} \frac{O_{inv}}{O_0} \) yields \( O_t = O_0 \left(\frac{O_{inv}}{O_0}\right)^t \). Convert back to probability:

\[ p_t = \frac{O_t}{1+O_t} = \frac{O_0 \left(\frac{O_{inv}}{O_0}\right)^t}{1 + O_0 \left(\frac{O_{inv}}{O_0}\right)^t}. \]

If \( p_{inv} < 0.5 \), then \( O_{inv} < 1 \) and the odds shrink with \( t \), driving \( p_t \) toward 0 (free).

Problem 5 (Inverse model design): Suppose you choose \( p_{occ}=0.6 \) and \( p_{free}=0.4 \). Explain qualitatively how the map will differ from the choice \( p_{occ}=0.9 \), \( p_{free}=0.1 \) after many updates in a static world.

Solution: Using log-odds increments, the “strength” of each update is \( \Delta\ell_{occ} = \operatorname{logit}(p_{occ})-\operatorname{logit}(p_0) \) and \( \Delta\ell_{free} = \operatorname{logit}(p_{free})-\operatorname{logit}(p_0) \). With \( p_{occ}=0.6 \) and \( p_{free}=0.4 \), the increments have small magnitude, so the map becomes conservative and “uncertain” (probabilities remain closer to 0.5). With \( p_{occ}=0.9 \) and \( p_{free}=0.1 \), increments have large magnitude, so the map saturates quickly (close to 0 or 1) and becomes overconfident under any model mismatch; clamping becomes more important.

We defined occupancy grids as Bernoulli random fields over a discretized workspace, formulated mapping as inference of \( p(m\mid x_{1:t}, z_{1:t}) \), and derived per-cell Bayesian updates using inverse sensor models. We motivated odds/log-odds representations for stable incremental updates and connected the update logic to practical ray traversal. Lesson 2 formalizes log-odds updates and Bayesian cell updates in greater detail and emphasizes efficient implementation patterns.