Let the (unknown) environment map be a random object \( M \) (e.g., an occupancy grid), and let the robot choose an action or viewpoint \( a \in \mathcal{A} \). Executing \( a \) yields measurements \( Z \) (LiDAR, depth, etc.). The core principle is: \( \text{choose } a \text{ to reduce uncertainty in } M \).

The canonical information objective is the expected entropy reduction:

\[ \operatorname{IG}(a) \;=\; H(M) \;-\; \mathbb{E}_{Z\,|\,a}\!\left[\,H\!\left(M \mid Z, a\right)\right]. \]

In practice we also penalize travel time, energy, or risk. Two common utilities are: \( U(a)=\operatorname{IG}(a)-\lambda\,C(a) \) and \( U(a)=\operatorname{IG}(a)/(C(a)+\epsilon) \), where \( C(a) \) is a cost (e.g., path length on the current map) and \( \lambda \) is a trade-off parameter.

flowchart TD
  S["Start"] --> B["Belief map M: occupancy probabilities"]
  B --> F["Extract candidate targets (frontiers + sampled viewpoints)"]
  F --> E["For each candidate a: simulate visibility (ray casting)"]
  E --> G["Compute expected IG(a) from visible uncertain cells"]
  G --> C["Compute cost C(a): path length / time / risk"]
  C --> U["Utility U(a) = IG(a) - lambda*C(a) or IG(a)/(C(a)+eps)"]
  U --> A["Select best action a*"]
  A --> P["Plan + execute motion to a*"]
  P --> Z["Acquire measurements Z"]
  Z --> UPD["Update map belief M using mapping (Chapter 9)"]
  UPD --> F
        

The rest of this lesson makes \( H(M) \) and \( \operatorname{IG}(a) \) explicit for occupancy grids and provides implementable approximations.

In an occupancy grid, each cell \( m_i \in \{0,1\} \) represents free (0) or occupied (1) with belief \( p_i = \Pr(m_i=1) \). The Shannon entropy (in nats) of a Bernoulli variable is:

\[ H(m_i) \;=\; H(p_i) \;=\; -p_i\ln(p_i) \;-\; (1-p_i)\ln(1-p_i), \quad 0 \,<\, p_i \,<\, 1. \]

A standard approximation (used widely in active mapping) assumes conditional independence across cells, giving total map entropy: \( H(M) \approx \sum_i H(p_i) \).

\[ H(M) \;\approx\; \sum_{i=1}^{N} \Big[-p_i\ln(p_i)-(1-p_i)\ln(1-p_i)\Big]. \]

Key property (maximum uncertainty at 0.5). The entropy is maximized at \( p_i=0.5 \) and decreases as the cell becomes certain (\( p_i \to 0 \) or \( p_i \to 1 \)).

Proof (calculus):

\[ \frac{d}{dp}H(p) \;=\; \ln\!\left(\frac{1-p}{p}\right), \qquad \frac{d^2}{dp^2}H(p) \;=\; -\frac{1}{p(1-p)} \,<\, 0. \]

Setting the first derivative to zero gives \( \ln\!\left(\frac{1-p}{p}\right)=0 \Rightarrow \frac{1-p}{p}=1 \Rightarrow p=0.5 \). The second derivative is negative for all \( 0<p<1 \), so the stationary point is a unique global maximum.

Connection to log-odds (from Chapter 9): if \( \ell_i=\ln\!\left(\frac{p_i}{1-p_i}\right) \), then \( p_i = \frac{1}{1+e^{-\ell_i} } \). Entropy can be seen as a concave function of \( \ell_i \), shrinking as \( |\ell_i| \) grows.

The expected entropy reduction objective is exactly a conditional mutual information:

\[ \operatorname{IG}(a) \;=\; H(M)-\mathbb{E}_{Z\,|\,a}\!\left[H(M\mid Z,a)\right] \;=\; I(M;Z\mid a). \]

Proof: By definition of conditional mutual information,

\[ I(M;Z\mid a) \;=\; H(M\mid a) - H(M\mid Z,a). \]

Since \( a \) is a chosen action (not a random variable generated by \( M \)), \( H(M\mid a)=H(M) \). Taking expectation over \( Z\mid a \) yields the expected posterior entropy term. Therefore the two objectives coincide.

This identity matters because it lets us import powerful results (e.g., diminishing returns/submodularity) and also derive implementable per-cell scoring rules.

For an occupancy cell \( m \in \{0,1\} \) with prior \( p=\Pr(m=1) \), assume a simplified binary sensor output \( z\in\{\text{occ},\text{free}\} \) with:

\[ \Pr(z=\text{occ}\mid m=1)=p_{\text{hit} },\qquad \Pr(z=\text{occ}\mid m=0)=p_{\text{false} }. \]

The measurement marginal is: \( \Pr(z=\text{occ}) = p_{\text{hit} }p + p_{\text{false} }(1-p) \). Bayes rule gives posteriors:

\[ \Pr(m=1\mid z=\text{occ}) = \frac{p_{\text{hit} }p}{p_{\text{hit} }p+p_{\text{false} }(1-p)}, \\ \Pr(m=1\mid z=\text{free}) = \frac{(1-p_{\text{hit} })p}{(1-p_{\text{hit} })p+(1-p_{\text{false} })(1-p)}. \]

The per-cell information gain is:

\[ \operatorname{IG}_{\text{cell} }(p) = H(p) - \sum_{z \in \{\text{occ},\text{free}\} } \Pr(z)\,H\!\left(\Pr(m=1\mid z)\right). \]

flowchart TD
  P["Prior p = P(m=1)"] --> H1["Compute H(p)"]
  P --> ZP["Compute P(z=occ), P(z=free)"]
  ZP --> B1["Bayes: p_post_occ = P(m=1|z=occ)"]
  ZP --> B2["Bayes: p_post_free = P(m=1|z=free)"]
  B1 --> H2["H(p_post_occ)"]
  B2 --> H3["H(p_post_free)"]
  H2 --> EH["E[H(post)] = P(z=occ)*H(p_post_occ) + P(z=free)*H(p_post_free)"]
  H3 --> EH
  H1 --> IG["IG_cell = H(p) - E[H(post)]"]
  EH --> IG
        

Interpretation: cells near \( p\approx 0.5 \) have large entropy and typically produce higher expected entropy reduction. Very certain cells (near 0 or 1) contribute little. This is why many systems restrict scoring to an “uncertainty band”, e.g. \( 0.4 \le p \le 0.6 \).

A viewpoint/action \( a \) induces a set of cells \( V(a) \) that are expected to be visible (within FOV and range, not occluded). Under independent-cell and independent-measurement approximations:

\[ \operatorname{IG}(a) \;\approx\; \sum_{i \in V(a)} \operatorname{IG}_{\text{cell} }(p_i). \]

Visibility approximation (ray-based): (i) cast rays through the grid, (ii) collect cells along each ray, (iii) stop a ray when it hits a likely-occupied cell (occlusion model), (iv) sum per-cell IG on uncertain cells.

If \( R \) rays are cast and each ray samples \( L \) grid steps, the naive complexity is \( \mathcal{O}(RL) \) per candidate viewpoint. This is often acceptable because we rank a moderate number of candidates per cycle (frontiers + samples).

Greedy selection insight (diminishing returns): When selecting multiple future viewpoints, the mutual-information objective \( F(S)=I(M;Z_S) \) is (under standard conditional-independence assumptions) a monotone submodular set function, implying a classic greedy strategy achieves a \( (1-1/e) \)-approximation to the optimal multi-view set. This is a research-grade justification for greedy exploration policies.

\[ \text{(Diminishing returns)}\quad A \subseteq B,\; s \notin B \;\Rightarrow\; I(M;Z_s \mid Z_A) \;\ge\; I(M;Z_s \mid Z_B). \]

One way to see this is via the chain rule \( I(M;Z_{A\cup\{s\} })=I(M;Z_A)+I(M;Z_s\mid Z_A) \) and the fact that conditioning on more measurements reduces uncertainty in \( Z_s \), shrinking its incremental contribution.

• Python: Use numpy for grids, and in ROS2 use rclpy with nav_msgs/OccupancyGrid and sensor_msgs/LaserScan. Convert map values (-1 unknown, 0 free, 100 occupied) into probabilities; unknown ← 0.5 is standard.
• C++: In ROS2 use rclcpp and the same message types. For performance, precompute ray traversal tables per angle (lookup per cell) or use Bresenham/DDA.
• Java: For research prototypes, keep the IG logic independent of middleware. If using ROS2 Java bindings (rcljava), the same occupancy conversion applies.
• MATLAB/Simulink: Robotics System Toolbox provides occupancy representations and ROS interfaces. Put the scoring function inside a MATLAB Function block if you want a Simulink loop; the math is identical.
• Wolfram Mathematica: Useful for symbolic checks (entropy derivatives), fast prototyping, and plotting IG(p) curves to understand sensor-parameter effects.

Each file below is included verbatim in the ZIP you downloaded. For HTML safety, characters < and > are escaped in the code blocks.

Chapter17_Lesson2.py

# -*- coding: utf-8 -*-
"""
Chapter 17 - Lesson 2: Information Gain and Entropy Reduction
Autonomous Mobile Robots (Control Engineering)

This file provides a minimal, dependency-light implementation of:
- Bernoulli (occupancy) entropy
- Per-cell mutual information (information gain) for a binary measurement model
- A simple expected information gain estimator for candidate viewpoints using ray casting

Optional integration notes for ROS/ROS2 occupancy grids are included at the end.
"""

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


def clip(x: float, lo: float, hi: float) -> float:
    return lo if x < lo else (hi if x > hi else x)


def bernoulli_entropy(p: float, eps: float = 1e-12) -> float:
    """
    Shannon entropy of Bernoulli(p) in nats:
        H(p) = -p ln p - (1-p) ln(1-p)
    """
    p = clip(p, eps, 1.0 - eps)
    return -p * math.log(p) - (1.0 - p) * math.log(1.0 - p)


def info_gain_cell(p: float, p_hit: float = 0.85, p_false: float = 0.15) -> float:
    """
    Mutual information I(M;Z) for a single Bernoulli occupancy variable M ~ Bernoulli(p)
    observed through a binary sensor Z in {occ, free} with:
        P(Z=occ | M=occ)  = p_hit
        P(Z=occ | M=free) = p_false

    IG = H(M) - E_Z[ H(M | Z) ].

    Returns IG in nats.
    """
    H_prior = bernoulli_entropy(p)

    # Marginal measurement probabilities
    P_z_occ = p_hit * p + p_false * (1.0 - p)
    P_z_free = 1.0 - P_z_occ

    eps = 1e-15
    P_z_occ = clip(P_z_occ, eps, 1.0 - eps)
    P_z_free = clip(P_z_free, eps, 1.0 - eps)

    # Posteriors
    p_post_given_z_occ = (p_hit * p) / P_z_occ
    p_post_given_z_free = ((1.0 - p_hit) * p) / P_z_free

    # Expected posterior entropy
    H_post = (
        P_z_occ * bernoulli_entropy(p_post_given_z_occ)
        + P_z_free * bernoulli_entropy(p_post_given_z_free)
    )

    return H_prior - H_post


@dataclass
class OccupancyGrid:
    """
    A simple occupancy grid storing probabilities p in [0,1].

    width, height: number of cells
    resolution: meters per cell
    origin_x, origin_y: world coordinate of cell (0,0) lower-left corner
    """
    width: int
    height: int
    resolution: float
    origin_x: float = 0.0
    origin_y: float = 0.0

    def __post_init__(self) -> None:
        self.p = [[0.5 for _ in range(self.width)] for _ in range(self.height)]

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

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

    def map_entropy(self) -> float:
        return sum(bernoulli_entropy(prob) for row in self.p for prob in row)


def ray_cast_cells(
    grid: OccupancyGrid,
    x0: float,
    y0: float,
    theta: float,
    max_range: float,
    step: float | None = None,
) -> List[Tuple[int, int]]:
    """
    Simple ray casting by stepping along the ray and collecting visited grid cells.
    For expected IG, we mainly need which cells are visible (approx).

    step default: half a cell.
    """
    if step is None:
        step = 0.5 * grid.resolution

    cells: List[Tuple[int, int]] = []
    seen: Set[Tuple[int, int]] = set()

    t = 0.0
    while t <= max_range:
        x = x0 + t * math.cos(theta)
        y = y0 + t * math.sin(theta)
        ix, iy = grid.world_to_grid(x, y)
        if not grid.in_bounds(ix, iy):
            break
        key = (ix, iy)
        if key not in seen:
            seen.add(key)
            cells.append(key)
        t += step

    return cells


def expected_info_gain_view(
    grid: OccupancyGrid,
    pose: Tuple[float, float, float],
    fov_deg: float = 180.0,
    n_rays: int = 181,
    max_range: float = 8.0,
    p_hit: float = 0.85,
    p_false: float = 0.15,
    unknown_band: Tuple[float, float] = (0.4, 0.6),
    occ_stop_threshold: float = 0.75,
) -> float:
    """
    Approximate expected information gain from a viewpoint pose=(x,y,yaw).

    We:
      1) Cast rays in a fan (FOV).
      2) Traverse cells along each ray until we hit a likely-occupied cell (occlusion).
      3) Accumulate per-cell IG for "uncertain" cells (near 0.5), under an independent-cell model.

    This is a common active-mapping approximation for occupancy grids.
    """
    x, y, yaw = pose
    fov = math.radians(fov_deg)

    if n_rays < 2:
        angles = [0.0]
    else:
        angles = [(-0.5 * fov) + i * (fov / (n_rays - 1)) for i in range(n_rays)]

    lo_u, hi_u = unknown_band
    visited: Set[Tuple[int, int]] = set()
    ig_total = 0.0

    for a in angles:
        theta = yaw + a
        cells = ray_cast_cells(grid, x, y, theta, max_range=max_range)

        for (ix, iy) in cells:
            if (ix, iy) in visited:
                continue
            visited.add((ix, iy))

            p = grid.p[iy][ix]

            # occlusion / stopping
            if p >= occ_stop_threshold:
                break

            # score only uncertain cells
            if lo_u <= p <= hi_u:
                ig_total += info_gain_cell(p, p_hit=p_hit, p_false=p_false)

    return ig_total


def demo() -> None:
    g = OccupancyGrid(width=120, height=120, resolution=0.1, origin_x=-6.0, origin_y=-6.0)

    random.seed(0)
    for _ in range(4000):
        ix = random.randint(0, g.width - 1)
        iy = random.randint(0, g.height - 1)
        g.p[iy][ix] = clip(random.gauss(0.15, 0.10), 0.01, 0.99)  # mostly free-ish
    for _ in range(1200):
        ix = random.randint(0, g.width - 1)
        iy = random.randint(0, g.height - 1)
        g.p[iy][ix] = clip(random.gauss(0.85, 0.08), 0.01, 0.99)  # mostly occupied-ish

    print("Initial map entropy (nats):", round(g.map_entropy(), 3))
    pose_candidates = [
        (0.0, 0.0, 0.0),
        (-2.0, 1.5, 0.7),
        (2.0, -1.0, -1.2),
    ]
    for pose in pose_candidates:
        ig = expected_info_gain_view(g, pose, fov_deg=180, n_rays=181, max_range=6.0)
        print(f"Pose={pose}  Expected IG≈ {ig:.3f} nats")


if __name__ == "__main__":
    demo()

"""
ROS/ROS2 NOTE (optional):
- nav_msgs/OccupancyGrid stores data as int8 in [-1,100]:
    -1 unknown, 0 free, 100 occupied.
  Typical conversion:
    p = 0.5 if v==-1 else v/100.0
- Use LaserScan angles and max range to define rays.
- The same IG scoring can be used to rank frontier candidates or next-best-view poses.
"""
      

Chapter17_Lesson2.cpp

/*
Chapter 17 - Lesson 2: Information Gain and Entropy Reduction
Autonomous Mobile Robots (Control Engineering)

Minimal C++17 implementation for:
- Bernoulli entropy
- Per-cell information gain for a binary sensor model
- A simple expected IG estimator for a candidate viewpoint (ray stepping)

Build:
  g++ -std=c++17 -O2 Chapter17_Lesson2.cpp -o ig_demo
Run:
  ./ig_demo
*/

#include <cmath>
#include <iostream>
#include <random>
#include <set>
#include <tuple>
#include <utility>
#include <vector>

static double clip(double x, double lo, double hi) {
  return (x < lo) ? lo : (x > hi) ? hi : x;
}

static double bernoulli_entropy(double p, double eps = 1e-12) {
  p = clip(p, eps, 1.0 - eps);
  return -p * std::log(p) - (1.0 - p) * std::log(1.0 - p);
}

static double info_gain_cell(double p, double p_hit = 0.85, double p_false = 0.15) {
  const double H_prior = bernoulli_entropy(p);

  double P_z_occ = p_hit * p + p_false * (1.0 - p);
  double P_z_free = 1.0 - P_z_occ;

  const double eps = 1e-15;
  P_z_occ = clip(P_z_occ, eps, 1.0 - eps);
  P_z_free = clip(P_z_free, eps, 1.0 - eps);

  const double p_post_z_occ = (p_hit * p) / P_z_occ;
  const double p_post_z_free = ((1.0 - p_hit) * p) / P_z_free;

  const double H_post =
      P_z_occ * bernoulli_entropy(p_post_z_occ) + P_z_free * bernoulli_entropy(p_post_z_free);

  return H_prior - H_post;
}

struct Grid {
  int width;
  int height;
  double resolution;
  double origin_x;
  double origin_y;
  std::vector<double> p; // row-major

  Grid(int w, int h, double res, double ox, double oy)
      : width(w), height(h), resolution(res), origin_x(ox), origin_y(oy), p(w * h, 0.5) {}

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

  std::pair<int, int> world_to_grid(double x, double y) const {
    const int ix = static_cast<int>(std::floor((x - origin_x) / resolution));
    const int iy = static_cast<int>(std::floor((y - origin_y) / resolution));
    return {ix, iy};
  }

  double &at(int ix, int iy) { return p[iy * width + ix]; }
  double at(int ix, int iy) const { return p[iy * width + ix]; }

  double map_entropy() const {
    double s = 0.0;
    for (double prob : p) s += bernoulli_entropy(prob);
    return s;
  }
};

static std::vector<std::pair<int, int>> ray_cast_cells(const Grid &g, double x0, double y0, double theta,
                                                       double max_range, double step = -1.0) {
  if (step <= 0.0) step = 0.5 * g.resolution;

  std::vector<std::pair<int, int>> cells;
  std::set<std::pair<int, int>> seen;

  for (double t = 0.0; t <= max_range; t += step) {
    const double x = x0 + t * std::cos(theta);
    const double y = y0 + t * std::sin(theta);
    auto [ix, iy] = g.world_to_grid(x, y);
    if (!g.in_bounds(ix, iy)) break;
    std::pair<int, int> key = {ix, iy};
    if (!seen.count(key)) {
      seen.insert(key);
      cells.push_back(key);
    }
  }
  return cells;
}

static double expected_info_gain_view(const Grid &g, std::tuple<double, double, double> pose,
                                      double fov_deg = 180.0, int n_rays = 181, double max_range = 8.0,
                                      double p_hit = 0.85, double p_false = 0.15,
                                      std::pair<double, double> unknown_band = {0.4, 0.6},
                                      double occ_stop_threshold = 0.75) {
  const auto [x, y, yaw] = pose;

  const double fov = fov_deg * M_PI / 180.0;
  std::vector<double> angles;
  if (n_rays < 2) {
    angles.push_back(0.0);
  } else {
    angles.reserve(n_rays);
    for (int i = 0; i < n_rays; ++i) {
      angles.push_back((-0.5 * fov) + i * (fov / (n_rays - 1)));
    }
  }

  const double lo_u = unknown_band.first;
  const double hi_u = unknown_band.second;

  std::set<std::pair<int, int>> visited;
  double ig_total = 0.0;

  for (double a : angles) {
    const double theta = yaw + a;
    auto cells = ray_cast_cells(g, x, y, theta, max_range);

    for (auto [ix, iy] : cells) {
      std::pair<int, int> key = {ix, iy};
      if (visited.count(key)) continue;
      visited.insert(key);

      const double p = g.at(ix, iy);

      if (p >= occ_stop_threshold) break; // occlusion

      if (lo_u <= p && p <= hi_u) {
        ig_total += info_gain_cell(p, p_hit, p_false);
      }
    }
  }

  return ig_total;
}

int main() {
  Grid g(120, 120, 0.1, -6.0, -6.0);

  std::mt19937 rng(0);
  std::normal_distribution<double> free_d(0.15, 0.10);
  std::normal_distribution<double> occ_d(0.85, 0.08);
  std::uniform_int_distribution<int> ix_d(0, g.width - 1);
  std::uniform_int_distribution<int> iy_d(0, g.height - 1);

  for (int k = 0; k < 4000; ++k) {
    int ix = ix_d(rng), iy = iy_d(rng);
    g.at(ix, iy) = clip(free_d(rng), 0.01, 0.99);
  }
  for (int k = 0; k < 1200; ++k) {
    int ix = ix_d(rng), iy = iy_d(rng);
    g.at(ix, iy) = clip(occ_d(rng), 0.01, 0.99);
  }

  std::cout << "Initial map entropy (nats): " << g.map_entropy() << "\n";

  std::vector<std::tuple<double, double, double>> poses = {
      {0.0, 0.0, 0.0},
      {-2.0, 1.5, 0.7},
      {2.0, -1.0, -1.2},
  };

  for (auto pose : poses) {
    double ig = expected_info_gain_view(g, pose, 180.0, 181, 6.0);
    std::cout << "Pose=(" << std::get<0>(pose) << "," << std::get<1>(pose) << "," << std::get<2>(pose)
              << ")  Expected IG≈ " << ig << " nats\n";
  }

  return 0;
}
      

Chapter17_Lesson2.java

/*
Chapter 17 - Lesson 2: Information Gain and Entropy Reduction
Autonomous Mobile Robots (Control Engineering)

Minimal Java implementation for:
- Bernoulli entropy
- Per-cell information gain for a binary sensor model
- Simple expected IG scoring for candidate viewpoints via ray stepping

Compile:
  javac Chapter17_Lesson2.java
Run:
  java Chapter17_Lesson2
*/

import java.util.HashSet;
import java.util.Random;
import java.util.Set;

public class Chapter17_Lesson2 {

  static double clip(double x, double lo, double hi) {
    return (x < lo) ? lo : ((x > hi) ? hi : x);
  }

  static double bernoulliEntropy(double p) {
    double eps = 1e-12;
    p = clip(p, eps, 1.0 - eps);
    return -p * Math.log(p) - (1.0 - p) * Math.log(1.0 - p);
  }

  static double infoGainCell(double p, double pHit, double pFalse) {
    double Hprior = bernoulliEntropy(p);

    double PzOcc = pHit * p + pFalse * (1.0 - p);
    double PzFree = 1.0 - PzOcc;

    double eps = 1e-15;
    PzOcc = clip(PzOcc, eps, 1.0 - eps);
    PzFree = clip(PzFree, eps, 1.0 - eps);

    double pPostOcc = (pHit * p) / PzOcc;
    double pPostFree = ((1.0 - pHit) * p) / PzFree;

    double Hpost = PzOcc * bernoulliEntropy(pPostOcc) + PzFree * bernoulliEntropy(pPostFree);
    return Hprior - Hpost;
  }

  static class Grid {
    final int width, height;
    final double resolution, originX, originY;
    final double[] p; // row-major

    Grid(int w, int h, double res, double ox, double oy) {
      width = w;
      height = h;
      resolution = res;
      originX = ox;
      originY = oy;
      p = new double[w * h];
      for (int i = 0; i < p.length; i++) p[i] = 0.5;
    }

    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;
    }

    double get(int ix, int iy) {
      return p[idx(ix, iy)];
    }

    void set(int ix, int iy, double v) {
      p[idx(ix, iy)] = v;
    }

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

    double mapEntropy() {
      double s = 0.0;
      for (double prob : p) s += bernoulliEntropy(prob);
      return s;
    }
  }

  static double expectedInfoGainView(
      Grid g,
      double x,
      double y,
      double yaw,
      double fovDeg,
      int nRays,
      double maxRange,
      double pHit,
      double pFalse,
      double unknownLo,
      double unknownHi,
      double occStopThreshold) {

    double fov = Math.toRadians(fovDeg);
    double[] angles = new double[Math.max(nRays, 1)];
    if (nRays < 2) {
      angles[0] = 0.0;
    } else {
      for (int i = 0; i < nRays; i++) {
        angles[i] = (-0.5 * fov) + i * (fov / (nRays - 1));
      }
    }

    Set<Long> visited = new HashSet<>();
    double igTotal = 0.0;
    double step = 0.5 * g.resolution;

    for (double a : angles) {
      double theta = yaw + a;

      for (double t = 0.0; t <= maxRange; t += step) {
        double xr = x + t * Math.cos(theta);
        double yr = y + t * Math.sin(theta);
        int[] ij = g.worldToGrid(xr, yr);
        int ix = ij[0], iy = ij[1];
        if (!g.inBounds(ix, iy)) break;

        long key = (((long) ix) << 32) ^ (iy & 0xffffffffL);
        if (visited.contains(key)) continue;
        visited.add(key);

        double p = g.get(ix, iy);
        if (p >= occStopThreshold) break; // occlusion
        if (unknownLo <= p && p <= unknownHi) {
          igTotal += infoGainCell(p, pHit, pFalse);
        }
      }
    }

    return igTotal;
  }

  public static void main(String[] args) {
    Grid g = new Grid(120, 120, 0.1, -6.0, -6.0);
    Random rng = new Random(0);

    for (int k = 0; k < 4000; k++) {
      int ix = rng.nextInt(g.width);
      int iy = rng.nextInt(g.height);
      double v = clip(0.15 + 0.10 * rng.nextGaussian(), 0.01, 0.99);
      g.set(ix, iy, v);
    }
    for (int k = 0; k < 1200; k++) {
      int ix = rng.nextInt(g.width);
      int iy = rng.nextInt(g.height);
      double v = clip(0.85 + 0.08 * rng.nextGaussian(), 0.01, 0.99);
      g.set(ix, iy, v);
    }

    System.out.println("Initial map entropy (nats): " + g.mapEntropy());

    double[][] poses = new double[][] {
      {0.0, 0.0, 0.0},
      {-2.0, 1.5, 0.7},
      {2.0, -1.0, -1.2}
    };

    for (double[] pose : poses) {
      double ig = expectedInfoGainView(g, pose[0], pose[1], pose[2],
          180.0, 181, 6.0, 0.85, 0.15, 0.4, 0.6, 0.75);
      System.out.printf("Pose=(%.2f, %.2f, %.2f)  Expected IG≈ %.3f nats%n",
          pose[0], pose[1], pose[2], ig);
    }
  }
}
      

Chapter17_Lesson2.m

% Chapter 17 - Lesson 2: Information Gain and Entropy Reduction
% Autonomous Mobile Robots (Control Engineering)
%
% MATLAB implementation for:
% - Bernoulli (occupancy) entropy
% - Per-cell information gain for a binary sensor model
% - Approx expected info gain for a viewpoint using ray stepping
%
% Simulink note:
%   You can place expected_info_gain_view(...) into a MATLAB Function block
%   to score candidate viewpoints generated elsewhere.
%
% Run:
%   Chapter17_Lesson2

function Chapter17_Lesson2()
  rng(0);

  % Build a grid
  g.width = 120; g.height = 120; g.resolution = 0.1;
  g.origin_x = -6.0; g.origin_y = -6.0;
  g.p = 0.5 * ones(g.height, g.width);

  % Random free-ish and occupied-ish patches
  for k = 1:4000
    ix = randi(g.width); iy = randi(g.height);
    g.p(iy, ix) = clip(0.15 + 0.10 * randn(), 0.01, 0.99);
  end
  for k = 1:1200
    ix = randi(g.width); iy = randi(g.height);
    g.p(iy, ix) = clip(0.85 + 0.08 * randn(), 0.01, 0.99);
  end

  H0 = map_entropy(g.p);
  fprintf('Initial map entropy (nats): %.3f\n', H0);

  poses = [
    0.0, 0.0, 0.0;
    -2.0, 1.5, 0.7;
    2.0, -1.0, -1.2
  ];

  for i = 1:size(poses,1)
    pose = poses(i,:);
    ig = expected_info_gain_view(g, pose, 180.0, 181, 6.0, 0.85, 0.15, [0.4, 0.6], 0.75);
    fprintf('Pose=(%.2f, %.2f, %.2f)  Expected IG≈ %.3f nats\n', pose(1), pose(2), pose(3), ig);
  end
end

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

function H = bernoulli_entropy(p)
  eps = 1e-12;
  p = clip(p, eps, 1-eps);
  H = -p .* log(p) - (1-p) .* log(1-p);
end

function IG = info_gain_cell(p, p_hit, p_false)
  Hprior = bernoulli_entropy(p);

  PzOcc = p_hit * p + p_false * (1 - p);
  PzFree = 1 - PzOcc;

  eps = 1e-15;
  PzOcc = clip(PzOcc, eps, 1-eps);
  PzFree = clip(PzFree, eps, 1-eps);

  pPostOcc  = (p_hit * p) / PzOcc;
  pPostFree = ((1 - p_hit) * p) / PzFree;

  Hpost = PzOcc * bernoulli_entropy(pPostOcc) + PzFree * bernoulli_entropy(pPostFree);
  IG = Hprior - Hpost;
end

function Hm = map_entropy(P)
  Hm = sum(bernoulli_entropy(P), 'all');
end

function [ix, iy] = world_to_grid(g, x, y)
  ix = floor((x - g.origin_x) / g.resolution) + 1; % MATLAB 1-based
  iy = floor((y - g.origin_y) / g.resolution) + 1;
end

function in = in_bounds(g, ix, iy)
  in = (ix >= 1) && (ix <= g.width) && (iy >= 1) && (iy <= g.height);
end

function cells = ray_cast_cells(g, x0, y0, theta, max_range)
  step = 0.5 * g.resolution;
  cells = zeros(0,2);
  seen = false(g.height, g.width);

  t = 0.0;
  while t <= max_range
    x = x0 + t * cos(theta);
    y = y0 + t * sin(theta);
    [ix, iy] = world_to_grid(g, x, y);
    if ~in_bounds(g, ix, iy)
      break;
    end
    if ~seen(iy, ix)
      seen(iy, ix) = true;
      cells(end+1,:) = [ix, iy]; %#ok<AGROW>
    end
    t = t + step;
  end
end

function ig_total = expected_info_gain_view(g, pose, fov_deg, n_rays, max_range, p_hit, p_false, unknown_band, occ_stop_threshold)
  x = pose(1); y = pose(2); yaw = pose(3);
  fov = deg2rad(fov_deg);

  if n_rays < 2
    angles = 0;
  else
    angles = linspace(-0.5*fov, 0.5*fov, n_rays);
  end

  lo_u = unknown_band(1); hi_u = unknown_band(2);
  visited = false(g.height, g.width);
  ig_total = 0.0;

  for a = angles
    theta = yaw + a;
    cells = ray_cast_cells(g, x, y, theta, max_range);

    for k = 1:size(cells,1)
      ix = cells(k,1); iy = cells(k,2);
      if visited(iy, ix)
        continue;
      end
      visited(iy, ix) = true;

      p = g.p(iy, ix);

      if p >= occ_stop_threshold
        break; % occlusion
      end

      if (p >= lo_u) && (p <= hi_u)
        ig_total = ig_total + info_gain_cell(p, p_hit, p_false);
      end
    end
  end
end
      

Chapter17_Lesson2.nb

(* Chapter 17 - Lesson 2: Information Gain and Entropy Reduction
   Autonomous Mobile Robots (Control Engineering)

   Wolfram Language (Mathematica) code for:
   - Bernoulli entropy
   - Per-cell information gain for a binary sensor model
   - Approx expected IG scoring for a viewpoint using ray stepping
*)

ClearAll[clip, Hbern, IGCell, WorldToGrid, InBoundsQ, RayCastCells, ExpectedIGView, MapEntropy];

clip[x_, lo_, hi_] := Min[Max[x, lo], hi];

Hbern[p_] := Module[{pp = clip[p, 10^-12, 1 - 10^-12]},
  -pp Log[pp] - (1 - pp) Log[1 - pp]
];

IGCell[p_, pHit_:0.85, pFalse_:0.15] := Module[
  {Hprior, PzOcc, PzFree, pPostOcc, pPostFree, Hpost},
  Hprior = Hbern[p];
  PzOcc = pHit p + pFalse (1 - p);
  PzFree = 1 - PzOcc;

  PzOcc = clip[PzOcc, 10^-15, 1 - 10^-15];
  PzFree = clip[PzFree, 10^-15, 1 - 10^-15];

  pPostOcc  = (pHit p)/PzOcc;
  pPostFree = ((1 - pHit) p)/PzFree;

  Hpost = PzOcc Hbern[pPostOcc] + PzFree Hbern[pPostFree];
  Hprior - Hpost
];

MapEntropy[P_] := Total[Hbern /@ Flatten[P]];

WorldToGrid[g_, x_, y_] := Module[{ix, iy},
  ix = Floor[(x - g["originX"])/g["resolution"]] + 1;
  iy = Floor[(y - g["originY"])/g["resolution"]] + 1;
  {ix, iy}
];

InBoundsQ[g_, ix_, iy_] := (1 <= ix <= g["width"]) && (1 <= iy <= g["height"]);

RayCastCells[g_, x0_, y0_, theta_, maxRange_] := Module[
  {step = 0.5 g["resolution"], t = 0., cells = {}, seen},
  seen = ConstantArray[False, {g["height"], g["width"]}];
  While[t <= maxRange,
    Module[{x = x0 + t Cos[theta], y = y0 + t Sin[theta], ix, iy},
      {ix, iy} = WorldToGrid[g, x, y];
      If[!InBoundsQ[g, ix, iy], Break[]];
      If[!seen[[iy, ix]],
        seen[[iy, ix]] = True;
        AppendTo[cells, {ix, iy}];
      ];
    ];
    t += step;
  ];
  cells
];

ExpectedIGView[g_, pose_, fovDeg_:180., nRays_:181, maxRange_:8., pHit_:0.85, pFalse_:0.15,
               unknownBand_:{0.4, 0.6}, occStop_:0.75] := Module[
  {x = pose[[1]], y = pose[[2]], yaw = pose[[3]],
   fov = fovDeg Degree, angles, lo = unknownBand[[1]], hi = unknownBand[[2]],
   visited, ig = 0.0},
  angles = If[nRays < 2, {0.0}, Table[-0.5 fov + (i-1) (fov/(nRays-1)), {i, 1, nRays}]];
  visited = ConstantArray[False, {g["height"], g["width"]}];

  Do[
    Module[{cells = RayCastCells[g, x, y, yaw + a, maxRange]},
      Do[
        Module[{ix = c[[1]], iy = c[[2]], p},
          If[visited[[iy, ix]], Continue[]];
          visited[[iy, ix]] = True;
          p = g["P"][[iy, ix]];
          If[p >= occStop, Break[]];
          If[lo <= p <= hi, ig += IGCell[p, pHit, pFalse]];
        ],
      {c, cells}]
    ],
  {a, angles}];

  ig
];

(* Demo *)
SeedRandom[0];
g = <|
  "width" -> 120, "height" -> 120, "resolution" -> 0.1,
  "originX" -> -6.0, "originY" -> -6.0,
  "P" -> ConstantArray[0.5, {120, 120}]
|>;

Do[
  g["P"][[RandomInteger[{1,120}], RandomInteger[{1,120}]]] = clip[RandomVariate[NormalDistribution[0.15, 0.10]], 0.01, 0.99],
{4000}];
Do[
  g["P"][[RandomInteger[{1,120}], RandomInteger[{1,120}]]] = clip[RandomVariate[NormalDistribution[0.85, 0.08]], 0.01, 0.99],
{1200}];

Print["Initial map entropy (nats): ", N[MapEntropy[g["P"]], 6]];

poses = { {0.,0.,0.}, {-2.,1.5,0.7}, {2.,-1.,-1.2} };
Do[
  ig = ExpectedIGView[g, pose, 180., 181, 6.0];
  Print["Pose=", pose, "  Expected IG≈ ", N[ig, 6], " nats"],
{pose, poses}];
      

Note: Replace the placeholders { {CODE_PY} }, { {CODE_CPP} }, { {CODE_JAVA} }, { {CODE_M} }, { {CODE_NB} } with the corresponding code blocks below (already provided in this chat response). If you paste the unified HTML into your site generator, you can also directly paste the code bodies into those placeholder locations.

Problem 1 (Entropy maximizer): For \( H(p)=-p\ln p-(1-p)\ln(1-p) \), prove that the unique maximizer over \( 0<p<1 \) is \( p=0.5 \).

Solution: Differentiate:

\[ H'(p)=\ln\!\left(\frac{1-p}{p}\right). \]

Setting \( H'(p)=0 \) implies \( \frac{1-p}{p}=1 \Rightarrow p=0.5 \). The second derivative is

\[ H''(p)=-\frac{1}{p(1-p)} \,<\, 0, \]

so \( H \) is strictly concave and the stationary point is the unique global maximum.

Problem 2 (Per-cell IG derivation): Under the binary sensor model \( \Pr(z=\text{occ}\mid m=1)=p_{\text{hit} } \), \( \Pr(z=\text{occ}\mid m=0)=p_{\text{false} } \), derive \( \operatorname{IG}_{\text{cell} }(p)=H(p)-\mathbb{E}_z[H(p\mid z)] \) explicitly.

Solution: First compute the measurement marginal:

\[ \Pr(z=\text{occ})=p_{\text{hit} }p+p_{\text{false} }(1-p),\qquad \Pr(z=\text{free})=1-\Pr(z=\text{occ}). \]

Then apply Bayes rule for the two posteriors (Section 4):

\[ p_{\text{post,occ} } = \frac{p_{\text{hit} }p}{p_{\text{hit} }p+p_{\text{false} }(1-p)}, \qquad p_{\text{post,free} } = \frac{(1-p_{\text{hit} })p}{(1-p_{\text{hit} })p+(1-p_{\text{false} })(1-p)}. \]

Finally, \( \operatorname{IG}_{\text{cell} }(p)=H(p)-\Pr(z=\text{occ})H(p_{\text{post,occ} })-\Pr(z=\text{free})H(p_{\text{post,free} }) \).

Problem 3 (IG equals conditional mutual information): Prove that \( H(M)-\mathbb{E}_{Z|a}[H(M\mid Z,a)] = I(M;Z\mid a) \).

Solution: By definition:

\[ I(M;Z\mid a)=H(M\mid a)-H(M\mid Z,a). \]

Since \( a \) is chosen, treat it as fixed so \( H(M\mid a)=H(M) \). Taking expectation over \( Z\mid a \) yields \( H(M)-\mathbb{E}_{Z|a}[H(M\mid Z,a)] \).

Problem 4 (Diminishing returns for multi-view selection): Let \( Z_S \) be measurements collected from a set of viewpoints \( S \). Assume measurements are conditionally independent given the map: \( p(Z_S\mid M)=\prod_{s\in S}p(Z_s\mid M) \). Show that for \( A\subseteq B \) and \( s\notin B \), the incremental information satisfies \( I(M;Z_s\mid Z_A)\ge I(M;Z_s\mid Z_B) \).

Solution (standard information argument):

Using the chain rule, \( I(M;Z_{A\cup\{s\} })=I(M;Z_A)+I(M;Z_s\mid Z_A) \). The marginal gain is the conditional MI term.

Write \( I(M;Z_s\mid Z_A)=H(Z_s\mid Z_A)-H(Z_s\mid M,Z_A) \). Under conditional independence, \( H(Z_s\mid M,Z_A)=H(Z_s\mid M) \) (conditioning on other measurements doesn’t add information once \( M \) is known). So the only term depending on \( A \) is \( H(Z_s\mid Z_A) \).

Since conditioning reduces entropy, \( A\subseteq B \Rightarrow H(Z_s\mid Z_A)\ge H(Z_s\mid Z_B) \), therefore \( I(M;Z_s\mid Z_A)\ge I(M;Z_s\mid Z_B) \). This is the diminishing returns property behind submodularity-based greedy exploration.

Problem 5 (Complexity + speedups): Suppose each IG evaluation casts \( R \) rays with \( L \) steps each. (a) What is the time complexity? (b) Give two principled speedups that preserve ranking quality.

Solution:

(a) The naive traversal is \( \mathcal{O}(RL) \) per viewpoint. If we evaluate \( K \) candidates per cycle, total cost is \( \mathcal{O}(KRL) \).

(b) Two speedups commonly used in AMR stacks:

• Ray lookup tables: precompute discrete ray offsets for each angle and reuse them for all candidates. This makes traversal mostly indexed memory access.
• Coarse-to-fine scoring: evaluate IG on a downsampled grid (or fewer rays) to prune candidates, then refine the top few with full resolution.

Problem 6 (Constrained formulation and Lagrangian): Consider maximizing information under a travel budget: \( \max_a \operatorname{IG}(a) \) subject to \( C(a)\le C_{\max} \). Show how this leads to an unconstrained utility with a multiplier \( \lambda \ge 0 \).

Solution: Form the Lagrangian

\[ \mathcal{L}(a,\lambda) = \operatorname{IG}(a) - \lambda\left(C(a)-C_{\max}\right). \]

For a fixed \( \lambda \), maximizing \( \mathcal{L} \) over \( a \) is equivalent to maximizing \( \operatorname{IG}(a)-\lambda C(a) \). Tuning \( \lambda \) controls how strictly the budget is enforced (KKT conditions).

We modeled exploration as expected entropy reduction of an uncertain map, derived occupancy entropy and its concavity, and connected information gain to conditional mutual information. We then built a practical viewpoint-scoring approximation by summing per-cell expected entropy reduction over ray-visible uncertain cells, and discussed how costs and diminishing returns justify greedy selection in multi-view settings.