In static worlds, collision avoidance can be handled by geometric clearance and reactive rules. In dynamic environments, however, the robot must reason about where obstacles will be at future times. If the robot uses only instantaneous obstacle positions/velocities, it can oscillate, stop unnecessarily, or enter unsafe time-delayed collisions (e.g., stepping into a corridor that will be occupied moments later).

Prediction-aware navigation introduces a time horizon \( T = N\Delta t \) and optimizes over a sequence of controls \( \mathbf{u}_0, \dots, \mathbf{u}_{N-1} \) while using a predicted obstacle distribution \( p\big( \mathbf{o}_k \mid \mathcal{Z}_{0:k_0} \big) \) for future steps \( k > k_0 \).

flowchart TD
  S["Sensors (LiDAR / camera / radar)"] --> TR["Track moving objects"]
  TR --> PR["Predict future motion (mean + uncertainty)"]
  PR --> LP["Local planner (receding horizon)"]
  LP --> CMD["Execute first control (v,w)"]
  CMD --> FB["Feedback: new measurements"]
  FB --> TR
        

The key technical question: how do we convert predictions into constraints and costs so that the planner is both safe and efficient?

We represent obstacle \( j \) by a (planar) state \( \mathbf{x}^o_j = [x_j, y_j, v_{x,j}, v_{y,j}]^\top \). A common baseline is a constant-velocity (CV) discrete-time model:

\[ \mathbf{x}^o_{j,k+1} = \mathbf{F}\mathbf{x}^o_{j,k} + \mathbf{w}_{j,k}, \quad \mathbf{F} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad \mathbf{w}_{j,k} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}) \]

With position observations \( \mathbf{z}_{j,k} = [x_j, y_j]^\top + \mathbf{v}_{j,k} \), \( \mathbf{v}_{j,k} \sim \mathcal{N}(\mathbf{0}, \mathbf{R}) \), a Kalman filter yields the posterior \( \mathbf{x}^o_{j,k} \sim \mathcal{N}(\boldsymbol{\mu}^o_{j,k}, \mathbf{P}^o_{j,k}) \). For prediction to step \( k+\ell \):

\[ \boldsymbol{\mu}^o_{j,k+\ell \mid k} = \mathbf{F}^\ell \boldsymbol{\mu}^o_{j,k}, \qquad \mathbf{P}^o_{j,k+\ell \mid k} = \mathbf{F}^\ell \mathbf{P}^o_{j,k} (\mathbf{F}^\ell)^\top + \sum_{i=0}^{\ell-1} \mathbf{F}^i \mathbf{Q} (\mathbf{F}^i)^\top. \]

In practice, crowd motion can be multi-modal. A compact way to represent this is a mixture: \( p(\mathbf{o}_{j,k+\ell}) = \sum_{m=1}^M \pi_m \, \mathcal{N}(\boldsymbol{\mu}_{m}, \mathbf{P}_{m}) \). A planner can handle this by: (i) choosing a conservative envelope, (ii) sampling modes, or (iii) allocating a risk budget \( \delta \) across modes.

For local planning we often use a low-order kinematic model. For a unicycle: robot state \( \mathbf{x}_k = [x_k, y_k, \theta_k]^\top \) and control \( \mathbf{u}_k = [v_k, \omega_k]^\top \) satisfy

\[ \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k) = \begin{bmatrix} x_k + \Delta t \, v_k \cos\theta_k \\ y_k + \Delta t \, v_k \sin\theta_k \\ \theta_k + \Delta t \, \omega_k \end{bmatrix}. \]

The canonical prediction-aware local objective over a horizon of \( N \) steps is

\[ J(\mathbf{u}_{0:N-1}) = \sum_{k=0}^{N-1} \Big( \|\mathbf{p}_k - \mathbf{p}^{\text{ref} }_k\|_{\mathbf{Q}_p}^2 + \|\mathbf{u}_k\|_{\mathbf{Q}_u}^2 + \lambda \, \rho_k \Big) + \|\mathbf{p}_N - \mathbf{p}_g\|_{\mathbf{Q}_g}^2, \]

where \( \mathbf{p}_k=[x_k,y_k]^\top \), \( \mathbf{p}_g \) is the local goal, and \( \rho_k \) is a risk term derived from predicted obstacle distributions (next section). The planner executes only the first control \( \mathbf{u}_0 \), then replans at the next timestep (receding horizon / MPC).

flowchart TD
  I["Inputs: robot state, local goal, tracks"] --> P1["Predict obstacles for horizon"]
  P1 --> P2["Generate candidate controls (samples or optimizer)"]
  P2 --> P3["Rollout robot motion for each candidate"]
  P3 --> P4["Compute cost: tracking + control + risk"]
  P4 --> SEL["Select best candidate"]
  SEL --> EX["Execute first control"]
  EX --> SH["Shift horizon and repeat"]
        

Deterministic collision avoidance enforces \( \|\mathbf{p}_k - \mathbf{o}_{j,k}\|_2 \ge R \) for all predicted times. With uncertainty, obstacle position becomes random. A probability-of-collision constraint is:

\[ \Pr\Big( \|\mathbf{p}_k - \mathbf{o}_{j,k}\|_2 \le R \Big) \le \delta, \qquad 0 < \delta < 1. \]

Exact evaluation is expensive (noncentral chi distribution). Instead we derive a conservative, planner-friendly bound. Let the relative position be the random vector \( \mathbf{r} = \mathbf{p}_k - \mathbf{o}_{j,k} \). Assume a Gaussian prediction \( \mathbf{r} \sim \mathcal{N}(\boldsymbol{\mu}, \mathbf{\Sigma}) \) (common in Kalman-based tracking), with \( \boldsymbol{\mu} = \mathbf{p}_k - \boldsymbol{\mu}^o_{j,k} \).

Define the unit direction \( \mathbf{n} = \boldsymbol{\mu} / \|\boldsymbol{\mu}\|_2 \). The scalar projection \( y = \mathbf{n}^\top \mathbf{r} \) is Gaussian: \( y \sim \mathcal{N}(m, s^2) \) where \( m=\mathbf{n}^\top \boldsymbol{\mu} = \|\boldsymbol{\mu}\|_2 \) and \( s^2=\mathbf{n}^\top \mathbf{\Sigma}\mathbf{n} \).

Since the collision event \( \|\mathbf{r}\|_2 \le R \) implies \( \mathbf{n}^\top \mathbf{r} \le R \), we get the conservative implication: \( \Pr(\|\mathbf{r}\|_2 \le R) \le \Pr(\mathbf{n}^\top \mathbf{r} \le R) \). Therefore it suffices to enforce \( \Pr(y \le R) \le \delta \).

\[ \Pr(y \le R) = \Phi\!\left( \frac{R - m}{s} \right) \le \delta \quad \Longleftrightarrow \quad m - R \ge z_{1-\delta} \, s, \]

where \( \Phi \) is the standard normal CDF and \( z_{1-\delta} = \Phi^{-1}(1-\delta) \). This yields a geometric “keep-out” margin: \( \|\boldsymbol{\mu}\|_2 \ge R + z_{1-\delta}\sqrt{\mathbf{n}^\top \mathbf{\Sigma}\mathbf{n} } \).

Proof (conservativeness):

If \( \|\mathbf{r}\|_2 \le R \), then for any unit vector \( \mathbf{n} \), by Cauchy–Schwarz, \( \mathbf{n}^\top \mathbf{r} \le \|\mathbf{n}\|_2 \|\mathbf{r}\|_2 \le R \). Hence the collision set is a subset of the half-space event \( \{\mathbf{n}^\top \mathbf{r} \le R\} \), implying the probability bound above.

In implementation, this constraint is often softened into a quadratic penalty \( \rho_k = \max(0, z_{1-\delta}s - (m-R))^2 \) to keep the planning problem smooth.

Besides explicit chance constraints, prediction enables richer objectives that improve fluency and reduce deadlocks:

• Time-to-collision (TTC): For predicted relative motion \( \mathbf{r}(t) \approx \mathbf{r}_0 + t \, \mathbf{v}_{rel} \), define the smallest \( t > 0 \) with \( \|\mathbf{r}(t)\|_2 = R \). Penalize small TTC to avoid “late braking.”
• Spatiotemporal clearance: A distance field \( d(x,y,t) \) on a 3D grid allows costs like \( \int_0^T \psi(d(\mathbf{p}(t),t)) \, dt \).
• Interaction-aware etiquette: In crowds, reduce predicted “intrusions” into personal space: \( \sum_j \exp(-\|\mathbf{p}-\boldsymbol{\mu}^o_j\|_2/\sigma_{ps}) \), using the predicted mean path rather than instantaneous positions.

A common engineering practice is to combine (i) a hard or soft chance-constraint safety term, and (ii) a smooth social/comfort term derived from predicted occupancy probability.

For sampling-based local planners (DWA-like), we can evaluate candidate rollouts directly. For optimization-based MPC, we often linearize dynamics around a nominal trajectory \( \bar{\mathbf{x} }_k, \bar{\mathbf{u} }_k \):

\[ \mathbf{x}_{k+1} \approx \bar{\mathbf{x} }_{k+1} + \mathbf{A}_k(\mathbf{x}_k - \bar{\mathbf{x} }_k) + \mathbf{B}_k(\mathbf{u}_k - \bar{\mathbf{u} }_k), \quad \mathbf{A}_k = \left.\frac{\partial f}{\partial \mathbf{x} }\right|_{\bar{\mathbf{x} }_k,\bar{\mathbf{u} }_k}, \ \mathbf{B}_k = \left.\frac{\partial f}{\partial \mathbf{u} }\right|_{\bar{\mathbf{x} }_k,\bar{\mathbf{u} }_k}. \]

With quadratic tracking and control terms, the resulting subproblem is a QP. The risk proxy from Section 4 can be approximated by a convex penalty if we fix \( \mathbf{n} \) using the current mean direction \( \mathbf{n} = \boldsymbol{\mu}/\|\boldsymbol{\mu}\|_2 \) (outer loop / SQP).

This gives a practical recipe used in many planners: (predict) → (linearize) → (solve) → (apply first control) → (repeat).

Prediction-aware local navigation is typically realized by adding two modules to the local planning loop:

1. Tracking + prediction: multi-target tracking (e.g., Kalman/UKF/particle filters) that outputs predicted means and covariances for the next \( N \) steps.
2. Time-indexed evaluation: local planner rollouts include timestamps; each rollout point queries predicted obstacle states at matching times, computing a risk-aware cost.

Related software libraries (typical choices):

• Python: ROS2 (rclpy), Nav2 interfaces, numpy, matplotlib; for optimization MPC: CasADi, OSQP.
• C++: ROS2 Nav2, Eigen (linear algebra), local planners (e.g., DWB/TEB-style), nonlinear solvers (IPOPT, SNOPT), trajectory optimization toolkits.
• Java: ROSJava (where available), EJML (linear algebra), Apache Commons Math (stats), custom local planner loops for research prototypes.
• MATLAB/Simulink: Robotics System Toolbox (ROS2), Navigation Toolbox, MPC Toolbox; Simulink for real-time testing (plant + controller + tracker).
• Wolfram Mathematica: rapid prototyping and analysis: NDSolve, InverseCDF, nonlinear optimization, symbolic checks of bounds.

The following reference implementations demonstrate a prediction-aware sampling MPC: we track obstacles (lightweight Gaussian model), predict their future mean/uncertainty, sample control pairs \( (v,\omega) \), roll out the robot for \( N \) steps, and score each rollout using (i) goal tracking, (ii) control regularization, and (iii) the chance-constraint penalty from Section 4.

Code: Chapter16_Lesson4.py

# Chapter16_Lesson4.py
# Prediction-Aware Local Navigation (sampling MPC with chance-constraint penalty)
# Dependencies: numpy, matplotlib (optional for plotting)

import math
import numpy as np

def wrap_angle(a: float) -> float:
    """Wrap angle to (-pi, pi]."""
    return (a + math.pi) % (2.0 * math.pi) - math.pi

class ObstacleTrack:
    """
    Simple 2D obstacle track with constant-velocity Gaussian prediction:
      state x = [px, py, vx, vy]^T
      covariance P (4x4)
    """
    def __init__(self, mean: np.ndarray, cov: np.ndarray, radius: float, name: str = "obs"):
        self.mean = mean.astype(float).copy().reshape(4)
        self.cov = cov.astype(float).copy().reshape(4, 4)
        self.radius = float(radius)
        self.name = name

    @staticmethod
    def from_position_velocity(px, py, vx, vy, pos_sigma=0.2, vel_sigma=0.5, radius=0.35, name="obs"):
        mu = np.array([px, py, vx, vy], dtype=float)
        P = np.diag([pos_sigma**2, pos_sigma**2, vel_sigma**2, vel_sigma**2])
        return ObstacleTrack(mu, P, radius=radius, name=name)

    def predict(self, dt: float, q_pos: float = 0.05, q_vel: float = 0.2) -> None:
        """
        One-step CV prediction:
          x_{k+1} = F x_k + w,  w ~ N(0,Q)
        """
        F = np.array([
            [1.0, 0.0, dt, 0.0],
            [0.0, 1.0, 0.0, dt],
            [0.0, 0.0, 1.0, 0.0],
            [0.0, 0.0, 0.0, 1.0],
        ], dtype=float)
        Q = np.diag([q_pos**2, q_pos**2, q_vel**2, q_vel**2]).astype(float)

        self.mean = F @ self.mean
        self.cov = F @ self.cov @ F.T + Q

def unicycle_step(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
    """
    x = [px, py, theta], u = [v, w]
    """
    px, py, th = float(x[0]), float(x[1]), float(x[2])
    v, w = float(u[0]), float(u[1])
    nx = px + dt * v * math.cos(th)
    ny = py + dt * v * math.sin(th)
    nth = wrap_angle(th + dt * w)
    return np.array([nx, ny, nth], dtype=float)

def chance_penalty_point(
    p_robot: np.ndarray,
    track_mu: np.ndarray,
    track_P: np.ndarray,
    R_safe: float,
    delta: float,
    eps: float = 1e-9,
) -> float:
    """
    Conservative 1D-projection chance-constraint penalty.

    Relative position r = p - o, where o ~ N(mu_o, Sigma_o)
    Treat p as deterministic, so r ~ N(mu_r, Sigma_o_pos).
    Let n = mu_r / ||mu_r||, y = n^T r ~ N(m, s^2), with:
      m = ||mu_r||, s^2 = n^T Sigma n
    Enforce m - R_safe >= z_{1-delta} s. Penalize violation squared.
    """
    # Use only 2D position block of obstacle covariance.
    Sigma = track_P[:2, :2]
    mu_o = track_mu[:2]
    mu_r = p_robot.reshape(2) - mu_o.reshape(2)
    dist = float(np.linalg.norm(mu_r)) + eps
    n = mu_r / dist

    m = dist
    s2 = float(n.T @ Sigma @ n)
    s = math.sqrt(max(s2, 0.0) + eps)

    # z_{1-delta} for common deltas (avoid scipy). Linear interp for nearby.
    # For delta in {0.1,0.05,0.02,0.01,0.005,0.001}
    z_table = {
        0.1: 1.281551565545,
        0.05: 1.644853626951,
        0.02: 2.053748910631,
        0.01: 2.326347874041,
        0.005: 2.575829303549,
        0.001: 3.090232306168,
    }
    if delta in z_table:
        z = z_table[delta]
    else:
        # crude fallback: clamp and interpolate on log10(delta)
        keys = sorted(z_table.keys())
        d = min(max(delta, keys[0]), keys[-1])
        # find bracketing
        lo = max([k for k in keys if k <= d])
        hi = min([k for k in keys if k >= d])
        if lo == hi:
            z = z_table[lo]
        else:
            t = (math.log10(d) - math.log10(lo)) / (math.log10(hi) - math.log10(lo))
            z = (1 - t) * z_table[lo] + t * z_table[hi]

    margin = z * s
    violation = max(0.0, margin - (m - R_safe))
    return violation * violation

def rollout_cost(
    x0: np.ndarray,
    u: np.ndarray,
    goal_xy: np.ndarray,
    tracks0: list,
    dt: float,
    N: int,
    weights: dict,
    delta: float,
    robot_radius: float,
) -> float:
    """
    Rollout with constant control u for N steps, while predicting obstacles forward.
    """
    x = x0.copy()
    # copy tracks so we can predict in-place
    tracks = [ObstacleTrack(t.mean.copy(), t.cov.copy(), t.radius, t.name) for t in tracks0]

    w_goal = float(weights.get("goal", 1.0))
    w_ctrl = float(weights.get("control", 0.05))
    w_risk = float(weights.get("risk", 8.0))

    cost = 0.0
    for k in range(N):
        # predict obstacles to this step (one-step ahead each loop)
        for t in tracks:
            t.predict(dt)

        x = unicycle_step(x, u, dt)
        p = x[:2]

        # quadratic goal tracking
        err = p - goal_xy.reshape(2)
        cost += w_goal * float(err.T @ err)

        # control regularization (constant control)
        cost += w_ctrl * float(u.T @ u)

        # risk penalty (sum over obstacles)
        risk_k = 0.0
        for t in tracks:
            R_safe = robot_radius + t.radius
            risk_k += chance_penalty_point(p, t.mean, t.cov, R_safe=R_safe, delta=delta)
        cost += w_risk * risk_k

    return cost

def sample_controls(v_range, w_range, n_v: int, n_w: int):
    vs = np.linspace(v_range[0], v_range[1], n_v)
    ws = np.linspace(w_range[0], w_range[1], n_w)
    for v in vs:
        for w in ws:
            yield np.array([v, w], dtype=float)

def prediction_aware_local_plan(
    x0: np.ndarray,
    goal_xy: np.ndarray,
    tracks: list,
    dt: float = 0.1,
    N: int = 20,
    v_range=(0.0, 1.0),
    w_range=(-1.5, 1.5),
    n_v: int = 11,
    n_w: int = 21,
    delta: float = 0.01,
    robot_radius: float = 0.25,
):
    """
    Returns best control u* from a sampled set, using prediction-aware risk scoring.
    """
    weights = {"goal": 1.0, "control": 0.05, "risk": 10.0}

    best_u = None
    best_J = float("inf")
    for u in sample_controls(v_range, w_range, n_v, n_w):
        J = rollout_cost(
            x0=x0,
            u=u,
            goal_xy=goal_xy,
            tracks0=tracks,
            dt=dt,
            N=N,
            weights=weights,
            delta=delta,
            robot_radius=robot_radius,
        )
        if J < best_J:
            best_J = J
            best_u = u
    return best_u, best_J

def demo():
    # Robot initial state and local goal
    x0 = np.array([0.0, 0.0, 0.0], dtype=float)
    goal = np.array([6.0, 0.0], dtype=float)

    # Two moving obstacles crossing the corridor
    tracks = [
        ObstacleTrack.from_position_velocity(px=3.0, py=1.0, vx=0.0, vy=-0.6, pos_sigma=0.15, vel_sigma=0.3, radius=0.35, name="p1"),
        ObstacleTrack.from_position_velocity(px=4.0, py=-1.2, vx=0.0, vy=0.7, pos_sigma=0.15, vel_sigma=0.3, radius=0.35, name="p2"),
    ]

    dt = 0.1
    N = 25
    u_star, J_star = prediction_aware_local_plan(
        x0=x0,
        goal_xy=goal,
        tracks=tracks,
        dt=dt,
        N=N,
        v_range=(0.0, 1.2),
        w_range=(-1.8, 1.8),
        n_v=13,
        n_w=31,
        delta=0.01,
        robot_radius=0.25,
    )
    print("Best control u* = [v, w] =", u_star, " cost =", J_star)

if __name__ == "__main__":
    demo()
      

Code: Chapter16_Lesson4.cpp

// Chapter16_Lesson4.cpp
// Prediction-Aware Local Navigation (sampling MPC with chance-constraint penalty)
// Minimal, self-contained (no Eigen). Compile: g++ -O2 -std=c++17 Chapter16_Lesson4.cpp -o demo

#include <cmath>
#include <cstddef>
#include <iostream>
#include <limits>
#include <string>
#include <utility>
#include <vector>

static double wrap_angle(double a) {
  const double pi = 3.14159265358979323846;
  a = std::fmod(a + pi, 2.0 * pi);
  if (a < 0.0) a += 2.0 * pi;
  return a - pi;
}

struct Vec2 {
  double x{0.0}, y{0.0};
};

struct Vec3 {
  double x{0.0}, y{0.0}, th{0.0};
};

struct Mat2 {
  // row-major
  double a11{0.0}, a12{0.0}, a21{0.0}, a22{0.0};
};

struct Track {
  // mean: [px, py, vx, vy]
  double px{0.0}, py{0.0}, vx{0.0}, vy{0.0};
  // covariance for position block (2x2) only (for penalty)
  Mat2 SigmaPos;
  double radius{0.35};
  std::string name{"obs"};

  void predict(double dt, double q_pos = 0.05, double q_vel = 0.2) {
    // constant velocity mean update
    px += dt * vx;
    py += dt * vy;

    // crude covariance inflation (position grows with velocity uncertainty)
    // For teaching: SigmaPos <- SigmaPos + dt^2 * q_vel^2 I + q_pos^2 I
    const double add = (dt * q_vel) * (dt * q_vel) + q_pos * q_pos;
    SigmaPos.a11 += add;
    SigmaPos.a22 += add;
  }
};

static Vec3 unicycle_step(const Vec3& x, double v, double w, double dt) {
  Vec3 nx;
  nx.x = x.x + dt * v * std::cos(x.th);
  nx.y = x.y + dt * v * std::sin(x.th);
  nx.th = wrap_angle(x.th + dt * w);
  return nx;
}

static double z_value(double delta) {
  // table for z_{1-delta}
  if (std::abs(delta - 0.1) < 1e-12) return 1.281551565545;
  if (std::abs(delta - 0.05) < 1e-12) return 1.644853626951;
  if (std::abs(delta - 0.02) < 1e-12) return 2.053748910631;
  if (std::abs(delta - 0.01) < 1e-12) return 2.326347874041;
  if (std::abs(delta - 0.005) < 1e-12) return 2.575829303549;
  if (std::abs(delta - 0.001) < 1e-12) return 3.090232306168;
  // fallback
  return 2.326347874041;
}

static double chance_penalty_point(const Vec2& p, const Track& t, double R_safe, double delta) {
  // mu_r = p - mu_o
  const double mux = p.x - t.px;
  const double muy = p.y - t.py;
  const double dist = std::sqrt(mux * mux + muy * muy) + 1e-9;

  // n = mu / ||mu||
  const double nx = mux / dist;
  const double ny = muy / dist;

  // s^2 = n^T Sigma n
  const double s2 = nx * (t.SigmaPos.a11 * nx + t.SigmaPos.a12 * ny)
                  + ny * (t.SigmaPos.a21 * nx + t.SigmaPos.a22 * ny);
  const double s = std::sqrt(std::max(0.0, s2) + 1e-12);

  const double z = z_value(delta);
  const double margin = z * s;
  const double violation = std::max(0.0, margin - (dist - R_safe));
  return violation * violation;
}

static double rollout_cost(
    const Vec3& x0,
    double v,
    double w,
    const Vec2& goal,
    const std::vector<Track>& tracks0,
    double dt,
    int N,
    double delta,
    double robot_radius) {

  const double w_goal = 1.0;
  const double w_ctrl = 0.05;
  const double w_risk = 10.0;

  Vec3 x = x0;
  std::vector<Track> tracks = tracks0; // copy to predict in-place

  double cost = 0.0;
  for (int k = 0; k < N; ++k) {
    for (auto& t : tracks) t.predict(dt);

    x = unicycle_step(x, v, w, dt);

    const double ex = x.x - goal.x;
    const double ey = x.y - goal.y;
    cost += w_goal * (ex * ex + ey * ey);

    cost += w_ctrl * (v * v + w * w);

    Vec2 p{x.x, x.y};
    double risk_k = 0.0;
    for (const auto& t : tracks) {
      const double R_safe = robot_radius + t.radius;
      risk_k += chance_penalty_point(p, t, R_safe, delta);
    }
    cost += w_risk * risk_k;
  }
  return cost;
}

int main() {
  Vec3 x0{0.0, 0.0, 0.0};
  Vec2 goal{6.0, 0.0};

  std::vector<Track> tracks;
  Track p1;
  p1.px = 3.0; p1.py = 1.0; p1.vx = 0.0; p1.vy = -0.6; p1.radius = 0.35; p1.name = "p1";
  p1.SigmaPos = Mat2{0.15 * 0.15, 0.0, 0.0, 0.15 * 0.15};
  tracks.push_back(p1);

  Track p2;
  p2.px = 4.0; p2.py = -1.2; p2.vx = 0.0; p2.vy = 0.7; p2.radius = 0.35; p2.name = "p2";
  p2.SigmaPos = Mat2{0.15 * 0.15, 0.0, 0.0, 0.15 * 0.15};
  tracks.push_back(p2);

  const double dt = 0.1;
  const int N = 25;
  const double delta = 0.01;
  const double robot_radius = 0.25;

  const double vmin = 0.0, vmax = 1.2;
  const double wmin = -1.8, wmax = 1.8;
  const int n_v = 13;
  const int n_w = 31;

  double best_v = 0.0, best_w = 0.0;
  double best_J = std::numeric_limits<double>::infinity();

  for (int i = 0; i < n_v; ++i) {
    const double v = vmin + (vmax - vmin) * (static_cast<double>(i) / (n_v - 1));
    for (int j = 0; j < n_w; ++j) {
      const double w = wmin + (wmax - wmin) * (static_cast<double>(j) / (n_w - 1));
      const double J = rollout_cost(x0, v, w, goal, tracks, dt, N, delta, robot_radius);
      if (J < best_J) {
        best_J = J;
        best_v = v;
        best_w = w;
      }
    }
  }

  std::cout << "Best control u* = [v,w] = [" << best_v << ", " << best_w << "], cost = " << best_J << "\n";
  return 0;
}
      

Code: Chapter16_Lesson4.java

// Chapter16_Lesson4.java
// Prediction-Aware Local Navigation (sampling MPC with chance-constraint penalty)
// Minimal self-contained Java. Compile: javac Chapter16_Lesson4.java ; Run: java Chapter16_Lesson4

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

public class Chapter16_Lesson4 {

  static double wrapAngle(double a) {
    final double pi = Math.PI;
    a = (a + pi) % (2.0 * pi);
    if (a < 0.0) a += 2.0 * pi;
    return a - pi;
  }

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

  static class Vec3 {
    double x, y, th;
    Vec3(double x, double y, double th) { this.x = x; this.y = y; this.th = th; }
  }

  static class Mat2 {
    // row-major
    double a11, a12, a21, a22;
    Mat2(double a11, double a12, double a21, double a22) {
      this.a11 = a11; this.a12 = a12; this.a21 = a21; this.a22 = a22;
    }
  }

  static class Track {
    double px, py, vx, vy;
    Mat2 SigmaPos;
    double radius;
    String name;

    Track(double px, double py, double vx, double vy, Mat2 SigmaPos, double radius, String name) {
      this.px = px; this.py = py; this.vx = vx; this.vy = vy;
      this.SigmaPos = SigmaPos;
      this.radius = radius;
      this.name = name;
    }

    void predict(double dt, double qPos, double qVel) {
      px += dt * vx;
      py += dt * vy;
      double add = (dt * qVel) * (dt * qVel) + qPos * qPos;
      SigmaPos.a11 += add;
      SigmaPos.a22 += add;
    }
  }

  static Vec3 unicycleStep(Vec3 x, double v, double w, double dt) {
    double nx = x.x + dt * v * Math.cos(x.th);
    double ny = x.y + dt * v * Math.sin(x.th);
    double nth = wrapAngle(x.th + dt * w);
    return new Vec3(nx, ny, nth);
  }

  static double zValue(double delta) {
    if (Math.abs(delta - 0.1) < 1e-12) return 1.281551565545;
    if (Math.abs(delta - 0.05) < 1e-12) return 1.644853626951;
    if (Math.abs(delta - 0.02) < 1e-12) return 2.053748910631;
    if (Math.abs(delta - 0.01) < 1e-12) return 2.326347874041;
    if (Math.abs(delta - 0.005) < 1e-12) return 2.575829303549;
    if (Math.abs(delta - 0.001) < 1e-12) return 3.090232306168;
    return 2.326347874041; // fallback
  }

  static double chancePenaltyPoint(Vec2 p, Track t, double RSafe, double delta) {
    double mux = p.x - t.px;
    double muy = p.y - t.py;
    double dist = Math.sqrt(mux * mux + muy * muy) + 1e-9;

    double nx = mux / dist;
    double ny = muy / dist;

    double s2 = nx * (t.SigmaPos.a11 * nx + t.SigmaPos.a12 * ny)
              + ny * (t.SigmaPos.a21 * nx + t.SigmaPos.a22 * ny);
    double s = Math.sqrt(Math.max(0.0, s2) + 1e-12);

    double z = zValue(delta);
    double margin = z * s;
    double violation = Math.max(0.0, margin - (dist - RSafe));
    return violation * violation;
  }

  static double rolloutCost(
      Vec3 x0,
      double v,
      double w,
      Vec2 goal,
      List<Track> tracks0,
      double dt,
      int N,
      double delta,
      double robotRadius) {

    double wGoal = 1.0;
    double wCtrl = 0.05;
    double wRisk = 10.0;

    Vec3 x = new Vec3(x0.x, x0.y, x0.th);
    List<Track> tracks = new ArrayList<>();
    for (Track t : tracks0) {
      tracks.add(new Track(t.px, t.py, t.vx, t.vy,
          new Mat2(t.SigmaPos.a11, t.SigmaPos.a12, t.SigmaPos.a21, t.SigmaPos.a22),
          t.radius, t.name));
    }

    double cost = 0.0;
    for (int k = 0; k < N; ++k) {
      for (Track t : tracks) t.predict(dt, 0.05, 0.2);

      x = unicycleStep(x, v, w, dt);

      double ex = x.x - goal.x;
      double ey = x.y - goal.y;
      cost += wGoal * (ex * ex + ey * ey);

      cost += wCtrl * (v * v + w * w);

      Vec2 p = new Vec2(x.x, x.y);
      double risk = 0.0;
      for (Track t : tracks) {
        double RSafe = robotRadius + t.radius;
        risk += chancePenaltyPoint(p, t, RSafe, delta);
      }
      cost += wRisk * risk;
    }
    return cost;
  }

  public static void main(String[] args) {
    Vec3 x0 = new Vec3(0.0, 0.0, 0.0);
    Vec2 goal = new Vec2(6.0, 0.0);

    List<Track> tracks = new ArrayList<>();
    tracks.add(new Track(3.0, 1.0, 0.0, -0.6, new Mat2(0.15*0.15, 0.0, 0.0, 0.15*0.15), 0.35, "p1"));
    tracks.add(new Track(4.0, -1.2, 0.0, 0.7, new Mat2(0.15*0.15, 0.0, 0.0, 0.15*0.15), 0.35, "p2"));

    double dt = 0.1;
    int N = 25;
    double delta = 0.01;
    double robotRadius = 0.25;

    double vmin = 0.0, vmax = 1.2;
    double wmin = -1.8, wmax = 1.8;
    int nV = 13, nW = 31;

    double bestV = 0.0, bestW = 0.0;
    double bestJ = Double.POSITIVE_INFINITY;

    for (int i = 0; i < nV; ++i) {
      double v = vmin + (vmax - vmin) * (i / (double)(nV - 1));
      for (int j = 0; j < nW; ++j) {
        double w = wmin + (wmax - wmin) * (j / (double)(nW - 1));
        double J = rolloutCost(x0, v, w, goal, tracks, dt, N, delta, robotRadius);
        if (J < bestJ) {
          bestJ = J;
          bestV = v;
          bestW = w;
        }
      }
    }

    System.out.println("Best control u* = [v,w] = [" + bestV + ", " + bestW + "], cost = " + bestJ);
  }
}
      

Code: Chapter16_Lesson4.m

% Chapter16_Lesson4.m
% Prediction-Aware Local Navigation (sampling MPC with chance-constraint penalty)
% Minimal MATLAB script (no toolboxes required).

function Chapter16_Lesson4()
  x0 = [0; 0; 0];           % [x;y;theta]
  goal = [6; 0];

  % Two moving obstacles (mean state + simple pos covariance)
  tracks(1).mu = [3; 1; 0; -0.6];
  tracks(1).Sigma = diag([0.15^2, 0.15^2, 0.3^2, 0.3^2]);
  tracks(1).r = 0.35;

  tracks(2).mu = [4; -1.2; 0; 0.7];
  tracks(2).Sigma = diag([0.15^2, 0.15^2, 0.3^2, 0.3^2]);
  tracks(2).r = 0.35;

  dt = 0.1; N = 25;
  delta = 0.01;
  robot_r = 0.25;

  v_range = [0, 1.2];
  w_range = [-1.8, 1.8];
  n_v = 13; n_w = 31;

  bestJ = inf; bestU = [0;0];
  for i = 1:n_v
    v = v_range(1) + (v_range(2)-v_range(1))*(i-1)/(n_v-1);
    for j = 1:n_w
      w = w_range(1) + (w_range(2)-w_range(1))*(j-1)/(n_w-1);
      J = rollout_cost(x0, [v; w], goal, tracks, dt, N, delta, robot_r);
      if J < bestJ
        bestJ = J;
        bestU = [v; w];
      end
    end
  end

  fprintf('Best control u* = [v,w] = [%.3f, %.3f], cost = %.3f\n', bestU(1), bestU(2), bestJ);
end

function x1 = unicycle_step(x, u, dt)
  v = u(1); w = u(2);
  x1 = zeros(3,1);
  x1(1) = x(1) + dt*v*cos(x(3));
  x1(2) = x(2) + dt*v*sin(x(3));
  x1(3) = wrapToPi(x(3) + dt*w);
end

function tracks1 = predict_tracks(tracks, dt)
  % constant velocity prediction for mean, covariance inflation
  F = [1 0 dt 0;
       0 1 0 dt;
       0 0 1 0;
       0 0 0 1];
  Q = diag([0.05^2, 0.05^2, 0.2^2, 0.2^2]);
  tracks1 = tracks;
  for k = 1:numel(tracks)
    tracks1(k).mu = F*tracks(k).mu;
    tracks1(k).Sigma = F*tracks(k).Sigma*F' + Q;
  end
end

function pen = chance_penalty_point(p, mu_o, Sigma_o, R_safe, delta)
  Sigma = Sigma_o(1:2,1:2);
  mu_r = p - mu_o(1:2);
  dist = norm(mu_r) + 1e-9;
  n = mu_r / dist;

  m = dist;
  s2 = n'*Sigma*n;
  s = sqrt(max(0,s2) + 1e-12);

  z = z_value(delta);
  margin = z*s;
  violation = max(0, margin - (m - R_safe));
  pen = violation^2;
end

function z = z_value(delta)
  % z_{1-delta} table
  if abs(delta-0.1) < 1e-12, z = 1.281551565545;
  elseif abs(delta-0.05) < 1e-12, z = 1.644853626951;
  elseif abs(delta-0.02) < 1e-12, z = 2.053748910631;
  elseif abs(delta-0.01) < 1e-12, z = 2.326347874041;
  elseif abs(delta-0.005) < 1e-12, z = 2.575829303549;
  elseif abs(delta-0.001) < 1e-12, z = 3.090232306168;
  else, z = 2.326347874041; % fallback
  end
end

function J = rollout_cost(x0, u, goal, tracks0, dt, N, delta, robot_r)
  w_goal = 1.0; w_ctrl = 0.05; w_risk = 10.0;

  x = x0; tracks = tracks0;
  J = 0;
  for k = 1:N
    tracks = predict_tracks(tracks, dt);
    x = unicycle_step(x, u, dt);

    e = x(1:2) - goal;
    J = J + w_goal*(e'*e) + w_ctrl*(u'*u);

    risk = 0;
    for j = 1:numel(tracks)
      R_safe = robot_r + tracks(j).r;
      risk = risk + chance_penalty_point(x(1:2), tracks(j).mu, tracks(j).Sigma, R_safe, delta);
    end
    J = J + w_risk*risk;
  end
end
      

Code: Chapter16_Lesson4.nb

(* ::Package:: *)

(* Chapter16_Lesson4.nb
   Prediction-Aware Local Navigation (sampling MPC with chance-constraint penalty)
   Mathematica / Wolfram Language script-style content. *)

ClearAll["Global`*"];

wrapAngle[a_] := Module[{pi = Pi, b = Mod[a + Pi, 2 Pi]},
  If[b < 0, b = b + 2 Pi];
  b - Pi
];

unicycleStep[x_, u_, dt_] := Module[{px, py, th, v, w},
  {px, py, th} = x; {v, w} = u;
  {
    px + dt*v*Cos[th],
    py + dt*v*Sin[th],
    wrapAngle[th + dt*w]
  }
];

zValue[delta_] := Which[
  Abs[delta - 0.1] < 10^-12, 1.281551565545,
  Abs[delta - 0.05] < 10^-12, 1.644853626951,
  Abs[delta - 0.02] < 10^-12, 2.053748910631,
  Abs[delta - 0.01] < 10^-12, 2.326347874041,
  Abs[delta - 0.005] < 10^-12, 2.575829303549,
  Abs[delta - 0.001] < 10^-12, 3.090232306168,
  True, 2.326347874041
];

predictTrack[track_, dt_] := Module[{F, Q, mu, Sigma},
  F = { {1, 0, dt, 0},
       {0, 1, 0, dt},
       {0, 0, 1, 0},
       {0, 0, 0, 1} };
  Q = DiagonalMatrix[{0.05^2, 0.05^2, 0.2^2, 0.2^2}];
  mu = F.track["mu"];
  Sigma = F.track["Sigma"].Transpose[F] + Q;
  Association[track, "mu" -> mu, "Sigma" -> Sigma]
];

chancePenaltyPoint[p_, track_, RSafe_, delta_] := Module[
  {muO, Sigma2, muR, dist, n, m, s2, s, z, margin, violation},
  muO = track["mu"][[{1, 2}]];
  Sigma2 = track["Sigma"][[{1, 2}, {1, 2}]];
  muR = p - muO;
  dist = Norm[muR] + 10^-9;
  n = muR/dist;
  m = dist;
  s2 = n.Sigma2.n;
  s = Sqrt[Max[0, s2] + 10^-12];
  z = zValue[delta];
  margin = z*s;
  violation = Max[0, margin - (m - RSafe)];
  violation^2
];

rolloutCost[x0_, u_, goal_, tracks0_, dt_, N_, delta_, robotR_] := Module[
  {wGoal = 1.0, wCtrl = 0.05, wRisk = 10.0, x = x0, tracks = tracks0, J = 0.0, k, e, risk},
  For[k = 1, k <= N, k++,
    tracks = predictTrack[#, dt] & /@ tracks;
    x = unicycleStep[x, u, dt];
    e = x[[{1, 2}]] - goal;
    J = J + wGoal*(e.e) + wCtrl*(u.u);
    risk = Total@Table[
      chancePenaltyPoint[x[[{1, 2}]], tracks[[j]], robotR + tracks[[j]]["r"], delta],
      {j, Length[tracks]}
    ];
    J = J + wRisk*risk;
  ];
  J
];

(* Demo setup *)
x0 = {0.0, 0.0, 0.0};
goal = {6.0, 0.0};
dt = 0.1; N = 25; delta = 0.01; robotR = 0.25;

tracks = {
  <|"mu" -> {3.0, 1.0, 0.0, -0.6},
    "Sigma" -> DiagonalMatrix[{0.15^2, 0.15^2, 0.3^2, 0.3^2}],
    "r" -> 0.35, "name" -> "p1"|>,
  <|"mu" -> {4.0, -1.2, 0.0, 0.7},
    "Sigma" -> DiagonalMatrix[{0.15^2, 0.15^2, 0.3^2, 0.3^2}],
    "r" -> 0.35, "name" -> "p2"|>
};

vRange = {0.0, 1.2};
wRange = {-1.8, 1.8};
nV = 13; nW = 31;

bestU = {0.0, 0.0};
bestJ = Infinity;

Do[
  v = vRange[[1]] + (vRange[[2]] - vRange[[1]])*(i - 1)/(nV - 1);
  Do[
    w = wRange[[1]] + (wRange[[2]] - wRange[[1]])*(j - 1)/(nW - 1);
    J = rolloutCost[x0, {v, w}, goal, tracks, dt, N, delta, robotR];
    If[J < bestJ, bestJ = J; bestU = {v, w};],
    {j, 1, nW}
  ],
  {i, 1, nV}
];

Print["Best control u* = [v,w] = ", bestU, ", cost = ", bestJ];
      

Problem 1 (Gaussian prediction propagation): Assume an obstacle CV model \( \mathbf{x}^o_{k+1}=\mathbf{F}\mathbf{x}^o_k+\mathbf{w}_k \), \( \mathbf{w}_k \sim \mathcal{N}(\mathbf{0},\mathbf{Q}) \). Prove the prediction recursion \( \boldsymbol{\mu}_{k+1|k}=\mathbf{F}\boldsymbol{\mu}_{k|k} \), \( \mathbf{P}_{k+1|k}=\mathbf{F}\mathbf{P}_{k|k}\mathbf{F}^\top+\mathbf{Q} \).

Solution: By linearity, \( E[\mathbf{x}_{k+1}] = E[\mathbf{F}\mathbf{x}_k+\mathbf{w}_k] = \mathbf{F}E[\mathbf{x}_k]+E[\mathbf{w}_k] \) and \( E[\mathbf{w}_k]=\mathbf{0} \), hence the mean recursion. For covariance, \( \mathbf{P}_{k+1}=E[(\mathbf{x}_{k+1}-\boldsymbol{\mu}_{k+1})(\cdot)^\top] \). Substitute \( \mathbf{x}_{k+1}-\boldsymbol{\mu}_{k+1}=\mathbf{F}(\mathbf{x}_k-\boldsymbol{\mu}_k)+\mathbf{w}_k \). Using independence of \( \mathbf{w}_k \) and \( \mathbf{x}_k \):

\[ \mathbf{P}_{k+1} = \mathbf{F}\mathbf{P}_k\mathbf{F}^\top + \mathbf{Q} + E[\mathbf{F}(\mathbf{x}_k-\boldsymbol{\mu}_k)\mathbf{w}_k^\top] + E[\mathbf{w}_k(\mathbf{x}_k-\boldsymbol{\mu}_k)^\top\mathbf{F}^\top] = \mathbf{F}\mathbf{P}_k\mathbf{F}^\top + \mathbf{Q}. \]

Problem 2 (Chance-constraint bound): Let \( y \sim \mathcal{N}(m,s^2) \). Show that \( \Pr(y \le R) \le \delta \) is equivalent to \( m - R \ge z_{1-\delta} s \).

Solution: Standardize: \( (y-m)/s \sim \mathcal{N}(0,1) \). Then \( \Pr(y \le R) = \Pr((y-m)/s \le (R-m)/s) = \Phi((R-m)/s) \). The inequality \( \Phi((R-m)/s) \le \delta \) implies \( (R-m)/s \le \Phi^{-1}(\delta) = -z_{1-\delta} \), hence \( m-R \ge z_{1-\delta}s \).

Problem 3 (Conservativeness of the projection test): Prove that for any random vector \( \mathbf{r} \in \mathbb{R}^2 \) and any unit \( \mathbf{n} \), the event \( \{\|\mathbf{r}\|_2 \le R\} \) implies \( \{\mathbf{n}^\top \mathbf{r} \le R\} \), and thus \( \Pr(\|\mathbf{r}\|_2 \le R) \le \Pr(\mathbf{n}^\top \mathbf{r} \le R) \).

Solution: If \( \|\mathbf{r}\|_2 \le R \), then by Cauchy–Schwarz, \( \mathbf{n}^\top \mathbf{r} \le \|\mathbf{n}\|_2\|\mathbf{r}\|_2 = \|\mathbf{r}\|_2 \le R \) since \( \|\mathbf{n}\|_2=1 \). Hence the implication of events, and probability monotonicity yields the inequality.

Problem 4 (Risk budget across time): Suppose you want an overall collision probability bound across the horizon: \( \Pr(\text{any collision in } k=1,\dots,N) \le \delta_{\text{tot} } \). Using the union bound, propose per-step risk allocations \( \delta_k \) that guarantee the bound.

Solution: Let \( A_k \) be the collision event at step \( k \). By union bound, \( \Pr(\cup_{k=1}^N A_k) \le \sum_{k=1}^N \Pr(A_k) \). If we enforce \( \Pr(A_k) \le \delta_k \) and choose \( \sum_{k=1}^N \delta_k \le \delta_{\text{tot} } \), then \( \Pr(\cup_k A_k) \le \delta_{\text{tot} } \). A simple allocation is uniform: \( \delta_k = \delta_{\text{tot} }/N \). A more conservative near-term allocation uses, for example, \( \delta_k \propto \gamma^{k-1} \) with \( 0<\gamma<1 \), normalized to sum to \( \delta_{\text{tot} } \).

Problem 5 (Sampling resolution trade-off): In a sampling planner, assume \( n_v \) speed samples, \( n_\omega \) turn-rate samples, horizon length \( N \), and \( M \) obstacles. Give the computational complexity per replanning step.

Solution: Each candidate control pair costs a rollout of \( N \) steps. At each step, the risk term checks \( M \) obstacles (and constant-time arithmetic). Therefore the dominant cost is \( \mathcal{O}(n_v n_\omega N M) \). This motivates: parallel evaluation, adaptive sampling, pruning unsafe rollouts early, and limiting \( M \) to nearby dynamic obstacles.

Problem 6 (Linearization Jacobians): For the unicycle update \( x_{k+1}=x_k+\Delta t v_k\cos\theta_k \), \( y_{k+1}=y_k+\Delta t v_k\sin\theta_k \), \( \theta_{k+1}=\theta_k+\Delta t \omega_k \), compute \( \mathbf{A}_k \) and \( \mathbf{B}_k \).

Solution: Let \( \mathbf{x}=[x,y,\theta]^\top \), \( \mathbf{u}=[v,\omega]^\top \). Then

\[ \mathbf{A}_k = \begin{bmatrix} 1 & 0 & -\Delta t\, v_k \sin\theta_k \\ 0 & 1 & \ \Delta t\, v_k \cos\theta_k \\ 0 & 0 & 1 \end{bmatrix}, \qquad \mathbf{B}_k = \begin{bmatrix} \Delta t \cos\theta_k & 0 \\ \Delta t \sin\theta_k & 0 \\ 0 & \Delta t \end{bmatrix}. \]

These Jacobians are used in SQP / iterative MPC to build local QP subproblems.

Prediction-aware local navigation augments the local planner with (i) tracking and probabilistic prediction of moving obstacles and (ii) time-indexed risk evaluation over a receding horizon. We derived a conservative and implementable chance-constraint bound \( m - R \ge z_{1-\delta} s \) based on a 1D projection, and showed how to embed it as a smooth penalty in sampling-based planners or as a constraint/penalty in MPC. This prepares you for Lesson 5, where we benchmark end-to-end navigation through crowds.