In an occupancy grid, the environment is discretized into cells \( m_i \), each representing a small region of space. A Bayesian cell models occupancy as a Bernoulli random variable: \( m_i \in \{0,1\} \), where \( m_i=1 \) means “occupied” and \( m_i=0 \) means “free.”

Define the occupancy belief at time \( t \) as \( p_t^i := \mathbb{P}(m_i=1 \mid z_{1:t}, x_{1:t}) \), where \( z_{1:t} \) are sensor measurements and \( x_{1:t} \) are robot poses (assumed given from localization).

The standard occupancy-grid assumption is that, conditioned on the history, cells update independently: the map posterior factorizes approximately as \( \mathbb{P}(m \mid z_{1:t}, x_{1:t}) \approx \prod_i \mathbb{P}(m_i \mid z_{1:t}, x_{1:t}) \). This is not strictly true in general (rays induce correlations), but it yields tractable updates.

The prior occupancy of a cell is \( p_0^i := \mathbb{P}(m_i=1) \), often set to \( 0.5 \) (“unknown”) or a weak bias reflecting typical occupancy in the environment.

\[ 0 \;<\; p_t^i \;<\; 1,\qquad p_t^i = \mathbb{P}(m_i=1 \mid z_{1:t}, x_{1:t}) \]

For a fixed cell \( m_i \), Bayes’ theorem gives the recursive update:

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

The denominator is a normalization constant w.r.t. \( m_i \). Writing the update in terms of \( m_i \in \{0,1\} \) yields a two-hypothesis Bayesian filter for each cell.

In practice, occupancy mapping commonly uses an inverse sensor model \( \mathbb{P}(m_i \mid z_t, x_t) \) (easier to specify for range sensors than \( \mathbb{P}(z_t \mid m_i, x_t) \)). The classical occupancy-grid recursion incorporates this inverse model while preserving the correct prior through a log-odds correction term (derived next).

Directly updating probabilities can be numerically fragile because repeated multiplication of likelihood ratios rapidly pushes beliefs toward \( 0 \) or \( 1 \). The standard solution is to work with odds and log-odds.

Define the odds and log-odds of occupancy for cell \( i \) at time \( t \):

\[ O_t^i \;:=\; \frac{p_t^i}{1-p_t^i},\qquad \ell_t^i \;:=\; \log O_t^i \;=\; \log\!\left(\frac{p_t^i}{1-p_t^i}\right). \]

The inverse transform (recover probability from log-odds) is the logistic function:

\[ p_t^i \;=\; \sigma(\ell_t^i) \;=\; \frac{1}{1+\exp(-\ell_t^i)}. \]

Let the inverse sensor model provide \( p(m_i \mid z_t, x_t) \), and let \( p_0^i \) be the prior. Define \( \ell_0^i = \log\!\left(\frac{p_0^i}{1-p_0^i}\right) \).

Key result (log-odds recursion). Under the occupancy-grid assumptions, the update can be written as:

\[ \ell_t^i \;=\; \ell_{t-1}^i \;+\; \log\!\left(\frac{\mathbb{P}(m_i=1 \mid z_t, x_t)}{1-\mathbb{P}(m_i=1 \mid z_t, x_t)}\right) \;-\; \ell_0^i. \]

The middle term is the log-odds “evidence” contributed by the new measurement at time \( t \); subtracting \( \ell_0^i \) prevents double-counting the prior at each update.

Proof sketch (odds ratio form):

Consider Bayes updates for the two hypotheses \( m_i=1 \) and \( m_i=0 \):

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

The left-hand side is \( O_t^i \) and the last ratio is \( O_{t-1}^i \). In practice, rather than modeling the forward likelihood ratio \( \mathbb{P}(z_t \mid m_i=1, x_t) / \mathbb{P}(z_t \mid m_i=0, x_t) \), we specify the inverse sensor odds \( \mathbb{P}(m_i=1 \mid z_t, x_t) / \mathbb{P}(m_i=0 \mid z_t, x_t) \). Correcting by the prior odds yields:

\[ O_t^i \;=\; O_{t-1}^i \cdot \frac{\mathbb{P}(m_i=1 \mid z_t, x_t)}{\mathbb{P}(m_i=0 \mid z_t, x_t)} \cdot \left(\frac{\mathbb{P}(m_i=0)}{\mathbb{P}(m_i=1)}\right). \]

Taking logs converts multiplication into addition, giving the stated additive recursion in \( \ell_t^i \). This is the main computational advantage of log-odds.

For a planar range sensor (LiDAR/sonar), a single beam suggests: (i) cells along the beam before the return are likely free, (ii) the cell at the return is likely occupied. Let \( z_t = (r_t, \alpha_t) \) denote range and bearing.

A common piecewise inverse model assigns two probabilities: \( p_\text{free} \) for cells in front of the hit and \( p_\text{occ} \) for the endpoint cell. In log-odds form, we add increments \( \Delta \ell_\text{free} = \log\!\left(\frac{p_\text{free}}{1-p_\text{free}}\right)-\ell_0 \) and \( \Delta \ell_\text{occ} = \log\!\left(\frac{p_\text{occ}}{1-p_\text{occ}}\right)-\ell_0 \).

\[ \mathbb{P}(m_i=1 \mid z_t, x_t) \;=\; \begin{cases} p_\text{free}, & \text{if cell } i \text{ lies on the ray before the hit}\\ p_\text{occ}, & \text{if cell } i \text{ is the endpoint (hit cell)}\\ p_0^i, & \text{otherwise (uninformative for this measurement)} \end{cases} \]

flowchart TD
  R["Robot pose (x, y, theta)"] --> B["Cast a beam with angle alpha"]
  B --> T["Traverse grid cells along the beam"]
  T --> F["Cells before endpoint: add 'free' evidence"]
  T --> E["Endpoint cell (if hit): add 'occupied' evidence"]
  F --> S["Clamp log-odds to [l_min, l_max]"]
  E --> S
  S --> P["Convert to probability for display: p = 1/(1+exp(-l))"]
        

Important limitation. Real beams induce correlations between cells (because the same measurement constrains a whole ray), and repeated updates can become overconfident if the independence assumptions are violated. This motivates careful tuning of \( p_\text{free} \), \( p_\text{occ} \), and saturation bounds \( \ell_{\min}, \ell_{\max} \).

Because \( \ell_t^i \) accumulates evidence additively, it can grow in magnitude quickly. In implementations, one typically enforces saturation: \( \ell_t^i \in [\ell_{\min}, \ell_{\max}] \), e.g., \( \ell_{\min}=-10 \), \( \ell_{\max}=10 \). This prevents numerical overflow in \( \exp(\pm \ell) \) and avoids irreversible certainty.

\[ \ell_t^i \;\leftarrow\; \min\!\Big(\ell_{\max},\;\max(\ell_{\min},\;\ell_t^i)\Big). \]

Consistency note. If the inverse sensor model is miscalibrated or if updates violate conditional independence, the map may become systematically biased or overconfident. Two practical mitigations are: (i) reduce evidence magnitudes (choose \( p_\text{occ} \) closer to 0.5 and \( p_\text{free} \) closer to 0.5), and (ii) clamp more aggressively.

Interpretation of log-odds magnitude. If \( \ell=0 \), then \( p=0.5 \). If \( \ell=2 \), then \( p \approx 0.881 \). If \( \ell=-2 \), then \( p \approx 0.119 \). Hence log-odds provide a linear “evidence scale.”

The mapping loop below assumes the robot pose \( x_t \) is available at each time step, and applies a scan update by ray-tracing each beam through the grid.

flowchart TD
  S["Start with prior log-odds grid l0"] --> T["At time t, get pose x_t and scan z_t"]
  T --> B["For each beam: compute endpoint in world"]
  B --> R["Ray-trace cells from robot cell to endpoint cell"]
  R --> F["Update traversed cells with 'free' increment"]
  R --> O["If beam hits: update endpoint \nwith 'occupied' increment"]
  F --> C["Clamp log-odds to [l_min, l_max]"]
  O --> C
  C --> N["Next time step (t = t+1)"]
        

Complexity per scan is roughly proportional to the total number of traversed grid cells across all beams. This motivates careful choice of grid resolution and beam subsampling in real-time systems.

This lab implements the additive log-odds update with a simple inverse sensor model and Bresenham ray traversal. Typical robotics libraries used with this idea include NumPy/SciPy for numerics and ROS/ROS2 interfaces for sensor ingestion (e.g., LaserScan), but the implementation below is dependency-light and self-contained.

Chapter9_Lesson2.py

"""
Chapter9_Lesson2.py
Autonomous Mobile Robots — Chapter 9, Lesson 2
Log-Odds Updates and Bayesian Cells (Occupancy Grid Mapping)

This script demonstrates a numerically stable log-odds occupancy grid update
using an inverse sensor model for a 2D range sensor (e.g., LiDAR).
"""

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:
    """Log-odds: log(p/(1-p))."""
    p = float(np.clip(p, 1e-9, 1.0 - 1e-9))
    return math.log(p / (1.0 - p))


def logistic(l: float) -> float:
    """Inverse logit: 1/(1+exp(-l))."""
    if l >= 0:
        z = math.exp(-l)
        return 1.0 / (1.0 + z)
    else:
        z = math.exp(l)
        return z / (1.0 + z)


def bresenham(x0: int, y0: int, x1: int, y1: int) -> List[Tuple[int, int]]:
    """Integer grid traversal between two cells, including endpoints."""
    points: List[Tuple[int, int]] = []
    dx = abs(x1 - x0)
    dy = abs(y1 - y0)
    x, y = x0, y0
    sx = 1 if x1 >= x0 else -1
    sy = 1 if y1 >= y0 else -1

    if dy <= dx:
        err = dx // 2
        while x != x1:
            points.append((x, y))
            err -= dy
            if err < 0:
                y += sy
                err += dx
            x += sx
        points.append((x1, y1))
    else:
        err = dy // 2
        while y != y1:
            points.append((x, y))
            err -= dx
            if err < 0:
                x += sx
                err += dy
            y += sy
        points.append((x1, y1))
    return points


@dataclass
class OccupancyGridLogOdds:
    width: int
    height: int
    resolution: float
    origin_x: float = 0.0
    origin_y: float = 0.0

    def __post_init__(self) -> None:
        self.log_odds = np.zeros((self.height, self.width), dtype=np.float32)

    def world_to_map(self, x: float, y: float) -> Tuple[int, int]:
        ix = int(round((x - self.origin_x) / self.resolution))
        iy = int(round((y - self.origin_y) / self.resolution))
        return ix, iy

    def in_bounds(self, ix: int, iy: int) -> bool:
        return 0 <= ix < self.width and 0 <= iy < self.height

    def update_cell(self, ix: int, iy: int, delta_l: float, l_min: float, l_max: float) -> None:
        if not self.in_bounds(ix, iy):
            return
        self.log_odds[iy, ix] = float(np.clip(self.log_odds[iy, ix] + delta_l, l_min, l_max))

    def update_scan(
        self,
        pose_xytheta: Tuple[float, float, float],
        angles: np.ndarray,
        ranges: np.ndarray,
        range_max: float,
        p0: float = 0.5,
        p_occ: float = 0.7,
        p_free: float = 0.3,
        l_min: float = -10.0,
        l_max: float = 10.0,
    ) -> None:
        """
        Additive log-odds update:
          l_t = l_{t-1} + logit(p(m|z,x)) - logit(p0)
        """
        x, y, th = pose_xytheta
        l0 = logit(p0)
        l_occ = logit(p_occ) - l0
        l_free = logit(p_free) - l0

        start_ix, start_iy = self.world_to_map(x, y)

        for a, r in zip(angles, ranges):
            if not np.isfinite(r) or r <= 0.0:
                continue
            r_eff = min(float(r), float(range_max))
            bx = x + r_eff * math.cos(th + float(a))
            by = y + r_eff * math.sin(th + float(a))
            end_ix, end_iy = self.world_to_map(bx, by)

            ray = bresenham(start_ix, start_iy, end_ix, end_iy)
            if len(ray) == 0:
                continue

            for (ix, iy) in ray[:-1]:
                self.update_cell(ix, iy, l_free, l_min, l_max)

            if float(r) < float(range_max) - 1e-9:
                ix, iy = ray[-1]
                self.update_cell(ix, iy, l_occ, l_min, l_max)

    def as_probability(self) -> np.ndarray:
        v = np.vectorize(logistic)
        return v(self.log_odds)


def demo() -> None:
    grid = OccupancyGridLogOdds(width=220, height=220, resolution=0.05, origin_x=-5.5, origin_y=-5.5)

    poses = [
        (-2.0, -2.0, 0.2),
        (0.0, -2.5, 0.6),
        (2.0, -1.5, 1.0),
        (2.0, 1.0, 1.6),
        (0.0, 2.5, 2.5),
        (-2.0, 2.0, -2.7),
    ]

    angles = np.linspace(-math.pi / 2, math.pi / 2, 121)
    range_max = 6.0

    obstacles = [(1.5, 0.5, 0.6), (-1.2, 1.2, 0.5)]

    def raycast(pose, a) -> float:
        x, y, th = pose
        dx = math.cos(th + a)
        dy = math.sin(th + a)
        step = 0.02
        t = 0.0
        while t <= range_max:
            px = x + t * dx
            py = y + t * dy
            for (cx, cy, rr) in obstacles:
                if (px - cx) ** 2 + (py - cy) ** 2 <= rr ** 2:
                    return max(0.05, t + np.random.normal(0, 0.03))
            t += step
        return range_max

    np.random.seed(7)
    for pose in poses:
        ranges = np.array([raycast(pose, float(a)) for a in angles], dtype=np.float32)
        grid.update_scan(pose, angles, ranges, range_max=range_max)

    P = grid.as_probability()
    plt.figure(figsize=(6.2, 6.0))
    plt.imshow(P, origin="lower",
               extent=[grid.origin_x, grid.origin_x + grid.width * grid.resolution,
                       grid.origin_y, grid.origin_y + grid.height * grid.resolution])
    plt.title("Occupancy probability (log-odds fused)")
    plt.xlabel("x [m]")
    plt.ylabel("y [m]")
    plt.colorbar()
    plt.tight_layout()
    plt.show()


if __name__ == "__main__":
    demo()
      

This C++ implementation mirrors the Python lab but is dependency-free and exports the occupancy grid to a PGM image. This is a common pattern when integrating mapping into embedded or real-time pipelines.

Chapter9_Lesson2.cpp

// Chapter9_Lesson2.cpp
// Autonomous Mobile Robots — Chapter 9, Lesson 2
// Log-Odds Updates and Bayesian Cells (Occupancy Grid Mapping)
//
// Minimal, dependency-free C++ example that:
//   1) maintains a log-odds occupancy grid
//   2) applies an inverse sensor model update along range rays (Bresenham)
//   3) exports the fused occupancy map as a PGM image (viewable with many image viewers).
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter9_Lesson2.cpp -o Chapter9_Lesson2
// Run:
//   ./Chapter9_Lesson2
//
// Output: occupancy.pgm
#include <cmath>
#include <cstdint>
#include <fstream>
#include <iostream>
#include <string>
#include <tuple>
#include <vector>

static inline float clampf(float v, float lo, float hi) {
  return (v < lo) ? lo : (v > hi) ? hi : v;
}

static inline float logit(float p) {
  const float eps = 1e-6f;
  p = clampf(p, eps, 1.0f - eps);
  return std::log(p / (1.0f - p));
}

static inline float logistic(float l) {
  // Stable sigmoid
  if (l >= 0.0f) {
    float z = std::exp(-l);
    return 1.0f / (1.0f + z);
  } else {
    float z = std::exp(l);
    return z / (1.0f + z);
  }
}

struct Grid {
  int width = 0;
  int height = 0;
  float res = 0.05f;        // m/cell
  float origin_x = -5.5f;   // world coord of cell (0,0) center
  float origin_y = -5.5f;
  std::vector log_odds; // row-major, size = width*height

  Grid(int w, int h, float resolution, float ox, float oy)
      : width(w), height(h), res(resolution), origin_x(ox), origin_y(oy),
        log_odds(static_cast(w) * static_cast(h), 0.0f) {}

  inline bool inBounds(int ix, int iy) const {
    return (0 <= ix && ix < width && 0 <= iy && iy < height);
  }

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

  inline std::pair worldToMap(float x, float y) const {
    int ix = static_cast(std::lround((x - origin_x) / res));
    int iy = static_cast(std::lround((y - origin_y) / res));
    return {ix, iy};
  }

  void updateCell(int ix, int iy, float delta_l, float lmin, float lmax) {
    if (!inBounds(ix, iy)) return;
    int k = idx(ix, iy);
    log_odds[k] = clampf(log_odds[k] + delta_l, lmin, lmax);
  }
};

static std::vector> bresenham(int x0, int y0, int x1, int y1) {
  std::vector> pts;
  int dx = std::abs(x1 - x0);
  int dy = std::abs(y1 - y0);
  int sx = (x1 >= x0) ? 1 : -1;
  int sy = (y1 >= y0) ? 1 : -1;

  int x = x0, y = y0;
  if (dy <= dx) {
    int err = dx / 2;
    while (x != x1) {
      pts.push_back({x, y});
      err -= dy;
      if (err < 0) {
        y += sy;
        err += dx;
      }
      x += sx;
    }
    pts.push_back({x1, y1});
  } else {
    int err = dy / 2;
    while (y != y1) {
      pts.push_back({x, y});
      err -= dx;
      if (err < 0) {
        x += sx;
        err += dy;
      }
      y += sy;
    }
    pts.push_back({x1, y1});
  }
  return pts;
}

// Synthetic raycast against circles
static float raycastCircles(float x, float y, float th, float a, float range_max,
                            const std::vector>& circles) {
  float dx = std::cos(th + a);
  float dy = std::sin(th + a);
  float step = 0.02f;
  for (float t = 0.0f; t <= range_max; t += step) {
    float px = x + t * dx;
    float py = y + t * dy;
    for (const auto& c : circles) {
      float cx, cy, rr;
      std::tie(cx, cy, rr) = c;
      float ex = px - cx;
      float ey = py - cy;
      if (ex * ex + ey * ey <= rr * rr) {
        return std::max(0.05f, t); // return hit range (no noise here)
      }
    }
  }
  return range_max;
}

static void updateScan(Grid& g,
                       float x, float y, float th,
                       const std::vector& angles,
                       const std::vector& ranges,
                       float range_max,
                       float p0 = 0.5f, float p_occ = 0.7f, float p_free = 0.3f,
                       float lmin = -10.0f, float lmax = 10.0f) {
  float l0 = logit(p0);
  float l_occ = logit(p_occ) - l0;
  float l_free = logit(p_free) - l0;

  auto [sx, sy] = g.worldToMap(x, y);

  for (size_t i = 0; i < angles.size(); ++i) {
    float a = angles[i];
    float r = ranges[i];
    if (!std::isfinite(r) || r <= 0.0f) continue;
    float r_eff = std::min(r, range_max);

    float bx = x + r_eff * std::cos(th + a);
    float by = y + r_eff * std::sin(th + a);
    auto [ex, ey] = g.worldToMap(bx, by);

    auto ray = bresenham(sx, sy, ex, ey);
    if (ray.empty()) continue;

    for (size_t k = 0; k + 1 < ray.size(); ++k) {
      g.updateCell(ray[k].first, ray[k].second, l_free, lmin, lmax);
    }
    if (r < range_max - 1e-6f) {
      g.updateCell(ray.back().first, ray.back().second, l_occ, lmin, lmax);
    }
  }
}

static void savePGM(const Grid& g, const std::string& path) {
  std::ofstream out(path, std::ios::binary);
  if (!out) {
    std::cerr << "Failed to open output file: " << path << "\n";
    return;
  }
  out << "P5\n" << g.width << " " << g.height << "\n255\n";

  // Convert probability to grayscale: occupied -> dark (low value), free -> bright.
  for (int iy = g.height - 1; iy >= 0; --iy) { // flip for nicer view
    for (int ix = 0; ix < g.width; ++ix) {
      float l = g.log_odds[g.idx(ix, iy)];
      float p = logistic(l);
      uint8_t v = static_cast(clampf(255.0f * (1.0f - p), 0.0f, 255.0f));
      out.write(reinterpret_cast(&v), 1);
    }
  }
}

int main() {
  Grid grid(220, 220, 0.05f, -5.5f, -5.5f);

  std::vector> circles = {
      {1.5f, 0.5f, 0.6f},
      {-1.2f, 1.2f, 0.5f},
  };

  // Angles for a coarse scan
  std::vector angles;
  const int N = 121;
  for (int i = 0; i < N; ++i) {
    float a = -static_cast(M_PI) / 2.0f + (static_cast(i) / (N - 1)) * static_cast(M_PI);
    angles.push_back(a);
  }
  float range_max = 6.0f;

  std::vector> poses = {
      {-2.0f, -2.0f, 0.2f},
      {0.0f, -2.5f, 0.6f},
      {2.0f, -1.5f, 1.0f},
      {2.0f, 1.0f, 1.6f},
      {0.0f, 2.5f, 2.5f},
      {-2.0f, 2.0f, -2.7f},
  };

  for (const auto& pose : poses) {
    float x, y, th;
    std::tie(x, y, th) = pose;
    std::vector ranges;
    ranges.reserve(angles.size());
    for (float a : angles) {
      ranges.push_back(raycastCircles(x, y, th, a, range_max, circles));
    }
    updateScan(grid, x, y, th, angles, ranges, range_max);
  }

  savePGM(grid, "occupancy.pgm");
  std::cout << "Wrote occupancy.pgm\n";
  return 0;
}
      

The downloadable file contains the full implementation including Bresenham traversal, additive log-odds update, synthetic scan generation, and PGM output.

The Java version follows the same algorithmic structure and is suitable for JVM-based robotics stacks or educational pipelines that emphasize portability.

Chapter9_Lesson2.java

// Chapter9_Lesson2.java
// Autonomous Mobile Robots — Chapter 9, Lesson 2
// Log-Odds Updates and Bayesian Cells (Occupancy Grid Mapping)
//
// Minimal Java example that maintains a log-odds occupancy grid, updates with an inverse
// sensor model along rays (Bresenham), and writes a PGM image.
//
// Compile:
//   javac Chapter9_Lesson2.java
// Run:
//   java Chapter9_Lesson2
//
// Output: occupancy_java.pgm

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

public class Chapter9_Lesson2 {

  static float clamp(float v, float lo, float hi) {
    return (v < lo) ? lo : (v > hi) ? hi : v;
  }

  static float logit(float p) {
    float eps = 1e-6f;
    p = clamp(p, eps, 1.0f - eps);
    return (float) Math.log(p / (1.0f - p));
  }

  static float logistic(float l) {
    if (l >= 0.0f) {
      float z = (float) Math.exp(-l);
      return 1.0f / (1.0f + z);
    } else {
      float z = (float) Math.exp(l);
      return z / (1.0f + z);
    }
  }

  static class Grid {
    final int width, height;
    final float res, originX, originY;
    final float[] logOdds; // row-major

    Grid(int w, int h, float res, float ox, float oy) {
      this.width = w;
      this.height = h;
      this.res = res;
      this.originX = ox;
      this.originY = oy;
      this.logOdds = new float[w * h];
    }

    boolean inBounds(int ix, int iy) {
      return (0 <= ix && ix < width && 0 <= iy && iy < height);
    }

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

    int[] worldToMap(float x, float y) {
      int ix = Math.round((x - originX) / res);
      int iy = Math.round((y - originY) / res);
      return new int[] {ix, iy};
    }

    void updateCell(int ix, int iy, float delta, float lmin, float lmax) {
      if (!inBounds(ix, iy)) return;
      int k = idx(ix, iy);
      logOdds[k] = clamp(logOdds[k] + delta, lmin, lmax);
    }
  }

  static List bresenham(int x0, int y0, int x1, int y1) {
    List pts = new ArrayList<>();
    int dx = Math.abs(x1 - x0);
    int dy = Math.abs(y1 - y0);
    int sx = (x1 >= x0) ? 1 : -1;
    int sy = (y1 >= y0) ? 1 : -1;
    int x = x0, y = y0;

    if (dy <= dx) {
      int err = dx / 2;
      while (x != x1) {
        pts.add(new int[] {x, y});
        err -= dy;
        if (err < 0) {
          y += sy;
          err += dx;
        }
        x += sx;
      }
      pts.add(new int[] {x1, y1});
    } else {
      int err = dy / 2;
      while (y != y1) {
        pts.add(new int[] {x, y});
        err -= dx;
        if (err < 0) {
          x += sx;
          err += dy;
        }
        y += sy;
      }
      pts.add(new int[] {x1, y1});
    }
    return pts;
  }

  static float raycastCircles(float x, float y, float th, float a, float rangeMax, float[][] circles) {
    float dx = (float) Math.cos(th + a);
    float dy = (float) Math.sin(th + a);
    float step = 0.02f;
    for (float t = 0.0f; t <= rangeMax; t += step) {
      float px = x + t * dx;
      float py = y + t * dy;
      for (float[] c : circles) {
        float cx = c[0], cy = c[1], rr = c[2];
        float ex = px - cx, ey = py - cy;
        if (ex * ex + ey * ey <= rr * rr) {
          return Math.max(0.05f, t);
        }
      }
    }
    return rangeMax;
  }

  static void updateScan(Grid g, float x, float y, float th,
                         float[] angles, float[] ranges, float rangeMax,
                         float p0, float pOcc, float pFree,
                         float lmin, float lmax) {
    float l0 = logit(p0);
    float lOcc = logit(pOcc) - l0;
    float lFree = logit(pFree) - l0;

    int[] s = g.worldToMap(x, y);
    int sx = s[0], sy = s[1];

    for (int i = 0; i < angles.length; i++) {
      float a = angles[i];
      float r = ranges[i];
      if (!Float.isFinite(r) || r <= 0.0f) continue;
      float rEff = Math.min(r, rangeMax);

      float bx = x + rEff * (float) Math.cos(th + a);
      float by = y + rEff * (float) Math.sin(th + a);
      int[] e = g.worldToMap(bx, by);
      int ex = e[0], ey = e[1];

      List ray = bresenham(sx, sy, ex, ey);
      if (ray.isEmpty()) continue;

      for (int k = 0; k + 1 < ray.size(); k++) {
        int[] p = ray.get(k);
        g.updateCell(p[0], p[1], lFree, lmin, lmax);
      }
      if (r < rangeMax - 1e-6f) {
        int[] p = ray.get(ray.size() - 1);
        g.updateCell(p[0], p[1], lOcc, lmin, lmax);
      }
    }
  }

  static void savePGM(Grid g, String path) throws IOException {
    try (FileOutputStream out = new FileOutputStream(path)) {
      String header = "P5\n" + g.width + " " + g.height + "\n255\n";
      out.write(header.getBytes(StandardCharsets.US_ASCII));

      for (int iy = g.height - 1; iy >= 0; iy--) {
        for (int ix = 0; ix < g.width; ix++) {
          float l = g.logOdds[g.idx(ix, iy)];
          float p = logistic(l);
          int v = Math.round(clamp(255.0f * (1.0f - p), 0.0f, 255.0f));
          out.write((byte) (v & 0xFF));
        }
      }
    }
  }

  public static void main(String[] args) throws IOException {
    Grid grid = new Grid(220, 220, 0.05f, -5.5f, -5.5f);

    float[][] circles = new float[][] {
      {1.5f, 0.5f, 0.6f},
      {-1.2f, 1.2f, 0.5f},
    };

    // Angles
    int N = 121;
    float[] angles = new float[N];
    for (int i = 0; i < N; i++) {
      angles[i] = (float) (-Math.PI / 2.0 + (double) i * (Math.PI / (N - 1)));
    }
    float rangeMax = 6.0f;

    float[][] poses = new float[][] {
      {-2.0f, -2.0f, 0.2f},
      {0.0f, -2.5f, 0.6f},
      {2.0f, -1.5f, 1.0f},
      {2.0f, 1.0f, 1.6f},
      {0.0f, 2.5f, 2.5f},
      {-2.0f, 2.0f, -2.7f},
    };

    for (float[] pose : poses) {
      float x = pose[0], y = pose[1], th = pose[2];
      float[] ranges = new float[N];
      for (int i = 0; i < N; i++) {
        ranges[i] = raycastCircles(x, y, th, angles[i], rangeMax, circles);
      }
      updateScan(grid, x, y, th, angles, ranges, rangeMax,
                 0.5f, 0.7f, 0.3f, -10.0f, 10.0f);
    }

    savePGM(grid, "occupancy_java.pgm");
    System.out.println("Wrote occupancy_java.pgm");
  }
}
      

MATLAB is commonly used to prototype mapping algorithms and to explore the effect of grid resolution, inverse sensor model parameters, and saturation bounds. The provided script implements the log-odds update and includes an optional programmatic Simulink block diagram for the “log-odds accumulator + saturation” concept.

Chapter9_Lesson2.m

% Chapter9_Lesson2.m
% Autonomous Mobile Robots — Chapter 9, Lesson 2
% Log-Odds Updates and Bayesian Cells (Occupancy Grid Mapping)
%
% This script demonstrates a log-odds occupancy grid update using a simple
% inverse sensor model and Bresenham ray traversal.
%
% Notes:
%   - Requires base MATLAB for the script portion.
%   - Optional: Simulink model construction is included at the end (requires Simulink).

clear; clc; close all;

%% Parameters
W = 220; H = 220;
res = 0.05;            % meters per cell
origin = [-5.5, -5.5]; % world coordinate of cell (1,1) center
rangeMax = 6.0;

p0 = 0.5;
pOcc = 0.7;
pFree = 0.3;
lMin = -10; lMax = 10;

logOdds = zeros(H, W, 'single');

%% Helpers
logit = @(p) log(max(min(p, 1-1e-6), 1e-6) ./ (1 - max(min(p, 1-1e-6), 1e-6)));
sigmoid = @(l) 1 ./ (1 + exp(-l));

worldToMap = @(x,y) [round((x - origin(1)) / res) + 1, round((y - origin(2)) / res) + 1]; % 1-based

inBounds = @(ix,iy) (ix >= 1 && ix <= W && iy >= 1 && iy <= H);

updateCell = @(ix,iy,dl) assignin('caller','logOdds', ...
  localUpdate(logOdds, ix, iy, dl, lMin, lMax, inBounds));

%% Synthetic environment: two circles
circles = [ 1.5, 0.5, 0.6;
           -1.2, 1.2, 0.5];

poses = [ -2.0, -2.0, 0.2;
           0.0, -2.5, 0.6;
           2.0, -1.5, 1.0;
           2.0,  1.0, 1.6;
           0.0,  2.5, 2.5;
          -2.0,  2.0,-2.7];

angles = linspace(-pi/2, pi/2, 121);

%% Main loop: update scans
l0 = logit(p0);
lOcc = logit(pOcc) - l0;
lFree = logit(pFree) - l0;

for k = 1:size(poses,1)
    x = poses(k,1); y = poses(k,2); th = poses(k,3);

    ranges = zeros(size(angles));
    for i = 1:numel(angles)
        ranges(i) = raycastCircles(x, y, th, angles(i), rangeMax, circles);
    end

    s = worldToMap(x,y);
    sx = s(1); sy = s(2);

    for i = 1:numel(angles)
        a = angles(i); r = ranges(i);
        if ~isfinite(r) || r <= 0, continue; end
        rEff = min(r, rangeMax);

        bx = x + rEff * cos(th + a);
        by = y + rEff * sin(th + a);
        e = worldToMap(bx,by);
        ex = e(1); ey = e(2);

        ray = bresenhamLine(sx, sy, ex, ey);
        if isempty(ray), continue; end

        % Free cells (excluding endpoint)
        for j = 1:size(ray,1)-1
            ix = ray(j,1); iy = ray(j,2);
            if inBounds(ix,iy)
                logOdds(iy,ix) = min(max(logOdds(iy,ix) + lFree, lMin), lMax);
            end
        end

        % Occupied endpoint if hit
        if r < rangeMax - 1e-9
            ix = ray(end,1); iy = ray(end,2);
            if inBounds(ix,iy)
                logOdds(iy,ix) = min(max(logOdds(iy,ix) + lOcc, lMin), lMax);
            end
        end
    end
end

%% Visualize
P = sigmoid(double(logOdds));
figure('Color','w'); imagesc([origin(1), origin(1)+W*res], [origin(2), origin(2)+H*res], P);
axis image; set(gca,'YDir','normal'); colorbar;
title('Occupancy probability (log-odds fused)');
xlabel('x [m]'); ylabel('y [m]');

%% ---- Optional: programmatically build a Simulink "log-odds accumulator" block diagram ----
% This section creates a *conceptual* model that integrates delta_l updates
% and applies saturation. A full LiDAR ray-tracing sensor model is typically
% implemented in MATLAB Function blocks or external code.
%
% Requires Simulink. Uncomment to generate.
%
%{
modelName = 'Chapter9_Lesson2_LogOdds_Accumulator';
new_system(modelName); open_system(modelName);

add_block('simulink/Sources/In1', [modelName '/delta_l'], 'Position',[30 50 60 70]);
add_block('simulink/Discrete/Unit Delay', [modelName '/z^-1'], 'Position',[110 45 160 75]);
add_block('simulink/Math Operations/Add', [modelName '/add'], 'Position',[200 45 240 75]);
add_block('simulink/Discontinuities/Saturation', [modelName '/sat'], 'Position',[280 45 340 75]);
add_block('simulink/Sinks/Out1', [modelName '/l_t'], 'Position',[380 50 410 70]);

set_param([modelName '/z^-1'], 'InitialCondition', '0');
set_param([modelName '/sat'], 'LowerLimit', num2str(lMin), 'UpperLimit', num2str(lMax));

add_line(modelName, 'delta_l/1', 'add/2');
add_line(modelName, 'z^-1/1', 'add/1');
add_line(modelName, 'add/1', 'sat/1');
add_line(modelName, 'sat/1', 'l_t/1');
add_line(modelName, 'sat/1', 'z^-1/1');

save_system(modelName);
%}

%% ---- Local functions ----
function logOddsOut = localUpdate(logOddsIn, ix, iy, dl, lMin, lMax, inBoundsFun)
    logOddsOut = logOddsIn;
    if inBoundsFun(ix,iy)
        logOddsOut(iy,ix) = min(max(logOddsOut(iy,ix) + dl, lMin), lMax);
    end
end

function r = raycastCircles(x,y,th,a,rangeMax,circles)
    dx = cos(th+a); dy = sin(th+a);
    step = 0.02;
    t = 0.0;
    while t <= rangeMax
        px = x + t*dx; py = y + t*dy;
        for i = 1:size(circles,1)
            cx = circles(i,1); cy = circles(i,2); rr = circles(i,3);
            if (px-cx)^2 + (py-cy)^2 <= rr^2
                r = max(0.05, t);
                return;
            end
        end
        t = t + step;
    end
    r = rangeMax;
end

function pts = bresenhamLine(x0,y0,x1,y1)
    dx = abs(x1-x0);
    dy = abs(y1-y0);
    sx = 1; if x1 < x0, sx = -1; end
    sy = 1; if y1 < y0, sy = -1; end

    x = x0; y = y0;
    pts = [];

    if dy <= dx
        err = floor(dx/2);
        while x ~= x1
            pts = [pts; x, y]; %#ok
            err = err - dy;
            if err < 0
                y = y + sy;
                err = err + dx;
            end
            x = x + sx;
        end
        pts = [pts; x1, y1];
    else
        err = floor(dy/2);
        while y ~= y1
            pts = [pts; x, y]; %#ok
            err = err - dx;
            if err < 0
                x = x + sx;
                err = err + dy;
            end
            y = y + sy;
        end
        pts = [pts; x1, y1];
    end
end
      

Mathematica is helpful for symbolic manipulation of Bayes recursions and for exploring how repeated evidence drives the posterior toward certainty in log-odds space.

Chapter9_Lesson2.nb

(* ::Package:: *)

(* Chapter9_Lesson2.nb
   Autonomous Mobile Robots — Chapter 9, Lesson 2
   Log-Odds Updates and Bayesian Cells (Occupancy Grid Mapping)

   This notebook illustrates the Bayesian cell update and the log-odds additivity
   for repeated independent measurements. *)

Notebook[{
  Cell["Chapter 9, Lesson 2 — Log-Odds Updates and Bayesian Cells", "Title"],
  Cell["1) Definitions: probability and log-odds", "Section"],
  Cell[BoxData@ToBoxes@HoldForm[
    logit[p_] := Log[p/(1 - p)];
    sigmoid[l_] := 1/(1 + Exp[-l]);
  ], "Input"],
  Cell["Assume a prior occupancy probability p0 and an inverse sensor model that returns p(m|z).", "Text"],
  Cell[BoxData@ToBoxes@HoldForm[
    p0 = 0.5;
    pOcc = 0.7;  (* endpoint occupied evidence *)
    pFree = 0.3; (* along-ray free evidence *)
    l0 = logit[p0];
    dOcc = logit[pOcc] - l0;
    dFree = logit[pFree] - l0;
  ], "Input"],
  Cell["2) Additive log-odds update for repeated identical evidence", "Section"],
  Cell[BoxData@ToBoxes@HoldForm[
    (* If the same 'occupied' evidence is seen n times independently, then log-odds add. *)
    l[n_] := l0 + n*dOcc;
    p[n_] := sigmoid[l[n]];
    Table[{n, N[p[n], 10]}, {n, 0, 20, 2}]
  ], "Input"],
  Cell["3) Plot the posterior occupancy vs number of updates", "Section"],
  Cell[BoxData@ToBoxes@HoldForm[
    ListLinePlot[Table[{n, p[n]}, {n, 0, 40}],
      AxesLabel -> {\"n\", \"p(occupied)\"},
      PlotRange -> {0, 1},
      GridLines -> Automatic]
  ], "Input"],
  Cell["4) Two independent sensors: combine evidence by adding their log-odds increments", "Section"],
  Cell[BoxData@ToBoxes@HoldForm[
    pOcc2 = 0.65; (* second sensor's occupied evidence *)
    dOcc2 = logit[pOcc2] - l0;
    l2[n1_, n2_] := l0 + n1*dOcc + n2*dOcc2;
    p2[n1_, n2_] := sigmoid[l2[n1, n2]];
    Table[{n1, n2, N[p2[n1, n2], 8]}, {n1, 0, 10, 5}, {n2, 0, 10, 5}] // Flatten[#, 1] &
  ], "Input"]
},
WindowSize -> {980, 740},
WindowMargins -> { {Automatic, 50}, {Automatic, 50} }
]
      

The downloadable notebook file contains additional cells including plots and a two-sensor fusion example by adding log-odds increments.

Problem 1 (Derive the additive log-odds update): Let \( p_t = \mathbb{P}(m=1 \mid z_{1:t}, x_{1:t}) \) and define odds \( O_t = \frac{p_t}{1-p_t} \). Show that if an inverse sensor model provides \( \mathbb{P}(m=1 \mid z_t, x_t) \), then the log-odds recursion \( \ell_t = \ell_{t-1} + \log\!\big(\frac{\mathbb{P}(m=1 \mid z_t, x_t)}{1-\mathbb{P}(m=1 \mid z_t, x_t)}\big) - \ell_0 \) follows from an odds-ratio update.

Solution:

Start from the two-hypothesis Bayes update: \( O_t = O_{t-1}\cdot \Lambda_t \), where \( \Lambda_t = \frac{\mathbb{P}(z_t \mid m=1, x_t)}{\mathbb{P}(z_t \mid m=0, x_t)} \). If we specify instead inverse odds \( \frac{\mathbb{P}(m=1 \mid z_t, x_t)}{\mathbb{P}(m=0 \mid z_t, x_t)} \), we correct by the prior odds \( O_0 \) to avoid reapplying the prior at each step:

\[ O_t \;=\; O_{t-1}\cdot \frac{\mathbb{P}(m=1 \mid z_t, x_t)}{\mathbb{P}(m=0 \mid z_t, x_t)} \cdot \frac{1}{O_0}. \]

Taking logs gives: \( \ell_t = \ell_{t-1} + \log\!\big(\frac{\mathbb{P}(m=1 \mid z_t, x_t)}{\mathbb{P}(m=0 \mid z_t, x_t)}\big) - \ell_0 \), which matches the stated recursion since \( \mathbb{P}(m=0 \mid \cdot)=1-\mathbb{P}(m=1 \mid \cdot) \).

Problem 2 (Evidence accumulation rate): Suppose a fixed cell repeatedly receives identical “occupied” evidence from an inverse sensor model: \( \mathbb{P}(m=1 \mid z_t, x_t) = p_\text{occ} \) for all \( t \), and prior \( p_0 \). Derive \( p_n \) after \( n \) updates.

Solution:

The log-odds update adds a constant increment \( \Delta = \log\!\left(\frac{p_\text{occ}}{1-p_\text{occ}}\right) - \log\!\left(\frac{p_0}{1-p_0}\right) \) each time. Hence \( \ell_n = \ell_0 + n\Delta \), and \( p_n = \frac{1}{1+\exp(-\ell_n)} \).

\[ \ell_n = \ell_0 + n\left[ \log\!\left(\frac{p_\text{occ}}{1-p_\text{occ}}\right) - \ell_0 \right], \qquad p_n = \frac{1}{1+\exp(-\ell_n)}. \]

Problem 3 (Saturation effect): Let saturation bounds be \( \ell_{\min} \), \( \ell_{\max} \). Show how saturation implies a minimum and maximum representable occupancy probability.

Solution:

Since \( p=\sigma(\ell)=\frac{1}{1+\exp(-\ell)} \) is monotone increasing in \( \ell \), saturation yields: \( p_{\min}=\sigma(\ell_{\min}) \) and \( p_{\max}=\sigma(\ell_{\max}) \).

\[ p_{\min} = \frac{1}{1+\exp(-\ell_{\min})},\qquad p_{\max} = \frac{1}{1+\exp(-\ell_{\max})}. \]

Problem 4 (Two independent sensors at one time step): Assume at time \( t \) two independent sensors provide inverse occupancy probabilities \( p^{(1)}=\mathbb{P}(m=1 \mid z_t^{(1)}, x_t) \) and \( p^{(2)}=\mathbb{P}(m=1 \mid z_t^{(2)}, x_t) \) for the same cell, with the same prior \( p_0 \). Derive the combined log-odds increment to apply at time \( t \).

Solution:

In log-odds space, independent evidence adds. Apply each inverse-model increment once (each corrected by the prior):

\[ \ell_t \;=\; \ell_{t-1} \;+\; \Big(\log\!\tfrac{p^{(1)}}{1-p^{(1)}} - \ell_0\Big) \;+\; \Big(\log\!\tfrac{p^{(2)}}{1-p^{(2)}} - \ell_0\Big). \]

Operationally, one can update sequentially with sensor 1 then sensor 2 (with the same \( \ell_0 \) correction), or sum the two increments and apply once.

Problem 5 (Map bias from miscalibrated inverse model): Suppose the true environment is free in a region, but due to multipath or specular effects a sensor frequently returns phantom hits, so your inverse model applies \( p_\text{occ} \) too aggressively. Explain (mathematically) why the map becomes biased, and propose a mitigation using the log-odds parameters.

Solution:

Each phantom hit adds a positive increment \( \Delta \ell_\text{occ} = \log\!\left(\frac{p_\text{occ}}{1-p_\text{occ}}\right) - \ell_0 \), so after many false hits the cell log-odds drifts upward: \( \ell_n \approx \ell_0 + n\Delta \ell_\text{occ} \), driving \( p_n=\sigma(\ell_n) \) toward 1 even when the cell is actually free.

Mitigations: choose \( p_\text{occ} \) closer to \( 0.5 \) (smaller increments), choose stronger clamping (smaller \( \ell_{\max} \)), or incorporate a more accurate inverse model that reduces phantom-hit probability (e.g., consider sensor-specific artifact models).

We modeled each occupancy-grid cell as a Bernoulli “Bayesian cell” and derived the log-odds mapping recursion as a sequential Bayesian fusion rule. The log-odds transform converts multiplicative odds updates into an additive accumulation of evidence, enabling stable long-horizon mapping with simple saturation. We then connected theory to practice via inverse sensor models for range sensors (free space along rays, occupied endpoints) and provided multi-language reference implementations that follow the same mathematical update.