A costmap is a discrete scalar field defined over a 2D grid (typically aligned with the ground plane). Each cell stores a cost that encodes traversal risk. Formally, let the grid index set be \( \mathcal{G}=\{(i,j)\mid 0\le i < W,\; 0\le j < H\} \) and define a cost function \( C:\mathcal{G}\to \{0,1,\dots,255\} \). Typical semantics are:

• \( C=0 \): free space (preferred).
• \( C\approx 255 \): lethal obstacle (collision if entered).
• \( 0 < C < 255 \): inflated/penalized region near obstacles (discourages proximity).

In layered navigation, the master costmap is produced by composing multiple layers (static map, sensor obstacles, inflation, etc.). The local navigation module then evaluates candidate motions using this cost field.

flowchart TD
  M["Static map layer (prior)"] --> MC["Master costmap C(i,j)"]
  S["Sensors (LiDAR/Depth)"] --> O["Obstacle layer (dynamic)"]
  O --> MC
  MC --> D["Distance field d(i,j)"]
  D --> I["Inflation (decay cost around obstacles)"]
  I --> MC
  MC --> L["Local navigation evaluates trajectories"]
        

Earlier (Chapter 9), you learned occupancy grids as probabilistic maps. Each cell stores an occupancy probability \( p_{ij} \in (0,1) \), interpreted as \( p_{ij} = \Pr(m_{ij}=\text{occ}\mid z_{1:t}) \) given sensor data \( z_{1:t} \). A numerically stable representation uses log-odds: \( L_{ij}=\log\!\frac{p_{ij}}{1-p_{ij}} \).

For an inverse sensor model producing an increment \( \ell(z_t,ij) \), the standard additive log-odds update is:

\[ L_{ij}^{(t)} = L_{ij}^{(t-1)} + \ell(z_t,ij). \]

Converting probabilities to costs is a decision-and-scaling step. A common threshold model is:

\[ C_{\text{base}}(i,j)= \begin{cases} C_{\text{lethal}} & \text{if } p_{ij} \ge p_{\text{occ}}, \\ C_{\text{unknown}} & \text{if } p_{\text{free}} < p_{ij} < p_{\text{occ}}, \\ 0 & \text{if } p_{ij} \le p_{\text{free}}. \end{cases} \]

Here \(p_{\text{occ}}\) and \(p_{\text{free}}\) implement a hysteresis band to avoid flicker under noise. The inflation layer then transforms the sparse obstacle set into a smooth safety buffer by assigning a graded penalty to cells near obstacles.

Important modeling note. The costmap is not a probability distribution; it is a control-oriented surrogate objective that trades off safety and efficiency. Its calibration depends on footprint geometry, dynamics margins, and the local navigation controller.

Let the robot footprint in its body frame be a compact set \( \mathcal{R}\subset\mathbb{R}^2 \) (polygon or circle approximation). Let workspace obstacles be \( \mathcal{O}\subset\mathbb{R}^2 \). A robot center position \( \mathbf{x}\in\mathbb{R}^2 \) is in collision iff:

\[ (\mathbf{x}+\mathcal{R}) \cap \mathcal{O} \ne \emptyset. \]

Define the reflected footprint \( -\mathcal{R}=\{-\mathbf{r}\mid \mathbf{r}\in\mathcal{R}\} \). The configuration-space obstacle set for the robot center is:

\[ \mathcal{O}_{\text{cs}} = \mathcal{O} \oplus (-\mathcal{R}) \;=\;\{\mathbf{o}-\mathbf{r}\mid \mathbf{o}\in\mathcal{O},\;\mathbf{r}\in\mathcal{R}\}. \]

This is the Minkowski sum dilation viewpoint: planning for the robot center is equivalent to planning a point robot among dilated obstacles. On a grid, inflation approximates this dilation plus an additional safety margin \(d_{\text{safe}}\).

Proposition (safety via inflated obstacles). Suppose the footprint is contained in a circle of radius \(r_c\) (circumscribed radius), i.e. \( \mathcal{R} \subset \{\mathbf{r}:\|\mathbf{r}\|_2 \le r_c\} \). If we mark all cells whose center is within distance \( r_c + d_{\text{safe}} \) of any obstacle as “forbidden,” then any path for the robot center that stays outside this forbidden set has clearance at least \(d_{\text{safe}}\).

Proof. Let \( \mathbf{x} \) be a robot center on the path and let \( \mathbf{o} \) be the closest obstacle point so that \( \|\mathbf{x}-\mathbf{o}\|_2 = \operatorname{dist}(\mathbf{x},\mathcal{O}) \). If the path stays outside the forbidden set, then \( \operatorname{dist}(\mathbf{x},\mathcal{O}) > r_c + d_{\text{safe}} \). For any footprint point \( \mathbf{r}\in\mathcal{R} \), we have \( \|\mathbf{r}\|_2 \le r_c \). By triangle inequality:

\[ \|\mathbf{x}+\mathbf{r}-\mathbf{o}\|_2 \ge \|\mathbf{x}-\mathbf{o}\|_2 - \|\mathbf{r}\|_2 > (r_c + d_{\text{safe}}) - r_c = d_{\text{safe}}. \]

Hence every point of the footprint stays at least \(d_{\text{safe}}\) away from obstacles. □

Practical stacks refine this using an inscribed radius \(r_{\text{in}}\) (largest circle contained inside the footprint) and a graded penalty between \(r_{\text{in}}\) and an inflation radius \(r_{\text{inf}}\).

Let \( d(i,j) \) be the Euclidean distance (in meters) from cell center \((i,j)\) to the nearest obstacle cell (or obstacle boundary, depending on convention). Inflation computes a cost \( C_{\text{infl}}(i,j) \) from \( d(i,j) \).

A standard piecewise model (used widely in practice) is:

\[ C_{\text{infl}}(i,j)= \begin{cases} C_{\text{lethal}} & \text{if } d(i,j)=0,\\ C_{\text{ins}} & \text{if } 0 < d(i,j) \le r_{\text{in}},\\ \Big\lfloor (C_{\text{ins}}-1)\exp\!\big(-k(d(i,j)-r_{\text{in}})\big)+1 \Big\rceil & \text{if } r_{\text{in}} < d(i,j) \le r_{\text{inf}},\\ 0 & \text{if } d(i,j) > r_{\text{inf}}, \end{cases} \]

where \(k>0\) is a scaling factor controlling how fast cost decays with distance, and \(\lfloor \cdot \rceil\) denotes rounding to an integer cost. The “inscribed” band enforces a hard safety core near obstacles for the footprint’s guaranteed inner circle.

Lemma (monotonicity). For \( r_{\text{in}} < d \le r_{\text{inf}} \), the continuous (pre-rounded) inflation cost \( \tilde{C}(d)=(C_{\text{ins}}-1)\exp(-k(d-r_{\text{in}}))+1 \) is strictly decreasing in \(d\).

Proof. Differentiate:

\[ \frac{d\tilde{C}}{dd} = -(C_{\text{ins}}-1)k \exp\!\big(-k(d-r_{\text{in}})\big) < 0 \quad \text{for } k>0. \]

Thus increasing clearance strictly reduces penalty (before discretization/rounding). □

flowchart TD
  O["Obstacle cells (binary grid)"] --> DT["Compute distance field d(i,j)"]
  DT --> B1["If d == 0 \nthen lethal"]
  DT --> B2["If d in (0, r_in] \nthen inscribed"]
  DT --> B3["If d in (r_in, r_inf] \nthen cost=(Cins-1)*exp(-k*(d-r_in))+1"]
  DT --> B4["If d in (r_inf, +inf) \nthen 0"]
  B1 --> C["Inflated costmap layer"]
  B2 --> C
  B3 --> C
  B4 --> C

Let grid resolution be \( \Delta \) meters/cell. The distance field ideally is: \( d(i,j)=\min_{(u,v)\in\mathcal{O}} \Delta\sqrt{(i-u)^2+(j-v)^2} \), where \(\mathcal{O}\subset\mathcal{G}\) is the obstacle cell set.

Exact Euclidean distance transforms can be computed in linear time in the number of cells, but many practical implementations use approximate multi-source wavefront propagation (often called “brushfire”) with 4- or 8-neighborhood updates. With 8-neighborhood steps of length \(\Delta\) and \(\sqrt{2}\Delta\), you obtain a tight approximation suitable for inflation.

Discretization bound (clearance error). If obstacle boundaries are represented at cell centers, the true continuous clearance differs from the grid distance by at most about half a cell diagonal: \( \epsilon \le \tfrac{\sqrt{2}}{2}\Delta \). This motivates choosing \(\Delta\) small enough that \(\epsilon\) is below the safety margin.

A common master costmap composition is the cellwise maximum: \( C(i,j)=\max_\ell C_\ell(i,j) \) over layers \(\ell\). This is conservative: any layer can veto traversal.

Key parameters (conceptually; naming varies across stacks):

• \( \Delta \) (resolution): smaller improves geometric fidelity but increases computation.
• \( r_{\text{in}} \) (inscribed radius): footprint inner safety core.
• \( r_{\text{inf}} \) (inflation radius): how far penalty spreads into free space.
• \( k \) (cost scaling): larger makes a steeper “repulsive wall”; smaller is smoother but may allow close passes.

Practical tuning rule: pick \( r_{\text{inf}} \ge r_c + d_{\text{safe}} \) for a desired clearance, then adjust \(k\) so that the cost at clearance distance is below the “acceptable” threshold used by the local navigation evaluator.

Chapter14_Lesson2.py

"""
Chapter14_Lesson2.py
Costmaps and Inflation Concepts — reference implementation (grid + inflation).
This script builds a toy occupancy grid, computes an approximate Euclidean distance
field via an 8-connected brushfire, and produces an inflated costmap.
"""

from collections import deque
import math
from typing import List, Tuple

Grid = List[List[int]]  # 0 free, 1 obstacle

def make_toy_map(w: int = 60, h: int = 40) -> Grid:
    g = [[0 for _ in range(w)] for _ in range(h)]
    # border
    for x in range(w):
        g[0][x] = 1
        g[h-1][x] = 1
    for y in range(h):
        g[y][0] = 1
        g[y][w-1] = 1
    # internal obstacles
    for x in range(10, 50):
        g[12][x] = 1
    for y in range(18, 33):
        g[y][28] = 1
    for x in range(35, 55):
        g[28][x] = 1
    return g

def brushfire_distance(obstacles: Grid, resolution: float) -> List[List[float]]:
    """
    Approximate Euclidean distance transform using multi-source BFS.
    8-neighborhood with step costs {1, sqrt(2)} * resolution.
    """
    h = len(obstacles)
    w = len(obstacles[0])
    INF = 1e9
    dist = [[INF for _ in range(w)] for _ in range(h)]
    q = deque()

    # initialize all obstacle cells
    for y in range(h):
        for x in range(w):
            if obstacles[y][x] == 1:
                dist[y][x] = 0.0
                q.append((x, y))

    nbrs = [(-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
            (-1, -1, math.sqrt(2)), (1, -1, math.sqrt(2)),
            (-1, 1, math.sqrt(2)), (1, 1, math.sqrt(2))]

    while q:
        x, y = q.popleft()
        dxy = dist[y][x]
        for dx, dy, step in nbrs:
            nx, ny = x + dx, y + dy
            if 0 <= nx < w and 0 <= ny < h:
                nd = dxy + step * resolution
                if nd < dist[ny][nx]:
                    dist[ny][nx] = nd
                    q.append((nx, ny))
    return dist

def inflation_cost_from_distance(d: float, r_inscribed: float, r_inflation: float,
                                cost_scaling: float,
                                lethal_cost: int = 254,
                                inscribed_cost: int = 253) -> int:
    """
    Common inflation cost model:
      - obstacle cell: lethal_cost
      - 0 < d <= r_inscribed: inscribed_cost
      - r_inscribed < d <= r_inflation: exponential decay
      - d > r_inflation: 0
    """
    if d <= 0.0:
        return lethal_cost
    if d <= r_inscribed:
        return inscribed_cost
    if d > r_inflation:
        return 0

    # decay from (inscribed_cost-1) down to 1, then clamp
    c = (inscribed_cost - 1) * math.exp(-cost_scaling * (d - r_inscribed)) + 1.0
    c_int = int(max(1, min(inscribed_cost - 1, round(c))))
    return c_int

def build_inflated_costmap(obstacles: Grid, resolution: float,
                           r_inscribed: float, r_inflation: float,
                           cost_scaling: float) -> List[List[int]]:
    dist = brushfire_distance(obstacles, resolution)
    h = len(obstacles)
    w = len(obstacles[0])
    cm = [[0 for _ in range(w)] for _ in range(h)]
    for y in range(h):
        for x in range(w):
            cm[y][x] = inflation_cost_from_distance(
                dist[y][x], r_inscribed, r_inflation, cost_scaling
            )
    return cm

def ascii_render_costmap(costmap: List[List[int]]) -> str:
    """
    Render costmap with ASCII characters:
      '#' lethal, '+' high, '.' low, ' ' free
    """
    lines = []
    for row in costmap:
        s = []
        for c in row:
            if c >= 254:
                s.append('#')
            elif c >= 200:
                s.append('X')
            elif c >= 120:
                s.append('+')
            elif c >= 40:
                s.append('.')
            else:
                s.append(' ')
        lines.append(''.join(s))
    return "\n".join(lines)

def main():
    resolution = 0.05  # m/cell
    r_inscribed = 0.25 # m
    r_inflation = 0.60 # m
    cost_scaling = 6.0 # 1/m

    obs = make_toy_map()
    costmap = build_inflated_costmap(obs, resolution, r_inscribed, r_inflation, cost_scaling)
    print("Inflated costmap (ASCII preview):")
    print(ascii_render_costmap(costmap))

if __name__ == "__main__":
    main()
      

This script is intentionally “stack-agnostic”: it isolates the mathematics of distance fields + decay costs. In a full AMR system, the obstacle grid would be updated by the sensor pipeline, and the inflated layer would be recomputed in a local rolling window around the robot.

Chapter14_Lesson2.cpp

// Chapter14_Lesson2.cpp
// Costmaps and Inflation Concepts — simple grid + brushfire inflation (no ROS deps).
#include <cmath>
#include <cstdint>
#include <deque>
#include <iostream>
#include <limits>
#include <vector>

struct Cell { int x, y; };

static std::vector<std::vector<int>> makeToyMap(int w=60, int h=40) {
    std::vector<std::vector<int>> g(h, std::vector<int>(w, 0));
    for (int x=0; x<w; ++x) { g[0][x]=1; g[h-1][x]=1; }
    for (int y=0; y<h; ++y) { g[y][0]=1; g[y][w-1]=1; }
    for (int x=10; x<50; ++x) g[12][x]=1;
    for (int y=18; y<33; ++y) g[y][28]=1;
    for (int x=35; x<55; ++x) g[28][x]=1;
    return g;
}

static std::vector<std::vector<double>> brushfireDistance(
    const std::vector<std::vector<int>>& obs, double resolution)
{
    const int h = (int)obs.size();
    const int w = (int)obs[0].size();
    const double INF = std::numeric_limits<double>::infinity();

    std::vector<std::vector<double>> dist(h, std::vector<double>(w, INF));
    std::deque<Cell> q;

    for (int y=0; y<h; ++y) for (int x=0; x<w; ++x) {
        if (obs[y][x] == 1) { dist[y][x] = 0.0; q.push_back({x,y}); }
    }

    struct Nbr { int dx, dy; double step; };
    const double s2 = std::sqrt(2.0);
    const Nbr nbrs[8] = {
        {-1,0,1.0},{1,0,1.0},{0,-1,1.0},{0,1,1.0},
        {-1,-1,s2},{1,-1,s2},{-1,1,s2},{1,1,s2}
    };

    while (!q.empty()) {
        Cell c = q.front(); q.pop_front();
        const double dxy = dist[c.y][c.x];
        for (const auto& nb : nbrs) {
            const int nx = c.x + nb.dx;
            const int ny = c.y + nb.dy;
            if (0 <= nx && nx < w && 0 <= ny && ny < h) {
                const double nd = dxy + nb.step * resolution;
                if (nd < dist[ny][nx]) { dist[ny][nx] = nd; q.push_back({nx,ny}); }
            }
        }
    }
    return dist;
}

static int inflationCost(double d, double r_inscribed, double r_inflation, double cost_scaling,
                         int lethal_cost=254, int inscribed_cost=253)
{
    if (d <= 0.0) return lethal_cost;
    if (d <= r_inscribed) return inscribed_cost;
    if (d > r_inflation) return 0;

    const double c = (inscribed_cost - 1) * std::exp(-cost_scaling * (d - r_inscribed)) + 1.0;
    int ci = (int)std::llround(c);
    if (ci < 1) ci = 1;
    if (ci > inscribed_cost - 1) ci = inscribed_cost - 1;
    return ci;
}

static std::vector<std::vector<int>> buildInflatedCostmap(
    const std::vector<std::vector<int>>& obs,
    double resolution, double r_inscribed, double r_inflation, double cost_scaling)
{
    auto dist = brushfireDistance(obs, resolution);
    const int h = (int)obs.size();
    const int w = (int)obs[0].size();
    std::vector<std::vector<int>> cm(h, std::vector<int>(w, 0));
    for (int y=0; y<h; ++y) for (int x=0; x<w; ++x)
        cm[y][x] = inflationCost(dist[y][x], r_inscribed, r_inflation, cost_scaling);
    return cm;
}

static void asciiRender(const std::vector<std::vector<int>>& cm) {
    for (const auto& row : cm) {
        for (int c : row) {
            char ch = ' ';
            if (c >= 254) ch = '#';
            else if (c >= 200) ch = 'X';
            else if (c >= 120) ch = '+';
            else if (c >= 40)  ch = '.';
            std::cout << ch;
        }
        std::cout << "\n";
    }
}

int main() {
    const double resolution = 0.05;
    const double r_inscribed = 0.25;
    const double r_inflation = 0.60;
    const double cost_scaling = 6.0;

    auto obs = makeToyMap();
    auto cm = buildInflatedCostmap(obs, resolution, r_inscribed, r_inflation, cost_scaling);
    std::cout << "Inflated costmap (ASCII preview):\n";
    asciiRender(cm);
    return 0;
}
      

Chapter14_Lesson2.java

// Chapter14_Lesson2.java
// Costmaps and Inflation Concepts — grid + brushfire inflation (pure Java).
import java.util.ArrayDeque;
import java.util.Arrays;

public class Chapter14_Lesson2 {

    static int[][] makeToyMap(int w, int h) {
        int[][] g = new int[h][w];
        for (int x=0; x<w; x++) { g[0][x]=1; g[h-1][x]=1; }
        for (int y=0; y<h; y++) { g[y][0]=1; g[y][w-1]=1; }
        for (int x=10; x<50; x++) g[12][x]=1;
        for (int y=18; y<33; y++) g[y][28]=1;
        for (int x=35; x<55; x++) g[28][x]=1;
        return g;
    }

    static double[][] brushfireDistance(int[][] obs, double resolution) {
        int h = obs.length, w = obs[0].length;
        double INF = 1e18;
        double[][] dist = new double[h][w];
        for (int y=0; y<h; y++) Arrays.fill(dist[y], INF);

        ArrayDeque<int[]> q = new ArrayDeque<>();
        for (int y=0; y<h; y++) for (int x=0; x<w; x++) {
            if (obs[y][x] == 1) { dist[y][x] = 0.0; q.add(new int[]{x,y}); }
        }

        int[] dx = {-1,1,0,0,-1,1,-1,1};
        int[] dy = {0,0,-1,1,-1,-1,1,1};
        double[] step = {1,1,1,1,Math.sqrt(2),Math.sqrt(2),Math.sqrt(2),Math.sqrt(2)};

        while (!q.isEmpty()) {
            int[] c = q.removeFirst();
            int x = c[0], y = c[1];
            double dxy = dist[y][x];
            for (int k=0; k<8; k++) {
                int nx = x + dx[k], ny = y + dy[k];
                if (0 <= nx && nx < w && 0 <= ny && ny < h) {
                    double nd = dxy + step[k]*resolution;
                    if (nd < dist[ny][nx]) {
                        dist[ny][nx] = nd;
                        q.add(new int[]{nx, ny});
                    }
                }
            }
        }
        return dist;
    }

    static int inflationCost(double d, double rInscribed, double rInflation, double costScaling) {
        int lethal = 254, inscribed = 253;
        if (d <= 0.0) return lethal;
        if (d <= rInscribed) return inscribed;
        if (d > rInflation) return 0;
        double c = (inscribed - 1) * Math.exp(-costScaling * (d - rInscribed)) + 1.0;
        long ci = Math.round(c);
        if (ci < 1) ci = 1;
        if (ci > inscribed - 1) ci = inscribed - 1;
        return (int)ci;
    }

    static int[][] buildInflatedCostmap(int[][] obs, double resolution,
                                       double rInscribed, double rInflation, double costScaling) {
        double[][] dist = brushfireDistance(obs, resolution);
        int h = obs.length, w = obs[0].length;
        int[][] cm = new int[h][w];
        for (int y=0; y<h; y++) for (int x=0; x<w; x++)
            cm[y][x] = inflationCost(dist[y][x], rInscribed, rInflation, costScaling);
        return cm;
    }

    static void asciiRender(int[][] cm) {
        for (int y=0; y<cm.length; y++) {
            StringBuilder sb = new StringBuilder();
            for (int x=0; x<cm[0].length; x++) {
                int c = cm[y][x];
                char ch = ' ';
                if (c >= 254) ch = '#';
                else if (c >= 200) ch = 'X';
                else if (c >= 120) ch = '+';
                else if (c >= 40)  ch = '.';
                sb.append(ch);
            }
            System.out.println(sb.toString());
        }
    }

    public static void main(String[] args) {
        double resolution = 0.05;
        double rInscribed = 0.25;
        double rInflation = 0.60;
        double costScaling = 6.0;

        int[][] obs = makeToyMap(60, 40);
        int[][] cm = buildInflatedCostmap(obs, resolution, rInscribed, rInflation, costScaling);

        System.out.println("Inflated costmap (ASCII preview):");
        asciiRender(cm);
    }
}
      

Chapter14_Lesson2.m

% Chapter14_Lesson2.m
% Costmaps and Inflation Concepts — build inflated costmap using bwdist.

clear; clc;

resolution = 0.05;      % m/cell
r_inscribed = 0.25;     % m
r_inflation = 0.60;     % m
cost_scaling = 6.0;     % 1/m

w = 60; h = 40;
obs = zeros(h, w);
obs(1,:) = 1; obs(end,:) = 1;
obs(:,1) = 1; obs(:,end) = 1;
obs(13, 11:50) = 1;         % MATLAB is 1-indexed
obs(19:33, 29) = 1;
obs(29, 36:55) = 1;

% Euclidean distance to nearest obstacle cell (in cells)
d_cells = bwdist(obs > 0);
d = d_cells * resolution;    % meters

lethal = 254;
inscribed = 253;

costmap = zeros(h, w);

% lethal cells
costmap(d <= 0) = lethal;

% inside inscribed radius
mask_in = (d > 0) & (d <= r_inscribed);
costmap(mask_in) = inscribed;

% inflation band
mask_inf = (d > r_inscribed) & (d <= r_inflation);
c = (inscribed - 1) .* exp(-cost_scaling .* (d(mask_inf) - r_inscribed)) + 1;
c = round(c);
c = max(1, min(inscribed-1, c));
costmap(mask_inf) = c;

% display
figure;
imagesc(costmap);
axis image; colorbar;
title('Inflated Costmap (0..254)');
      

In Simulink, the same concept is realized by (i) generating a binary obstacle raster, (ii) applying a distance transform (or chamfer approximation), and (iii) mapping distance to cost via a Lookup Table block implementing the exponential decay.

Chapter14_Lesson2.nb

(* Chapter14_Lesson2.nb
   Costmaps and Inflation Concepts — minimal Mathematica notebook expression. *)

Notebook[{
  Cell["Chapter 14 — Lesson 2: Costmaps and Inflation Concepts", "Title"],
  Cell["Toy occupancy grid -> distance transform -> inflated costmap.", "Text"],
  Cell[BoxData[
    ToBoxes@Module[
      {w=60, h=40, resolution=0.05, rIns=0.25, rInf=0.60, k=6.0,
       obs, dist, cost},
      obs = ConstantArray[0, {h,w}];
      obs[[1, All]] = 1; obs[[h, All]] = 1;
      obs[[All, 1]] = 1; obs[[All, w]] = 1;
      obs[[13, 11 ;; 50]] = 1;
      obs[[19 ;; 33, 29]] = 1;
      obs[[29, 36 ;; 55]] = 1;

      (* Euclidean distance (in cells) to nearest obstacle *)
      dist = DistanceTransform[obs, WorkingPrecision -> MachinePrecision] * resolution;

      cost[d_] := Which[
        d <= 0, 254,
        d <= rIns, 253,
        d > rInf, 0,
        True, Round[(253 - 1) Exp[-k (d - rIns)] + 1]
      ];

      ArrayPlot[cost /@ dist,
        PlotRange -> {0, 254},
        Frame -> True,
        PlotLabel -> "Inflated Costmap (0..254)"
      ]
    ]
  ], "Input"]
}]
      

Problem 1 (Log-odds update derivation): Let \(p_t=\Pr(m=\text{occ}\mid z_{1:t})\). Show that under conditional independence of measurements given \(m\), the log-odds satisfy an additive update \(L_t = L_{t-1} + \ell(z_t)\) for an appropriate \(\ell(z_t)\).

Solution: Bayes rule gives \( \Pr(m\mid z_{1:t}) \propto \Pr(z_t\mid m)\Pr(m\mid z_{1:t-1}) \). Taking odds:

\[ \frac{p_t}{1-p_t} = \frac{\Pr(z_t\mid m=\text{occ})}{\Pr(z_t\mid m=\text{free})} \cdot \frac{p_{t-1}}{1-p_{t-1}}. \]

Taking log yields \( L_t = \log\!\frac{p_t}{1-p_t} = \log\!\frac{p_{t-1}}{1-p_{t-1}} + \log\!\frac{\Pr(z_t\mid \text{occ})}{\Pr(z_t\mid \text{free})} \). Hence \( L_t = L_{t-1} + \ell(z_t) \) with \( \ell(z_t)=\log\!\frac{\Pr(z_t\mid \text{occ})}{\Pr(z_t\mid \text{free})} \). □

Problem 2 (Inflation monotonicity and sensitivity): For \( d \in (r_{\text{in}}, r_{\text{inf}}] \), define \( \tilde{C}(d)=(C_{\text{ins}}-1)\exp(-k(d-r_{\text{in}}))+1 \). (a) Prove it is strictly decreasing in \(d\). (b) Show that increasing \(k\) makes the decay “steeper” by proving \( \frac{\partial \tilde{C}}{\partial k} < 0 \) for \(d>r_{\text{in}}\).

Solution: (a) From Section 4:

\[ \frac{d\tilde{C}}{dd} = -(C_{\text{ins}}-1)k\exp(-k(d-r_{\text{in}})) < 0. \]

(b) Differentiate w.r.t. \(k\):

\[ \frac{\partial \tilde{C}}{\partial k} = (C_{\text{ins}}-1)\exp(-k(d-r_{\text{in}}))\cdot (-(d-r_{\text{in}})) < 0 \quad \text{for } d>r_{\text{in}}. \]

Thus larger \(k\) reduces costs faster away from obstacles (a sharper penalty wall). □

Problem 3 (Choosing inflation radius for clearance): The robot footprint is bounded by a circumscribed radius \(r_c\). You want a guaranteed clearance \(d_{\text{safe}}\) from obstacles. What condition should \(r_{\text{inf}}\) satisfy so that “avoiding inflated lethal cells” ensures clearance at least \(d_{\text{safe}}\) (ignoring discretization)?

Solution: From the Proposition in Section 3, mark forbidden all cells whose centers are within \(r_c+d_{\text{safe}}\) of obstacles. Therefore choose

\[ r_{\text{inf}} \ge r_c + d_{\text{safe}}. \]

Then any robot-center path that stays outside the forbidden inflated set yields clearance at least \(d_{\text{safe}}\) for the entire footprint. □

Problem 4 (Resolution vs clearance error): Assume obstacle boundaries are represented at cell centers and the grid resolution is \(\Delta\). Give a conservative bound on the maximum error between the true continuous distance-to-obstacle and the distance computed between cell centers.

Solution: The farthest point in a cell from its center is half the diagonal length: \( \tfrac{\sqrt{2}}{2}\Delta \). Therefore a conservative bound on distance error is

\[ \epsilon \le \frac{\sqrt{2}}{2}\Delta. \]

In practice, to maintain an effective safety margin \(d_{\text{safe}}\), choose \(\Delta\) so that \(\epsilon\) is small relative to \(d_{\text{safe}}\), and inflate accordingly. □

Programming Exercise (monotonicity test): Write a script that computes \(C_{\text{infl}}(d)\) for sampled distances and verifies it is non-increasing for \(d\ge r_{\text{in}}\).

Solution code:

Chapter14_Lesson2_Ex1.py

"""
Chapter14_Lesson2_Ex1.py
Exercise: empirically verify monotonicity of the inflation cost and visualize cost(d).
"""

import math

def inflation_cost(d: float, r_inscribed: float, r_inflation: float, k: float,
                   inscribed_cost: int = 253) -> int:
    if d <= 0.0:
        return 254
    if d <= r_inscribed:
        return inscribed_cost
    if d > r_inflation:
        return 0
    c = (inscribed_cost - 1) * math.exp(-k * (d - r_inscribed)) + 1.0
    return int(max(1, min(inscribed_cost - 1, round(c))))

def main():
    r_ins, r_inf, k = 0.25, 0.60, 6.0
    ds = [i * 0.01 for i in range(0, 121)]
    cs = [inflation_cost(d, r_ins, r_inf, k) for d in ds]

    # check monotone non-increasing for d >= r_ins
    ok = True
    for i in range(1, len(ds)):
        if ds[i] >= r_ins and cs[i] > cs[i-1]:
            ok = False
            print("Violation at d=", ds[i-1], "->", ds[i], "cost", cs[i-1], "->", cs[i])
            break
    print("Monotone non-increasing beyond inscribed radius:", ok)

    # print a small table
    for d in [0.0, 0.1, 0.25, 0.30, 0.40, 0.60, 0.80]:
        print(f"d={d:0.2f} m -> cost={inflation_cost(d, r_ins, r_inf, k)}")

if __name__ == "__main__":
    main()
      

We formalized costmaps as discrete risk fields, connected them to probabilistic occupancy estimates, and derived the geometric reason for inflation through Minkowski sums (configuration-space dilation). We then modeled inflation as a distance transform followed by a monotone decay kernel, proving key properties (monotonicity and parameter sensitivity) and discussing discretization error. These concepts are the bridge between perception/mapping and the decision logic of local navigation.