In exploration, the robot repeatedly chooses a viewpoint \( \mathbf{x}_t \), acquires measurements \( \mathbf{z}_t \), updates its map belief, and then decides where to go next. The NBV principle chooses the next viewpoint that maximizes a utility function.

Unlike pure frontier-following, NBV explicitly reasons about \( \mathbb{E}[\Delta H] \) (expected entropy reduction), measurement occlusions, and the travel/time budget. This provides a principled way to decide between: (i) close but low-information moves, and (ii) farther but high-information moves.

flowchart TD
  A["Current belief map b_t and pose x_t"] --> B["Generate candidate viewpoints V"]
  B --> C["For each v in V: predict observations (ray casting)"]
  C --> D["Compute expected info gain IG(v)"]
  D --> E["Compute motion cost C(v) and feasibility"]
  E --> F["Utility U(v)=IG(v)-lambda*C(v)"]
  F --> G["Select NBV: v* = argmax U(v)"]
  G --> H["Plan path to v* (global/local layers)"]
  H --> I["Move + sense + update belief map"]
  I --> A
        

In a deployed system, the candidate set \( V \) is usually restricted to reachable poses in free space, often near frontiers, so the computational budget focuses on the most promising regions.

Let the map be a random variable \( \mathbf{M} \) (e.g., an occupancy grid), and let the robot action be a candidate viewpoint \( \mathbf{v} \in \mathcal{V} \) (pose plus sensor heading). With accumulated data \( \mathcal{D}_t \) (all past controls and measurements), the current belief is \( p(\mathbf{M}\mid \mathcal{D}_t) \).

Executing viewpoint \( \mathbf{v} \) produces a random measurement \( \mathbf{Z}_{t+1} \) with likelihood \( p(\mathbf{Z}_{t+1}\mid \mathbf{M}, \mathbf{v}) \). NBV chooses the action maximizing a utility:

\[ \mathbf{v}^\star \;=\; \arg\max_{\mathbf{v}\in\mathcal{V}} \; U(\mathbf{v}) \quad \text{with} \quad U(\mathbf{v}) \;=\; \underbrace{I(\mathbf{M};\mathbf{Z}_{t+1}\mid \mathcal{D}_t,\mathbf{v})}_{\text{expected info gain}} \;-\; \lambda\, \underbrace{J(\mathbf{v}\mid \mathbf{x}_t)}_{\text{travel/time/risk cost}}. \]

The expected information gain is a conditional mutual information:

\[ I(\mathbf{M};\mathbf{Z}_{t+1}\mid \mathcal{D}_t,\mathbf{v}) \;=\; H(\mathbf{M}\mid \mathcal{D}_t) \;-\; \mathbb{E}_{\mathbf{Z}_{t+1}\sim p(\cdot\mid \mathcal{D}_t,\mathbf{v})} \!\left[\,H(\mathbf{M}\mid \mathcal{D}_t,\mathbf{Z}_{t+1},\mathbf{v})\,\right]. \]

Here \( H(\cdot) \) is Shannon entropy (in nats if using natural log). The cost term \( J(\mathbf{v}\mid \mathbf{x}_t) \) can model distance, time, energy, collision risk, or battery impact (handled more explicitly in Lesson 4).

A key identity connects mutual information to an expected KL divergence. This matters because it shows that NBV can be seen as maximizing the expected discrepancy between posterior and prior beliefs over the map.

Theorem (Mutual information as expected KL). For any candidate viewpoint \( \mathbf{v} \): \( I(\mathbf{M};\mathbf{Z}\mid \mathcal{D}_t,\mathbf{v}) = \mathbb{E}_{\mathbf{Z}}\!\left[ D_{\mathrm{KL}}\!\left(p(\mathbf{M}\mid \mathcal{D}_t,\mathbf{Z},\mathbf{v}) \,\|\, p(\mathbf{M}\mid \mathcal{D}_t)\right)\right] \).

Proof. Start from the definition of conditional mutual information:

\[ I(\mathbf{M};\mathbf{Z}\mid \mathcal{D}_t,\mathbf{v}) \;=\; \mathbb{E}\!\left[\log\frac{p(\mathbf{M},\mathbf{Z}\mid \mathcal{D}_t,\mathbf{v})}{p(\mathbf{M}\mid \mathcal{D}_t)\,p(\mathbf{Z}\mid \mathcal{D}_t,\mathbf{v})}\right]. \]

Using Bayes rule, \( p(\mathbf{M},\mathbf{Z}\mid \mathcal{D}_t,\mathbf{v}) = p(\mathbf{M}\mid \mathcal{D}_t)\,p(\mathbf{Z}\mid \mathbf{M},\mathbf{v}) \) and \( p(\mathbf{M}\mid \mathcal{D}_t,\mathbf{Z},\mathbf{v}) = \frac{p(\mathbf{Z}\mid \mathbf{M},\mathbf{v})\,p(\mathbf{M}\mid \mathcal{D}_t)}{p(\mathbf{Z}\mid \mathcal{D}_t,\mathbf{v})} \). Substitute these into the log ratio to obtain:

\[ I(\mathbf{M};\mathbf{Z}\mid \mathcal{D}_t,\mathbf{v}) \;=\; \mathbb{E}_{\mathbf{Z}}\!\left[ \int p(\mathbf{M}\mid \mathcal{D}_t,\mathbf{Z},\mathbf{v}) \log\frac{p(\mathbf{M}\mid \mathcal{D}_t,\mathbf{Z},\mathbf{v})}{p(\mathbf{M}\mid \mathcal{D}_t)}\,d\mathbf{M} \right] \;=\\ \mathbb{E}_{\mathbf{Z}}\!\left[D_{\mathrm{KL}}\!\left(p(\mathbf{M}\mid \mathcal{D}_t,\mathbf{Z},\mathbf{v}) \,\|\, p(\mathbf{M}\mid \mathcal{D}_t)\right)\right]. \]

Since \( D_{\mathrm{KL}}(\cdot\|\cdot)\ge 0 \), the theorem implies \( I(\mathbf{M};\mathbf{Z}\mid \mathcal{D}_t,\mathbf{v}) \ge 0 \). Therefore NBV never expects to increase map uncertainty; it only differs in how efficiently it reduces it per unit cost.

In AMR, a common map is an occupancy grid with binary variables \( m_i \in \{0,1\} \) for each cell \( i \). Under the (approximate) conditional independence assumption, \( p(\mathbf{M}\mid \mathcal{D}_t) \approx \prod_i p(m_i\mid \mathcal{D}_t) \), the entropy decomposes:

\[ H(\mathbf{M}\mid \mathcal{D}_t) \;\approx\; \sum_{i=1}^{N} H\!\left(p_i\right), \\ \text{where } p_i \;=\; \Pr(m_i=1\mid \mathcal{D}_t) \text{ and } H(p_i) \;=\; -p_i\log p_i - (1-p_i)\log(1-p_i). \]

Suppose a viewpoint \( \mathbf{v} \) induces a set of observable cells \( \mathcal{S}(\mathbf{v}) \) (via ray casting within sensor range and field-of-view). Then a standard NBV approximation is:

\[ I(\mathbf{M};\mathbf{Z}\mid \mathcal{D}_t,\mathbf{v}) \;\approx\; \sum_{i \in \mathcal{S}(\mathbf{v})} \mathrm{IG}_i, \quad \mathrm{IG}_i \;=\; H(p_i) \;-\; \mathbb{E}_{z_i}\!\left[H\!\left(p_i \mid z_i\right)\right]. \]

For a binary per-cell measurement \( z_i \in \{\text{occ},\text{free}\} \) with forward model \( \Pr(z_i=\text{occ}\mid m_i=1)=p_{\text{hit}} \) and \( \Pr(z_i=\text{occ}\mid m_i=0)=p_{\text{false}} \), Bayes updates are:

\[ \Pr(m_i=1 \mid z_i=\text{occ}) \;=\; \frac{p_{\text{hit}}\,p_i}{p_{\text{hit}}\,p_i + p_{\text{false}}\,(1-p_i)}, \\ \Pr(m_i=1 \mid z_i=\text{free}) \;=\; \frac{(1-p_{\text{hit}})\,p_i}{(1-p_{\text{hit}})\,p_i + (1-p_{\text{false}})\,(1-p_i)}. \]

The expectation uses \( \Pr(z_i=\text{occ}) = p_{\text{hit}}\,p_i + p_{\text{false}}(1-p_i) \) and \( \Pr(z_i=\text{free}) = 1 - \Pr(z_i=\text{occ}) \). Cells with \( p_i \approx 0.5 \) yield the largest entropy and typically the largest potential gain.

flowchart TD
  V["Candidate view v"] --> R["Cast rays in FOV and range"]
  R --> S["Visible cell set S(v) with occlusion stop"]
  S --> G["Per-cell IG: H(p)-E[H(p|z)]"]
  G --> SUM["Approx IG(v)=sum_{i in S(v)} IG_i"]
  SUM --> U["Utility U(v)=IG(v)-lambda*cost(v)"]
        

NBV is rarely “maximize information at any cost.” You must also account for feasibility and operational constraints:

• \( J_{\text{time}}(\mathbf{v}) \): travel time using the navigation stack (global path length / speed),
• \( J_{\text{risk}}(\mathbf{v}) \): proximity to obstacles (costmap inflation), narrow passages,
• \( J_{\text{local}}(\mathbf{v}) \): local planner difficulty (e.g., expected oscillations),
• \( J_{\text{energy}}(\mathbf{v}) \): energy or battery drain (Lesson 4 expands this).

A common composite cost is:

\[ J(\mathbf{v}\mid \mathbf{x}_t) \;=\; w_L \, L(\mathbf{x}_t,\mathbf{v}) \;+\; w_R \, \mathbb{E}\!\left[\text{risk along path}\right] \;+\; w_T \, \Delta t(\mathbf{x}_t,\mathbf{v}), \quad w_L,w_R,w_T \;\ge\; 0. \]

In implementations, \( L(\mathbf{x}_t,\mathbf{v}) \) is usually the global-planner path length, not Euclidean distance, because nonholonomic constraints, obstacles, and local planner limits matter. Candidate filtering often enforces a hard constraint such as \( J(\mathbf{v}\mid \mathbf{x}_t) \le J_{\max} \) before utility ranking.

The candidate set \( \mathcal{V} \) determines both performance and runtime. Typical strategies:

• Frontier-constrained sampling: pick viewpoints that “see” frontier segments (unknown cells adjacent to known free cells).
• Free-space sampling: sample reachable free cells (biased toward unexplored regions).
• Viewpoint shells: around each frontier cluster, sample poses on a ring of radius \( r_s \) so that rays intersect the unknown boundary with high probability.
• Heading discretization: for each position, evaluate a small set of headings \( \theta \in \{\theta_1,\dots,\theta_K\} \) instead of continuous optimization.

A practical selection pipeline is:

\[ \mathcal{V} \;=\; \Big\{\; (x,y,\theta) \;\Big|\; (x,y)\in \text{FreeSpace},\;\text{dist-to-obstacle}(x,y) > r_{\min},\; \theta\in\Theta_K \;\Big\} \;\cap\; \\ \text{Reachable}(\mathbf{x}_t). \]

Note that the reachability test is delegated to the navigation stack (Chapter 14–15): if the global planner fails or the local planner predicts infeasible motion, the candidate is discarded.

A single-step NBV can be myopic (greedy): it may choose a view with high immediate gain but that positions the robot poorly. A standard remedy is receding-horizon NBV (lookahead planning) over a short horizon \( h \).

Let \( \mathbf{v}_{t:t+h-1} \) be a sequence of viewpoints. A rollout objective is:

\[ \max_{\mathbf{v}_{t:t+h-1}} \;\; \mathbb{E}\!\left[ \sum_{\tau=t}^{t+h-1} \gamma^{\tau-t}\, I(\mathbf{M};\mathbf{Z}_{\tau+1}\mid \mathcal{D}_\tau,\mathbf{v}_\tau) \right] \;-\; \lambda \sum_{\tau=t}^{t+h-1}\gamma^{\tau-t} J(\mathbf{v}_\tau\mid \mathbf{x}_\tau), \quad 0 < \gamma \le 1. \]

Exact optimization is expensive because the belief evolves stochastically. Common approximations:

• Determinized rollout: approximate observations by their expected effect (e.g., using current beliefs),
• Greedy multi-view selection: pick a small set of views sequentially, re-evaluating marginal gains,
• Sampling-based planning: evaluate a small number of random sequences (Monte Carlo),
• Submodular view selection: under independence/coverage assumptions, total information is often approximately submodular, giving a near-optimality guarantee for greedy view sets.

Submodularity insight (sketch). Let \( F(S) \) be the information captured by a set of viewpoints \( S \). If \( F \) is monotone submodular and costs are uniform, greedy selection achieves at least \( (1-1/e) \) of the optimal set value. This motivates picking several good views ahead, even if execution is receding-horizon (execute only the first view and replan).

Practical NBV systems integrate with the mapping and navigation modules already built in earlier chapters:

• Mapping interface: occupancy grid probabilities \( p_i \) (or log-odds) plus a ray-casting function.
• Navigation interface: query global path length/time to candidates; filter unreachable candidates.
• Obstacle inflation: reject candidates with insufficient clearance; compute risk cost.

Suggested language-specific tooling:

• Python: numpy for grids; ROS 2 integration via rclpy, nav_msgs, nav2_msgs, and occupancy grid topics; ray casting via custom Bresenham or fast C++ extensions.
• C++: ROS 2 rclcpp; Nav2 costmaps and planners; fast grid operations; optional Eigen for math.
• Java: algorithm prototyping with standard collections + math; optimization/graphs via libraries like EJML or JGraphT; robotics middleware via ROSJava or custom IPC (depending on your stack).
• MATLAB/Simulink: Robotics System Toolbox provides occupancy grids and ray intersection utilities; Simulink can host the NBV selector as a MATLAB Function block to test discrete-time exploration loops.
• Wolfram Mathematica: symbolic derivations (entropy/IG) and rapid prototyping with list-based grid computation.

The Python reference implementation below demonstrates: (i) expected per-cell information gain under a binary sensor model, (ii) ray-cast accumulation for each candidate view, and (iii) utility-based selection. It also includes a small simulation loop (with a hidden ground truth) to show entropy decreasing over steps.

File: Chapter17_Lesson3.py

# Chapter17_Lesson3.py
# Autonomous Mobile Robots (Control Engineering) — Chapter 17, Lesson 3
# Next-Best-View (NBV) Strategies for Exploration and Active Mapping
#
# This script provides:
#   1) A minimal occupancy-grid belief representation (probabilities per cell)
#   2) Expected Information Gain (EIG) per candidate viewpoint via a simple sensor model
#   3) A utility function U(v) = EIG(v) - lambda * travel_cost(v)
#   4) A small simulation loop using a hidden "ground-truth" grid world
#
# Dependencies: numpy only (optional matplotlib is NOT used to keep the script minimal)

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

import numpy as np


def clamp(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:
    """
    Entropy of Bernoulli(p) in nats: H(p) = -p ln p - (1-p) ln (1-p).
    """
    p = clamp(p, eps, 1.0 - eps)
    return -p * math.log(p) - (1.0 - p) * math.log(1.0 - p)


@dataclass
class SensorBinaryModel:
    """
    Binary measurement model z in {occ, free} for each cell (independent approximation).

    Parameters:
      p_hit   = P(z=occ | m=occ)
      p_false = P(z=occ | m=free)   (false positive)
    """
    p_hit: float = 0.85
    p_false: float = 0.15

    def posterior(self, p_occ: float, z_occ: bool) -> float:
        """
        Bayes update for P(m=occ | z).
        """
        p = clamp(p_occ, 1e-12, 1.0 - 1e-12)
        if z_occ:
            num = self.p_hit * p
            den = self.p_hit * p + self.p_false * (1.0 - p)
        else:
            num = (1.0 - self.p_hit) * p
            den = (1.0 - self.p_hit) * p + (1.0 - self.p_false) * (1.0 - p)
        return clamp(num / den, 1e-12, 1.0 - 1e-12)

    def expected_posterior_entropy(self, p_occ: float) -> float:
        """
        E_z[ H(P(m=occ | z)) ] where z ∈ {occ, free}.
        """
        p = clamp(p_occ, 1e-12, 1.0 - 1e-12)
        # P(z=occ) = P(z=occ|m=occ)P(m=occ) + P(z=occ|m=free)P(m=free)
        p_z_occ = self.p_hit * p + self.p_false * (1.0 - p)
        p_z_free = 1.0 - p_z_occ

        p_post_occ = self.posterior(p, True)
        p_post_free = self.posterior(p, False)

        return p_z_occ * bernoulli_entropy(p_post_occ) + p_z_free * bernoulli_entropy(p_post_free)

    def expected_information_gain(self, p_occ: float) -> float:
        """
        IG(p) = H(prior) - E_z[H(posterior)].
        """
        return bernoulli_entropy(p_occ) - self.expected_posterior_entropy(p_occ)


@dataclass
class GridWorld:
    """
    Occupancy grid belief:
      belief[y, x] = P(cell is occupied)
    Ground-truth:
      truth[y, x] ∈ {0,1} (1=occupied)
    """
    width: int
    height: int
    belief: np.ndarray
    truth: np.ndarray

    @staticmethod
    def random_world(width: int, height: int, obstacle_prob: float = 0.18, seed: int = 0) -> "GridWorld":
        rng = np.random.default_rng(seed)
        truth = (rng.random((height, width)) < obstacle_prob).astype(np.int8)
        belief = np.full((height, width), 0.5, dtype=float)  # unknown prior
        return GridWorld(width, height, belief, truth)

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

    def is_free_truth(self, x: int, y: int) -> bool:
        return self.truth[y, x] == 0

    def entropy_total(self) -> float:
        H = 0.0
        for y in range(self.height):
            for x in range(self.width):
                H += bernoulli_entropy(float(self.belief[y, x]))
        return H


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


def ray_endpoints(cx: int, cy: int, theta: float, r: float) -> Tuple[int, int]:
    ex = int(round(cx + r * math.cos(theta)))
    ey = int(round(cy + r * math.sin(theta)))
    return ex, ey


def expected_ig_view(
    world: GridWorld,
    cx: int,
    cy: int,
    heading: float,
    sensor: SensorBinaryModel,
    fov: float = math.radians(180.0),
    n_rays: int = 45,
    max_range: float = 10.0,
    occ_stop: float = 0.70,
) -> float:
    """
    Expected information gain of a viewpoint, approximated as sum of per-cell IG along rays.
    Occlusion is approximated by stopping a ray once belief occupancy exceeds occ_stop.
    """
    if not world.in_bounds(cx, cy):
        return -1e9

    total = 0.0
    angles = np.linspace(heading - 0.5 * fov, heading + 0.5 * fov, n_rays)
    for ang in angles:
        ex, ey = ray_endpoints(cx, cy, float(ang), max_range)
        line = bresenham(cx, cy, ex, ey)
        # skip first cell (robot position)
        for (x, y) in line[1:]:
            if not world.in_bounds(x, y):
                break
            p_occ = float(world.belief[y, x])
            total += sensor.expected_information_gain(p_occ)

            # occlusion approximation: stop ray if likely-occupied
            if p_occ > occ_stop:
                break
    return total


def travel_cost_grid(cx: int, cy: int, vx: int, vy: int) -> float:
    """
    Simple proxy: Euclidean distance on grid.
    """
    return math.hypot(vx - cx, vy - cy)


def sample_candidate_views(
    world: GridWorld,
    n_candidates: int,
    rng: random.Random,
) -> List[Tuple[int, int, float]]:
    """
    Candidate generation (simplified):
      sample random grid cells that are free in ground truth (sim-only),
      and assign random headings.

    In a real robot, candidates come from:
      - frontier cells (Lesson 1),
      - reachable free-space samples,
      - visibility constraints + collision checking.
    """
    candidates: List[Tuple[int, int, float]] = []
    tries = 0
    while len(candidates) < n_candidates and tries < 50 * n_candidates:
        tries += 1
        x = rng.randrange(world.width)
        y = rng.randrange(world.height)
        if world.is_free_truth(x, y):
            th = rng.random() * 2.0 * math.pi
            candidates.append((x, y, th))
    return candidates


def choose_next_best_view(
    world: GridWorld,
    current: Tuple[int, int, float],
    candidates: List[Tuple[int, int, float]],
    sensor: SensorBinaryModel,
    lam: float = 0.25,
) -> Tuple[int, int, float, float, float]:
    """
    Returns (best_x, best_y, best_theta, best_ig, best_u).
    """
    cx, cy, _ = current
    best = (cx, cy, 0.0, -1e9, -1e9)
    for (vx, vy, th) in candidates:
        ig = expected_ig_view(world, vx, vy, th, sensor)
        cost = travel_cost_grid(cx, cy, vx, vy)
        u = ig - lam * cost
        if u > best[4]:
            best = (vx, vy, th, ig, u)
    return best


def simulate_measurement_and_update(
    world: GridWorld,
    pose: Tuple[int, int, float],
    sensor: SensorBinaryModel,
    fov: float = math.radians(180.0),
    n_rays: int = 60,
    max_range: float = 10.0,
    p_hit_inv: float = 0.70,
    p_false_inv: float = 0.30,
) -> None:
    """
    Simulated scan on ground truth, then Bayesian update of each visited cell with an inverse model.

    Inverse model parameters are distinct from forward model (common practice):
      - If the ray passes through a cell before a hit: treat as 'free' evidence.
      - The first occupied cell on the ray: treat as 'occupied' evidence.
    """
    cx, cy, heading = pose
    angles = np.linspace(heading - 0.5 * fov, heading + 0.5 * fov, n_rays)

    def update_cell(x: int, y: int, z_occ: bool) -> None:
        p = float(world.belief[y, x])
        # Use a simple forward model for the cell update (like SensorBinaryModel but with inverse-tuned params)
        if z_occ:
            p_hit, p_false = p_hit_inv, 1.0 - p_false_inv
        else:
            p_hit, p_false = 1.0 - p_hit_inv, p_false_inv

        # Bayes update:
        num = p_hit * p
        den = p_hit * p + p_false * (1.0 - p)
        world.belief[y, x] = clamp(num / den, 1e-6, 1.0 - 1e-6)

    for ang in angles:
        ex, ey = ray_endpoints(cx, cy, float(ang), max_range)
        line = bresenham(cx, cy, ex, ey)
        for (x, y) in line[1:]:
            if not world.in_bounds(x, y):
                break
            if world.truth[y, x] == 1:
                update_cell(x, y, True)
                break
            else:
                update_cell(x, y, False)


def ascii_map(world: GridWorld, robot_xy: Tuple[int, int]) -> str:
    """
    Simple text view:
      '?' = unknown-ish belief (p around 0.5)
      'o' = believed free (p < 0.35)
      'X' = believed occupied (p > 0.65)
      'R' = robot
    """
    rx, ry = robot_xy
    rows = []
    for y in range(world.height):
        row = []
        for x in range(world.width):
            if (x, y) == (rx, ry):
                row.append("R")
                continue
            p = float(world.belief[y, x])
            if p > 0.65:
                row.append("X")
            elif p < 0.35:
                row.append("o")
            else:
                row.append("?")
        rows.append("".join(row))
    return "\n".join(rows)


def main() -> None:
    rng = random.Random(7)
    world = GridWorld.random_world(width=30, height=18, obstacle_prob=0.16, seed=3)
    sensor = SensorBinaryModel(p_hit=0.85, p_false=0.15)

    # Start pose
    pose = (2, 2, 0.0)
    world.truth[pose[1], pose[0]] = 0  # ensure free in the sim

    print("Initial total entropy (nats):", world.entropy_total())
    for t in range(10):
        candidates = sample_candidate_views(world, n_candidates=80, rng=rng)
        vx, vy, vth, ig, u = choose_next_best_view(world, pose, candidates, sensor, lam=0.22)

        print(f"\nStep {t+1}: NBV = ({vx},{vy}), IG={ig:.2f}, U={u:.2f}")
        pose = (vx, vy, vth)
        simulate_measurement_and_update(world, pose, sensor)

        print("Total entropy (nats):", f"{world.entropy_total():.2f}")
        print(ascii_map(world, (pose[0], pose[1])))

    print("\nDone.")


if __name__ == "__main__":
    main()
      

The C++ implementation focuses on the NBV scoring core (expected IG + utility), suitable for embedding into a higher-performance stack. In a full AMR, you would replace the Euclidean travel proxy with the navigation stack's path-length/time query and add collision checks.

File: Chapter17_Lesson3.cpp

// Chapter17_Lesson3.cpp
// Autonomous Mobile Robots (Control Engineering) — Chapter 17, Lesson 3
// Next-Best-View (NBV) Strategies — minimal occupancy-grid expected-IG utility
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter17_Lesson3.cpp -o nbv
//
// This is a compact, dependency-free reference implementation (no ROS).
// It computes the best candidate viewpoint among random samples using:
//   U(v) = EIG(v) - lambda * dist(v, current)
//
// Notes:
// - Expected information gain is computed per cell using a binary sensor model
// - View EIG is approximated as a sum along ray casts with simple occlusion stopping

#include <cmath>
#include <cstdint>
#include <iostream>
#include <random>
#include <tuple>
#include <vector>
#include <algorithm>

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

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

struct SensorBinaryModel {
  double p_hit{0.85};   // P(z=occ | m=occ)
  double p_false{0.15}; // P(z=occ | m=free)

  double posterior(double p_occ, bool z_occ) const {
    double p = clamp(p_occ, 1e-12, 1.0 - 1e-12);
    double num = 0.0, den = 1.0;
    if (z_occ) {
      num = p_hit * p;
      den = p_hit * p + p_false * (1.0 - p);
    } else {
      num = (1.0 - p_hit) * p;
      den = (1.0 - p_hit) * p + (1.0 - p_false) * (1.0 - p);
    }
    return clamp(num / den, 1e-12, 1.0 - 1e-12);
  }

  double expected_posterior_entropy(double p_occ) const {
    double p = clamp(p_occ, 1e-12, 1.0 - 1e-12);
    double p_z_occ = p_hit * p + p_false * (1.0 - p);
    double p_z_free = 1.0 - p_z_occ;
    double p_post_occ = posterior(p, true);
    double p_post_free = posterior(p, false);
    return p_z_occ * bernoulli_entropy(p_post_occ) + p_z_free * bernoulli_entropy(p_post_free);
  }

  double expected_information_gain(double p_occ) const {
    return bernoulli_entropy(p_occ) - expected_posterior_entropy(p_occ);
  }
};

struct GridBelief {
  int W{0}, H{0};
  std::vector<double> p; // size H*W, p[y*W + x]

  GridBelief(int w, int h) : W(w), H(h), p(static_cast<size_t>(w*h), 0.5) {}

  inline bool in_bounds(int x, int y) const { return (0 <= x && x < W && 0 <= y && y < H); }
  inline double at(int x, int y) const { return p[static_cast<size_t>(y)*W + static_cast<size_t>(x)]; }
};

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

static std::pair<int,int> ray_endpoint(int cx, int cy, double theta, double r) {
  int ex = static_cast<int>(std::llround(cx + r * std::cos(theta)));
  int ey = static_cast<int>(std::llround(cy + r * std::sin(theta)));
  return {ex, ey};
}

static double expected_ig_view(
  const GridBelief& grid,
  int cx, int cy, double heading,
  const SensorBinaryModel& sensor,
  double fov_rad = M_PI, int n_rays = 45, double max_range = 10.0,
  double occ_stop = 0.70
) {
  if (!grid.in_bounds(cx, cy)) return -1e9;

  double total = 0.0;
  for (int i = 0; i < n_rays; ++i) {
    double a = heading - 0.5*fov_rad + (static_cast<double>(i) / std::max(1, n_rays-1)) * fov_rad;
    auto [ex, ey] = ray_endpoint(cx, cy, a, max_range);
    auto line = bresenham(cx, cy, ex, ey);
    for (size_t k = 1; k < line.size(); ++k) {
      int x = line[k].first, y = line[k].second;
      if (!grid.in_bounds(x, y)) break;
      double p_occ = grid.at(x, y);
      total += sensor.expected_information_gain(p_occ);
      if (p_occ > occ_stop) break;
    }
  }
  return total;
}

static double dist_cost(int x0, int y0, int x1, int y1) {
  return std::hypot(static_cast<double>(x1-x0), static_cast<double>(y1-y0));
}

int main() {
  const int W = 30, H = 18;
  GridBelief grid(W, H);
  SensorBinaryModel sensor;

  std::mt19937 rng(7);
  std::uniform_int_distribution<int> ux(0, W-1), uy(0, H-1);
  std::uniform_real_distribution<double> uth(0.0, 2.0*M_PI);

  // Current pose (grid coordinates)
  int cx = 2, cy = 2;
  double heading = 0.0;

  const int N = 120;     // candidates
  const double lambda = 0.22;

  double bestU = -1e18;
  std::tuple<int,int,double,double,double> best = {cx,cy,heading,0.0,bestU};

  for (int i = 0; i < N; ++i) {
    int vx = ux(rng);
    int vy = uy(rng);
    double th = uth(rng);

    double ig = expected_ig_view(grid, vx, vy, th, sensor);
    double c = dist_cost(cx, cy, vx, vy);
    double u = ig - lambda * c;

    if (u > bestU) {
      bestU = u;
      best = {vx, vy, th, ig, u};
    }
  }

  auto [vx, vy, vth, ig, u] = best;
  std::cout << "NBV = (" << vx << "," << vy << "), heading=" << vth
            << ", IG=" << ig << ", U=" << u << "\n";
  std::cout << "(This demo only selects a view; a real system would plan & move, then update belief.)\n";
  return 0;
}
      

The Java implementation mirrors the C++ demo. It is useful for teaching and for integrating NBV ideas into Java-based research toolchains.

File: Chapter17_Lesson3.java

// Chapter17_Lesson3.java
// Autonomous Mobile Robots (Control Engineering) — Chapter 17, Lesson 3
// Next-Best-View (NBV) Strategies — compact Java reference implementation
//
// Compile:
//   javac Chapter17_Lesson3.java
// Run:
//   java Chapter17_Lesson3
//
// This code mirrors the logic of the C++ demo: choose an NBV among random candidates using
// U(v) = EIG(v) - lambda * distance(v,current).

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

public class Chapter17_Lesson3 {

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

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

  static class SensorBinaryModel {
    double pHit = 0.85;   // P(z=occ | m=occ)
    double pFalse = 0.15; // P(z=occ | m=free)

    double posterior(double pOcc, boolean zOcc) {
      double p = clamp(pOcc, 1e-12, 1.0 - 1e-12);
      double num, den;
      if (zOcc) {
        num = pHit * p;
        den = pHit * p + pFalse * (1.0 - p);
      } else {
        num = (1.0 - pHit) * p;
        den = (1.0 - pHit) * p + (1.0 - pFalse) * (1.0 - p);
      }
      return clamp(num / den, 1e-12, 1.0 - 1e-12);
    }

    double expectedPosteriorEntropy(double pOcc) {
      double p = clamp(pOcc, 1e-12, 1.0 - 1e-12);
      double pZOcc = pHit * p + pFalse * (1.0 - p);
      double pZFree = 1.0 - pZOcc;
      double pPostOcc = posterior(p, true);
      double pPostFree = posterior(p, false);
      return pZOcc * bernoulliEntropy(pPostOcc) + pZFree * bernoulliEntropy(pPostFree);
    }

    double expectedInformationGain(double pOcc) {
      return bernoulliEntropy(pOcc) - expectedPosteriorEntropy(pOcc);
    }
  }

  static class GridBelief {
    int W, H;
    double[] p; // p[y*W + x]

    GridBelief(int w, int h) {
      W = w; H = h;
      p = new double[w*h];
      for (int i = 0; i < p.length; i++) p[i] = 0.5;
    }

    boolean inBounds(int x, int y) {
      return (0 <= x && x < W && 0 <= y && y < H);
    }

    double at(int x, int y) {
      return p[y*W + x];
    }
  }

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

  static int[] rayEndpoint(int cx, int cy, double theta, double r) {
    int ex = (int)Math.round(cx + r * Math.cos(theta));
    int ey = (int)Math.round(cy + r * Math.sin(theta));
    return new int[]{ex, ey};
  }

  static double expectedIgView(
      GridBelief grid,
      int cx, int cy, double heading,
      SensorBinaryModel sensor,
      double fovRad, int nRays, double maxRange,
      double occStop
  ) {
    if (!grid.inBounds(cx, cy)) return -1e9;
    double total = 0.0;
    for (int i = 0; i < nRays; i++) {
      double a = heading - 0.5*fovRad + ((double)i / Math.max(1, nRays-1)) * fovRad;
      int[] end = rayEndpoint(cx, cy, a, maxRange);
      List<int[]> line = bresenham(cx, cy, end[0], end[1]);
      for (int k = 1; k < line.size(); k++) {
        int x = line.get(k)[0], y = line.get(k)[1];
        if (!grid.inBounds(x, y)) break;
        double pOcc = grid.at(x, y);
        total += sensor.expectedInformationGain(pOcc);
        if (pOcc > occStop) break;
      }
    }
    return total;
  }

  static double distCost(int x0, int y0, int x1, int y1) {
    return Math.hypot(x1 - x0, y1 - y0);
  }

  public static void main(String[] args) {
    int W = 30, H = 18;
    GridBelief grid = new GridBelief(W, H);
    SensorBinaryModel sensor = new SensorBinaryModel();
    Random rng = new Random(7);

    int cx = 2, cy = 2;
    double heading = 0.0;

    int N = 120;
    double lambda = 0.22;

    double bestU = -1e18;
    int bestX = cx, bestY = cy;
    double bestTh = heading, bestIg = 0.0;

    for (int i = 0; i < N; i++) {
      int vx = rng.nextInt(W);
      int vy = rng.nextInt(H);
      double th = rng.nextDouble() * 2.0 * Math.PI;

      double ig = expectedIgView(grid, vx, vy, th, sensor, Math.PI, 45, 10.0, 0.70);
      double c = distCost(cx, cy, vx, vy);
      double u = ig - lambda * c;

      if (u > bestU) {
        bestU = u;
        bestX = vx; bestY = vy; bestTh = th; bestIg = ig;
      }
    }

    System.out.println("NBV = (" + bestX + "," + bestY + "), heading=" + bestTh
        + ", IG=" + bestIg + ", U=" + bestU);
    System.out.println("(This demo only selects a view; a real system would plan & move, then update belief.)");
  }
}
      

The MATLAB script computes an NBV among random candidates and (optionally) creates a Simulink skeleton model that shows how the NBV selector can be embedded as a MATLAB Function block in a discrete-time exploration loop.

File: Chapter17_Lesson3.m

% Chapter17_Lesson3.m
% Autonomous Mobile Robots (Control Engineering) — Chapter 17, Lesson 3
% Next-Best-View (NBV) Strategies — MATLAB/Simulink-oriented reference
%
% This script:
%   1) Builds a belief occupancy grid (probabilities per cell)
%   2) Computes expected information gain (EIG) for candidate viewpoints
%   3) Chooses the NBV via U(v) = EIG(v) - lambda * travel_cost
%   4) (Optional) Creates a Simulink skeleton model programmatically
%
% Requirements: base MATLAB. (Robotics System Toolbox can be used for real robots.)

function Chapter17_Lesson3()
  rng(7);

  W = 30; H = 18;
  belief = 0.5 * ones(H, W); % P(occupied), unknown prior

  % Sensor model parameters
  p_hit = 0.85;   % P(z=occ | m=occ)
  p_false = 0.15; % P(z=occ | m=free)

  % Current pose (grid coords)
  cx = 2; cy = 2; heading = 0.0;

  % Candidates
  N = 120;
  cand = [randi([1 W], N, 1), randi([1 H], N, 1), 2*pi*rand(N,1)]; % [x y theta]

  lambda = 0.22;

  bestU = -1e18;
  best = [cx cy heading 0 bestU]; % [x y theta IG U]

  for i = 1:N
    vx = cand(i,1); vy = cand(i,2); th = cand(i,3);
    ig = expectedIgView(belief, vx, vy, th, p_hit, p_false, pi, 45, 10, 0.70);
    cost = hypot(vx - cx, vy - cy);
    u = ig - lambda * cost;
    if u > bestU
      bestU = u;
      best = [vx vy th ig u];
    end
  end

  fprintf('NBV = (%d,%d), heading=%.3f, IG=%.3f, U=%.3f\n', best(1), best(2), best(3), best(4), best(5));
  fprintf('(This demo only selects a view; a real system would plan & move, then update belief.)\n');

  % --- Optional: create a Simulink skeleton model (no .slx included in this package) ---
  % createSimulinkSkeleton();
end

function H = bernoulliEntropy(p)
  eps = 1e-12;
  p = min(max(p, eps), 1-eps);
  H = -p*log(p) - (1-p)*log(1-p);
end

function ppost = posterior(p, zOcc, p_hit, p_false)
  eps = 1e-12;
  p = min(max(p, eps), 1-eps);
  if zOcc
    num = p_hit * p;
    den = p_hit * p + p_false * (1-p);
  else
    num = (1-p_hit) * p;
    den = (1-p_hit) * p + (1-p_false) * (1-p);
  end
  ppost = min(max(num/den, eps), 1-eps);
end

function E = expectedPosteriorEntropy(p, p_hit, p_false)
  pzOcc = p_hit * p + p_false * (1-p);
  pzFree = 1 - pzOcc;
  pPostOcc = posterior(p, true,  p_hit, p_false);
  pPostFree = posterior(p, false, p_hit, p_false);
  E = pzOcc * bernoulliEntropy(pPostOcc) + pzFree * bernoulliEntropy(pPostFree);
end

function ig = expectedInformationGain(p, p_hit, p_false)
  ig = bernoulliEntropy(p) - expectedPosteriorEntropy(p, p_hit, p_false);
end

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

function [ex, ey] = rayEndpoint(cx, cy, theta, r)
  ex = round(cx + r*cos(theta));
  ey = round(cy + r*sin(theta));
end

function total = expectedIgView(belief, cx, cy, heading, p_hit, p_false, fov, nRays, maxRange, occStop)
  [H, W] = size(belief);
  if cx < 1 || cx > W || cy < 1 || cy > H
    total = -1e9;
    return;
  end
  total = 0.0;
  angles = linspace(heading - 0.5*fov, heading + 0.5*fov, nRays);
  for a = angles
    [ex, ey] = rayEndpoint(cx, cy, a, maxRange);
    line = bresenham(cx, cy, ex, ey);
    for k = 2:size(line,1)
      x = line(k,1); y = line(k,2);
      if x < 1 || x > W || y < 1 || y > H
        break;
      end
      pOcc = belief(y,x);
      total = total + expectedInformationGain(pOcc, p_hit, p_false);
      if pOcc > occStop
        break;
      end
    end
  end
end

function createSimulinkSkeleton()
  % This creates a basic Simulink model showing where NBV selection would live.
  % Students can extend it by adding a MATLAB Function block that calls expectedIgView().
  mdl = 'NBV_Skeleton_Chapter17_Lesson3';
  if bdIsLoaded(mdl); close_system(mdl, 0); end
  new_system(mdl);
  open_system(mdl);

  add_block('simulink/Sources/Constant', [mdl '/BeliefGrid'], 'Value', '0.5');
  add_block('simulink/Sources/Constant', [mdl '/CurrentPose'], 'Value', '[2 2 0]');
  add_block('simulink/User-Defined Functions/MATLAB Function', [mdl '/NBV_Selector']);
  add_block('simulink/Sinks/Display', [mdl '/BestView']);

  set_param([mdl '/NBV_Selector'], 'Position', [250 100 450 170]);
  set_param([mdl '/BestView'], 'Position', [520 115 650 155]);

  add_line(mdl, 'BeliefGrid/1', 'NBV_Selector/1');
  add_line(mdl, 'CurrentPose/1', 'NBV_Selector/2');
  add_line(mdl, 'NBV_Selector/1', 'BestView/1');

  save_system(mdl);
  fprintf('Created Simulink skeleton model: %s.slx\n', mdl);
end
      

The Mathematica notebook (provided as a plain-text .nb expression) implements entropy, Bayes update, expected IG, and NBV selection.

File: Chapter17_Lesson3.nb

(* Chapter17_Lesson3.nb
   Autonomous Mobile Robots (Control Engineering) — Chapter 17, Lesson 3
   Next-Best-View (NBV) Strategies — Wolfram Mathematica Notebook (plain text)
   -------------------------------------------------------------------------
   This .nb is provided as a Notebook expression. Open it in Mathematica.

   It implements:
     - Bernoulli entropy
     - A binary sensor Bayes update
     - Expected information gain per cell
     - Ray-cast view scoring on a probability grid
     - NBV selection among random candidates
*)

Notebook[{
  Cell["Chapter 17, Lesson 3 — Next-Best-View (NBV) Strategies", "Title"],
  Cell["Core math utilities", "Section"],

  Cell[BoxData @ ToBoxes @ HoldForm[
    clamp[x_, lo_, hi_] := Min[Max[x, lo], hi];
    bernoulliEntropy[p_] := Module[{q = clamp[p, 10^-12, 1 - 10^-12]},
      -q Log[q] - (1 - q) Log[1 - q]
    ];

    posterior[pOcc_, zOcc_, pHit_:0.85, pFalse_:0.15] := Module[{p = clamp[pOcc, 10^-12, 1 - 10^-12], num, den},
      If[zOcc,
        num = pHit p; den = pHit p + pFalse (1 - p),
        num = (1 - pHit) p; den = (1 - pHit) p + (1 - pFalse) (1 - p)
      ];
      clamp[num/den, 10^-12, 1 - 10^-12]
    ];

    expectedPosteriorEntropy[pOcc_, pHit_:0.85, pFalse_:0.15] := Module[
      {p = clamp[pOcc, 10^-12, 1 - 10^-12], pzOcc, pzFree, pPostOcc, pPostFree},
      pzOcc = pHit p + pFalse (1 - p);
      pzFree = 1 - pzOcc;
      pPostOcc = posterior[p, True, pHit, pFalse];
      pPostFree = posterior[p, False, pHit, pFalse];
      pzOcc bernoulliEntropy[pPostOcc] + pzFree bernoulliEntropy[pPostFree]
    ];

    expectedInformationGain[pOcc_, pHit_:0.85, pFalse_:0.15] :=
      bernoulliEntropy[pOcc] - expectedPosteriorEntropy[pOcc, pHit, pFalse];
  ], "Input"],

  Cell["Ray casting (grid line traversal)", "Section"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    bresenham[{x0_, y0_}, {x1_, y1_}] := Module[
      {dx = Abs[x1 - x0], dy = -Abs[y1 - y0], sx = If[x0 < x1, 1, -1], sy = If[y0 < y1, 1, -1],
       err, x = x0, y = y0, pts = {}},
      err = dx + dy;
      While[True,
        AppendTo[pts, {x, y}];
        If[x == x1 && y == y1, Break[]];
        With[{e2 = 2 err},
          If[e2 >= dy, err = err + dy; x = x + sx];
          If[e2 <= dx, err = err + dx; y = y + sy];
        ];
      ];
      pts
    ];

    rayEndpoint[{cx_, cy_}, theta_, r_] := {Round[cx + r Cos[theta]], Round[cy + r Sin[theta]]};
  ], "Input"],

  Cell["Expected IG of a view + NBV selection", "Section"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    inBoundsQ[grid_, {x_, y_}] := Module[{h = Length[grid], w = Length[grid[[1]]]},
      1 <= x <= w && 1 <= y <= h
    ];

    expectedIgView[grid_, pose:{cx_, cy_, heading_}, fov_:Pi, nRays_:45, maxRange_:10, occStop_:0.70] := Module[
      {angles, total = 0, end, line, pOcc},
      If[!inBoundsQ[grid, {cx, cy}], Return[-10^9]];
      angles = Table[heading - fov/2 + (i/(Max[1, nRays - 1])) fov, {i, 0, nRays - 1}];
      Do[
        end = rayEndpoint[{cx, cy}, a, maxRange];
        line = bresenham[{cx, cy}, end];
        Do[
          If[k == 1, Continue[]];
          If[!inBoundsQ[grid, line[[k]]], Break[]];
          pOcc = grid[[ line[[k,2]], line[[k,1]] ]];
          total += expectedInformationGain[pOcc];
          If[pOcc > occStop, Break[]];
          , {k, 1, Length[line]}
        ];
        , {a, angles}
      ];
      total
    ];

    travelCost[{cx_, cy_}, {vx_, vy_}] := Sqrt[(vx - cx)^2 + (vy - cy)^2];

    chooseNBV[grid_, current:{cx_, cy_, heading_}, candidates_, lambda_:0.22] := Module[
      {best = {cx, cy, heading}, bestU = -Infinity, ig, u, c},
      Do[
        ig = expectedIgView[grid, cand];
        c = travelCost[{cx, cy}, cand[[{1,2}]]];
        u = ig - lambda c;
        If[u > bestU, bestU = u; best = cand;];
        , {cand, candidates}
      ];
      {best, bestU}
    ];
  ], "Input"],

  Cell["Demo run", "Section"],
  Cell[BoxData @ ToBoxes @ HoldForm[
    SeedRandom[7];
    W = 30; H = 18;
    grid = ConstantArray[0.5, {H, W}];
    current = {2, 2, 0.0};

    candidates = Table[{RandomInteger[{1, W}], RandomInteger[{1, H}], RandomReal[{0, 2 Pi}]}, {120}];
    {best, bestU} = chooseNBV[grid, current, candidates, 0.22];
    bestIG = expectedIgView[grid, best];

    Print["NBV = ", best, " IG = ", bestIG, " U = ", bestU];
  ], "Input"]
}]
      

The following problems reinforce the NBV derivations and help you reason about practical approximations.

Problem 1 (Cell-wise IG formula): Let a cell occupancy variable be \( m \in \{0,1\} \) with prior \( p=\Pr(m=1) \). A binary measurement \( z \in \{\text{occ},\text{free}\} \) satisfies \( \Pr(z=\text{occ}\mid m=1)=p_{\text{hit}} \) and \( \Pr(z=\text{occ}\mid m=0)=p_{\text{false}} \). Derive \( \mathrm{IG}(p) = H(p) - \mathbb{E}_z[H(p\mid z)] \) explicitly.

Solution: First compute \( \Pr(z=\text{occ}) = p_{\text{hit}}p + p_{\text{false}}(1-p) \) and \( \Pr(z=\text{free}) = 1 - \Pr(z=\text{occ}) \). By Bayes rule (Section 4), \( p_{\text{occ}} = \Pr(m=1\mid z=\text{occ}) \) and \( p_{\text{free}} = \Pr(m=1\mid z=\text{free}) \). Then:

\[ \mathrm{IG}(p) \;=\; H(p) \;-\; \Pr(z=\text{occ})\,H(p_{\text{occ}}) \;-\; \Pr(z=\text{free})\,H(p_{\text{free}}). \]

This is the scalar function used in the provided implementations.

Problem 2 (Maximum entropy and informativeness): Show that the Bernoulli entropy \( H(p) \) is maximized at \( p=0.5 \), and conclude why NBV tends to prioritize unknown frontier-adjacent cells.

Solution: Differentiate: \( H'(p) = -\log p + \log(1-p) \). Setting \( H'(p)=0 \) gives \( p=0.5 \). Also \( H''(p)= -\frac{1}{p} - \frac{1}{1-p} < 0 \) for \( 0 < p < 1 \), so it is a maximum. Therefore cells with \( p \approx 0.5 \) contribute the most uncertainty and usually the most potential IG.

Problem 3 (Utility scaling): Suppose a candidate view has \( \mathrm{IG}_1 \) and cost \( J_1 \), while another has \( \mathrm{IG}_2 \) and cost \( J_2 \). Derive the condition on \( \lambda \) such that view 1 is preferred, i.e., \( U_1 > U_2 \).

Solution: With \( U=\mathrm{IG}-\lambda J \), preference is: \( \mathrm{IG}_1 - \lambda J_1 > \mathrm{IG}_2 - \lambda J_2 \). Rearranging:

\[ \lambda \;<\; \frac{\mathrm{IG}_1 - \mathrm{IG}_2}{J_1 - J_2} \quad \text{(when } J_1 \ne J_2 \text{)}. \]

This inequality shows \( \lambda \) controls how aggressively the robot pays travel cost for information.

Problem 4 (Occlusion-aware IG approximation): In a ray model, assume that the ray stops at the first occupied cell. Explain why summing IG over all cells along a ray can overestimate IG and propose a conservative correction.

Solution: If the ray hits an occupied cell early, cells behind it are not observed. Summing IG over all cells ignores this and thus overestimates gain. A conservative correction is to stop accumulating IG once the belief occupancy exceeds a threshold \( p_i > p_{\text{stop}} \), or to weight each cell by an estimated visibility probability (free-space probability of previous cells).

Problem 5 (Two-step NBV lookahead): Consider a horizon-2 plan with discount \( 0 < \gamma \le 1 \). Write the determinized objective when you approximate the posterior after the first view by its expected update, and explain the benefit.

Solution: A determinized horizon-2 objective is:

\[ \max_{\mathbf{v}_t,\mathbf{v}_{t+1}} \left( \widehat{I}_t(\mathbf{v}_t) - \lambda J(\mathbf{v}_t\mid \mathbf{x}_t) \right) \;+\; \gamma\left( \widehat{I}_{t+1}(\mathbf{v}_{t+1}\mid \widehat{\mathcal{D}}_{t+1}) - \lambda J(\mathbf{v}_{t+1}\mid \widehat{\mathbf{x}}_{t+1}) \right), \]

where \( \widehat{\mathcal{D}}_{t+1} \) is the “expected” updated data/belief after executing \( \mathbf{v}_t \). The benefit is reduced myopia: the first action is chosen not only for immediate IG, but also for positioning and future IG.

Next-best-view strategies turn exploration into a principled decision problem: evaluate candidate viewpoints, estimate their expected information gain under a sensing model and occlusion approximation, and trade that gain against travel/risk costs using a utility function. This lesson provided the core derivations (mutual information and its KL form), practical occupancy-grid approximations, and multi-language reference implementations. In Lesson 4, we incorporate explicit battery/time budgeting to ensure exploration remains feasible under limited resources.

1. Yamauchi, B. (1997). A frontier-based approach for autonomous exploration. Proceedings of IEEE International Symposium on Computational Intelligence in Robotics and Automation (CIRA).
2. Bourgault, F., Makarenko, A.A., Williams, S.B., Grocholsky, B., & Durrant-Whyte, H.F. (2002). Information based adaptive robotic exploration. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).
3. Stachniss, C., Grisetti, G., & Burgard, W. (2005). Information gain-based exploration using Rao-Blackwellized particle filters. Robotics: Science and Systems (RSS).
4. Sim, R., & Roy, N. (2005). Global A-optimal robot exploration in SLAM. IEEE International Conference on Robotics and Automation (ICRA).
5. Juliá, M., Gil, A., & Reinoso, O. (2012). A comparison of path planning strategies for autonomous exploration and mapping of unknown environments. Autonomous Robots, 33(4), 427–444.
6. Charrow, B., Liu, S., Kumar, V., & Michael, N. (2015). Information-theoretic mapping using Cauchy-Schwarz quadratic mutual information. IEEE International Conference on Robotics and Automation (ICRA).
7. Bircher, A., Kamel, M., Alexis, K., Oleynikova, H., & Siegwart, R. (2016). Receding horizon “next-best-view” planning for 3D exploration. IEEE International Conference on Robotics and Automation (ICRA).
8. Lauri, M., & Ritala, R. (2017). Planning for robotic exploration based on forward simulation. Autonomous Robots, 41(5), 1113–1132.
9. Schmid, L., Delmerico, J., Schröder, M., et al. (2020). On the evaluation of exploration strategies for aerial robots. IEEE Robotics and Automation Letters (RA-L).