By the end of this lab, you should be able to build an occupancy grid map \( m \) from a sequence of LiDAR scans \( z_{1:T} \) and robot poses \( x_{1:T} \). We make the standard mapping-only assumption (no SLAM yet): the pose trajectory is treated as known (from odometry / localization output).

The occupancy grid discretizes the plane into cells \( c_i \), each having a binary random variable \( m_i \in \{0,1\} \) (0 = free, 1 = occupied). The goal is to compute the posterior \( p(m \mid z_{1:T}, x_{1:T}) \) or, cellwise, \( p(m_i \mid z_{1:T}, x_{1:T}) \).

\[ p(m \mid z_{1:T}, x_{1:T}) \;\propto\; p(z_{1:T} \mid m, x_{1:T})\, p(m). \]

In practice we update each cell recursively with an inverse sensor model \( p(m_i \mid z_t, x_t) \) and log-odds, as introduced in the previous lessons.

Let the robot pose at time \( t \) be \( x_t = (x_t, y_t, \theta_t) \). A 2D LiDAR returns a set of range measurements \( z_t = \{r_1,\dots,r_K\} \) at fixed beam angles \( \phi_k \) in the robot frame.

For beam \( k \), define the unit direction in world frame as \( \mathbf{d}_k(x_t) \):

\[ \mathbf{d}_k(x_t) = \begin{bmatrix} \cos(\theta_t + \phi_k) \\ \sin(\theta_t + \phi_k) \end{bmatrix}, \quad \mathbf{p}_t = \begin{bmatrix} x_t \\ y_t \end{bmatrix}. \]

The ideal (noise-free) range is the first intersection between the ray \( \mathbf{p}_t + s\,\mathbf{d}_k \) (for \( s \ge 0 \)) and the environment boundary/obstacles. With additive range noise, a simple model is:

\[ r_k = r^*_k(m, x_t) + \varepsilon_k,\quad \varepsilon_k \sim \mathcal{N}(0,\sigma_r^2), \quad 0 \le r_k \le r_{\max}. \]

For the occupancy-grid update, we do not directly evaluate \( p(z_t \mid m, x_t) \). Instead, we use the inverse model \( p(m_i \mid z_t, x_t) \).

Choose grid resolution \( \Delta \) (meters/cell) and an origin \( (x_{\min}, y_{\min}) \). For a world coordinate \( (x,y) \), the grid indices \( (i,j) \) are:

\[ i = \left\lfloor \frac{x - x_{\min}}{\Delta} \right\rfloor,\quad j = \left\lfloor \frac{y - y_{\min}}{\Delta} \right\rfloor. \]

Conversely, the center of cell \( (i,j) \) maps back to:

\[ x_{c}(i) = x_{\min} + \left(i + \tfrac{1}{2}\right)\Delta,\quad y_{c}(j) = y_{\min} + \left(j + \tfrac{1}{2}\right)\Delta. \]

Design note. Smaller \( \Delta \) increases spatial fidelity but also increases memory and compute. For a map of physical size \( A \), the number of cells scales as \( O(A/\Delta^2) \).

Let cell center be \( \mathbf{c}_i \in \mathbb{R}^2 \). Define the cell in the robot frame: \( \mathbf{u} = \mathbf{R}(\theta_t)^\top (\mathbf{c}_i - \mathbf{p}_t) \), where \( \mathbf{R}(\theta) \) is the planar rotation matrix. Then \( \rho = \|\mathbf{u}\| \) and \( \varphi = \operatorname{atan2}(u_y,u_x) \).

Beam association uses the closest beam angle. If the angular resolution is \( \beta \), then cell \( i \) is “seen” by beam \( k \) if \( |\varphi - \phi_k| \le \beta/2 \).

A widely used piecewise-constant inverse sensor model assigns:

\[ p(m_i = 1 \mid z_t, x_t) = \begin{cases} p_{\mathrm{occ}} & \text{if } |\varphi - \phi_k| \le \beta/2 \text{ and } \left|\rho - r_k\right| \le \alpha/2 \\ p_{\mathrm{free}} & \text{if } |\varphi - \phi_k| \le \beta/2 \text{ and } \rho \le r_k - \alpha/2 \\ p_0 & \text{otherwise} \end{cases} \]

Here \( p_0 \) is the prior occupancy (often 0.5), \( p_{\mathrm{occ}} \) and \( p_{\mathrm{free}} \) are design parameters (e.g., 0.7 and 0.3), and \( \alpha \) is an “obstacle thickness” band (meters).

flowchart TD
  R["Robot cell (start)"] --> F["Cells along ray: mark free"]
  F --> H["Endpoint band: mark occupied if hit"]
  H --> U["Beyond endpoint: leave unknown"]
        

Max-range case. If a beam reports near maximum range ( \( r_k \approx r_{\max} \)), it is common to treat it as “no hit” and update free cells along the ray, without creating an occupied endpoint. In code we use a threshold: \( r_k < r_{\max} - \alpha/2 \) implies a hit.

Define log-odds for cell \( i \) at time \( t \): \( l_{t,i} = \log \frac{p(m_i=1 \mid z_{1:t},x_{1:t})}{1 - p(m_i=1 \mid z_{1:t},x_{1:t})} \). The central computational identity is the additive update:

\[ l_{t,i} = l_{t-1,i} + \operatorname{logit}\big(p(m_i=1 \mid z_t, x_t)\big) - \operatorname{logit}(p_0). \]

Proof sketch (odds form). Under the conditional independence assumption used in occupancy grids, we approximate the cellwise Bayes update by: \( p(m_i \mid z_{1:t},x_{1:t}) \propto p(z_t \mid m_i, x_t)\, p(m_i \mid z_{1:t-1},x_{1:t-1}) \). Taking odds ratio and logarithm yields:

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

The inverse model is introduced by a standard rearrangement that makes the update depend on \( p(m_i \mid z_t,x_t) \) instead of \( p(z_t \mid m_i,x_t) \), producing the \( -\operatorname{logit}(p_0) \) correction term.

To recover occupancy probabilities for visualization: \( p_{t,i} = \sigma(l_{t,i}) = \frac{1}{1+\exp(-l_{t,i})} \). In implementations, clamp \( l_{t,i} \) to \( [l_{\min},l_{\max}] \) to prevent numeric saturation.

The lab pipeline is deterministic given poses and scans: for each pose, for each beam, traverse grid cells and update log-odds. The traversal uses Bresenham (integer grid) or DDA (floating stepping).

flowchart TD
  A["Inputs: poses x_t and scans z_t"] --> B["For each time t"]
  B --> C["For each beam k"]
  C --> D["Compute endpoint in world (x_t + r_k * dir)"]
  D --> E["Convert start/end to grid indices"]
  E --> F["Traverse cells along ray (Bresenham/DDA)"]
  F --> G["Update free cells"]
  G --> H["If hit: update endpoint as occupied"]
  H --> I["Clamp log-odds to [lmin, lmax]"]
  I --> J["After all scans: p = sigmoid(log-odds), visualize"]
        

Complexity. For grid resolution \( \Delta \), maximum range \( r_{\max} \), and \( K \) beams, a worst-case per-scan cost is \( O\left(K\,\frac{r_{\max}}{\Delta}\right) \), ignoring constant factors (beam association and trig evaluations).

File: Chapter9_Lesson5.py

<!-- See downloadable zip for the exact raw code (HTML escaping removed). The below is the same content but HTML-escaped. -->
""""""
Chapter 9 — Mapping Representations for Mobile Robots
Lesson 5 (Lab): Build a 2D Occupancy Grid from LiDAR

This script is self-contained:
1) It defines a simple 2D world with circular obstacles and boundary walls.
2) It simulates a robot trajectory and a 2D LiDAR (range-only beams).
3) It builds an occupancy grid using an inverse sensor model + log-odds updates.
4) It visualizes the resulting occupancy probabilities.

Dependencies: numpy, matplotlib
Install: pip install numpy matplotlib
""""""

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 = min(max(p, 1e-9), 1.0 - 1e-9)
    return math.log(p / (1.0 - p))


def logistic(l: np.ndarray) -> np.ndarray:
    return 1.0 / (1.0 + np.exp(-l))


@dataclass
class Pose2D:
    x: float
    y: float
    theta: float  # radians


@dataclass
class CircleObs:
    cx: float
    cy: float
    r: float


@dataclass
class World:
    # axis-aligned bounding box and a list of circular obstacles
    xmin: float
    xmax: float
    ymin: float
    ymax: float
    circles: List[CircleObs]


def ray_circle_intersection(px: float, py: float, dx: float, dy: float, c: CircleObs) -> float | None:
    """
    Ray: p + t d, with t >= 0, ||d|| = 1.
    Returns smallest t (distance) to circle boundary if intersection exists.
    """
    ox, oy = px - c.cx, py - c.cy
    b = 2.0 * (ox * dx + oy * dy)
    cterm = ox * ox + oy * oy - c.r * c.r
    disc = b * b - 4.0 * cterm
    if disc < 0:
        return None
    s = math.sqrt(disc)
    t1 = (-b - s) / 2.0
    t2 = (-b + s) / 2.0
    ts = [t for t in (t1, t2) if t >= 0.0]
    return min(ts) if ts else None


def ray_aabb_intersection(px: float, py: float, dx: float, dy: float, xmin: float, xmax: float, ymin: float, ymax: float) -> float | None:
    """
    Ray-AABB intersection using slab method.
    Returns smallest positive t where ray hits boundary.
    """
    tmin, tmax = -math.inf, math.inf

    if abs(dx) < 1e-12:
        if px < xmin or px > xmax:
            return None
    else:
        tx1 = (xmin - px) / dx
        tx2 = (xmax - px) / dx
        tmin = max(tmin, min(tx1, tx2))
        tmax = min(tmax, max(tx1, tx2))

    if abs(dy) < 1e-12:
        if py < ymin or py > ymax:
            return None
    else:
        ty1 = (ymin - py) / dy
        ty2 = (ymax - py) / dy
        tmin = max(tmin, min(ty1, ty2))
        tmax = min(tmax, max(ty1, ty2))

    if tmax < 0.0 or tmin > tmax:
        return None
    # We want first intersection in front of ray origin
    t = tmin if tmin >= 0.0 else tmax
    return t if t >= 0.0 else None


def simulate_lidar(world: World, pose: Pose2D, angles_body: np.ndarray, z_max: float, sigma_r: float = 0.01) -> np.ndarray:
    """
    Returns ranges for each beam angle (in robot/body frame).
    """
    ranges = np.full_like(angles_body, z_max, dtype=float)
    px, py, th = pose.x, pose.y, pose.theta

    for k, a in enumerate(angles_body):
        ang = th + a
        dx, dy = math.cos(ang), math.sin(ang)

        hits = []
        # boundary walls
        tb = ray_aabb_intersection(px, py, dx, dy, world.xmin, world.xmax, world.ymin, world.ymax)
        if tb is not None:
            hits.append(tb)
        # circles
        for c in world.circles:
            t = ray_circle_intersection(px, py, dx, dy, c)
            if t is not None:
                hits.append(t)

        if hits:
            r = min(hits)
            if r <= z_max:
                r = r + np.random.normal(0.0, sigma_r)
                ranges[k] = float(np.clip(r, 0.0, z_max))

    return ranges


class OccupancyGrid:
    def __init__(self, width_m: float, height_m: float, res: float, origin_x: float, origin_y: float,
                 p0: float = 0.5, l_min: float = -10.0, l_max: float = 10.0):
        self.res = float(res)
        self.origin_x = float(origin_x)
        self.origin_y = float(origin_y)
        self.w = int(math.ceil(width_m / res))
        self.h = int(math.ceil(height_m / res))
        self.l0 = logit(p0)
        self.l_min = float(l_min)
        self.l_max = float(l_max)
        self.log_odds = np.full((self.h, self.w), self.l0, dtype=float)

    def world_to_grid(self, x: float, y: float) -> Tuple[int, int] | None:
        i = int(math.floor((x - self.origin_x) / self.res))
        j = int(math.floor((y - self.origin_y) / self.res))
        if 0 <= i < self.w and 0 <= j < self.h:
            return i, j
        return None

    def grid_to_world_center(self, i: int, j: int) -> Tuple[float, float]:
        x = self.origin_x + (i + 0.5) * self.res
        y = self.origin_y + (j + 0.5) * self.res
        return x, y

    @staticmethod
    def bresenham(i0: int, j0: int, i1: int, j1: int) -> List[Tuple[int, int]]:
        """
        2D Bresenham line traversal (grid indices).
        Returns list of (i,j) cells on the line including both endpoints.
        """
        cells = []
        di = abs(i1 - i0)
        dj = abs(j1 - j0)
        si = 1 if i0 < i1 else -1
        sj = 1 if j0 < j1 else -1
        err = di - dj
        i, j = i0, j0
        while True:
            cells.append((i, j))
            if i == i1 and j == j1:
                break
            e2 = 2 * err
            if e2 > -dj:
                err -= dj
                i += si
            if e2 < di:
                err += di
                j += sj
        return cells

    def update_ray(self, pose: Pose2D, angle_body: float, r: float, z_max: float,
                   l_occ: float, l_free: float, alpha: float = 0.2):
        """
        Inverse sensor model update along one LiDAR ray.
        - Cells before endpoint: free
        - Endpoint cell (if r < z_max - alpha/2): occupied
        - If no hit (r close to z_max): free up to max range, no occupied endpoint
        """
        start = self.world_to_grid(pose.x, pose.y)
        if start is None:
            return
        i0, j0 = start

        ang = pose.theta + angle_body
        ex = pose.x + r * math.cos(ang)
        ey = pose.y + r * math.sin(ang)
        end = self.world_to_grid(ex, ey)
        if end is None:
            # ray endpoint outside map; clip by stepping until outside (simple approach)
            return
        i1, j1 = end

        cells = self.bresenham(i0, j0, i1, j1)
        if len(cells) <= 1:
            return

        hit = (r < (z_max - 0.5 * alpha))
        # free cells: exclude the endpoint; for no-hit, all cells on line are free (excluding robot cell is optional)
        free_cells = cells[1:-1] if hit else cells[1:]
        for (i, j) in free_cells:
            self.log_odds[j, i] = np.clip(self.log_odds[j, i] + (l_free - self.l0), self.l_min, self.l_max)

        if hit:
            (ie, je) = cells[-1]
            self.log_odds[je, ie] = np.clip(self.log_odds[je, ie] + (l_occ - self.l0), self.l_min, self.l_max)

    def probs(self) -> np.ndarray:
        return logistic(self.log_odds)


def main():
    np.random.seed(7)

    # World definition (meters)
    world = World(
        xmin=-10.0, xmax=10.0, ymin=-10.0, ymax=10.0,
        circles=[
            CircleObs(-3.0, 2.0, 1.2),
            CircleObs(2.5, -1.0, 1.0),
            CircleObs(4.0, 4.0, 1.5),
            CircleObs(-4.5, -4.0, 1.0),
        ]
    )

    # LiDAR parameters
    n_beams = 360
    z_max = 8.0
    angles = np.linspace(-math.pi, math.pi, n_beams, endpoint=False)

    # Occupancy grid parameters
    res = 0.05  # meters/cell
    grid = OccupancyGrid(width_m=20.0, height_m=20.0, res=0.1, origin_x=-10.0, origin_y=-10.0,
                         p0=0.5, l_min=-8.0, l_max=8.0)

    # Inverse sensor model probabilities
    p_occ = 0.70
    p_free = 0.30
    l_occ = logit(p_occ)
    l_free = logit(p_free)
    alpha = 0.2  # obstacle thickness (m), affects "hit" decision band

    # Robot trajectory (known poses)
    T = 220
    poses = []
    for t in range(T):
        # a smooth loop
        ang = 2.0 * math.pi * t / T
        x = 6.0 * math.cos(ang)
        y = 6.0 * math.sin(ang)
        theta = ang + math.pi / 2.0  # tangential heading
        poses.append(Pose2D(x, y, theta))

    # Mapping loop
    for pose in poses:
        z = simulate_lidar(world, pose, angles, z_max=z_max, sigma_r=0.02)
        for a, r in zip(angles, z):
            grid.update_ray(pose, a, r, z_max=z_max, l_occ=l_occ, l_free=l_free, alpha=alpha)

    # Visualization
    P = grid.probs()
    # Convert to occupancy map: 1=occupied, 0=free
    fig = plt.figure(figsize=(7, 6))
    plt.imshow(P, origin='lower', extent=[world.xmin, world.xmax, world.ymin, world.ymax])
    plt.colorbar(label="p(occupied)")
    plt.title("2D Occupancy Grid from Simulated LiDAR")
    plt.xlabel("x [m]")
    plt.ylabel("y [m]")

    # Plot true obstacles
    th = np.linspace(0, 2 * math.pi, 200)
    for c in world.circles:
        plt.plot(c.cx + c.r * np.cos(th), c.cy + c.r * np.sin(th), linewidth=1.5)

    # Plot trajectory
    xs = [p.x for p in poses]
    ys = [p.y for p in poses]
    plt.plot(xs, ys, linewidth=1.0)

    plt.tight_layout()
    plt.show()


if __name__ == "__main__":
    main()
      

Practical extensions (optional): swap the simulator with real data by replacing the simulate_lidar(...) function with your dataset loader and by feeding measured \( x_t \) and \( z_t \).

File: Chapter9_Lesson5.cpp

// Chapter 9 — Mapping Representations for Mobile Robots
// Lesson 5 (Lab): Build a 2D Occupancy Grid from LiDAR
//
// Self-contained C++17 program:
// - Simulates a robot trajectory and 2D LiDAR in a 2D world with circular obstacles + boundary walls
// - Builds occupancy grid using inverse sensor model + log-odds updates + Bresenham ray traversal
// - Writes result as a PGM image (occupancy probability) so you can view it with any image viewer
//
// Build (Linux/macOS):
//   g++ -O2 -std=c++17 Chapter9_Lesson5.cpp -o ogm
//   ./ogm
//
// Build (Windows, MSVC):
//   cl /O2 /std:c++17 Chapter9_Lesson5.cpp
//   Chapter9_Lesson5.exe

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

struct Pose2D {
  double x, y, theta;
};

struct Circle {
  double cx, cy, r;
};

static inline double clamp(double v, double lo, double hi) {
  return std::max(lo, std::min(hi, v));
}

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

// Ray p + t d with t >= 0, ||d||=1
static bool rayCircleIntersection(double px, double py, double dx, double dy, const Circle& c, double& t_out) {
  const double ox = px - c.cx;
  const double oy = py - c.cy;
  const double b = 2.0 * (ox * dx + oy * dy);
  const double cterm = ox * ox + oy * oy - c.r * c.r;
  const double disc = b * b - 4.0 * cterm;
  if (disc < 0.0) return false;
  const double s = std::sqrt(disc);
  const double t1 = (-b - s) / 2.0;
  const double t2 = (-b + s) / 2.0;
  double best = 1e300;
  bool ok = false;
  if (t1 >= 0.0) { best = std::min(best, t1); ok = true; }
  if (t2 >= 0.0) { best = std::min(best, t2); ok = true; }
  if (!ok) return false;
  t_out = best;
  return true;
}

static bool rayAABBIntersection(double px, double py, double dx, double dy,
                                double xmin, double xmax, double ymin, double ymax,
                                double& t_out) {
  double tmin = -1e300, tmax = 1e300;

  if (std::abs(dx) < 1e-12) {
    if (px < xmin || px > xmax) return false;
  } else {
    const double tx1 = (xmin - px) / dx;
    const double tx2 = (xmax - px) / dx;
    tmin = std::max(tmin, std::min(tx1, tx2));
    tmax = std::min(tmax, std::max(tx1, tx2));
  }

  if (std::abs(dy) < 1e-12) {
    if (py < ymin || py > ymax) return false;
  } else {
    const double ty1 = (ymin - py) / dy;
    const double ty2 = (ymax - py) / dy;
    tmin = std::max(tmin, std::min(ty1, ty2));
    tmax = std::min(tmax, std::max(ty1, ty2));
  }

  if (tmax < 0.0 || tmin > tmax) return false;
  const double t = (tmin >= 0.0) ? tmin : tmax;
  if (t < 0.0) return false;
  t_out = t;
  return true;
}

struct OccupancyGrid {
  double res;
  double origin_x, origin_y;
  int w, h;
  double l0, lmin, lmax;
  std::vector<double> logodds; // row-major: j*w + i

  OccupancyGrid(double width_m, double height_m, double res_, double ox, double oy,
                double p0=0.5, double lmin_=-8.0, double lmax_=8.0)
      : res(res_), origin_x(ox), origin_y(oy),
        w((int)std::ceil(width_m / res_)), h((int)std::ceil(height_m / res_)),
        l0(logit(p0)), lmin(lmin_), lmax(lmax_), logodds((size_t)w * (size_t)h, logit(p0)) {}

  bool worldToGrid(double x, double y, int& i, int& j) const {
    i = (int)std::floor((x - origin_x) / res);
    j = (int)std::floor((y - origin_y) / res);
    if (0 <= i && i < w && 0 <= j && j < h) return true;
    return false;
  }

  double& at(int i, int j) { return logodds[(size_t)j * (size_t)w + (size_t)i]; }
  const double& at(int i, int j) const { return logodds[(size_t)j * (size_t)w + (size_t)i]; }

  static std::vector<std::pair<int,int>> bresenham(int i0, int j0, int i1, int j1) {
    std::vector<std::pair<int,int>> cells;
    int di = std::abs(i1 - i0);
    int dj = std::abs(j1 - j0);
    int si = (i0 < i1) ? 1 : -1;
    int sj = (j0 < j1) ? 1 : -1;
    int err = di - dj;
    int i = i0, j = j0;
    while (true) {
      cells.emplace_back(i, j);
      if (i == i1 && j == j1) break;
      int e2 = 2 * err;
      if (e2 > -dj) { err -= dj; i += si; }
      if (e2 <  di) { err += di; j += sj; }
    }
    return cells;
  }

  void updateRay(const Pose2D& pose, double angle_body, double r, double zmax,
                 double l_occ, double l_free, double alpha=0.2) {
    int i0, j0;
    if (!worldToGrid(pose.x, pose.y, i0, j0)) return;

    const double ang = pose.theta + angle_body;
    const double ex = pose.x + r * std::cos(ang);
    const double ey = pose.y + r * std::sin(ang);

    int i1, j1;
    if (!worldToGrid(ex, ey, i1, j1)) return;

    auto cells = bresenham(i0, j0, i1, j1);
    if (cells.size() <= 1) return;

    const bool hit = (r < (zmax - 0.5 * alpha));
    const int last = (int)cells.size() - 1;

    // free cells: exclude endpoint if hit; else include all (except robot cell)
    const int free_end = hit ? (last - 1) : last;
    for (int k = 1; k <= free_end; ++k) {
      const auto [i, j] = cells[(size_t)k];
      at(i, j) = clamp(at(i, j) + (l_free - l0), lmin, lmax);
    }

    if (hit) {
      const auto [ie, je] = cells[(size_t)last];
      at(ie, je) = clamp(at(ie, je) + (l_occ - l0), lmin, lmax);
    }
  }

  void writePGM(const std::string& path) const {
    // p = logistic(l) in [0,1], map to [0..255]
    std::ofstream f(path, std::ios::binary);
    if (!f) {
      std::cerr << "Failed to open " << path << "\n";
      return;
    }
    f << "P5\n" << w << " " << h << "\n255\n";
    for (int j = 0; j < h; ++j) {
      for (int i = 0; i < w; ++i) {
        const double l = at(i, j);
        const double p = 1.0 / (1.0 + std::exp(-l));
        const uint8_t v = (uint8_t)clamp(std::round(p * 255.0), 0.0, 255.0);
        f.write((const char*)&v, 1);
      }
    }
  }
};

int main() {
  // World (meters)
  const double xmin = -10.0, xmax = 10.0, ymin = -10.0, ymax = 10.0;
  std::vector<Circle> circles = {
      {-3.0,  2.0, 1.2},
      { 2.5, -1.0, 1.0},
      { 4.0,  4.0, 1.5},
      {-4.5, -4.0, 1.0},
  };

  // LiDAR
  const int n_beams = 360;
  const double zmax = 8.0;

  // Grid
  const double res = 0.1;
  OccupancyGrid grid(/*width=*/20.0, /*height=*/20.0, res, /*origin_x=*/-10.0, /*origin_y=*/-10.0,
                     /*p0=*/0.5, /*lmin=*/-8.0, /*lmax=*/8.0);

  // Inverse sensor model params
  const double p_occ = 0.70, p_free = 0.30;
  const double l_occ = logit(p_occ), l_free = logit(p_free);
  const double alpha = 0.2;

  // Randomness
  std::mt19937 rng(7);
  std::normal_distribution<double> noise_r(0.0, 0.02);

  // Trajectory
  const int T = 220;
  std::vector<Pose2D> poses;
  poses.reserve(T);
  for (int t = 0; t < T; ++t) {
    const double ang = 2.0 * M_PI * (double)t / (double)T;
    const double x = 6.0 * std::cos(ang);
    const double y = 6.0 * std::sin(ang);
    const double theta = ang + M_PI / 2.0;
    poses.push_back({x, y, theta});
  }

  // Mapping
  for (const auto& pose : poses) {
    for (int k = 0; k < n_beams; ++k) {
      const double a_body = -M_PI + (2.0 * M_PI) * (double)k / (double)n_beams;
      const double ang = pose.theta + a_body;
      const double dx = std::cos(ang);
      const double dy = std::sin(ang);

      // find nearest hit among walls and circles
      double best = 1e300;
      double t_wall;
      if (rayAABBIntersection(pose.x, pose.y, dx, dy, xmin, xmax, ymin, ymax, t_wall)) {
        best = std::min(best, t_wall);
      }
      for (const auto& c : circles) {
        double t;
        if (rayCircleIntersection(pose.x, pose.y, dx, dy, c, t)) {
          best = std::min(best, t);
        }
      }

      double r = zmax;
      if (best < 1e200 && best <= zmax) {
        r = clamp(best + noise_r(rng), 0.0, zmax);
      }
      grid.updateRay(pose, a_body, r, zmax, l_occ, l_free, alpha);
    }
  }

  grid.writePGM("Chapter9_Lesson5_map.pgm");
  std::cout << "Wrote occupancy probability PGM: Chapter9_Lesson5_map.pgm\n";
  std::cout << "Tip: open the PGM with an image viewer, or convert it with ImageMagick.\n";
  return 0;
}
      

The output image Chapter9_Lesson5_map.pgm stores \( p(m_i=1) \) mapped to 0–255. Higher intensity means higher occupancy probability.

File: Chapter9_Lesson5.java

// Chapter 9 — Mapping Representations for Mobile Robots
// Lesson 5 (Lab): Build a 2D Occupancy Grid from LiDAR
//
// Self-contained Java program (no external libraries):
// - Simulates robot trajectory and 2D LiDAR in a 2D world with circles + boundary walls
// - Builds occupancy grid using log-odds + Bresenham traversal
// - Writes PGM image (occupancy probability)
//
// Compile:
//   javac Chapter9_Lesson5.java
// Run:
//   java Chapter9_Lesson5

import java.io.FileOutputStream;
import java.io.IOException;
import java.nio.charset.StandardCharsets;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;

public class Chapter9_Lesson5 {

  static class Pose2D {
    double x, y, theta;
    Pose2D(double x, double y, double theta) { this.x = x; this.y = y; this.theta = theta; }
  }

  static class Circle {
    double cx, cy, r;
    Circle(double cx, double cy, double r) { this.cx = cx; this.cy = cy; this.r = r; }
  }

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

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

  static boolean rayCircleIntersection(double px, double py, double dx, double dy, Circle c, double[] tOut) {
    double ox = px - c.cx;
    double oy = py - c.cy;
    double b = 2.0 * (ox * dx + oy * dy);
    double cterm = ox * ox + oy * oy - c.r * c.r;
    double disc = b * b - 4.0 * cterm;
    if (disc < 0.0) return false;
    double s = Math.sqrt(disc);
    double t1 = (-b - s) / 2.0;
    double t2 = (-b + s) / 2.0;
    double best = 1e300;
    boolean ok = false;
    if (t1 >= 0.0) { best = Math.min(best, t1); ok = true; }
    if (t2 >= 0.0) { best = Math.min(best, t2); ok = true; }
    if (!ok) return false;
    tOut[0] = best;
    return true;
  }

  static boolean rayAABBIntersection(double px, double py, double dx, double dy,
                                     double xmin, double xmax, double ymin, double ymax,
                                     double[] tOut) {
    double tmin = -1e300, tmax = 1e300;

    if (Math.abs(dx) < 1e-12) {
      if (px < xmin || px > xmax) return false;
    } else {
      double tx1 = (xmin - px) / dx;
      double tx2 = (xmax - px) / dx;
      tmin = Math.max(tmin, Math.min(tx1, tx2));
      tmax = Math.min(tmax, Math.max(tx1, tx2));
    }

    if (Math.abs(dy) < 1e-12) {
      if (py < ymin || py > ymax) return false;
    } else {
      double ty1 = (ymin - py) / dy;
      double ty2 = (ymax - py) / dy;
      tmin = Math.max(tmin, Math.min(ty1, ty2));
      tmax = Math.min(tmax, Math.max(ty1, ty2));
    }

    if (tmax < 0.0 || tmin > tmax) return false;
    double t = (tmin >= 0.0) ? tmin : tmax;
    if (t < 0.0) return false;
    tOut[0] = t;
    return true;
  }

  static class OccupancyGrid {
    double res;
    double originX, originY;
    int w, h;
    double l0, lmin, lmax;
    double[] logOdds; // row-major

    OccupancyGrid(double widthM, double heightM, double res, double originX, double originY,
                  double p0, double lmin, double lmax) {
      this.res = res;
      this.originX = originX;
      this.originY = originY;
      this.w = (int)Math.ceil(widthM / res);
      this.h = (int)Math.ceil(heightM / res);
      this.l0 = logit(p0);
      this.lmin = lmin;
      this.lmax = lmax;
      this.logOdds = new double[w * h];
      for (int i = 0; i < w * h; i++) logOdds[i] = this.l0;
    }

    boolean worldToGrid(double x, double y, int[] outIJ) {
      int i = (int)Math.floor((x - originX) / res);
      int j = (int)Math.floor((y - originY) / res);
      if (0 <= i && i < w && 0 <= j && j < h) {
        outIJ[0] = i; outIJ[1] = j;
        return true;
      }
      return false;
    }

    double at(int i, int j) { return logOdds[j * w + i]; }
    void set(int i, int j, double v) { logOdds[j * w + i] = v; }

    static List<int[]> bresenham(int i0, int j0, int i1, int j1) {
      List<int[]> cells = new ArrayList<>();
      int di = Math.abs(i1 - i0);
      int dj = Math.abs(j1 - j0);
      int si = (i0 < i1) ? 1 : -1;
      int sj = (j0 < j1) ? 1 : -1;
      int err = di - dj;
      int i = i0, j = j0;
      while (true) {
        cells.add(new int[]{i, j});
        if (i == i1 && j == j1) break;
        int e2 = 2 * err;
        if (e2 > -dj) { err -= dj; i += si; }
        if (e2 <  di) { err += di; j += sj; }
      }
      return cells;
    }

    void updateRay(Pose2D pose, double angleBody, double r, double zmax,
                   double lOcc, double lFree, double alpha) {
      int[] s = new int[2];
      if (!worldToGrid(pose.x, pose.y, s)) return;
      int i0 = s[0], j0 = s[1];

      double ang = pose.theta + angleBody;
      double ex = pose.x + r * Math.cos(ang);
      double ey = pose.y + r * Math.sin(ang);

      int[] e = new int[2];
      if (!worldToGrid(ex, ey, e)) return;
      int i1 = e[0], j1 = e[1];

      List<int[]> cells = bresenham(i0, j0, i1, j1);
      if (cells.size() <= 1) return;

      boolean hit = (r < (zmax - 0.5 * alpha));
      int last = cells.size() - 1;

      int freeEnd = hit ? (last - 1) : last;
      for (int k = 1; k <= freeEnd; k++) {
        int[] c = cells.get(k);
        int i = c[0], j = c[1];
        double v = clamp(at(i, j) + (lFree - l0), lmin, lmax);
        set(i, j, v);
      }

      if (hit) {
        int[] c = cells.get(last);
        int i = c[0], j = c[1];
        double v = clamp(at(i, j) + (lOcc - l0), lmin, lmax);
        set(i, j, v);
      }
    }

    void writePGM(String path) throws IOException {
      try (FileOutputStream f = new FileOutputStream(path)) {
        String header = "P5\n" + w + " " + h + "\n255\n";
        f.write(header.getBytes(StandardCharsets.US_ASCII));
        for (int j = 0; j < h; j++) {
          for (int i = 0; i < w; i++) {
            double l = at(i, j);
            double p = 1.0 / (1.0 + Math.exp(-l));
            int v = (int)Math.round(clamp(p * 255.0, 0.0, 255.0));
            f.write((byte)(v & 0xFF));
          }
        }
      }
    }
  }

  static double gaussian(Random rng, double mean, double std) {
    // Box-Muller
    double u1 = Math.max(rng.nextDouble(), 1e-12);
    double u2 = rng.nextDouble();
    double z = Math.sqrt(-2.0 * Math.log(u1)) * Math.cos(2.0 * Math.PI * u2);
    return mean + std * z;
  }

  public static void main(String[] args) throws Exception {
    // World
    double xmin = -10.0, xmax = 10.0, ymin = -10.0, ymax = 10.0;
    List<Circle> circles = List.of(
        new Circle(-3.0,  2.0, 1.2),
        new Circle( 2.5, -1.0, 1.0),
        new Circle( 4.0,  4.0, 1.5),
        new Circle(-4.5, -4.0, 1.0)
    );

    // LiDAR
    int nBeams = 360;
    double zmax = 8.0;

    // Grid
    double res = 0.1;
    OccupancyGrid grid = new OccupancyGrid(20.0, 20.0, res, -10.0, -10.0, 0.5, -8.0, 8.0);

    // Inverse sensor model params
    double pOcc = 0.70, pFree = 0.30;
    double lOcc = logit(pOcc), lFree = logit(pFree);
    double alpha = 0.2;

    Random rng = new Random(7);

    // Trajectory
    int T = 220;
    List<Pose2D> poses = new ArrayList<>();
    for (int t = 0; t < T; t++) {
      double ang = 2.0 * Math.PI * t / (double)T;
      double x = 6.0 * Math.cos(ang);
      double y = 6.0 * Math.sin(ang);
      double theta = ang + Math.PI / 2.0;
      poses.add(new Pose2D(x, y, theta));
    }

    // Mapping
    double[] tBuf = new double[1];
    for (Pose2D pose : poses) {
      for (int k = 0; k < nBeams; k++) {
        double aBody = -Math.PI + (2.0 * Math.PI) * k / (double)nBeams;
        double ang = pose.theta + aBody;
        double dx = Math.cos(ang);
        double dy = Math.sin(ang);

        double best = 1e300;
        if (rayAABBIntersection(pose.x, pose.y, dx, dy, xmin, xmax, ymin, ymax, tBuf)) {
          best = Math.min(best, tBuf[0]);
        }
        for (Circle c : circles) {
          if (rayCircleIntersection(pose.x, pose.y, dx, dy, c, tBuf)) {
            best = Math.min(best, tBuf[0]);
          }
        }

        double r = zmax;
        if (best < 1e200 && best <= zmax) {
          r = clamp(best + gaussian(rng, 0.0, 0.02), 0.0, zmax);
        }
        grid.updateRay(pose, aBody, r, zmax, lOcc, lFree, alpha);
      }
    }

    grid.writePGM("Chapter9_Lesson5_map_java.pgm");
    System.out.println("Wrote occupancy probability PGM: Chapter9_Lesson5_map_java.pgm");
  }
}
      

MATLAB code below implements the same logic. For a Simulink-style workflow, you can: (i) represent the log-odds grid as a Data Store Memory block or persistent state in a MATLAB Function block, (ii) stream pose and scan vectors as signals, and (iii) update the grid at each sample time. The core update step is identical (ray traversal + log-odds increments).

File: Chapter9_Lesson5.m

% Chapter 9 — Mapping Representations for Mobile Robots
% Lesson 5 (Lab): Build a 2D Occupancy Grid from LiDAR
%
% Self-contained MATLAB script:
% - Defines a world with circular obstacles + boundary walls
% - Simulates robot trajectory and 2D LiDAR ranges
% - Builds occupancy grid using inverse sensor model + log-odds updates
% - Visualizes occupancy probabilities
%
% Run:
%   Chapter9_Lesson5

function Chapter9_Lesson5()
  rng(7);

  % World (meters)
  world.xmin = -10; world.xmax = 10;
  world.ymin = -10; world.ymax = 10;
  circles = [
    -3.0,  2.0, 1.2;
     2.5, -1.0, 1.0;
     4.0,  4.0, 1.5;
    -4.5, -4.0, 1.0
  ];

  % LiDAR
  nBeams = 360;
  zMax = 8.0;
  angles = linspace(-pi, pi, nBeams+1); angles(end) = [];

  % Grid
  res = 0.1;
  origin = [-10, -10];
  widthM = 20; heightM = 20;
  w = ceil(widthM / res);
  h = ceil(heightM / res);

  p0 = 0.5;
  l0 = logit(p0);
  lMin = -8; lMax = 8;
  logOdds = l0 * ones(h, w);

  % Inverse sensor model params
  pOcc = 0.70; pFree = 0.30;
  lOcc = logit(pOcc); lFree = logit(pFree);
  alpha = 0.2;

  % Trajectory (known poses)
  T = 220;
  poses = zeros(T, 3);
  for t = 1:T
    ang = 2*pi*(t-1)/T;
    x = 6*cos(ang);
    y = 6*sin(ang);
    theta = ang + pi/2;
    poses(t, :) = [x, y, theta];
  end

  % Mapping
  for t = 1:T
    pose = poses(t, :);
    z = simulateLidar(world, circles, pose, angles, zMax, 0.02);
    for k = 1:nBeams
      logOdds = updateRay(logOdds, origin, res, pose, angles(k), z(k), zMax, l0, lOcc, lFree, lMin, lMax, alpha);
    end
  end

  % Visualization
  P = logistic(logOdds);
  figure('Color','w');
  imagesc([world.xmin world.xmax], [world.ymin world.ymax], P);
  axis xy equal tight;
  colormap(parula); colorbar;
  title('2D Occupancy Grid from Simulated LiDAR');
  xlabel('x [m]'); ylabel('y [m]');
  hold on;

  th = linspace(0, 2*pi, 200);
  for i = 1:size(circles,1)
    cx = circles(i,1); cy = circles(i,2); r = circles(i,3);
    plot(cx + r*cos(th), cy + r*sin(th), 'LineWidth', 1.5);
  end
  plot(poses(:,1), poses(:,2), 'LineWidth', 1.0);
  hold off;

  % Optional: save PGM-like image
  % imwrite(uint8(P*255), 'Chapter9_Lesson5_map_matlab.png');
end

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

function P = logistic(L)
  P = 1 ./ (1 + exp(-L));
end

function z = simulateLidar(world, circles, pose, angles, zMax, sigmaR)
  n = numel(angles);
  z = zMax * ones(n,1);
  px = pose(1); py = pose(2); th = pose(3);
  for k = 1:n
    ang = th + angles(k);
    dx = cos(ang); dy = sin(ang);

    hits = [];

    % boundary walls
    tb = rayAABB(px, py, dx, dy, world.xmin, world.xmax, world.ymin, world.ymax);
    if ~isnan(tb), hits(end+1) = tb; end

    % circles
    for i = 1:size(circles,1)
      c = circles(i,:);
      t = rayCircle(px, py, dx, dy, c(1), c(2), c(3));
      if ~isnan(t), hits(end+1) = t; end
    end

    if ~isempty(hits)
      r = min(hits);
      if r <= zMax
        z(k) = min(max(r + sigmaR*randn(), 0), zMax);
      end
    end
  end
end

function t = rayCircle(px, py, dx, dy, cx, cy, r)
  ox = px - cx; oy = py - cy;
  b = 2*(ox*dx + oy*dy);
  cterm = ox^2 + oy^2 - r^2;
  disc = b^2 - 4*cterm;
  if disc < 0
    t = NaN; return;
  end
  s = sqrt(disc);
  t1 = (-b - s)/2;
  t2 = (-b + s)/2;
  cand = [t1 t2];
  cand = cand(cand >= 0);
  if isempty(cand), t = NaN; else, t = min(cand); end
end

function t = rayAABB(px, py, dx, dy, xmin, xmax, ymin, ymax)
  tmin = -Inf; tmax = Inf;
  if abs(dx) < 1e-12
    if px < xmin || px > xmax, t = NaN; return; end
  else
    tx1 = (xmin - px)/dx; tx2 = (xmax - px)/dx;
    tmin = max(tmin, min(tx1, tx2));
    tmax = min(tmax, max(tx1, tx2));
  end
  if abs(dy) < 1e-12
    if py < ymin || py > ymax, t = NaN; return; end
  else
    ty1 = (ymin - py)/dy; ty2 = (ymax - py)/dy;
    tmin = max(tmin, min(ty1, ty2));
    tmax = min(tmax, max(ty1, ty2));
  end
  if tmax < 0 || tmin > tmax, t = NaN; return; end
  if tmin >= 0, t = tmin; else, t = tmax; end
  if t < 0, t = NaN; end
end

function L = updateRay(L, origin, res, pose, angleBody, r, zMax, l0, lOcc, lFree, lMin, lMax, alpha)
  s = worldToGrid(origin, res, pose(1), pose(2), size(L,2), size(L,1));
  if any(isnan(s)), return; end
  i0 = s(1); j0 = s(2);

  ang = pose(3) + angleBody;
  ex = pose(1) + r*cos(ang);
  ey = pose(2) + r*sin(ang);
  e = worldToGrid(origin, res, ex, ey, size(L,2), size(L,1));
  if any(isnan(e)), return; end
  i1 = e(1); j1 = e(2);

  cells = bresenham(i0, j0, i1, j1);
  if size(cells,1) <= 1, return; end

  hit = (r < (zMax - 0.5*alpha));
  if hit
    freeCells = cells(2:end-1, :);
  else
    freeCells = cells(2:end, :);
  end

  for idx = 1:size(freeCells,1)
    i = freeCells(idx,1); j = freeCells(idx,2);
    L(j,i) = min(max(L(j,i) + (lFree - l0), lMin), lMax);
  end

  if hit
    ie = cells(end,1); je = cells(end,2);
    L(je,ie) = min(max(L(je,ie) + (lOcc - l0), lMin), lMax);
  end
end

function ij = worldToGrid(origin, res, x, y, w, h)
  i = floor((x - origin(1))/res) + 1; % MATLAB 1-based
  j = floor((y - origin(2))/res) + 1;
  if i < 1 || i > w || j < 1 || j > h
    ij = [NaN, NaN];
  else
    ij = [i, j];
  end
end

function cells = bresenham(i0, j0, i1, j1)
  di = abs(i1 - i0);
  dj = abs(j1 - j0);
  si = 1; if i0 > i1, si = -1; end
  sj = 1; if j0 > j1, sj = -1; end
  err = di - dj;
  i = i0; j = j0;
  cells = [];
  while true
    cells(end+1,:) = [i, j]; %#ok
    if i == i1 && j == j1, break; end
    e2 = 2*err;
    if e2 > -dj
      err = err - dj;
      i = i + si;
    end
    if e2 < di
      err = err + di;
      j = j + sj;
    end
  end
end
      

File: Chapter9_Lesson5.nb

(* ::Package:: *)

(* Chapter 9 — Mapping Representations for Mobile Robots
   Lesson 5 (Lab): Build a 2D Occupancy Grid from LiDAR

   This Mathematica notebook is provided as plain-text .nb content.
   Open it in Wolfram Mathematica; it will evaluate as a notebook expression.

   It simulates a 2D world, LiDAR scans, and constructs an occupancy grid using log-odds.
*)

Notebook[{
  Cell["Chapter 9 — Mapping Representations for Mobile Robots\nLesson 5 (Lab): Build a 2D Occupancy Grid from LiDAR", "Title"],

  Cell["1. Utilities: logit/logistic and simple ray intersections", "Section"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    logit[p_] := Log[ Clip[p, {10^-9, 1 - 10^-9}] / (1 - Clip[p, {10^-9, 1 - 10^-9}]) ];
    logistic[l_] := 1/(1 + Exp[-l]);

    rayCircleIntersection[p_, d_, c_, r_] := Module[{o = p - c, b, cterm, disc, s, t1, t2, cand},
      b = 2 (o.d);
      cterm = (o.o) - r^2;
      disc = b^2 - 4 cterm;
      If[disc < 0, Infinity,
        s = Sqrt[disc];
        t1 = (-b - s)/2; t2 = (-b + s)/2;
        cand = Select[{t1, t2}, # >= 0 &];
        If[cand === {}, Infinity, Min[cand]]
      ]
    ];

    rayAABBIntersection[p_, d_, xmin_, xmax_, ymin_, ymax_] := Module[{px=p[[1]], py=p[[2]], dx=d[[1]], dy=d[[2]],
      tmin=-Infinity, tmax=Infinity, tx1, tx2, ty1, ty2},
      If[Abs[dx] < 10^-12, If[px < xmin || px > xmax, Return[Infinity]],
        tx1 = (xmin - px)/dx; tx2 = (xmax - px)/dx;
        tmin = Max[tmin, Min[tx1, tx2]];
        tmax = Min[tmax, Max[tx1, tx2]];
      ];
      If[Abs[dy] < 10^-12, If[py < ymin || py > ymax, Return[Infinity]],
        ty1 = (ymin - py)/dy; ty2 = (ymax - py)/dy;
        tmin = Max[tmin, Min[ty1, ty2]];
        tmax = Min[tmax, Max[ty1, ty2]];
      ];
      If[tmax < 0 || tmin > tmax, Infinity, If[tmin >= 0, tmin, tmax]]
    ];
  ], "Input"],

  Cell["2. World, LiDAR simulation, and occupancy-grid mapping", "Section"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    SeedRandom[7];

    (* World bounds and obstacles *)
    bounds = <|"xmin"->-10, "xmax"->10, "ymin"->-10, "ymax"->10|>;
    circles = {
      <|"c"->{-3.0,  2.0}, "r"->1.2|>,
      <|"c"->{ 2.5, -1.0}, "r"->1.0|>,
      <|"c"->{ 4.0,  4.0}, "r"->1.5|>,
      <|"c"->{-4.5, -4.0}, "r"->1.0|>
    };

    (* LiDAR params *)
    nBeams = 360; zMax = 8.0;
    angles = Subdivide[-Pi, Pi, nBeams + 1][[;; -2]];

    (* Grid params *)
    res = 0.1; origin = {-10, -10}; widthM = 20; heightM = 20;
    w = Ceiling[widthM/res]; h = Ceiling[heightM/res];
    l0 = logit[0.5]; lMin = -8; lMax = 8;
    logOdds = ConstantArray[l0, {h, w}];

    (* Sensor model *)
    pOcc = 0.70; pFree = 0.30;
    lOcc = logit[pOcc]; lFree = logit[pFree];
    alpha = 0.2;

    worldToGrid[{x_, y_}] := Module[{i, j},
      i = Floor[(x - origin[[1]])/res] + 1;
      j = Floor[(y - origin[[2]])/res] + 1;
      If[1 <= i <= w && 1 <= j <= h, {i, j}, Missing["OutOfBounds"]]
    ];

    bresenham[{i0_, j0_}, {i1_, j1_}] := Module[{di = Abs[i1 - i0], dj = Abs[j1 - j0],
      si = If[i0 < i1, 1, -1], sj = If[j0 < j1, 1, -1], err, i=i0, j=j0, cells = {} , e2},
      err = di - dj;
      While[True,
        AppendTo[cells, {i, j}];
        If[i == i1 && j == j1, Break[]];
        e2 = 2 err;
        If[e2 > -dj, err -= dj; i += si];
        If[e2 <  di, err += di; j += sj];
      ];
      cells
    ];

    simulateScan[{x_, y_, th_}] := Module[{ranges, px=x, py=y, hits, dx, dy, tWall, tCirc, a, ang, best},
      ranges = ConstantArray[zMax, nBeams];
      Do[
        a = angles[[k]];
        ang = th + a;
        dx = Cos[ang]; dy = Sin[ang];
        hits = {};
        tWall = rayAABBIntersection[{px, py}, {dx, dy}, bounds["xmin"], bounds["xmax"], bounds["ymin"], bounds["ymax"]];
        If[tWall < Infinity, AppendTo[hits, tWall]];
        Do[
          tCirc = rayCircleIntersection[{px, py}, {dx, dy}, circles[[m]]["c"], circles[[m]]["r"]];
          If[tCirc < Infinity, AppendTo[hits, tCirc]],
          {m, Length[circles]}
        ];
        If[hits =!= {}, best = Min[hits]; If[best <= zMax, ranges[[k]] = Clip[best + RandomVariate[NormalDistribution[0, 0.02]], {0, zMax}]]],
        {k, nBeams}
      ];
      ranges
    ];

    updateRay[{x_, y_, th_}, aBody_, r_] := Module[{start, end, cells, hit, freeCells, i, j, ie, je, ex, ey, ang},
      start = worldToGrid[{x, y}];
      If[start === Missing["OutOfBounds"], Return[]];
      ang = th + aBody;
      ex = x + r Cos[ang]; ey = y + r Sin[ang];
      end = worldToGrid[{ex, ey}];
      If[end === Missing["OutOfBounds"], Return[]];
      cells = bresenham[start, end];
      If[Length[cells] <= 1, Return[]];
      hit = r < (zMax - 0.5 alpha);
      freeCells = If[hit, cells[[2 ;; -2]], cells[[2 ;;]]];
      Do[
        {i, j} = freeCells[[q]];
        logOdds[[j, i]] = Clip[logOdds[[j, i]] + (lFree - l0), {lMin, lMax}],
        {q, Length[freeCells]}
      ];
      If[hit,
        {ie, je} = Last[cells];
        logOdds[[je, ie]] = Clip[logOdds[[je, ie]] + (lOcc - l0), {lMin, lMax}]
      ];
    ];

    (* Trajectory *)
    T = 220;
    poses = Table[
      ang = 2 Pi (t - 1)/T;
      {6 Cos[ang], 6 Sin[ang], ang + Pi/2},
      {t, T}
    ];

    Do[
      scan = simulateScan[poses[[t]]];
      Do[ updateRay[poses[[t]], angles[[k]], scan[[k]]], {k, nBeams} ],
      {t, T}
    ];

    P = logistic[logOdds];

    ArrayPlot[P, ColorFunction -> "SolarColors", Frame -> False,
      PlotLabel -> "2D Occupancy Grid from Simulated LiDAR (p(occ))"]
  ], "Input"]
}]
      

Problem 1 (Derive the log-odds recursion): Starting from Bayes’ rule and the odds definition, derive the update \( l_{t,i} = l_{t-1,i} + \operatorname{logit}(p(m_i=1 \mid z_t, x_t)) - \operatorname{logit}(p_0) \).

Solution: Write posterior odds: \( O_{t,i} = \frac{p(m_i=1 \mid z_{1:t},x_{1:t})}{p(m_i=0 \mid z_{1:t},x_{1:t})} \). Under the occupancy-grid conditional independence approximation, \( O_{t,i} = \frac{p(z_t \mid m_i=1,x_t)}{p(z_t \mid m_i=0,x_t)} O_{t-1,i} \). Taking log gives additivity: \( l_{t,i} = l_{t-1,i} + \log\frac{p(z_t \mid m_i=1,x_t)}{p(z_t \mid m_i=0,x_t)} \). The inverse-model form follows from Bayes’ rule applied to \( p(m_i \mid z_t,x_t) \) and subtracting the prior term \( \operatorname{logit}(p_0) \).

Problem 2 (Repeated updates and saturation): Suppose a fixed cell is observed as “free” \( n_f \) times and “occupied” \( n_o \) times, with constants \( p_{\mathrm{free}} \) and \( p_{\mathrm{occ}} \). Show that without clamping: \( l = l_0 + n_f(\operatorname{logit}(p_{\mathrm{free}})-l_0) + n_o(\operatorname{logit}(p_{\mathrm{occ}})-l_0) \).

Solution: Each event adds a constant increment to log-odds. Summing increments yields the expression. Clamping to \( [l_{\min},l_{\max}] \) prevents extreme probabilities from dominating later evidence.

Problem 3 (Resolution vs. runtime scaling): Assume \( K \) beams per scan, maximum range \( r_{\max} \), and resolution \( \Delta \). Estimate the asymptotic runtime for processing \( T \) scans.

Solution: Each ray traverses at most \( O(r_{\max}/\Delta) \) cells, so total work is:

\[ O\left(T\,K\,\frac{r_{\max}}{\Delta}\right). \]

Problem 4 (Noise band and hit decision): Suppose range noise is Gaussian with standard deviation \( \sigma_r \). If you declare a “hit” when \( r_k < r_{\max} - \alpha/2 \), how should \( \alpha \) scale with \( \sigma_r \) to reduce false occupied endpoints caused by near-max-range returns?

Solution: A practical guideline is \( \alpha \approx c\,\sigma_r \) with \( c \in [3,6] \). Larger \( \alpha \) increases robustness by widening the ambiguous band near \( r_{\max} \), but also reduces the spatial sharpness of occupied markings. This is a bias–variance tradeoff.

You implemented a complete 2D occupancy-grid mapping loop from LiDAR scans under the known-pose assumption. The key technical steps were (i) an inverse sensor model that classifies cells along each beam as free/occupied/unknown, (ii) a log-odds recursion derived from Bayes’ rule enabling stable incremental updates, and (iii) discrete ray traversal (Bresenham/DDA) to update exactly the set of cells intersected by each beam. This lab output (an occupancy probability grid) is the primary map representation used by many AMR stacks.