In AMR benchmarking, a single number such as \( \text{success rate} \) is insufficient because two planners may reach the same number of goals while exhibiting radically different safety margins, intervention frequency, or failure recovery behavior. Robustness evaluation must therefore use a metric family defined on repeated missions under controlled scenarios.

Let a benchmark scenario be repeated over \( N \) episodes. For episode \( i \), denote the logged trajectory and events by \( \mathcal{E}_i \). A robustness benchmark is a mapping

\[ \mathcal{M}:\{\mathcal{E}_i\}_{i=1}^{N} \mapsto \left(\mathbf{m}_{\text{binary}}, \mathbf{m}_{\text{continuous}}, \mathbf{m}_{\text{reliability}}, \mathbf{m}_{\text{aggregate}}\right). \]

The design principle is \( \text{separation of concerns} \): safety metrics should not be hidden inside efficiency metrics, and confidence intervals should be reported for proportions to avoid overinterpreting small benchmark sets.

flowchart TD
  A["Scenario set + robot config"] --> B["Run repeated episodes"]
  B --> C["Log states, commands, events"]
  C --> D["Compute binary metrics"]
  C --> E["Compute continuous margins"]
  C --> F["Compute reliability curves"]
  D --> G["Statistical uncertainty"]
  E --> G
  F --> G
  G --> H["Protocol report + leaderboard table"]
        

Assume a sampled mission log at times \( t_k = k\Delta t \), with robot pose samples \( \mathbf{p}_k=[x_k,y_k]^\top \), control commands \( \mathbf{u}_k=[v_k,\omega_k]^\top \), and event flags. We define the per-step log tuple:

\[ \ell_k = \left(t_k,\mathbf{p}_k,\mathbf{u}_k,c_k,e_k^{\text{col}},e_k^{\text{int}},e_k^{\text{rec}},e_k^{\text{goal}}\right), \]

where \( c_k \) is clearance (meters), \( e_k^{\text{col}} \in \{0,1\} \) collision flag, \( e_k^{\text{int}} \in \{0,1\} \) external intervention flag, \( e_k^{\text{rec}} \in \{0,1\} \) recovery-event flag, and \( e_k^{\text{goal}} \in \{0,1\} \) goal reached flag.

For episode \( i \), the logged sequence is \( \mathcal{E}_i = \{\ell_k^{(i)}\}_{k=0}^{K_i} \). The benchmark protocol must fix:

• Scenario geometry and obstacle dynamics
• Robot footprint and safety inflation
• Termination rules (goal reached, timeout, collision, manual stop)
• Reset policy and intervention definition
• Sampling rate and logging interface

Without a fixed protocol, metric values are not comparable across papers or implementations.

Define per-episode indicators:

\[ S_i = \mathbf{1}\{\text{goal reached and no terminal unsafe event}\}, \quad C_i = \mathbf{1}\{\text{no collision in episode } i\}, \\ I_i = \mathbf{1}\{\text{no intervention in episode } i\}. \]

The empirical rates are \( \hat{p}_S = \frac{1}{N}\sum_{i=1}^{N} S_i \), \( \hat{p}_C = \frac{1}{N}\sum_{i=1}^{N} C_i \), and \( \hat{p}_I = \frac{1}{N}\sum_{i=1}^{N} I_i \).

Since these are Bernoulli proportions, uncertainty should be reported. A robust choice is the Wilson interval (better coverage than the plain normal approximation when \( N \) is moderate):

\[ \hat{p}_{W} = \frac{\hat{p} + \frac{z^2}{2N}}{1 + \frac{z^2}{N}}, \qquad r_{W} = \frac{z}{1 + \frac{z^2}{N}} \sqrt{ \frac{\hat{p}(1-\hat{p})}{N} + \frac{z^2}{4N^2} }, \]

\[ \text{CI}_{1-\alpha}^{\text{Wilson}} = [\hat{p}_{W} - r_W,\; \hat{p}_{W} + r_W], \qquad z = z_{1-\alpha/2}. \]

Interpretation: for navigation robustness, \( \hat{p}_C \) and \( \hat{p}_I \) often matter more than \( \hat{p}_S \) in safety-critical systems, because a fast but collision-prone planner can still score well on success-only leaderboards.

Sample-size bound (Hoeffding): if episodes are i.i.d. Bernoulli for a metric \( X_i \in \{0,1\} \), then

\[ \Pr\left(|\hat{p}-p| \ge \varepsilon\right) \le 2\exp(-2N\varepsilon^2). \]

Therefore, to guarantee \( |\hat{p}-p| < \varepsilon \) with probability at least \( 1-\delta \), it is sufficient that

\[ N \ge \frac{1}{2\varepsilon^2}\log\!\left(\frac{2}{\delta}\right). \]

Binary metrics do not reveal how close the robot operated to failure. We therefore define continuous margins.

Clearance profile: Let \( c_k \) be obstacle clearance after footprint inflation. Episode-level safety margins include:

\[ c_{\min}^{(i)} = \min_{0 \le k \le K_i} c_k, \qquad c_{q}^{(i)} = \text{quantile}_q\left(\{c_k\}_{k=0}^{K_i}\right). \]

The lower quantile (e.g., \( q=0.05 \)) is usually more stable than the minimum, which can be dominated by a single sensor glitch.

Tracking quality: with path-tracking error samples \( e_k \), define

\[ e_{\text{RMSE}}^{(i)} = \sqrt{\frac{1}{K_i+1}\sum_{k=0}^{K_i} e_k^2 }. \]

Actuation saturation ratio: if command bounds are \( |v_k| \le v_{\max,k} \) and \( |\omega_k| \le \omega_{\max,k} \), define

\[ \rho_{\text{sat}}^{(i)} = \frac{1}{K_i+1}\sum_{k=0}^{K_i} \mathbf{1}\left\{ |v_k| \ge \eta_v v_{\max,k} \;\vee\; |\omega_k| \ge \eta_\omega \omega_{\max,k} \right\}, \]

where \( \eta_v,\eta_\omega \in (0,1) \) are protocol thresholds (commonly near 0.95–0.99). A high saturation ratio signals fragility, aggressive control, or poor planner-controller matching.

Proposition (hidden-collision exclusion under sampled clearance): Suppose the continuous clearance function \( c(t) \) is Lipschitz with constant \( L_c \), i.e., \( |c(t)-c(s)| \le L_c |t-s| \). If at sample time \( t_k \) we have \( c(t_k) > L_c \Delta t \), then \( c(t) > 0 \) for all \( t \in [t_k,t_{k+1}] \).

Proof: For any \( t \in [t_k,t_{k+1}] \), Lipschitz continuity gives \( c(t) \ge c(t_k) - L_c|t-t_k| \). Since \( |t-t_k| \le \Delta t \), \( c(t) \ge c(t_k) - L_c\Delta t > 0 \). Thus no unobserved collision can occur between samples. \( \square \)

This result is important when comparing planners at different control rates: lower logging rates can systematically under-report near-collision events unless a sampling bound is stated.

Robust navigation must still remain operationally efficient. For successful episodes, define completion time \( T_i = t_{K_i}-t_0 \) and path length \( L_i = \sum_{k=1}^{K_i}\|\mathbf{p}_k-\mathbf{p}_{k-1}\|_2 \).

Let \( d_i^{\text{ref}} \) be a reference distance (often start-to-goal Euclidean distance for local tests, or global path length for full-stack tests). The path efficiency metric is

\[ \eta_i = \begin{cases} \min\!\left(1,\dfrac{d_i^{\text{ref}}}{L_i}\right), & \text{if episode } i \text{ succeeds} \\ 0, & \text{otherwise.} \end{cases} \]

Bound proof: For a successful trajectory, the polygonal path length satisfies \( L_i \ge \|\mathbf{p}_{K_i}-\mathbf{p}_0\|_2 \) by the triangle inequality. If \( d_i^{\text{ref}} \) is chosen as a lower bound on any feasible path length (e.g., Euclidean start-goal distance), then \( 0 \le d_i^{\text{ref}}/L_i \le 1 \). The clipping only protects numerical edge cases. \( \square \)

Reporting \( (\hat{p}_S, \bar{T}, \bar{\eta}) \) together prevents pathological rankings where a planner is “safe” only because it barely moves.

Binary end-of-episode metrics discard when failures occur. For robustness engineering, define episode failure time \( \tau_i \) as the first time a collision or intervention occurs, with right-censoring if no failure occurs before episode termination.

The survival function is \( S_f(t)=\Pr(\tau > t) \), estimated nonparametrically by Kaplan–Meier:

\[ \widehat{S}_f(t) = \prod_{t_j \le t}\left(1-\frac{d_j}{n_j}\right), \]

where \( d_j \) is the number of failures at event time \( t_j \) and \( n_j \) is the risk set size just before \( t_j \).

The hazard rate (instantaneous failure tendency) is

\[ h(t) = \lim_{\Delta t \downarrow 0} \frac{\Pr(t \le \tau < t+\Delta t \mid \tau \ge t)}{\Delta t}. \]

In AMR benchmarking, comparing \( \widehat{S}_f(t) \) curves can reveal whether one planner fails early in cluttered passages while another degrades only on long runs (e.g., due to drift or local minima).

Leaderboards often require a single scalar. A principled composite score should use normalized terms with protocol-fixed anchors:

\[ r_C = \hat{p}_C,\quad r_I = \hat{p}_I,\quad r_S = \hat{p}_S,\quad r_{\text{clr}} = \frac{1}{N}\sum_{i=1}^{N}\operatorname{clip}\!\left(\frac{c_{0.05}^{(i)}}{c_{\text{safe}}},0,1\right), \]

\[ r_{\text{trk}} = \frac{1}{N}\sum_{i=1}^{N}\exp\!\left(-\frac{e_{\text{RMSE}}^{(i)}}{e_0}\right), \qquad r_T = \frac{1}{N}\sum_{i=1}^{N}\exp\!\left(-\frac{T_i}{T_0}\right)S_i, \qquad r_{\eta} = \frac{1}{N}\sum_{i=1}^{N}\eta_i. \]

A weighted composite is then

\[ R = w_C r_C + w_I r_I + w_S r_S + w_{\text{clr}}r_{\text{clr}} + w_{\eta}r_{\eta} + w_{\text{trk}}r_{\text{trk}} + w_T r_T, \]

\[ w_j \ge 0,\qquad \sum_j w_j = 1. \]

Proposition (boundedness): If each component score \( r_j \in [0,1] \) and weights satisfy the simplex constraints above, then \( R \in [0,1] \).

Proof: Since \( w_j \ge 0 \) and \( r_j \ge 0 \), we have \( R \ge 0 \). Also, because \( r_j \le 1 \), \( R = \sum_j w_j r_j \le \sum_j w_j = 1 \). Therefore \( 0 \le R \le 1 \). \( \square \)

Important protocol rule: anchors \( c_{\text{safe}}, e_0, T_0 \) and weights \( w_j \) must be fixed before comparing methods. Tuning them per planner invalidates benchmarking.

flowchart TD
  A["Episode logs"] --> B["Binary rates: \nsuccess / collision-free / \nintervention-free"]
  A --> C["Continuous scores: \nclearance / tracking / \nefficiency / time"]
  A --> D["Reliability: failure-time survival"]
  B --> E["Normalize to [0,1] with protocol anchors"]
  C --> E
  D --> F["Report separately (do not hide)"]
  E --> G["Weighted composite score R"]
  G --> H["Table + uncertainty intervals"]
  F --> H
        

Suppose two planners \( A \) and \( B \) are evaluated on the same scenario set. If episodes are paired by seed or obstacle realization, a paired analysis is more powerful than an unpaired analysis. Let \( Z_i = m_i^{(A)} - m_i^{(B)} \) be a paired metric difference (for example, path efficiency or completion time on successful paired trials).

A confidence interval for the mean paired difference is

\[ \bar{Z} \pm t_{1-\alpha/2,\;N-1}\frac{s_Z}{\sqrt{N}}, \]

where \( s_Z \) is the sample standard deviation of \( Z_i \). For heavy-tailed metrics (common in navigation due to timeouts), bootstrap confidence intervals are often more robust:

\[ \widehat{\Delta}^{\ast b} = \frac{1}{N}\sum_{i=1}^{N} Z_{I_i^{(b)}}, \qquad I_i^{(b)} \sim \text{Unif}\{1,\dots,N\}, \]

with \( b=1,\dots,B \) bootstrap replicates and percentile bounds from the empirical distribution of \( \widehat{\Delta}^{\ast b} \).

For binary outcomes (success, collision-free), paired tests such as McNemar’s test are appropriate when exactly the same episodes are matched across planners.

The following reference implementations compute a practical robustness metric suite from navigation episode logs. They include Wilson intervals, clearance/tracking metrics, path efficiency, intervention/collision rates, and a protocol-fixed composite score.

9.1 Python Implementation — Chapter19_Lesson2.py

Uses numpy and pandas. In a real AMR stack, CSV logs can be exported from ROS 2 / Nav2 bags (odom, cmd_vel, costmap-clearance proxies, and behavior-tree events).

# Chapter19_Lesson2.py
"""
Metrics for Navigation Robustness (Autonomous Mobile Robots)
------------------------------------------------------------
This script computes mission-level robustness metrics from an episode log.

CSV schema (one row per control cycle, sorted by episode_id,time_s):
episode_id,time_s,x,y,goal_x,goal_y,clearance_m,collision,intervention,
goal_reached,tracking_error_m,cmd_v,cmd_v_max,cmd_w,cmd_w_max,recovery_event

Notes:
- collision, intervention, goal_reached, recovery_event are 0/1.
- If no CSV is supplied, the script generates a synthetic dataset and evaluates it.

Robotics library integration notes:
- ROS 2 / Nav2: export odom, cmd_vel, local_costmap clearance, and behavior-tree events
  into this CSV using rosbag2 + a small conversion node/script.
- Python stack: numpy, pandas are used for numerical/statistical processing.
"""

from __future__ import annotations
import sys
from dataclasses import dataclass
from typing import Dict, List, Tuple
import numpy as np
import pandas as pd


@dataclass
class WilsonInterval:
    p_hat: float
    lower: float
    upper: float


def wilson_interval(k: int, n: int, z: float = 1.96) -> WilsonInterval:
    if n <= 0:
        return WilsonInterval(np.nan, np.nan, np.nan)
    p = k / n
    denom = 1.0 + (z * z) / n
    center = (p + (z * z) / (2.0 * n)) / denom
    radius = (z / denom) * np.sqrt((p * (1.0 - p) / n) + (z * z) / (4.0 * n * n))
    return WilsonInterval(p, center - radius, center + radius)


def generate_synthetic_log(num_episodes: int = 20, seed: int = 7) -> pd.DataFrame:
    rng = np.random.default_rng(seed)
    rows = []
    for ep in range(num_episodes):
        T = int(rng.integers(120, 280))
        dt = 0.1
        x, y = 0.0, 0.0
        goal_x, goal_y = 10.0 + rng.normal(0, 0.5), rng.normal(0, 1.0)
        collision_happened = False
        intervention_happened = False
        recovered = False
        for k in range(T):
            t = k * dt
            dxg = goal_x - x
            dyg = goal_y - y
            dist = float(np.hypot(dxg, dyg))
            heading = np.arctan2(dyg, dxg)
            cmd_v_max = 0.8
            cmd_w_max = 1.2
            cmd_v = float(np.clip(0.55 + 0.05 * rng.normal(), -cmd_v_max, cmd_v_max))
            cmd_w = float(np.clip(0.4 * np.sin(0.05 * k) + 0.15 * rng.normal(), -cmd_w_max, cmd_w_max))
            x += cmd_v * dt * np.cos(heading) + 0.01 * rng.normal()
            y += cmd_v * dt * np.sin(heading) + 0.01 * rng.normal()

            # Synthetic clearance with occasional near-obstacle events
            base_clear = 0.9 + 0.25 * np.sin(0.08 * k + ep) + 0.12 * rng.normal()
            if rng.random() < 0.02:
                base_clear -= rng.uniform(0.4, 0.9)
            clearance = max(0.0, base_clear)

            collision = 1 if (clearance < 0.05 and rng.random() < 0.5) else 0
            intervention = 1 if (clearance < 0.12 and rng.random() < 0.2) else 0
            if collision:
                collision_happened = True
            if intervention:
                intervention_happened = True
            recovery_event = 0
            if (collision or intervention) and (not recovered) and rng.random() < 0.7:
                recovery_event = 1
                recovered = True

            tracking_error = abs(0.08 * np.sin(0.03 * k) + 0.04 * rng.normal())
            goal_reached = 1 if dist < 0.35 and not collision_happened else 0

            rows.append(
                [
                    ep, t, x, y, goal_x, goal_y, clearance, collision, intervention,
                    goal_reached, tracking_error, cmd_v, cmd_v_max, cmd_w, cmd_w_max, recovery_event
                ]
            )
            if goal_reached or collision:
                break

    cols = [
        "episode_id", "time_s", "x", "y", "goal_x", "goal_y", "clearance_m", "collision", "intervention",
        "goal_reached", "tracking_error_m", "cmd_v", "cmd_v_max", "cmd_w", "cmd_w_max", "recovery_event"
    ]
    return pd.DataFrame(rows, columns=cols)


def path_length(xs: np.ndarray, ys: np.ndarray) -> float:
    if len(xs) < 2:
        return 0.0
    dx = np.diff(xs)
    dy = np.diff(ys)
    return float(np.sum(np.sqrt(dx * dx + dy * dy)))


def kaplan_meier_failure_survival(episode_df: pd.DataFrame) -> pd.DataFrame:
    """
    Failure event = first collision or intervention in each episode.
    Right-censored if no failure before episode ends.
    """
    records = []
    for ep, g in episode_df.groupby("episode_id"):
        g = g.sort_values("time_s")
        failure_mask = (g["collision"].values > 0) | (g["intervention"].values > 0)
        if np.any(failure_mask):
            idx = int(np.argmax(failure_mask))
            t = float(g.iloc[idx]["time_s"])
            event = 1
        else:
            t = float(g["time_s"].iloc[-1])
            event = 0
        records.append((ep, t, event))
    T = pd.DataFrame(records, columns=["episode_id", "time", "event"]).sort_values("time")

    unique_times = sorted(T.loc[T["event"] == 1, "time"].unique().tolist())
    n = len(T)
    surv = 1.0
    out = []
    for t in unique_times:
        d = int(((T["time"] == t) & (T["event"] == 1)).sum())
        at_risk = int((T["time"] >= t).sum())
        if at_risk > 0:
            surv *= (1.0 - d / at_risk)
        out.append((t, at_risk, d, surv))
    return pd.DataFrame(out, columns=["time_s", "at_risk", "failures", "survival"])


def evaluate_navigation_robustness(df: pd.DataFrame) -> Tuple[pd.DataFrame, Dict[str, float]]:
    required = {
        "episode_id", "time_s", "x", "y", "goal_x", "goal_y", "clearance_m", "collision",
        "intervention", "goal_reached", "tracking_error_m", "cmd_v", "cmd_v_max", "cmd_w", "cmd_w_max", "recovery_event"
    }
    missing = required - set(df.columns)
    if missing:
        raise ValueError(f"Missing columns: {sorted(missing)}")

    episode_rows: List[Dict[str, float]] = []
    for ep, g in df.groupby("episode_id"):
        g = g.sort_values("time_s").reset_index(drop=True)
        success = int(g["goal_reached"].max() > 0)
        collision_free = int(g["collision"].sum() == 0)
        intervention_free = int(g["intervention"].sum() == 0)
        had_failure = int((g["collision"].sum() + g["intervention"].sum()) > 0)
        recovered = int((g["recovery_event"].sum() > 0) and success)

        t_final = float(g["time_s"].iloc[-1] - g["time_s"].iloc[0])
        L = path_length(g["x"].to_numpy(), g["y"].to_numpy())
        start = g.iloc[0][["x", "y"]].to_numpy(dtype=float)
        goal = g.iloc[0][["goal_x", "goal_y"]].to_numpy(dtype=float)
        d_ref = float(np.linalg.norm(goal - start))
        path_eff = float(np.clip(d_ref / max(L, 1e-9), 0.0, 1.0)) if success else 0.0

        min_clear = float(g["clearance_m"].min())
        p05_clear = float(np.quantile(g["clearance_m"].to_numpy(), 0.05))
        track_rmse = float(np.sqrt(np.mean(np.square(g["tracking_error_m"].to_numpy()))))

        v_sat = np.abs(g["cmd_v"].to_numpy()) >= 0.98 * np.maximum(g["cmd_v_max"].to_numpy(), 1e-9)
        w_sat = np.abs(g["cmd_w"].to_numpy()) >= 0.98 * np.maximum(g["cmd_w_max"].to_numpy(), 1e-9)
        sat_ratio = float(np.mean(v_sat | w_sat))

        episode_rows.append(
            dict(
                episode_id=int(ep),
                success=success,
                collision_free=collision_free,
                intervention_free=intervention_free,
                had_failure=had_failure,
                recovered_after_failure=recovered,
                completion_time_s=t_final,
                path_length_m=L,
                reference_distance_m=d_ref,
                path_efficiency=path_eff,
                min_clearance_m=min_clear,
                p05_clearance_m=p05_clear,
                tracking_rmse_m=track_rmse,
                saturation_ratio=sat_ratio,
            )
        )

    ep_df = pd.DataFrame(episode_rows)

    n = len(ep_df)
    k_success = int(ep_df["success"].sum())
    k_collision_free = int(ep_df["collision_free"].sum())
    k_intervention_free = int(ep_df["intervention_free"].sum())
    k_recovered = int(ep_df["recovered_after_failure"].sum())
    n_failed = int(ep_df["had_failure"].sum())

    success_ci = wilson_interval(k_success, n)
    collisionfree_ci = wilson_interval(k_collision_free, n)

    # Normalize continuous metrics to [0,1] with task-dependent anchors
    # Anchors should be fixed by the benchmark protocol, not tuned per method.
    tau_time = 60.0  # s
    tau_track = 0.25 # m
    c_safe = 0.30    # m
    time_score = float(np.mean(np.exp(-ep_df["completion_time_s"].to_numpy() / tau_time) * ep_df["success"].to_numpy()))
    track_score = float(np.mean(np.exp(-ep_df["tracking_rmse_m"].to_numpy() / tau_track)))
    clearance_score = float(np.mean(np.clip(ep_df["p05_clearance_m"].to_numpy() / c_safe, 0.0, 1.0)))
    efficiency_score = float(np.mean(ep_df["path_efficiency"].to_numpy()))
    intervention_score = float(k_intervention_free / n)
    collisionfree_score = float(k_collision_free / n)

    weights = {
        "collisionfree": 0.30,
        "intervention_free": 0.15,
        "success": 0.20,
        "clearance": 0.15,
        "efficiency": 0.10,
        "tracking": 0.05,
        "time": 0.05,
    }
    robustness_score = (
        weights["collisionfree"] * collisionfree_score
        + weights["intervention_free"] * intervention_score
        + weights["success"] * (k_success / n)
        + weights["clearance"] * clearance_score
        + weights["efficiency"] * efficiency_score
        + weights["tracking"] * track_score
        + weights["time"] * time_score
    )

    km = kaplan_meier_failure_survival(df)

    summary = {
        "num_episodes": float(n),
        "success_rate": success_ci.p_hat,
        "success_rate_wilson_low": success_ci.lower,
        "success_rate_wilson_high": success_ci.upper,
        "collision_free_rate": collisionfree_ci.p_hat,
        "collision_free_wilson_low": collisionfree_ci.lower,
        "collision_free_wilson_high": collisionfree_ci.upper,
        "intervention_free_rate": float(k_intervention_free / n),
        "mean_completion_time_success_s": float(ep_df.loc[ep_df["success"] == 1, "completion_time_s"].mean()) if k_success > 0 else np.nan,
        "mean_path_efficiency": float(ep_df["path_efficiency"].mean()),
        "mean_p05_clearance_m": float(ep_df["p05_clearance_m"].mean()),
        "mean_tracking_rmse_m": float(ep_df["tracking_rmse_m"].mean()),
        "mean_saturation_ratio": float(ep_df["saturation_ratio"].mean()),
        "recovery_after_failure_rate": float(k_recovered / n_failed) if n_failed > 0 else np.nan,
        "composite_robustness_score": float(robustness_score),
        "km_last_survival": float(km["survival"].iloc[-1]) if len(km) else 1.0,
    }
    return ep_df, summary


def main():
    if len(sys.argv) >= 2:
        df = pd.read_csv(sys.argv[1])
    else:
        df = generate_synthetic_log()
        df.to_csv("Chapter19_Lesson2_synthetic_log.csv", index=False)
        print("Wrote synthetic log to Chapter19_Lesson2_synthetic_log.csv")

    ep_metrics, summary = evaluate_navigation_robustness(df)

    pd.set_option("display.width", 140)
    pd.set_option("display.max_columns", 20)
    print("\nPer-episode metrics (first 10 rows):")
    print(ep_metrics.head(10).to_string(index=False))

    print("\nSummary metrics:")
    for k, v in summary.items():
        print(f"{k:35s} = {v:.6f}" if isinstance(v, (float, np.floating)) else f"{k:35s} = {v}")

    # Save outputs for downstream reporting
    ep_metrics.to_csv("Chapter19_Lesson2_episode_metrics.csv", index=False)
    with open("Chapter19_Lesson2_summary.txt", "w", encoding="utf-8") as f:
        for k, v in summary.items():
            f.write(f"{k},{v}\n")
    print("\nSaved Chapter19_Lesson2_episode_metrics.csv and Chapter19_Lesson2_summary.txt")


if __name__ == "__main__":
    main()

9.2 C++ Implementation — Chapter19_Lesson2.cpp

Portable C++17 implementation (no external dependencies). In production, integrate with rclcpp, nav_msgs, and geometry_msgs for online logging and evaluation.

// Chapter19_Lesson2.cpp
/*
Metrics for Navigation Robustness (Autonomous Mobile Robots)

Build:
  g++ -std=c++17 -O2 -o Chapter19_Lesson2 Chapter19_Lesson2.cpp

Usage:
  ./Chapter19_Lesson2            # runs on embedded synthetic episodes
  ./Chapter19_Lesson2 log.csv    # simple CSV parser (schema documented below)

CSV schema (header required):
episode_id,time_s,x,y,goal_x,goal_y,clearance_m,collision,intervention,goal_reached,
tracking_error_m,cmd_v,cmd_v_max,cmd_w,cmd_w_max,recovery_event

Robotics library integration notes:
- For ROS 2 / Nav2, export odom (x,y), goal pose, cmd_vel, local minimum clearance,
  and behavior-tree events (collision, intervention, recovery) into a CSV logger.
- Suggested libraries in production: rclcpp, nav_msgs, geometry_msgs, Eigen.
*/

#include <algorithm>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <map>
#include <sstream>
#include <string>
#include <tuple>
#include <vector>

struct Sample {
    int episode_id{};
    double time_s{};
    double x{}, y{};
    double goal_x{}, goal_y{};
    double clearance_m{};
    int collision{};
    int intervention{};
    int goal_reached{};
    double tracking_error_m{};
    double cmd_v{}, cmd_v_max{};
    double cmd_w{}, cmd_w_max{};
    int recovery_event{};
};

struct EpisodeMetrics {
    int episode_id{};
    int success{};
    int collision_free{};
    int intervention_free{};
    int had_failure{};
    int recovered_after_failure{};
    double completion_time_s{};
    double path_length_m{};
    double reference_distance_m{};
    double path_efficiency{};
    double min_clearance_m{};
    double tracking_rmse_m{};
    double saturation_ratio{};
};

struct Wilson {
    double p_hat{};
    double lo{};
    double hi{};
};

static Wilson wilson_interval(int k, int n, double z = 1.96) {
    if (n <= 0) return {NAN, NAN, NAN};
    double p = static_cast<double>(k) / n;
    double denom = 1.0 + (z * z) / n;
    double center = (p + (z * z) / (2.0 * n)) / denom;
    double rad = (z / denom) * std::sqrt((p * (1.0 - p) / n) + (z * z) / (4.0 * n * n));
    return {p, center - rad, center + rad};
}

static double dist2d(double x1, double y1, double x2, double y2) {
    double dx = x2 - x1, dy = y2 - y1;
    return std::sqrt(dx * dx + dy * dy);
}

static std::vector<Sample> make_synthetic_data() {
    // deterministic pseudo-data for reproducibility (no RNG dependency)
    std::vector<Sample> out;
    for (int ep = 0; ep < 8; ++ep) {
        double x = 0.0, y = 0.2 * (ep % 2);
        double gx = 8.0 + 0.3 * ep;
        double gy = (ep % 3) - 1.0;
        bool terminated = false;
        for (int k = 0; k < 160 && !terminated; ++k) {
            double t = 0.1 * k;
            double dx = gx - x;
            double dy = gy - y;
            double d = std::sqrt(dx * dx + dy * dy);
            double theta = std::atan2(dy, dx);
            double cmd_v_max = 0.8, cmd_w_max = 1.2;
            double cmd_v = std::min(cmd_v_max, 0.55 + 0.05 * std::sin(0.07 * k + ep));
            double cmd_w = std::max(-cmd_w_max, std::min(cmd_w_max, 0.35 * std::sin(0.05 * k)));
            x += 0.1 * cmd_v * std::cos(theta);
            y += 0.1 * cmd_v * std::sin(theta);

            double clearance = 0.65 + 0.22 * std::sin(0.11 * k + 0.4 * ep);
            if (ep == 2 && k > 60 && k < 75) clearance -= 0.55;
            if (ep == 5 && k > 90 && k < 102) clearance -= 0.48;
            if (clearance < 0.0) clearance = 0.0;

            int collision = (clearance < 0.05) ? 1 : 0;
            int intervention = (clearance < 0.12 && !collision) ? 1 : 0;
            int goal_reached = (d < 0.30 && collision == 0) ? 1 : 0;
            int recovery = ((intervention == 1) && (k % 5 == 0)) ? 1 : 0;
            double e_track = std::fabs(0.05 * std::sin(0.02 * k + ep));

            out.push_back({ep, t, x, y, gx, gy, clearance, collision, intervention, goal_reached,
                           e_track, cmd_v, cmd_v_max, cmd_w, cmd_w_max, recovery});

            if (collision || goal_reached) terminated = true;
        }
    }
    return out;
}

static std::vector<std::string> split_csv_line(const std::string& line) {
    std::vector<std::string> cols;
    std::stringstream ss(line);
    std::string item;
    while (std::getline(ss, item, ',')) cols.push_back(item);
    return cols;
}

static bool read_csv(const std::string& path, std::vector<Sample>& out) {
    std::ifstream fin(path);
    if (!fin) return false;
    std::string line;
    if (!std::getline(fin, line)) return false; // header
    while (std::getline(fin, line)) {
        if (line.empty()) continue;
        auto c = split_csv_line(line);
        if (c.size() < 16) continue;
        Sample s;
        s.episode_id = std::stoi(c[0]);
        s.time_s = std::stod(c[1]);
        s.x = std::stod(c[2]); s.y = std::stod(c[3]);
        s.goal_x = std::stod(c[4]); s.goal_y = std::stod(c[5]);
        s.clearance_m = std::stod(c[6]);
        s.collision = std::stoi(c[7]);
        s.intervention = std::stoi(c[8]);
        s.goal_reached = std::stoi(c[9]);
        s.tracking_error_m = std::stod(c[10]);
        s.cmd_v = std::stod(c[11]); s.cmd_v_max = std::stod(c[12]);
        s.cmd_w = std::stod(c[13]); s.cmd_w_max = std::stod(c[14]);
        s.recovery_event = std::stoi(c[15]);
        out.push_back(s);
    }
    return !out.empty();
}

int main(int argc, char** argv) {
    std::vector<Sample> data;
    if (argc >= 2) {
        if (!read_csv(argv[1], data)) {
            std::cerr << "Failed to read CSV. Falling back to synthetic data.\n";
            data = make_synthetic_data();
        }
    } else {
        data = make_synthetic_data();
    }

    std::map<int, std::vector<Sample>> by_ep;
    for (const auto& s : data) by_ep[s.episode_id].push_back(s);

    std::vector<EpisodeMetrics> eps;
    for (auto& kv : by_ep) {
        auto& g = kv.second;
        std::sort(g.begin(), g.end(), [](const Sample& a, const Sample& b){ return a.time_s < b.time_s; });

        int success = 0, collisions = 0, interventions = 0, rec = 0;
        double min_clear = 1e9;
        double sum_e2 = 0.0;
        int sat_count = 0;
        double path_len = 0.0;
        for (size_t i = 0; i < g.size(); ++i) {
            success = std::max(success, g[i].goal_reached);
            collisions += g[i].collision;
            interventions += g[i].intervention;
            rec += g[i].recovery_event;
            min_clear = std::min(min_clear, g[i].clearance_m);
            sum_e2 += g[i].tracking_error_m * g[i].tracking_error_m;
            bool sat_v = std::fabs(g[i].cmd_v) >= 0.98 * std::max(g[i].cmd_v_max, 1e-9);
            bool sat_w = std::fabs(g[i].cmd_w) >= 0.98 * std::max(g[i].cmd_w_max, 1e-9);
            if (sat_v || sat_w) sat_count++;
            if (i > 0) path_len += dist2d(g[i-1].x, g[i-1].y, g[i].x, g[i].y);
        }
        double t_final = g.back().time_s - g.front().time_s;
        double d_ref = dist2d(g.front().x, g.front().y, g.front().goal_x, g.front().goal_y);
        double eta = success ? std::min(1.0, d_ref / std::max(path_len, 1e-9)) : 0.0;
        int had_failure = (collisions + interventions) > 0 ? 1 : 0;
        int recovered_after_failure = (had_failure && rec > 0 && success) ? 1 : 0;

        eps.push_back({
            kv.first,
            success,
            collisions == 0 ? 1 : 0,
            interventions == 0 ? 1 : 0,
            had_failure,
            recovered_after_failure,
            t_final,
            path_len,
            d_ref,
            eta,
            min_clear,
            std::sqrt(sum_e2 / std::max<size_t>(1, g.size())),
            static_cast<double>(sat_count) / std::max<size_t>(1, g.size())
        });
    }

    int n = static_cast<int>(eps.size());
    int k_success = 0, k_collision_free = 0, k_intervention_free = 0, k_recovered = 0, n_failed = 0;
    double mean_eta = 0.0, mean_clear = 0.0, mean_track = 0.0, mean_t = 0.0;
    int n_success = 0;
    for (const auto& e : eps) {
        k_success += e.success;
        k_collision_free += e.collision_free;
        k_intervention_free += e.intervention_free;
        k_recovered += e.recovered_after_failure;
        n_failed += e.had_failure;
        mean_eta += e.path_efficiency;
        mean_clear += e.min_clearance_m;
        mean_track += e.tracking_rmse_m;
        if (e.success) {
            mean_t += e.completion_time_s;
            n_success++;
        }
    }
    mean_eta /= std::max(1, n);
    mean_clear /= std::max(1, n);
    mean_track /= std::max(1, n);
    mean_t = (n_success > 0) ? mean_t / n_success : NAN;

    Wilson w_succ = wilson_interval(k_success, n);
    Wilson w_safe = wilson_interval(k_collision_free, n);

    auto exp_score = [](double x, double tau){ return std::exp(-x / tau); };
    double time_score = 0.0, track_score = 0.0, clear_score = 0.0;
    for (const auto& e : eps) {
        time_score += exp_score(e.completion_time_s, 60.0) * e.success;
        track_score += exp_score(e.tracking_rmse_m, 0.25);
        clear_score += std::min(1.0, std::max(0.0, e.min_clearance_m / 0.30));
    }
    time_score /= std::max(1, n);
    track_score /= std::max(1, n);
    clear_score /= std::max(1, n);

    double success_rate = static_cast<double>(k_success) / std::max(1, n);
    double collision_free_rate = static_cast<double>(k_collision_free) / std::max(1, n);
    double intervention_free_rate = static_cast<double>(k_intervention_free) / std::max(1, n);
    double robustness =
        0.30 * collision_free_rate +
        0.15 * intervention_free_rate +
        0.20 * success_rate +
        0.15 * clear_score +
        0.10 * mean_eta +
        0.05 * track_score +
        0.05 * time_score;

    std::cout << std::fixed << std::setprecision(4);
    std::cout << "Per-episode metrics:\n";
    for (const auto& e : eps) {
        std::cout << "ep=" << e.episode_id
                  << " success=" << e.success
                  << " collision_free=" << e.collision_free
                  << " intervention_free=" << e.intervention_free
                  << " T=" << e.completion_time_s
                  << " L=" << e.path_length_m
                  << " eta=" << e.path_efficiency
                  << " min_clear=" << e.min_clearance_m
                  << " track_rmse=" << e.tracking_rmse_m
                  << " sat=" << e.saturation_ratio
                  << "\n";
    }

    std::cout << "\nSummary metrics:\n";
    std::cout << "N episodes                    = " << n << "\n";
    std::cout << "Success rate                 = " << success_rate
              << " (Wilson 95%: " << w_succ.lo << ", " << w_succ.hi << ")\n";
    std::cout << "Collision-free rate          = " << collision_free_rate
              << " (Wilson 95%: " << w_safe.lo << ", " << w_safe.hi << ")\n";
    std::cout << "Intervention-free rate       = " << intervention_free_rate << "\n";
    std::cout << "Mean completion time (succ)  = " << mean_t << " s\n";
    std::cout << "Mean path efficiency         = " << mean_eta << "\n";
    std::cout << "Mean min clearance           = " << mean_clear << " m\n";
    std::cout << "Mean tracking RMSE           = " << mean_track << " m\n";
    std::cout << "Recovery-after-failure rate  = "
              << ((n_failed > 0) ? (static_cast<double>(k_recovered) / n_failed) : NAN) << "\n";
    std::cout << "Composite robustness score   = " << robustness << "\n";

    return 0;
}

9.3 Java Implementation — Chapter19_Lesson2.java

Java implementation with synthetic benchmark generation and episode-level aggregation. For larger experiments, combine with ROSJava/rosbridge logging and Apache Commons Math for advanced statistics.

// Chapter19_Lesson2.java
/*
Metrics for Navigation Robustness (Autonomous Mobile Robots)

Compile:
  javac Chapter19_Lesson2.java
Run:
  java Chapter19_Lesson2

This Java example creates a synthetic mission log and computes a core set of robustness metrics.
Production integration notes:
- ROSJava / rosbridge clients can stream odometry, goal state, and event flags into CSV or DB.
- Apache Commons Math can be added for advanced statistics (bootstrap, hypothesis tests).
*/

import java.util.*;
import java.util.stream.*;

public class Chapter19_Lesson2 {

    static class Sample {
        int episodeId;
        double timeS;
        double x, y;
        double goalX, goalY;
        double clearanceM;
        int collision;
        int intervention;
        int goalReached;
        double trackingErrorM;
        double cmdV, cmdVMax;
        double cmdW, cmdWMax;
        int recoveryEvent;
    }

    static class EpisodeMetrics {
        int episodeId;
        int success;
        int collisionFree;
        int interventionFree;
        int hadFailure;
        int recoveredAfterFailure;
        double completionTimeS;
        double pathLengthM;
        double referenceDistanceM;
        double pathEfficiency;
        double minClearanceM;
        double trackingRmseM;
        double saturationRatio;
    }

    static class Wilson {
        double pHat, low, high;
        Wilson(double pHat, double low, double high) {
            this.pHat = pHat; this.low = low; this.high = high;
        }
    }

    static Wilson wilsonInterval(int k, int n, double z) {
        if (n <= 0) return new Wilson(Double.NaN, Double.NaN, Double.NaN);
        double p = (double) k / n;
        double denom = 1.0 + (z * z) / n;
        double center = (p + (z * z) / (2.0 * n)) / denom;
        double rad = (z / denom) * Math.sqrt((p * (1.0 - p) / n) + (z * z) / (4.0 * n * n));
        return new Wilson(p, center - rad, center + rad);
    }

    static double dist(double x1, double y1, double x2, double y2) {
        double dx = x2 - x1, dy = y2 - y1;
        return Math.hypot(dx, dy);
    }

    static List<Sample> syntheticData() {
        List<Sample> list = new ArrayList<>();
        Random rng = new Random(19);
        for (int ep = 0; ep < 10; ep++) {
            double x = 0.0, y = 0.0;
            double gx = 9.0 + 0.3 * ep;
            double gy = (ep % 4) - 1.5;
            boolean stop = false;
            for (int k = 0; k < 180 && !stop; k++) {
                double t = 0.1 * k;
                double d = dist(x, y, gx, gy);
                double theta = Math.atan2(gy - y, gx - x);
                double vMax = 0.8, wMax = 1.2;
                double v = Math.max(-vMax, Math.min(vMax, 0.55 + 0.04 * Math.sin(0.07 * k + ep)));
                double w = Math.max(-wMax, Math.min(wMax, 0.30 * Math.sin(0.06 * k)));
                x += 0.1 * v * Math.cos(theta);
                y += 0.1 * v * Math.sin(theta);

                double clearance = 0.70 + 0.20 * Math.sin(0.09 * k + ep) + 0.02 * (rng.nextDouble() - 0.5);
                if (ep == 3 && k > 70 && k < 85) clearance -= 0.58;
                if (ep == 7 && k > 40 && k < 54) clearance -= 0.47;
                clearance = Math.max(0.0, clearance);

                int collision = (clearance < 0.05) ? 1 : 0;
                int intervention = (clearance < 0.12 && collision == 0) ? 1 : 0;
                int goalReached = (d < 0.33 && collision == 0) ? 1 : 0;
                int recovery = (intervention == 1 && (k % 6 == 0)) ? 1 : 0;
                double eTrack = Math.abs(0.06 * Math.sin(0.025 * k + 0.5 * ep));

                Sample s = new Sample();
                s.episodeId = ep; s.timeS = t;
                s.x = x; s.y = y;
                s.goalX = gx; s.goalY = gy;
                s.clearanceM = clearance;
                s.collision = collision; s.intervention = intervention;
                s.goalReached = goalReached;
                s.trackingErrorM = eTrack;
                s.cmdV = v; s.cmdVMax = vMax;
                s.cmdW = w; s.cmdWMax = wMax;
                s.recoveryEvent = recovery;
                list.add(s);

                if (collision == 1 || goalReached == 1) stop = true;
            }
        }
        return list;
    }

    static List<EpisodeMetrics> evaluate(List<Sample> data) {
        Map<Integer, List<Sample>> grouped = new TreeMap<>();
        for (Sample s : data) grouped.computeIfAbsent(s.episodeId, k -> new ArrayList<>()).add(s);

        List<EpisodeMetrics> out = new ArrayList<>();
        for (Map.Entry<Integer, List<Sample>> e : grouped.entrySet()) {
            List<Sample> g = e.getValue();
            g.sort(Comparator.comparingDouble(a -> a.timeS));

            EpisodeMetrics m = new EpisodeMetrics();
            m.episodeId = e.getKey();
            m.minClearanceM = Double.POSITIVE_INFINITY;
            m.collisionFree = 1;
            m.interventionFree = 1;

            int satCount = 0;
            double sumE2 = 0.0;
            for (int i = 0; i < g.size(); i++) {
                Sample s = g.get(i);
                m.success = Math.max(m.success, s.goalReached);
                                if (s.collision == 1) m.collisionFree = 0;
                if (s.intervention == 1) m.interventionFree = 0;
                if (s.collision + s.intervention > 0) m.hadFailure = 1;
                if (s.recoveryEvent == 1) m.recoveredAfterFailure = 1;

                m.minClearanceM = Math.min(m.minClearanceM, s.clearanceM);
                sumE2 += s.trackingErrorM * s.trackingErrorM;

                boolean satV = Math.abs(s.cmdV) >= 0.98 * Math.max(s.cmdVMax, 1e-9);
                boolean satW = Math.abs(s.cmdW) >= 0.98 * Math.max(s.cmdWMax, 1e-9);
                if (satV || satW) satCount++;

                if (i > 0) {
                    Sample p = g.get(i - 1);
                    m.pathLengthM += dist(p.x, p.y, s.x, s.y);
                }
            }
            Sample first = g.get(0), last = g.get(g.size() - 1);
            m.completionTimeS = last.timeS - first.timeS;
            m.referenceDistanceM = dist(first.x, first.y, first.goalX, first.goalY);
            m.pathEfficiency = (m.success == 1) ? Math.min(1.0, m.referenceDistanceM / Math.max(m.pathLengthM, 1e-9)) : 0.0;
            m.trackingRmseM = Math.sqrt(sumE2 / Math.max(1, g.size()));
            m.saturationRatio = (double) satCount / Math.max(1, g.size());
            if (!(m.hadFailure == 1 && m.recoveredAfterFailure == 1 && m.success == 1)) {
                m.recoveredAfterFailure = 0;
            }
            out.add(m);
        }
        return out;
    }

    public static void main(String[] args) {
        List<Sample> data = syntheticData();
        List<EpisodeMetrics> metrics = evaluate(data);

        int n = metrics.size();
        int kSuccess = 0, kCollisionFree = 0, kInterventionFree = 0, kRecovered = 0, nFailed = 0;
        double meanEta = 0, meanClear = 0, meanTrack = 0, meanTimeSucc = 0;
        int nSucc = 0;

        for (EpisodeMetrics m : metrics) {
            kSuccess += m.success;
            kCollisionFree += m.collisionFree;
            kInterventionFree += m.interventionFree;
            kRecovered += m.recoveredAfterFailure;
            nFailed += m.hadFailure;
            meanEta += m.pathEfficiency;
            meanClear += m.minClearanceM;
            meanTrack += m.trackingRmseM;
            if (m.success == 1) { meanTimeSucc += m.completionTimeS; nSucc++; }
        }

        meanEta /= Math.max(1, n);
        meanClear /= Math.max(1, n);
        meanTrack /= Math.max(1, n);
        meanTimeSucc = (nSucc > 0) ? meanTimeSucc / nSucc : Double.NaN;

        Wilson wSuccess = wilsonInterval(kSuccess, n, 1.96);
        Wilson wSafe = wilsonInterval(kCollisionFree, n, 1.96);

        double successRate = (double) kSuccess / Math.max(1, n);
        double collisionFreeRate = (double) kCollisionFree / Math.max(1, n);
        double interventionFreeRate = (double) kInterventionFree / Math.max(1, n);

        double timeScore = 0, trackScore = 0, clearScore = 0;
        for (EpisodeMetrics m : metrics) {
            timeScore += Math.exp(-m.completionTimeS / 60.0) * m.success;
            trackScore += Math.exp(-m.trackingRmseM / 0.25);
            clearScore += Math.min(1.0, Math.max(0.0, m.minClearanceM / 0.30));
        }
        timeScore /= Math.max(1, n);
        trackScore /= Math.max(1, n);
        clearScore /= Math.max(1, n);

        double robustness =
            0.30 * collisionFreeRate +
            0.15 * interventionFreeRate +
            0.20 * successRate +
            0.15 * clearScore +
            0.10 * meanEta +
            0.05 * trackScore +
            0.05 * timeScore;

        System.out.println("Per-episode metrics:");
        for (EpisodeMetrics m : metrics) {
            System.out.printf(
                Locale.US,
                "ep=%d success=%d collisionFree=%d interventionFree=%d T=%.2f L=%.2f eta=%.3f minClear=%.3f trackRMSE=%.3f sat=%.3f%n",
                m.episodeId, m.success, m.collisionFree, m.interventionFree, m.completionTimeS, m.pathLengthM,
                m.pathEfficiency, m.minClearanceM, m.trackingRmseM, m.saturationRatio
            );
        }

        System.out.println("\nSummary metrics:");
        System.out.printf(Locale.US, "Success rate: %.4f (Wilson95%% [%.4f, %.4f])%n", wSuccess.pHat, wSuccess.low, wSuccess.high);
        System.out.printf(Locale.US, "Collision-free rate: %.4f (Wilson95%% [%.4f, %.4f])%n", wSafe.pHat, wSafe.low, wSafe.high);
        System.out.printf(Locale.US, "Intervention-free rate: %.4f%n", interventionFreeRate);
        System.out.printf(Locale.US, "Mean completion time (success): %.4f s%n", meanTimeSucc);
        System.out.printf(Locale.US, "Mean path efficiency: %.4f%n", meanEta);
        System.out.printf(Locale.US, "Mean min clearance: %.4f m%n", meanClear);
        System.out.printf(Locale.US, "Mean tracking RMSE: %.4f m%n", meanTrack);
        System.out.printf(Locale.US, "Recovery-after-failure rate: %.4f%n", (nFailed > 0 ? (double) kRecovered / nFailed : Double.NaN));
        System.out.printf(Locale.US, "Composite robustness score: %.4f%n", robustness);
    }
}

9.4 MATLAB/Simulink Implementation — Chapter19_Lesson2.m

MATLAB script computes robustness metrics from tables and includes a Simulink-compatible helper function (MATLAB Function block style) for step-level scoring.

% Chapter19_Lesson2.m
% Metrics for Navigation Robustness (Autonomous Mobile Robots)
%
% This MATLAB script computes robustness metrics from a navigation log table.
% It also includes a Simulink-compatible helper function for online metric updates.
%
% Required columns:
% episode_id, time_s, x, y, goal_x, goal_y, clearance_m, collision, intervention,
% goal_reached, tracking_error_m, cmd_v, cmd_v_max, cmd_w, cmd_w_max, recovery_event
%
% Robotics toolbox notes:
% - Robotics System Toolbox / ROS Toolbox can be used to import rosbag logs.
% - In Simulink, connect signals to a MATLAB Function block that calls
%   Chapter19_Lesson2_stepMetric() for cycle-level scoring.

clear; clc;

if exist('Chapter19_Lesson2_log.csv', 'file')
    T = readtable('Chapter19_Lesson2_log.csv');
else
    T = localGenerateSyntheticLog(12);
    writetable(T, 'Chapter19_Lesson2_log.csv');
    fprintf('Wrote synthetic log to Chapter19_Lesson2_log.csv\n');
end

episodes = unique(T.episode_id);
N = numel(episodes);

M = table('Size',[N 13], ...
    'VariableTypes', {'double','double','double','double','double','double','double','double','double','double','double','double','double'}, ...
    'VariableNames', {'episode_id','success','collision_free','intervention_free','had_failure','recovered_after_failure', ...
                      'completion_time_s','path_length_m','reference_distance_m','path_efficiency','min_clearance_m', ...
                      'tracking_rmse_m','saturation_ratio'});

for ii = 1:N
    ep = episodes(ii);
    G = sortrows(T(T.episode_id == ep, :), 'time_s');

    success = any(G.goal_reached > 0);
    collision_free = all(G.collision == 0);
    intervention_free = all(G.intervention == 0);
    had_failure = any((G.collision + G.intervention) > 0);
    recovered_after_failure = had_failure && any(G.recovery_event > 0) && success;

    dt = G.time_s(end) - G.time_s(1);
    dx = diff(G.x); dy = diff(G.y);
    L = sum(sqrt(dx.^2 + dy.^2));
    dref = hypot(G.goal_x(1)-G.x(1), G.goal_y(1)-G.y(1));
    eta = 0;
    if success
        eta = min(1, dref / max(L, eps));
    end

    min_clear = min(G.clearance_m);
    track_rmse = sqrt(mean(G.tracking_error_m.^2));
    sat = mean(abs(G.cmd_v) >= 0.98*max(G.cmd_v_max, eps) | abs(G.cmd_w) >= 0.98*max(G.cmd_w_max, eps));

    M{ii,:} = [ep, success, collision_free, intervention_free, had_failure, recovered_after_failure, ...
               dt, L, dref, eta, min_clear, track_rmse, sat];
end

k_success = sum(M.success);
k_collision_free = sum(M.collision_free);
k_intervention_free = sum(M.intervention_free);
n_failed = sum(M.had_failure);
k_recovered = sum(M.recovered_after_failure);

[p_succ, lo_succ, hi_succ] = localWilson(k_success, N, 1.96);
[p_safe, lo_safe, hi_safe] = localWilson(k_collision_free, N, 1.96);

time_score = mean(exp(-M.completion_time_s/60) .* M.success);
track_score = mean(exp(-M.tracking_rmse_m/0.25));
clearance_score = mean(min(max(M.min_clearance_m/0.30, 0), 1));
eff_score = mean(M.path_efficiency);

robustness = 0.30*p_safe + 0.15*(k_intervention_free/N) + 0.20*p_succ + ...
             0.15*clearance_score + 0.10*eff_score + 0.05*track_score + 0.05*time_score;

fprintf('\nPer-episode metrics:\n');
disp(M);

fprintf('\nSummary metrics:\n');
fprintf('Success rate               = %.4f (Wilson95%% [%.4f, %.4f])\n', p_succ, lo_succ, hi_succ);
fprintf('Collision-free rate        = %.4f (Wilson95%% [%.4f, %.4f])\n', p_safe, lo_safe, hi_safe);
fprintf('Intervention-free rate     = %.4f\n', k_intervention_free/N);
fprintf('Mean completion time (succ)= %.4f s\n', mean(M.completion_time_s(M.success>0)));
fprintf('Mean path efficiency       = %.4f\n', mean(M.path_efficiency));
fprintf('Mean min clearance         = %.4f m\n', mean(M.min_clearance_m));
fprintf('Mean tracking RMSE         = %.4f m\n', mean(M.tracking_rmse_m));
if n_failed > 0
    fprintf('Recovery-after-failure rate= %.4f\n', k_recovered/n_failed);
else
    fprintf('Recovery-after-failure rate= NaN\n');
end
fprintf('Composite robustness score = %.4f\n', robustness);

writetable(M, 'Chapter19_Lesson2_episode_metrics_matlab.csv');

%% Simulink-friendly helper (can be used inside a MATLAB Function block)
% score_t = Chapter19_Lesson2_stepMetric(clearance_m, track_err_m, collision, intervention)

function score_t = Chapter19_Lesson2_stepMetric(clearance_m, track_err_m, collision, intervention)
%#codegen
clear_score = min(max(clearance_m / 0.30, 0), 1);
track_score = exp(-track_err_m / 0.25);
safety_gate = 1.0;
if (collision ~= 0) || (intervention ~= 0)
    safety_gate = 0.0;
end
score_t = safety_gate * (0.7 * clear_score + 0.3 * track_score);
end

function T = localGenerateSyntheticLog(numEpisodes)
rows = [];
for ep = 0:(numEpisodes-1)
    x = 0; y = 0;
    gx = 9 + 0.2*ep; gy = mod(ep,3)-1;
    for k = 0:180
        t = 0.1*k;
        d = hypot(gx-x, gy-y);
        th = atan2(gy-y, gx-x);
        vMax = 0.8; wMax = 1.2;
        v = min(vMax, 0.55 + 0.05*sin(0.07*k + ep));
        w = max(-wMax, min(wMax, 0.32*sin(0.05*k)));
        x = x + 0.1*v*cos(th);
        y = y + 0.1*v*sin(th);

        clearance = 0.7 + 0.2*sin(0.1*k + ep);
        if ep == 4 && k > 80 && k < 95
            clearance = clearance - 0.60;
        end
        if ep == 9 && k > 55 && k < 70
            clearance = clearance - 0.45;
        end
        clearance = max(0, clearance);

        collision = double(clearance < 0.05);
        intervention = double(clearance < 0.12 && collision == 0);
        goal_reached = double(d < 0.33 && collision == 0);
        tracking_error_m = abs(0.05*sin(0.02*k + 0.1*ep));
        recovery_event = double(intervention == 1 && mod(k,6) == 0);

        rows = [rows; ep, t, x, y, gx, gy, clearance, collision, intervention, goal_reached, ...
                     tracking_error_m, v, vMax, w, wMax, recovery_event]; %#ok<AGROW>
        if collision || goal_reached
            break;
        end
    end
end

T = array2table(rows, 'VariableNames', ...
    {'episode_id','time_s','x','y','goal_x','goal_y','clearance_m','collision','intervention', ...
     'goal_reached','tracking_error_m','cmd_v','cmd_v_max','cmd_w','cmd_w_max','recovery_event'});
end

function [p, lo, hi] = localWilson(k, n, z)
if n <= 0
    p = NaN; lo = NaN; hi = NaN; return;
end
p = k / n;
den = 1 + z^2/n;
ctr = (p + z^2/(2*n)) / den;
rad = (z/den) * sqrt(p*(1-p)/n + z^2/(4*n^2));
lo = ctr - rad;
hi = ctr + rad;
end

9.5 Wolfram Mathematica Implementation — Chapter19_Lesson2.nb

Wolfram Language implementation for symbolic/statistical analysis and rapid metric prototyping. In your site template, Mathematica code blocks should use the Mathematica language class exactly as shown.

(* Chapter19_Lesson2.nb *)
(* Metrics for Navigation Robustness (Autonomous Mobile Robots)
   Wolfram Language implementation. Save as .nb or paste into a notebook.

   Expected columns in CSV:
   episode_id,time_s,x,y,goal_x,goal_y,clearance_m,collision,intervention,
   goal_reached,tracking_error_m,cmd_v,cmd_v_max,cmd_w,cmd_w_max,recovery_event

   Robotics integration notes:
   - ROS logs can be exported to CSV and imported with Import[..., "CSV"].
   - For symbolic work, Wolfram can derive confidence interval approximations,
     error propagation, and optimization-based composite scores.
*)

ClearAll["Global`*"];

WilsonInterval[k_Integer, n_Integer, z_: 1.96] := Module[
  {p, den, ctr, rad},
  If[n <= 0, Return[<|"pHat" -> Indeterminate, "Low" -> Indeterminate, "High" -> Indeterminate|>]];
  p = N[k/n];
  den = 1 + z^2/n;
  ctr = (p + z^2/(2 n))/den;
  rad = (z/den) Sqrt[p (1 - p)/n + z^2/(4 n^2)];
  <|"pHat" -> p, "Low" -> ctr - rad, "High" -> ctr + rad|>
];

SyntheticLog[numEpisodes_: 10] := Module[
  {rows = {}, x, y, gx, gy, k, t, d, th, vMax, wMax, v, w, clearance, collision,
   intervention, goalReached, eTrack, recovery},
  Do[
    x = 0.; y = 0.;
    gx = 9.0 + 0.25 ep; gy = Mod[ep, 4] - 1.5;
    For[k = 0, k <= 180, k++,
      t = 0.1 k;
      d = Sqrt[(gx - x)^2 + (gy - y)^2];
      th = ArcTan[gx - x, gy - y]; (* angle form in WL *)
      vMax = 0.8; wMax = 1.2;
      v = Min[vMax, 0.55 + 0.05 Sin[0.07 k + ep]];
      w = Clip[0.30 Sin[0.05 k], {-wMax, wMax}];
      x = x + 0.1 v Cos[th];
      y = y + 0.1 v Sin[th];

      clearance = 0.72 + 0.20 Sin[0.1 k + ep];
      If[ep == 3 && 70 < k < 85, clearance = clearance - 0.58];
      If[ep == 8 && 45 < k < 58, clearance = clearance - 0.48];
      clearance = Max[0., clearance];

      collision = If[clearance < 0.05, 1, 0];
      intervention = If[clearance < 0.12 && collision == 0, 1, 0];
      goalReached = If[d < 0.33 && collision == 0, 1, 0];
      eTrack = Abs[0.06 Sin[0.02 k + 0.3 ep]];
      recovery = If[intervention == 1 && Mod[k, 6] == 0, 1, 0];

      AppendTo[rows, <|
        "episode_id" -> ep, "time_s" -> t, "x" -> x, "y" -> y,
        "goal_x" -> gx, "goal_y" -> gy, "clearance_m" -> clearance,
        "collision" -> collision, "intervention" -> intervention,
        "goal_reached" -> goalReached, "tracking_error_m" -> eTrack,
        "cmd_v" -> v, "cmd_v_max" -> vMax, "cmd_w" -> w, "cmd_w_max" -> wMax,
        "recovery_event" -> recovery
      |>];
      If[collision == 1 || goalReached == 1, Break[]];
    ],
    {ep, 0, numEpisodes - 1}
  ];
  rows
];

PathLength[pts_List] := If[Length[pts] < 2, 0., Total[Norm /@ Differences[pts]]];

EvaluateRobustness[data_List] := Module[
  {groups, episodes, metrics, n, kSuccess, kSafe, kIntFree, nFailed, kRec,
   succCI, safeCI, timeScore, trackScore, clearScore, effScore, robustness},
  groups = GatherBy[data, #episode_id &];
  metrics = Table[
    Module[{g = SortBy[grp, #time_s &], success, collFree, intFree, hadFail, recov,
            tFinal, L, dRef, eta, minClear, trackRMSE, satRatio, satFlags},
      success = Max[g[[All, "goal_reached"]]];
      collFree = If[Total[g[[All, "collision"]]] == 0, 1, 0];
      intFree = If[Total[g[[All, "intervention"]]] == 0, 1, 0];
      hadFail = If[Total[g[[All, "collision"]] + g[[All, "intervention"]]] > 0, 1, 0];
      recov = If[hadFail == 1 && Total[g[[All, "recovery_event"]]] > 0 && success == 1, 1, 0];
      tFinal = g[[-1, "time_s"]] - g[[1, "time_s"]];
      L = PathLength[g[[All, {"x", "y"}]] /. Association -> List];
      dRef = Norm[{g[[1, "goal_x"]] - g[[1, "x"]], g[[1, "goal_y"]] - g[[1, "y"]]}];
      eta = If[success == 1, Min[1., dRef/Max[L, 10^-9]], 0.];
      minClear = Min[g[[All, "clearance_m"]]];
      trackRMSE = Sqrt[Mean[(g[[All, "tracking_error_m"]])^2]];
      satFlags = Map[
        Function[s, Boole[Abs[s["cmd_v"]] >= 0.98 Max[s["cmd_v_max"], 10^-9] ||
                         Abs[s["cmd_w"]] >= 0.98 Max[s["cmd_w_max"], 10^-9]]],
        g
      ];
      satRatio = Mean[satFlags];
      <|
        "episode_id" -> g[[1, "episode_id"]],
        "success" -> success,
        "collision_free" -> collFree,
        "intervention_free" -> intFree,
        "had_failure" -> hadFail,
        "recovered_after_failure" -> recov,
        "completion_time_s" -> tFinal,
        "path_length_m" -> L,
        "reference_distance_m" -> dRef,
        "path_efficiency" -> eta,
        "min_clearance_m" -> minClear,
        "tracking_rmse_m" -> trackRMSE,
        "saturation_ratio" -> satRatio
      |>
    ],
    {grp, groups}
  ];

  n = Length[metrics];
  kSuccess = Total[metrics[[All, "success"]]];
  kSafe = Total[metrics[[All, "collision_free"]]];
  kIntFree = Total[metrics[[All, "intervention_free"]]];
  nFailed = Total[metrics[[All, "had_failure"]]];
  kRec = Total[metrics[[All, "recovered_after_failure"]]];

  succCI = WilsonInterval[kSuccess, n];
  safeCI = WilsonInterval[kSafe, n];

  timeScore = Mean[Map[Exp[-#completion_time_s/60.] #success &, metrics]];
  trackScore = Mean[Map[Exp[-#tracking_rmse_m/0.25] &, metrics]];
  clearScore = Mean[Map[Clip[#min_clearance_m/0.30, {0., 1.}] &, metrics]];
  effScore = Mean[metrics[[All, "path_efficiency"]]];

    robustness = 0.30 safeCI["pHat"] + 0.15 (N[kIntFree/n]) + 0.20 succCI["pHat"] +
               0.15 clearScore + 0.10 effScore + 0.05 trackScore + 0.05 timeScore;

  <|
    "EpisodeMetrics" -> Dataset[metrics],
    "Summary" -> <|
      "num_episodes" -> n,
      "success_rate" -> succCI,
      "collision_free_rate" -> safeCI,
      "intervention_free_rate" -> N[kIntFree/n],
      "mean_completion_time_success_s" -> Mean[Cases[metrics[[All, "completion_time_s"]], _?NumericQ]],
      "mean_path_efficiency" -> effScore,
      "mean_min_clearance_m" -> Mean[metrics[[All, "min_clearance_m"]]],
      "mean_tracking_rmse_m" -> Mean[metrics[[All, "tracking_rmse_m"]]],
      "recovery_after_failure_rate" -> If[nFailed > 0, N[kRec/nFailed], Indeterminate],
      "composite_robustness_score" -> robustness
    |>
  |>
];

data = SyntheticLog[12];
result = EvaluateRobustness[data];

Print["Per-episode metrics:"];
Print[result["EpisodeMetrics"]];

Print["Summary metrics:"];
Print[result["Summary"]];

Problem 1 (Wilson interval for collision-free rate): A planner completes \( N=50 \) episodes and remains collision-free in \( k=46 \) of them. Compute the 95% Wilson interval for the collision-free probability.

Solution: Here \( \hat{p}=46/50=0.92 \), \( z=1.96 \). Using \( \hat{p}_{W} = \dfrac{\hat{p} + z^2/(2N)}{1+z^2/N} \) and \( r_W = \dfrac{z}{1+z^2/N}\sqrt{\hat{p}(1-\hat{p})/N + z^2/(4N^2)} \), we obtain numerically

\[ \hat{p}_W \approx 0.8857,\qquad r_W \approx 0.0797, \qquad \text{CI}_{0.95}^{\text{Wilson}} \approx [0.8060,\;0.9654]. \]

Even with a high observed rate (0.92), the uncertainty is not negligible; this is why reporting only \( \hat{p} \) is insufficient.

Problem 2 (Minimum sample size for success-rate precision): You want the success-rate estimate to satisfy \( |\hat{p}-p| < 0.05 \) with confidence at least 95% using Hoeffding’s inequality. What sample size is sufficient?

Solution: Set \( \varepsilon=0.05 \) and \( \delta=0.05 \) in \( N \ge \frac{1}{2\varepsilon^2}\log(2/\delta) \):

\[ N \ge \frac{1}{2(0.05)^2}\log\!\left(\frac{2}{0.05}\right) = 200 \log(40) \approx 737.8. \]

Thus a sufficient integer choice is \( N=738 \) episodes. This is conservative; Wilson/Beta intervals can reduce sample counts in practice, but Hoeffding gives a distribution-free guarantee.

Problem 3 (Proof of bounded composite score): Let \( R=\sum_{j=1}^{m} w_j r_j \), with \( w_j \ge 0 \), \( \sum_j w_j=1 \), and \( r_j \in [0,1] \). Prove \( R \in [0,1] \).

Solution: Since each term satisfies \( w_j r_j \ge 0 \), summing gives \( R \ge 0 \). Also, because \( r_j \le 1 \), \( w_j r_j \le w_j \), so \( R=\sum_j w_j r_j \le \sum_j w_j = 1 \). Therefore \( 0 \le R \le 1 \). \( \square \)

Problem 4 (Clearance sampling guarantee): Suppose the clearance signal is Lipschitz with constant \( L_c=0.8 \,\text{m/s} \) and the logger samples at \( \Delta t=0.1 \,\text{s} \). What minimum sampled clearance guarantees no hidden collision between samples?

Solution: From the sampled-clearance proposition, it is sufficient that \( c(t_k) > L_c\Delta t \). Therefore

\[ c(t_k) > 0.8 \times 0.1 = 0.08 \text{ m}. \]

Any sampled clearance strictly greater than 8 cm guarantees \( c(t) > 0 \) throughout the next sampling interval.

Problem 5 (Paired comparison of path efficiency): Two planners are tested on the same \( N=25 \) seeds. The paired differences in path efficiency have sample mean \( \bar{Z}=0.06 \) and standard deviation \( s_Z=0.10 \). Compute a 95% confidence interval for the mean difference.

Solution: The paired t-interval is \( \bar{Z} \pm t_{0.975,24}\,s_Z/\sqrt{25} \). Using \( t_{0.975,24}\approx 2.064 \):

\[ 0.06 \pm 2.064\left(\frac{0.10}{5}\right) = 0.06 \pm 0.04128. \]

Hence the interval is approximately \( [0.0187,\;0.1013] \), which excludes zero. Under the pairing assumptions, planner A has significantly higher mean path efficiency.

Robustness benchmarking for AMR navigation should combine binary safety and completion rates, continuous safety/quality margins, reliability-over-time analysis, and protocol-fixed aggregate scores. The key technical points are: (1) report uncertainty for proportions, (2) separate safety from efficiency, (3) specify logging/protocol assumptions explicitly, and (4) freeze normalization and weights before planner comparisons. This framework directly supports Chapter 19 labs on standard datasets, simulated stress tests, and full-system evaluation reports.