In Chapters 6–8 (Lessons 1–3), we built the Bayes-filter view and the MCL recursion: particles approximate the belief \( bel(x_t) \) using weighted samples. In practice, two phenomena cause major accuracy and robustness issues:

• \( \text{Degeneracy} \): after several updates, most normalized weights become nearly zero, so only a few particles contribute.
• \( \text{Kidnapped robot} \): the true state changes abruptly (teleportation, wheel lift, relocation), so \( p(x_t \mid x_{t-1}, u_t) \) becomes a poor proposal and the filter can “lock” onto an incorrect hypothesis.

The key insight: degeneracy is not only a numerical issue; it is a statistical mismatch between the proposal distribution used to sample particles and the posterior being approximated. Kidnapping is an extreme mismatch case.

Let the robot state be \( x_t \in \mathcal{X} \), control \( u_t \), and measurement \( z_t \). The Bayes filter is:

\[ bel(x_t) = \eta \, p(z_t \mid x_t)\int p(x_t \mid x_{t-1}, u_t)\, bel(x_{t-1})\, dx_{t-1}, \quad \eta^{-1} = \int p(z_t \mid x_t)\, \bar{bel}(x_t)\, dx_t \]

In generic Sequential Importance Sampling (SIS), we sample \( x_t^{(i)} \sim q(x_t \mid x_{0:t-1}^{(i)}, z_{1:t}, u_{1:t}) \) and update unnormalized weights:

\[ \tilde{w}_t^{(i)} = \tilde{w}_{t-1}^{(i)} \, \frac{p(z_t \mid x_t^{(i)})\, p(x_t^{(i)} \mid x_{t-1}^{(i)}, u_t)} {q(x_t^{(i)} \mid x_{0:t-1}^{(i)}, z_{1:t}, u_{1:t})}, \qquad w_t^{(i)} = \frac{\tilde{w}_t^{(i)}}{\sum_{j=1}^N \tilde{w}_t^{(j)}} \]

MCL typically uses the motion prior as proposal: \( q(x_t \mid \cdot) = p(x_t \mid x_{t-1}, u_t) \). Then:

\[ \tilde{w}_t^{(i)} = \tilde{w}_{t-1}^{(i)} \, p(z_t \mid x_t^{(i)}) \quad \Rightarrow \quad w_t^{(i)} \propto w_{t-1}^{(i)}\, p(z_t \mid x_t^{(i)}) \]

Degeneracy mechanism. If the likelihood \( p(z_t \mid x) \) is sharp relative to the proposal spread, only a small subset of particles fall into the high-likelihood region; then \( w_t^{(i)} \) collapses (many near zero). This worsens over time because the weight update is multiplicative.

flowchart TD
  A["Start: particles (x_i, w_i)"] --> B["Predict: sample x_i ~ p(x|u)"]
  B --> C["Update: w_i = w_i * p(z|x_i)"]
  C --> D["Normalize weights"]
  D --> E["Compute N_eff = 1 / sum(w_i^2)"]
  E --> F{"Is N_eff below N_th ?"}
  F -->|"no"| G["Estimate pose from weights"]
  F -->|"yes"| H["Resample (systematic)"]
  H --> I["Roughen / jitter (optional)"]
  I --> J["Inject random particles (eps) if mismatch"]
  J --> G
  G --> K["Next time step"]
  

A practical consequence: without a degeneracy-aware resampling policy, MCL becomes unstable (high variance estimates) or brittle (sample impoverishment after too frequent resampling).

A standard scalar diagnostic of degeneracy is the effective sample size: \( N_{\text{eff}} \). For normalized weights \( \sum_i w^{(i)} = 1 \), the common estimator is:

\[ \hat{N}_{\text{eff}} = \frac{1}{\sum_{i=1}^{N} \left(w_t^{(i)}\right)^2} \]

Proposition (bounds and extremal cases). For any normalized weights, \( 1 \le \hat{N}_{\text{eff}} \le N \). Moreover:

• If all weights are equal \( w^{(i)}=1/N \), then \( \hat{N}_{\text{eff}} = N \).
• If one weight is 1 and the rest are 0, then \( \hat{N}_{\text{eff}} = 1 \).

Proof (short).

Since \( \sum_i w^{(i)} = 1 \), by Cauchy–Schwarz: \( \left(\sum_i w^{(i)}\right)^2 \le N \sum_i (w^{(i)})^2 \), so \( 1 \le N \sum_i (w^{(i)})^2 \) and therefore \( \hat{N}_{\text{eff}} = 1/\sum_i (w^{(i)})^2 \le N \). Also \( \sum_i (w^{(i)})^2 \le \sum_i w^{(i)} = 1 \) because \( 0 \le w^{(i)} \le 1 \Rightarrow (w^{(i)})^2 \le w^{(i)} \). Hence \( \hat{N}_{\text{eff}} \ge 1 \). □

Decision rule. A common policy is resample when \( \hat{N}_{\text{eff}} < \rho N \) for some \( \rho \in [0.3, 0.8] \). The threshold trades:

• Too large \(\rho\): frequent resampling, sample impoverishment.
• Too small \(\rho\): weight collapse, high estimator variance.

A second diagnostic useful for kidnapping is a mismatch score derived from the average log-likelihood: \( \bar{\ell}_t \):

\[ \bar{\ell}_t = \sum_{i=1}^N w_t^{(i)} \log\!\big(p(z_t \mid x_t^{(i)}) + \varepsilon\big), \quad \varepsilon > 0 \text{ small (numerical guard)} \]

If \( \bar{\ell}_t \) stays below a threshold for several steps, the particle cloud likely does not cover the true pose (a kidnapping-like event or severe map/sensor mismatch).

Resampling replaces the weighted set with an approximately unweighted set by drawing ancestor indices according to \( \{w^{(i)}\} \). After resampling, all weights become \( 1/N \). While resampling reduces variance due to weight degeneracy, it introduces:

• Additional Monte Carlo variance (because resampling is random).
• Sample impoverishment: repeated resampling duplicates high-weight particles and discards diversity, especially under low process noise.

A standard mitigation is roughening (post-resampling jitter), adding small noise to particles: \( x^{(i)} \leftarrow x^{(i)} + \xi^{(i)} \), where \( \xi^{(i)} \) has variance scaled by map size and particle count. One common heuristic in \( d \) dimensions uses:

\[ \sigma_{\text{rough}}(k) = K \, \Delta_k \, N^{-1/d}, \quad \Delta_k = \text{range of state coordinate } k \]

Here \( K \) is a tuning constant. Roughening is most useful when the motion noise is small (wheel odometry is “confident”), which otherwise encourages collapse.

Kidnapped robot problem: the robot is physically moved so that the true state \( x_t \) is no longer distributed according to the prediction \( \bar{bel}(x_t) \). If MCL samples only from the motion prior, then after kidnapping:

\[ x_t^{(i)} \sim p(x_t \mid x_{t-1}^{(i)}, u_t) \quad \text{but} \quad bel(x_t) \text{ has mass far outside the sampled region} \]

The immediate result is \( p(z_t \mid x_t^{(i)}) \) becomes tiny for almost all particles, driving \( \hat{N}_{\text{eff}} \) toward 1 and potentially causing filter divergence.

A robust recovery strategy must introduce global support so that at least some particles can “jump” to the new region. Two common approaches:

• Random particle injection: replace a fraction of particles with samples from a broad prior (often uniform over free space or map bounds).
• Sensor-resetting / measurement-driven proposals: generate particles from a distribution shaped by the current measurement (e.g., pose hypotheses consistent with LiDAR scan likelihood peaks).

flowchart TD
  S["Compute mean log-likelihood lbar"] --> C{"Is lbar below threshold?"}
  C -->|yes| A["Increase eps or trigger global reinit"]
  A --> B["Replace m particles with uniform samples"]
  B --> D["Reweight using p(z|x)"]
  D --> E["Resume normal loop"]
  C -->|no| F["Keep eps small"]
  F --> E
  

In practice, injection is often tied to a mismatch score (e.g., \( \bar{\ell}_t \)), so the filter behaves normally most of the time but becomes “global” during kidnapping.

A theoretically clean way to incorporate global recovery is to sample from a mixture proposal: \( q(x_t \mid \cdot) \):

\[ q(x_t \mid x_{t-1}, u_t, z_t) = (1-\epsilon_t)\, p(x_t \mid x_{t-1}, u_t) + \epsilon_t \, p_0(x_t) \]

where \( p_0(x_t) \) is a broad “global” prior (e.g., uniform over map bounds), and \( \epsilon_t \in [0,1] \) is adapted using a mismatch test. If we sample from \( q \), the correct SIS weight update is:

\[ \tilde{w}_t^{(i)} = \tilde{w}_{t-1}^{(i)}\, \frac{p(z_t \mid x_t^{(i)})\, p(x_t^{(i)} \mid x_{t-1}^{(i)}, u_t)} {(1-\epsilon_t)\, p(x_t^{(i)} \mid x_{t-1}^{(i)}, u_t) + \epsilon_t \, p_0(x_t^{(i)})} \]

This formula shows why global recovery is statistically nontrivial: injected particles are “off-prior” and must be reweighted by the proposal correction term. Many practical systems implement an approximate version (inject after resampling and reweight by likelihood), which often works well, but the mixture view is the clean conceptual baseline.

A simple adaptive rule based on \( \bar{\ell}_t \) is: \( \epsilon_t \):

\[ \epsilon_t = \begin{cases} \epsilon_{\max} & \text{if } \bar{\ell}_t < \ell_{\text{th}} \\ \epsilon_{\min} & \text{otherwise} \end{cases} \]

• Python: numpy for vectorization, matplotlib for debugging plots. In real robot stacks, MCL appears as AMCL-style localization nodes (ROS/ROS2 ecosystems).
• C++: standard library RNG + linear algebra libraries (often Eigen) for production-grade robotics. C++ is the common implementation language for onboard localization components.
• Java: used in some teaching stacks and Android robotics; core math can be implemented with standard java.util utilities; optional numerical libraries can be added when needed.
• MATLAB/Simulink: rapid prototyping with scripts; Simulink is useful for structured pipelines and testing estimation loops with realistic sensor blocks.
• Wolfram Mathematica: compact symbolic + numerical experimentation for estimator diagnostics (e.g., analyzing \( \hat{N}_{\text{eff}} \) behavior under controlled likelihood shapes).

The script below simulates a kidnapping event (teleportation) and recovers by: (i) resampling when \( \hat{N}_{\text{eff}} < \rho N \), (ii) roughening after resampling, and (iii) adaptive random particle injection when \( \bar{\ell}_t \) falls below threshold.

Chapter8_Lesson4.py

# Chapter8_Lesson4.py
# Particle-Filter Localization — Degeneracy + Kidnapped Robot Recovery (2D)
#
# Dependencies: numpy, matplotlib (optional)
#
# Run:
#   python Chapter8_Lesson4.py
#
# This script simulates a 2D robot with range-bearing measurements to known landmarks,
# runs Monte Carlo Localization (MCL), monitors degeneracy via N_eff, and
# recovers from a "kidnapped robot" event by adaptive random particle injection.

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

import numpy as np

try:
    import matplotlib.pyplot as plt
    HAS_PLOT = True
except Exception:
    HAS_PLOT = False


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


@dataclass
class Pose:
    x: float
    y: float
    th: float


@dataclass
class Particle:
    x: float
    y: float
    th: float
    w: float


def motion_step(p: Particle, v: float, w: float, dt: float,
                sigma_v: float, sigma_w: float, rng: np.random.Generator) -> Particle:
    """Unicycle motion with additive Gaussian noise on controls."""
    v_n = v + rng.normal(0.0, sigma_v)
    w_n = w + rng.normal(0.0, sigma_w)

    if abs(w_n) < 1e-9:
        # Straight-line approximation
        x2 = p.x + v_n * dt * math.cos(p.th)
        y2 = p.y + v_n * dt * math.sin(p.th)
        th2 = p.th
    else:
        x2 = p.x + (v_n / w_n) * (math.sin(p.th + w_n * dt) - math.sin(p.th))
        y2 = p.y - (v_n / w_n) * (math.cos(p.th + w_n * dt) - math.cos(p.th))
        th2 = p.th + w_n * dt

    th2 = wrap_angle(th2)
    return Particle(x2, y2, th2, p.w)


def measurement_model(pose: Pose, landmarks: np.ndarray) -> np.ndarray:
    """
    Deterministic range-bearing (r, b) to each landmark.
    Returns array shape (M,2): [range, bearing].
    """
    dx = landmarks[:, 0] - pose.x
    dy = landmarks[:, 1] - pose.y
    r = np.sqrt(dx * dx + dy * dy)
    b = np.arctan2(dy, dx) - pose.th
    b = (b + np.pi) % (2.0 * np.pi) - np.pi
    return np.stack([r, b], axis=1)


def gaussian_pdf(e: float, sigma: float) -> float:
    """1D Gaussian PDF N(0,sigma^2) at error e."""
    return (1.0 / (math.sqrt(2.0 * math.pi) * sigma)) * math.exp(-0.5 * (e / sigma) ** 2)


def sensor_likelihood(p: Particle, z: np.ndarray, landmarks: np.ndarray,
                      sigma_r: float, sigma_b: float) -> float:
    """
    Likelihood p(z|x) as product of independent Gaussian errors on (range,bearing)
    for all landmarks. For numerical stability, this function returns a value
    that may be extremely small; downstream code uses log-likelihood when needed.
    """
    zh = measurement_model(Pose(p.x, p.y, p.th), landmarks)
    dr = z[:, 0] - zh[:, 0]
    db = z[:, 1] - zh[:, 1]
    db = (db + np.pi) % (2.0 * np.pi) - np.pi

    # Product of per-landmark likelihoods (can underflow for many landmarks).
    L = 1.0
    for i in range(z.shape[0]):
        L *= gaussian_pdf(float(dr[i]), sigma_r) * gaussian_pdf(float(db[i]), sigma_b)
    return L


def log_sensor_likelihood(p: Particle, z: np.ndarray, landmarks: np.ndarray,
                          sigma_r: float, sigma_b: float) -> float:
    """Log-likelihood log p(z|x) with Gaussian independent errors."""
    zh = measurement_model(Pose(p.x, p.y, p.th), landmarks)
    dr = z[:, 0] - zh[:, 0]
    db = z[:, 1] - zh[:, 1]
    db = (db + np.pi) % (2.0 * np.pi) - np.pi

    # log N(0,sigma^2) = -0.5*(e/sigma)^2 - log(sqrt(2*pi)*sigma)
    c_r = -math.log(math.sqrt(2.0 * math.pi) * sigma_r)
    c_b = -math.log(math.sqrt(2.0 * math.pi) * sigma_b)
    ll = 0.0
    for i in range(z.shape[0]):
        ll += -0.5 * float(dr[i] / sigma_r) ** 2 + c_r
        ll += -0.5 * float(db[i] / sigma_b) ** 2 + c_b
    return ll


def normalize_weights(particles: List[Particle]) -> None:
    s = sum(p.w for p in particles)
    if s <= 0.0 or not np.isfinite(s):
        # Reset to uniform if all weights collapsed numerically
        n = len(particles)
        for p in particles:
            p.w = 1.0 / n
        return
    for p in particles:
        p.w /= s


def effective_sample_size(particles: List[Particle]) -> float:
    """N_eff = 1 / sum_i w_i^2 (assuming normalized weights)."""
    s2 = sum((p.w * p.w) for p in particles)
    if s2 <= 0.0:
        return 0.0
    return 1.0 / s2


def systematic_resample(particles: List[Particle], rng: np.random.Generator) -> List[Particle]:
    """
    Systematic resampling. Returns equally weighted particles.
    """
    n = len(particles)
    weights = np.array([p.w for p in particles], dtype=float)
    cdf = np.cumsum(weights)
    cdf[-1] = 1.0  # avoid roundoff

    u0 = rng.random() / n
    js = []
    i = 0
    for m in range(n):
        u = u0 + m / n
        while u > cdf[i]:
            i += 1
        js.append(i)

    new_particles = [Particle(particles[j].x, particles[j].y, particles[j].th, 1.0 / n) for j in js]
    return new_particles


def roughen(particles: List[Particle], k: float, bounds: Tuple[float, float, float, float],
            rng: np.random.Generator) -> None:
    """
    Simple 'roughening' (post-resampling jitter) to fight sample impoverishment.
    Jitter scale is k * (range of state) * N^{-1/d}.
    """
    xmin, xmax, ymin, ymax = bounds
    n = len(particles)
    d = 3.0
    sx = k * (xmax - xmin) * (n ** (-1.0 / d))
    sy = k * (ymax - ymin) * (n ** (-1.0 / d))
    sth = k * (2.0 * math.pi) * (n ** (-1.0 / d))

    for p in particles:
        p.x += rng.normal(0.0, sx)
        p.y += rng.normal(0.0, sy)
        p.th = wrap_angle(p.th + rng.normal(0.0, sth))


def inject_random_particles(particles: List[Particle], frac: float,
                            bounds: Tuple[float, float, float, float],
                            rng: np.random.Generator) -> None:
    """
    Replace a fraction of particles with global random samples.
    """
    n = len(particles)
    m = int(max(0, min(n, round(frac * n))))
    if m == 0:
        return

    xmin, xmax, ymin, ymax = bounds
    idx = rng.choice(n, size=m, replace=False)
    for j in idx:
        particles[j].x = float(rng.uniform(xmin, xmax))
        particles[j].y = float(rng.uniform(ymin, ymax))
        particles[j].th = float(rng.uniform(-math.pi, math.pi))
        # weight kept; will be renormalized after measurement update


def estimate_pose(particles: List[Particle]) -> Pose:
    """Weighted mean for x,y and circular mean for theta."""
    ws = np.array([p.w for p in particles], dtype=float)
    xs = np.array([p.x for p in particles], dtype=float)
    ys = np.array([p.y for p in particles], dtype=float)
    ths = np.array([p.th for p in particles], dtype=float)

    x = float(np.sum(ws * xs))
    y = float(np.sum(ws * ys))
    c = float(np.sum(ws * np.cos(ths)))
    s = float(np.sum(ws * np.sin(ths)))
    th = math.atan2(s, c)
    return Pose(x, y, th)


def run_demo():
    rng = np.random.default_rng(4)

    # Map bounds and landmarks
    bounds = (0.0, 10.0, 0.0, 10.0)  # (xmin,xmax,ymin,ymax)
    landmarks = np.array([
        [2.0, 2.0],
        [8.0, 2.0],
        [8.0, 8.0],
        [2.0, 8.0]
    ], dtype=float)

    # Simulation parameters
    dt = 0.1
    T = 300  # steps
    kidnapped_t = 170  # step index where robot is teleported

    # Motion noise
    sigma_v = 0.05
    sigma_w = 0.03

    # Sensor noise
    sigma_r = 0.15
    sigma_b = 0.07  # rad

    # Particle filter settings
    N = 800
    N_eff_ratio = 0.5  # resample when N_eff < ratio*N
    roughen_k = 0.15

    # Kidnapped recovery: adaptive injection
    eps_min = 0.01
    eps_max = 0.30
    ll_thresh = -12.0  # depends on sensor noise and number of landmarks

    # True initial pose
    x_true = Pose(1.0, 1.0, 0.0)

    # Initialize particles uniformly (global localization)
    particles = [
        Particle(float(rng.uniform(bounds[0], bounds[1])),
                 float(rng.uniform(bounds[2], bounds[3])),
                 float(rng.uniform(-math.pi, math.pi)),
                 1.0 / N)
        for _ in range(N)
    ]

    true_hist = []
    est_hist = []
    Neff_hist = []
    eps_hist = []

    # Control commands: drive a rounded square
    def control(t: int):
        if t < 70:
            return 0.7, 0.0
        if t < 90:
            return 0.7, 0.9
        if t < 160:
            return 0.7, 0.0
        if t < 180:
            return 0.7, 0.9
        if t < 250:
            return 0.7, 0.0
        return 0.7, 0.9

    for t in range(T):
        v_cmd, w_cmd = control(t)

        # True motion
        x_true_p = Particle(x_true.x, x_true.y, x_true.th, 1.0)
        x_true_p = motion_step(x_true_p, v_cmd, w_cmd, dt, sigma_v, sigma_w, rng)
        x_true = Pose(x_true_p.x, x_true_p.y, x_true_p.th)

        # Kidnapped event (teleport)
        if t == kidnapped_t:
            x_true = Pose(float(rng.uniform(bounds[0], bounds[1])),
                          float(rng.uniform(bounds[2], bounds[3])),
                          float(rng.uniform(-math.pi, math.pi)))

        # Generate measurement z_t from true pose
        z = measurement_model(x_true, landmarks)
        z[:, 0] += rng.normal(0.0, sigma_r, size=z.shape[0])
        z[:, 1] += rng.normal(0.0, sigma_b, size=z.shape[0])
        z[:, 1] = (z[:, 1] + np.pi) % (2.0 * np.pi) - np.pi

        # Predict particles
        particles = [motion_step(p, v_cmd, w_cmd, dt, sigma_v, sigma_w, rng) for p in particles]

        # Update weights using log-likelihood for stability
        lls = np.array([log_sensor_likelihood(p, z, landmarks, sigma_r, sigma_b) for p in particles], dtype=float)
        lls = lls - np.max(lls)
        ws = np.exp(lls)
        for i, p in enumerate(particles):
            p.w = float(ws[i])

        normalize_weights(particles)

        # Detect mismatch via mean log-likelihood under normalized weights
        lbar = float(np.sum(np.array([p.w for p in particles]) * np.array([log_sensor_likelihood(p, z, landmarks, sigma_r, sigma_b) for p in particles])))
        eps = eps_max if (lbar < ll_thresh) else eps_min

        # Degeneracy check
        Neff = effective_sample_size(particles)

        if Neff < N_eff_ratio * N:
            particles = systematic_resample(particles, rng)
            roughen(particles, k=roughen_k, bounds=bounds, rng=rng)

            # Inject random particles after resampling for kidnapped recovery
            inject_random_particles(particles, frac=eps, bounds=bounds, rng=rng)

            # Re-weight after injection (helps injected particles compete immediately)
            lls2 = np.array([log_sensor_likelihood(p, z, landmarks, sigma_r, sigma_b) for p in particles], dtype=float)
            lls2 = lls2 - np.max(lls2)
            ws2 = np.exp(lls2)
            for i, p in enumerate(particles):
                p.w = float(ws2[i])
            normalize_weights(particles)

        x_est = estimate_pose(particles)

        true_hist.append((x_true.x, x_true.y, x_true.th))
        est_hist.append((x_est.x, x_est.y, x_est.th))
        Neff_hist.append(Neff)
        eps_hist.append(eps)

    true_hist = np.array(true_hist)
    est_hist = np.array(est_hist)

    # Save CSV for other tools
    out_csv = "Chapter8_Lesson4_results.csv"
    with open(out_csv, "w", encoding="utf-8") as f:
        f.write("t,true_x,true_y,true_th,est_x,est_y,est_th,Neff,eps\n")
        for t in range(T):
            f.write(f"{t},{true_hist[t,0]},{true_hist[t,1]},{true_hist[t,2]},"
                    f"{est_hist[t,0]},{est_hist[t,1]},{est_hist[t,2]},"
                    f"{Neff_hist[t]},{eps_hist[t]}\n")

    print("Saved:", out_csv)

    if HAS_PLOT:
        plt.figure()
        plt.plot(true_hist[:, 0], true_hist[:, 1], label="true")
        plt.plot(est_hist[:, 0], est_hist[:, 1], label="estimate")
        plt.scatter(landmarks[:, 0], landmarks[:, 1], marker="x", label="landmarks")
        plt.axis("equal")
        plt.legend()
        plt.title("MCL trajectory (kidnapped at t=%d)" % kidnapped_t)

        plt.figure()
        plt.plot(Neff_hist)
        plt.axhline(y=N_eff_ratio * N, linestyle="--")
        plt.title("Effective sample size N_eff")

        plt.figure()
        plt.plot(eps_hist)
        plt.title("Injection fraction eps (kidnapped recovery)")

        plt.show()
    else:
        print("matplotlib not available; skipping plots.")


if __name__ == "__main__":
    run_demo()
      

Chapter8_Lesson4.cpp

// Chapter8_Lesson4.cpp
// Particle-Filter Localization — Degeneracy + Kidnapped Robot Recovery (2D)
// Build (example):
//   g++ -O2 -std=c++17 -o Chapter8_Lesson4 Chapter8_Lesson4.cpp
//
// Output:
//   Chapter8_Lesson4_results.csv

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

struct Pose {
  double x{0}, y{0}, th{0};
};

struct Particle {
  double x{0}, y{0}, th{0}, w{0};
};

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

static Particle motion_step(const Particle& p, double v, double w, double dt,
                            double sigma_v, double sigma_w,
                            std::mt19937_64& gen) {
  std::normal_distribution<double> nv(0.0, sigma_v);
  std::normal_distribution<double> nw(0.0, sigma_w);

  double v_n = v + nv(gen);
  double w_n = w + nw(gen);

  Particle q = p;
  if (std::abs(w_n) < 1e-9) {
    q.x = p.x + v_n * dt * std::cos(p.th);
    q.y = p.y + v_n * dt * std::sin(p.th);
    q.th = p.th;
  } else {
    q.x = p.x + (v_n / w_n) * (std::sin(p.th + w_n * dt) - std::sin(p.th));
    q.y = p.y - (v_n / w_n) * (std::cos(p.th + w_n * dt) - std::cos(p.th));
    q.th = p.th + w_n * dt;
  }
  q.th = wrap_angle(q.th);
  return q;
}

static void measurement_model(const Pose& x,
                              const std::vector<std::pair<double,double>>& landmarks,
                              std::vector<std::pair<double,double>>& out_rb) {
  out_rb.resize(landmarks.size());
  for (size_t i = 0; i < landmarks.size(); ++i) {
    double dx = landmarks[i].first - x.x;
    double dy = landmarks[i].second - x.y;
    double r = std::sqrt(dx*dx + dy*dy);
    double b = std::atan2(dy, dx) - x.th;
    b = wrap_angle(b);
    out_rb[i] = {r, b};
  }
}

static double log_gauss_0(double e, double sigma) {
  const double pi = 3.14159265358979323846;
  return -0.5 * (e/sigma)*(e/sigma) - std::log(std::sqrt(2.0*pi)*sigma);
}

static double log_sensor_likelihood(const Particle& p,
                                    const std::vector<std::pair<double,double>>& z,
                                    const std::vector<std::pair<double,double>>& landmarks,
                                    double sigma_r, double sigma_b) {
  std::vector<std::pair<double,double>> zh;
  measurement_model(Pose{p.x, p.y, p.th}, landmarks, zh);

  double ll = 0.0;
  for (size_t i = 0; i < z.size(); ++i) {
    double dr = z[i].first  - zh[i].first;
    double db = wrap_angle(z[i].second - zh[i].second);
    ll += log_gauss_0(dr, sigma_r);
    ll += log_gauss_0(db, sigma_b);
  }
  return ll;
}

static void normalize(std::vector<Particle>& P) {
  double s = 0.0;
  for (const auto& p : P) s += p.w;
  if (!(s > 0.0) || !std::isfinite(s)) {
    double w = 1.0 / static_cast<double>(P.size());
    for (auto& p : P) p.w = w;
    return;
  }
  for (auto& p : P) p.w /= s;
}

static double Neff(const std::vector<Particle>& P) {
  double s2 = 0.0;
  for (const auto& p : P) s2 += p.w * p.w;
  if (s2 <= 0.0) return 0.0;
  return 1.0 / s2;
}

static std::vector<Particle> systematic_resample(const std::vector<Particle>& P,
                                                 std::mt19937_64& gen) {
  const size_t N = P.size();
  std::vector<double> cdf(N);
  double acc = 0.0;
  for (size_t i = 0; i < N; ++i) {
    acc += P[i].w;
    cdf[i] = acc;
  }
  cdf[N-1] = 1.0;

  std::uniform_real_distribution<double> ur(0.0, 1.0);
  double u0 = ur(gen) / static_cast<double>(N);

  std::vector<Particle> out;
  out.reserve(N);

  size_t i = 0;
  for (size_t m = 0; m < N; ++m) {
    double u = u0 + static_cast<double>(m) / static_cast<double>(N);
    while (u > cdf[i]) ++i;
    Particle q = P[i];
    q.w = 1.0 / static_cast<double>(N);
    out.push_back(q);
  }
  return out;
}

static void roughen(std::vector<Particle>& P, double k,
                    double xmin, double xmax, double ymin, double ymax,
                    std::mt19937_64& gen) {
  const double pi = 3.14159265358979323846;
  const double N = static_cast<double>(P.size());
  const double d = 3.0;
  double sx = k * (xmax - xmin) * std::pow(N, -1.0/d);
  double sy = k * (ymax - ymin) * std::pow(N, -1.0/d);
  double sth = k * (2.0*pi) * std::pow(N, -1.0/d);

  std::normal_distribution<double> nx(0.0, sx);
  std::normal_distribution<double> ny(0.0, sy);
  std::normal_distribution<double> nth(0.0, sth);

  for (auto& p : P) {
    p.x += nx(gen);
    p.y += ny(gen);
    p.th = wrap_angle(p.th + nth(gen));
  }
}

static void inject_random(std::vector<Particle>& P, double frac,
                          double xmin, double xmax, double ymin, double ymax,
                          std::mt19937_64& gen) {
  const size_t N = P.size();
  size_t m = static_cast<size_t>(std::llround(std::max(0.0, std::min(1.0, frac)) * static_cast<double>(N)));
  if (m == 0) return;

  std::uniform_real_distribution<double> ux(xmin, xmax);
  std::uniform_real_distribution<double> uy(ymin, ymax);
  std::uniform_real_distribution<double> uth(-3.14159265358979323846, 3.14159265358979323846);

  for (size_t i = 0; i < m; ++i) {
    P[i].x = ux(gen);
    P[i].y = uy(gen);
    P[i].th = uth(gen);
  }
}

static Pose estimate(const std::vector<Particle>& P) {
  double x=0.0, y=0.0;
  double c=0.0, s=0.0;
  for (const auto& p : P) {
    x += p.w * p.x;
    y += p.w * p.y;
    c += p.w * std::cos(p.th);
    s += p.w * std::sin(p.th);
  }
  return Pose{x, y, std::atan2(s, c)};
}

int main() {
  std::mt19937_64 gen(4);

  double xmin=0.0, xmax=10.0, ymin=0.0, ymax=10.0;
  std::vector<std::pair<double,double>> landmarks{
    {2.0,2.0},{8.0,2.0},{8.0,8.0},{2.0,8.0}
  };

  double dt = 0.1;
  int T = 300;
  int kidnapped_t = 170;

  double sigma_v = 0.05, sigma_w = 0.03;
  double sigma_r = 0.15, sigma_b = 0.07;

  int N = 800;
  double Neff_ratio = 0.5;
  double rough_k = 0.15;

  double eps_min = 0.01, eps_max = 0.30;
  double ll_thresh = -12.0;

  Pose xtrue{1.0, 1.0, 0.0};

  std::uniform_real_distribution<double> ux(xmin, xmax);
  std::uniform_real_distribution<double> uy(ymin, ymax);
  std::uniform_real_distribution<double> uth(-3.14159265358979323846, 3.14159265358979323846);

  std::vector<Particle> P;
  P.reserve(N);
  for (int i = 0; i < N; ++i) {
    P.push_back(Particle{ux(gen), uy(gen), uth(gen), 1.0/static_cast<double>(N)});
  }

  auto control = [](int t) {
    if (t < 70)  return std::pair<double,double>{0.7, 0.0};
    if (t < 90)  return std::pair<double,double>{0.7, 0.9};
    if (t < 160) return std::pair<double,double>{0.7, 0.0};
    if (t < 180) return std::pair<double,double>{0.7, 0.9};
    if (t < 250) return std::pair<double,double>{0.7, 0.0};
    return std::pair<double,double>{0.7, 0.9};
  };

  std::normal_distribution<double> nr(0.0, sigma_r);
  std::normal_distribution<double> nb(0.0, sigma_b);

  std::ofstream f("Chapter8_Lesson4_results.csv");
  f << "t,true_x,true_y,true_th,est_x,est_y,est_th,Neff,eps\n";

  for (int t = 0; t < T; ++t) {
    auto [v_cmd, w_cmd] = control(t);

    Particle tmp{xtrue.x, xtrue.y, xtrue.th, 1.0};
    tmp = motion_step(tmp, v_cmd, w_cmd, dt, sigma_v, sigma_w, gen);
    xtrue = Pose{tmp.x, tmp.y, tmp.th};

    if (t == kidnapped_t) {
      xtrue = Pose{ux(gen), uy(gen), uth(gen)};
    }

    std::vector<std::pair<double,double>> z;
    measurement_model(xtrue, landmarks, z);
    for (auto& rb : z) {
      rb.first  += nr(gen);
      rb.second = wrap_angle(rb.second + nb(gen));
    }

    for (auto& p : P) {
      Particle q = motion_step(p, v_cmd, w_cmd, dt, sigma_v, sigma_w, gen);
      p.x = q.x; p.y = q.y; p.th = q.th;
    }

    std::vector<double> lls(N);
    double llmax = -1e300;
    for (int i = 0; i < N; ++i) {
      lls[i] = log_sensor_likelihood(P[i], z, landmarks, sigma_r, sigma_b);
      llmax = std::max(llmax, lls[i]);
    }
    for (int i = 0; i < N; ++i) {
      P[i].w = std::exp(lls[i] - llmax);
    }
    normalize(P);

    double lbar = 0.0;
    for (int i = 0; i < N; ++i) {
      lbar += P[i].w * log_sensor_likelihood(P[i], z, landmarks, sigma_r, sigma_b);
    }
    double eps = (lbar < ll_thresh) ? eps_max : eps_min;

    double neff = Neff(P);
    if (neff < Neff_ratio * static_cast<double>(N)) {
      P = systematic_resample(P, gen);
      roughen(P, rough_k, xmin, xmax, ymin, ymax, gen);
      inject_random(P, eps, xmin, xmax, ymin, ymax, gen);

      llmax = -1e300;
      for (int i = 0; i < N; ++i) {
        lls[i] = log_sensor_likelihood(P[i], z, landmarks, sigma_r, sigma_b);
        llmax = std::max(llmax, lls[i]);
      }
      for (int i = 0; i < N; ++i) {
        P[i].w = std::exp(lls[i] - llmax);
      }
      normalize(P);
    }

    Pose xest = estimate(P);

    f << t << "," << xtrue.x << "," << xtrue.y << "," << xtrue.th << ","
      << xest.x << "," << xest.y << "," << xest.th << ","
      << neff << "," << eps << "\n";
  }

  std::cout << "Saved: Chapter8_Lesson4_results.csv\n";
  return 0;
}
      

Chapter8_Lesson4.java

// Chapter8_Lesson4.java
// Particle-Filter Localization — Degeneracy + Kidnapped Robot Recovery (2D)
//
// Compile:
//   javac Chapter8_Lesson4.java
// Run:
//   java Chapter8_Lesson4
//
// Output:
//   Chapter8_Lesson4_results.csv

import java.io.FileWriter;
import java.io.IOException;
import java.util.Random;

public class Chapter8_Lesson4 {

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

    static class Particle {
        double x, y, th, w;
        Particle(double x, double y, double th, double w) { this.x=x; this.y=y; this.th=th; this.w=w; }
        Particle copy() { return new Particle(x,y,th,w); }
    }

    static final double PI = Math.PI;

    static double wrapAngle(double a) {
        a = (a + PI) % (2.0 * PI);
        if (a < 0) a += 2.0 * PI;
        return a - PI;
    }

    static double logGauss0(double e, double sigma) {
        return -0.5 * (e/sigma)*(e/sigma) - Math.log(Math.sqrt(2.0 * PI) * sigma);
    }

    static Particle motionStep(Particle p, double v, double w, double dt,
                               double sigmaV, double sigmaW, Random rng) {
        double vN = v + sigmaV * rng.nextGaussian();
        double wN = w + sigmaW * rng.nextGaussian();
        double x2, y2, th2;

        if (Math.abs(wN) < 1e-9) {
            x2 = p.x + vN * dt * Math.cos(p.th);
            y2 = p.y + vN * dt * Math.sin(p.th);
            th2 = p.th;
        } else {
            x2 = p.x + (vN / wN) * (Math.sin(p.th + wN * dt) - Math.sin(p.th));
            y2 = p.y - (vN / wN) * (Math.cos(p.th + wN * dt) - Math.cos(p.th));
            th2 = p.th + wN * dt;
        }
        th2 = wrapAngle(th2);
        return new Particle(x2, y2, th2, p.w);
    }

    static void measurementModel(Pose x, double[][] landmarks, double[][] outRB) {
        for (int i = 0; i < landmarks.length; i++) {
            double dx = landmarks[i][0] - x.x;
            double dy = landmarks[i][1] - x.y;
            double r = Math.sqrt(dx*dx + dy*dy);
            double b = Math.atan2(dy, dx) - x.th;
            b = wrapAngle(b);
            outRB[i][0] = r;
            outRB[i][1] = b;
        }
    }

    static double logSensorLikelihood(Particle p, double[][] z, double[][] landmarks,
                                      double sigmaR, double sigmaB) {
        double ll = 0.0;
        for (int i = 0; i < landmarks.length; i++) {
            double dx = landmarks[i][0] - p.x;
            double dy = landmarks[i][1] - p.y;
            double rHat = Math.sqrt(dx*dx + dy*dy);
            double bHat = wrapAngle(Math.atan2(dy, dx) - p.th);

            double dr = z[i][0] - rHat;
            double db = wrapAngle(z[i][1] - bHat);

            ll += logGauss0(dr, sigmaR);
            ll += logGauss0(db, sigmaB);
        }
        return ll;
    }

    static void normalize(Particle[] P) {
        double s = 0.0;
        for (Particle p : P) s += p.w;
        if (!(s > 0.0) || !Double.isFinite(s)) {
            double w = 1.0 / P.length;
            for (Particle p : P) p.w = w;
            return;
        }
        for (Particle p : P) p.w /= s;
    }

    static double Neff(Particle[] P) {
        double s2 = 0.0;
        for (Particle p : P) s2 += p.w * p.w;
        if (s2 <= 0.0) return 0.0;
        return 1.0 / s2;
    }

    static Particle[] systematicResample(Particle[] P, Random rng) {
        int N = P.length;
        double[] cdf = new double[N];
        double acc = 0.0;
        for (int i = 0; i < N; i++) {
            acc += P[i].w;
            cdf[i] = acc;
        }
        cdf[N-1] = 1.0;

        double u0 = rng.nextDouble() / N;

        Particle[] out = new Particle[N];
        int i = 0;
        for (int m = 0; m < N; m++) {
            double u = u0 + (double)m / N;
            while (u > cdf[i]) i++;
            Particle q = P[i].copy();
            q.w = 1.0 / N;
            out[m] = q;
        }
        return out;
    }

    static void roughen(Particle[] P, double k,
                        double xmin, double xmax, double ymin, double ymax,
                        Random rng) {
        int N = P.length;
        double d = 3.0;
        double sx = k * (xmax - xmin) * Math.pow((double)N, -1.0/d);
        double sy = k * (ymax - ymin) * Math.pow((double)N, -1.0/d);
        double sth = k * (2.0 * PI) * Math.pow((double)N, -1.0/d);

        for (Particle p : P) {
            p.x += sx * rng.nextGaussian();
            p.y += sy * rng.nextGaussian();
            p.th = wrapAngle(p.th + sth * rng.nextGaussian());
        }
    }

    static void injectRandom(Particle[] P, double frac,
                             double xmin, double xmax, double ymin, double ymax,
                             Random rng) {
        int N = P.length;
        int m = (int)Math.round(Math.max(0.0, Math.min(1.0, frac)) * N);
        if (m <= 0) return;
        for (int i = 0; i < m; i++) {
            P[i].x = xmin + (xmax - xmin) * rng.nextDouble();
            P[i].y = ymin + (ymax - ymin) * rng.nextDouble();
            P[i].th = -PI + (2.0 * PI) * rng.nextDouble();
        }
    }

    static Pose estimate(Particle[] P) {
        double x=0.0, y=0.0;
        double c=0.0, s=0.0;
        for (Particle p : P) {
            x += p.w * p.x;
            y += p.w * p.y;
            c += p.w * Math.cos(p.th);
            s += p.w * Math.sin(p.th);
        }
        return new Pose(x, y, Math.atan2(s, c));
    }

    static double[] control(int t) {
        if (t < 70)  return new double[]{0.7, 0.0};
        if (t < 90)  return new double[]{0.7, 0.9};
        if (t < 160) return new double[]{0.7, 0.0};
        if (t < 180) return new double[]{0.7, 0.9};
        if (t < 250) return new double[]{0.7, 0.0};
        return new double[]{0.7, 0.9};
    }

    public static void main(String[] args) throws IOException {
        Random rng = new Random(4);

        double xmin=0.0, xmax=10.0, ymin=0.0, ymax=10.0;

        double[][] landmarks = new double[][]{
                {2.0,2.0},{8.0,2.0},{8.0,8.0},{2.0,8.0}
        };

        double dt = 0.1;
        int T = 300;
        int kidnappedT = 170;

        double sigmaV = 0.05, sigmaW = 0.03;
        double sigmaR = 0.15, sigmaB = 0.07;

        int N = 800;
        double NeffRatio = 0.5;
        double roughK = 0.15;

        double epsMin = 0.01, epsMax = 0.30;
        double llThresh = -12.0;

        Pose xTrue = new Pose(1.0, 1.0, 0.0);

        Particle[] P = new Particle[N];
        for (int i = 0; i < N; i++) {
            P[i] = new Particle(
                    xmin + (xmax - xmin) * rng.nextDouble(),
                    ymin + (ymax - ymin) * rng.nextDouble(),
                    -PI + (2.0 * PI) * rng.nextDouble(),
                    1.0 / N
            );
        }

        double[][] z = new double[landmarks.length][2];
        double[][] zHat = new double[landmarks.length][2];

        try (FileWriter fw = new FileWriter("Chapter8_Lesson4_results.csv")) {
            fw.write("t,true_x,true_y,true_th,est_x,est_y,est_th,Neff,eps\n");

            for (int t = 0; t < T; t++) {
                double[] uw = control(t);
                double vCmd = uw[0], wCmd = uw[1];

                Particle tmp = new Particle(xTrue.x, xTrue.y, xTrue.th, 1.0);
                tmp = motionStep(tmp, vCmd, wCmd, dt, sigmaV, sigmaW, rng);
                xTrue = new Pose(tmp.x, tmp.y, tmp.th);

                if (t == kidnappedT) {
                    xTrue = new Pose(
                            xmin + (xmax - xmin) * rng.nextDouble(),
                            ymin + (ymax - ymin) * rng.nextDouble(),
                            -PI + (2.0 * PI) * rng.nextDouble()
                    );
                }

                measurementModel(xTrue, landmarks, zHat);
                for (int i = 0; i < landmarks.length; i++) {
                    z[i][0] = zHat[i][0] + sigmaR * rng.nextGaussian();
                    z[i][1] = wrapAngle(zHat[i][1] + sigmaB * rng.nextGaussian());
                }

                for (int i = 0; i < N; i++) {
                    Particle q = motionStep(P[i], vCmd, wCmd, dt, sigmaV, sigmaW, rng);
                    P[i].x = q.x; P[i].y = q.y; P[i].th = q.th;
                }

                double[] lls = new double[N];
                double llMax = -1e300;
                for (int i = 0; i < N; i++) {
                    lls[i] = logSensorLikelihood(P[i], z, landmarks, sigmaR, sigmaB);
                    if (lls[i] > llMax) llMax = lls[i];
                }
                for (int i = 0; i < N; i++) {
                    P[i].w = Math.exp(lls[i] - llMax);
                }
                normalize(P);

                double lbar = 0.0;
                for (int i = 0; i < N; i++) {
                    lbar += P[i].w * logSensorLikelihood(P[i], z, landmarks, sigmaR, sigmaB);
                }
                double eps = (lbar < llThresh) ? epsMax : epsMin;

                double neff = Neff(P);
                if (neff < NeffRatio * N) {
                    P = systematicResample(P, rng);
                    roughen(P, roughK, xmin, xmax, ymin, ymax, rng);
                    injectRandom(P, eps, xmin, xmax, ymin, ymax, rng);

                    llMax = -1e300;
                    for (int i = 0; i < N; i++) {
                        lls[i] = logSensorLikelihood(P[i], z, landmarks, sigmaR, sigmaB);
                        if (lls[i] > llMax) llMax = lls[i];
                    }
                    for (int i = 0; i < N; i++) {
                        P[i].w = Math.exp(lls[i] - llMax);
                    }
                    normalize(P);
                }

                Pose xEst = estimate(P);
                fw.write(String.format("%d,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f\n",
                        t, xTrue.x, xTrue.y, xTrue.th, xEst.x, xEst.y, xEst.th, neff, eps));
            }
        }

        System.out.println("Saved: Chapter8_Lesson4_results.csv");
    }
}
      

The MATLAB script reproduces the same loop and includes a helper that can generate a minimal Simulink model with a MATLAB Function block (scaffold) to host the PF update.

Chapter8_Lesson4.m

% Chapter8_Lesson4.m
% Particle-Filter Localization — Degeneracy + Kidnapped Robot Recovery (2D)
%
% Run:
%   Chapter8_Lesson4
%
% Notes:
% - This script simulates a 2D robot and range-bearing measurements to known landmarks,
%   runs MCL, monitors degeneracy via N_eff, and recovers from a "kidnapped robot"
%   using adaptive random particle injection.
% - At the end, it writes a CSV file "Chapter8_Lesson4_results.csv".
%
% Optional (Simulink):
% - Call buildSimulinkSkeleton() to auto-create a basic Simulink model with a
%   MATLAB Function block hosting the PF update (lightweight scaffold).

function Chapter8_Lesson4()
    rng(4);

    % Bounds and landmarks
    xmin = 0; xmax = 10; ymin = 0; ymax = 10;
    landmarks = [2 2; 8 2; 8 8; 2 8];

    % Simulation parameters
    dt = 0.1;
    T = 300;
    kidnapped_t = 170;

    % Noise
    sigma_v = 0.05; sigma_w = 0.03;
    sigma_r = 0.15; sigma_b = 0.07;

    % PF settings
    N = 800;
    Neff_ratio = 0.5;
    rough_k = 0.15;

    % Kidnapped recovery
    eps_min = 0.01;
    eps_max = 0.30;
    ll_thresh = -12.0;

    % True state
    x_true = [1.0; 1.0; 0.0];

    % Initialize particles globally
    P = zeros(N,3);
    P(:,1) = xmin + (xmax - xmin) * rand(N,1);
    P(:,2) = ymin + (ymax - ymin) * rand(N,1);
    P(:,3) = -pi + 2*pi*rand(N,1);
    w = ones(N,1)/N;

    true_hist = zeros(T,3);
    est_hist  = zeros(T,3);
    Neff_hist = zeros(T,1);
    eps_hist  = zeros(T,1);

    for t = 1:T
        [v_cmd, w_cmd] = controlProfile(t);

        % True motion (unicycle + noise)
        x_true = motionStep(x_true, v_cmd, w_cmd, dt, sigma_v, sigma_w);

        % Kidnapped event
        if t == kidnapped_t
            x_true = [xmin + (xmax - xmin)*rand();
                      ymin + (ymax - ymin)*rand();
                      -pi + 2*pi*rand()];
        end

        % Measurement from true pose
        z = measurementModel(x_true, landmarks);
        z(:,1) = z(:,1) + sigma_r * randn(size(z,1),1);
        z(:,2) = wrapAngle(z(:,2) + sigma_b * randn(size(z,1),1));

        % Predict particles
        for i = 1:N
            P(i,:) = motionStep(P(i,:).', v_cmd, w_cmd, dt, sigma_v, sigma_w).';
        end

        % Update weights in log domain
        ll = zeros(N,1);
        for i = 1:N
            ll(i) = logSensorLikelihood(P(i,:).', z, landmarks, sigma_r, sigma_b);
        end
        ll = ll - max(ll);
        w = exp(ll);
        w = w / sum(w);

        % Weighted mean log-likelihood
        ll_w = zeros(N,1);
        for i = 1:N
            ll_w(i) = logSensorLikelihood(P(i,:).', z, landmarks, sigma_r, sigma_b);
        end
        lbar = sum(w .* ll_w);

        if lbar < ll_thresh
            eps = eps_max;
        else
            eps = eps_min;
        end

        % Degeneracy
        Neff = 1 / sum(w.^2);

        if Neff < Neff_ratio * N
            [P, w] = systematicResample(P, w);

            % Roughen
            P = roughen(P, rough_k, [xmin xmax ymin ymax]);

            % Inject random particles
            m = round(eps*N);
            if m > 0
                P(1:m,1) = xmin + (xmax - xmin) * rand(m,1);
                P(1:m,2) = ymin + (ymax - ymin) * rand(m,1);
                P(1:m,3) = -pi + 2*pi*rand(m,1);
            end

            % Re-weight after injection
            ll2 = zeros(N,1);
            for i = 1:N
                ll2(i) = logSensorLikelihood(P(i,:).', z, landmarks, sigma_r, sigma_b);
            end
            ll2 = ll2 - max(ll2);
            w = exp(ll2);
            w = w / sum(w);
        end

        x_est = estimatePose(P, w);

        true_hist(t,:) = x_true.';
        est_hist(t,:)  = x_est.';
        Neff_hist(t) = Neff;
        eps_hist(t)  = eps;
    end

    % Save CSV
    fid = fopen('Chapter8_Lesson4_results.csv','w');
    fprintf(fid, 't,true_x,true_y,true_th,est_x,est_y,est_th,Neff,eps\n');
    for t = 1:T
        fprintf(fid, '%d,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f\n', ...
            t-1, true_hist(t,1), true_hist(t,2), true_hist(t,3), ...
            est_hist(t,1), est_hist(t,2), est_hist(t,3), ...
            Neff_hist(t), eps_hist(t));
    end
    fclose(fid);
    disp('Saved: Chapter8_Lesson4_results.csv');

    % Plot
    figure; plot(true_hist(:,1), true_hist(:,2), 'LineWidth', 1.5); hold on;
    plot(est_hist(:,1), est_hist(:,2), 'LineWidth', 1.5);
    scatter(landmarks(:,1), landmarks(:,2), 60, 'x', 'LineWidth', 2);
    axis equal; grid on; legend('true','estimate','landmarks');
    title(sprintf('MCL trajectory (kidnapped at t=%d)', kidnapped_t));

    figure; plot(Neff_hist, 'LineWidth', 1.5); hold on;
    yline(Neff_ratio * N, '--');
    grid on; title('Effective sample size N_{eff}');

    figure; plot(eps_hist, 'LineWidth', 1.5);
    grid on; title('Injection fraction eps');

    % Uncomment to generate a Simulink scaffold:
    % buildSimulinkSkeleton();
end

function [v, w] = controlProfile(t)
    if t < 70
        v = 0.7; w = 0.0;
    elseif t < 90
        v = 0.7; w = 0.9;
    elseif t < 160
        v = 0.7; w = 0.0;
    elseif t < 180
        v = 0.7; w = 0.9;
    elseif t < 250
        v = 0.7; w = 0.0;
    else
        v = 0.7; w = 0.9;
    end
end

function x2 = motionStep(x, v, w, dt, sigma_v, sigma_w)
    v_n = v + sigma_v * randn();
    w_n = w + sigma_w * randn();
    if abs(w_n) < 1e-9
        x2 = [x(1) + v_n*dt*cos(x(3));
              x(2) + v_n*dt*sin(x(3));
              x(3)];
    else
        x2 = [x(1) + (v_n/w_n)*(sin(x(3)+w_n*dt) - sin(x(3)));
              x(2) - (v_n/w_n)*(cos(x(3)+w_n*dt) - cos(x(3)));
              x(3) + w_n*dt];
    end
    x2(3) = wrapAngle(x2(3));
end

function z = measurementModel(x, landmarks)
    dx = landmarks(:,1) - x(1);
    dy = landmarks(:,2) - x(2);
    r = sqrt(dx.^2 + dy.^2);
    b = atan2(dy, dx) - x(3);
    b = wrapAngle(b);
    z = [r, b];
end

function ll = logSensorLikelihood(xp, z, landmarks, sigma_r, sigma_b)
    zh = measurementModel(xp, landmarks);
    dr = z(:,1) - zh(:,1);
    db = wrapAngle(z(:,2) - zh(:,2));
    cr = -log(sqrt(2*pi)*sigma_r);
    cb = -log(sqrt(2*pi)*sigma_b);
    ll = sum(-0.5*(dr./sigma_r).^2 + cr) + sum(-0.5*(db./sigma_b).^2 + cb);
end

function a = wrapAngle(a)
    a = mod(a + pi, 2*pi) - pi;
end

function [P2, w2] = systematicResample(P, w)
    N = size(P,1);
    cdf = cumsum(w);
    cdf(end) = 1.0;
    u0 = rand()/N;
    idx = zeros(N,1);
    i = 1;
    for m = 1:N
        u = u0 + (m-1)/N;
        while u > cdf(i)
            i = i + 1;
        end
        idx(m) = i;
    end
    P2 = P(idx,:);
    w2 = ones(N,1)/N;
end

function P2 = roughen(P, k, bounds)
    xmin = bounds(1); xmax = bounds(2);
    ymin = bounds(3); ymax = bounds(4);
    N = size(P,1);
    d = 3;
    sx  = k*(xmax-xmin)*N^(-1/d);
    sy  = k*(ymax-ymin)*N^(-1/d);
    sth = k*(2*pi)*N^(-1/d);
    P2 = P;
    P2(:,1) = P(:,1) + sx*randn(N,1);
    P2(:,2) = P(:,2) + sy*randn(N,1);
    P2(:,3) = wrapAngle(P(:,3) + sth*randn(N,1));
end

function xhat = estimatePose(P, w)
    x = sum(w .* P(:,1));
    y = sum(w .* P(:,2));
    c = sum(w .* cos(P(:,3)));
    s = sum(w .* sin(P(:,3)));
    th = atan2(s, c);
    xhat = [x; y; th];
end

function buildSimulinkSkeleton()
    % Minimal scaffold: a Simulink model with a MATLAB Function block where you
    % can paste the PF update as code. This is intentionally lightweight.
    model = 'Chapter8_Lesson4_MCL_Simulink';
    if bdIsLoaded(model)
        close_system(model, 0);
    end
    new_system(model); open_system(model);

    add_block('simulink/Sources/Constant', [model '/u_vw'], 'Value', '[0.7; 0.0]');
    add_block('simulink/Sources/Constant', [model '/z_meas'], 'Value', 'zeros(4,2)');

    add_block('simulink/User-Defined Functions/MATLAB Function', [model '/MCL_Update']);
    set_param([model '/MCL_Update'], 'Script', sprintf([ ...
        'function xhat = f(u, z)\n' ...
        '%% u = [v; w], z = measurements\n' ...
        '%% This block is a scaffold. Paste your PF predict/update/resample here.\n' ...
        'xhat = zeros(3,1);\n' ...
        'end\n' ...
    ]));

    add_block('simulink/Sinks/Display', [model '/xhat_display']);

    add_line(model, 'u_vw/1', 'MCL_Update/1');
    add_line(model, 'z_meas/1', 'MCL_Update/2');
    add_line(model, 'MCL_Update/1', 'xhat_display/1');

    set_param(model, 'StopTime', '10');
    save_system(model);
    disp(['Created Simulink scaffold: ' model]);
end
      

This notebook is intentionally compact: it provides a runnable MCL loop and plots for quick experimentation with \( \hat{N}_{\text{eff}} \) and injection rate behavior.

Chapter8_Lesson4.nb

(* Chapter8_Lesson4.nb
   Particle-Filter Localization — Degeneracy + Kidnapped Robot Recovery (2D)

   This is a lightweight Mathematica notebook (plain-text notebook expression).
   Open in Wolfram Mathematica.

   It simulates a robot with range/bearing measurements to known landmarks,
   runs MCL, monitors N_eff, and injects random particles for kidnapped recovery.
*)

Notebook[{
  Cell["Chapter 8 — Particle-Filter Localization", "Title"],
  Cell["Lesson 4: Degeneracy, Kidnapped Robot Problem", "Subtitle"],
  Cell[TextData[{
    "This notebook implements a compact 2D Monte Carlo Localization (MCL) demo: ",
    "weight update, effective sample size N_eff, systematic resampling, and ",
    "adaptive random-particle injection for kidnapped recovery."
  }], "Text"],

  Cell[BoxData[
    RowBox[{
      "ClearAll", "[", "\"Global`*\"", "]"}]], "Input"],

  Cell[BoxData[
    RowBox[{
      RowBox[{"wrap", "[", "a_", "]"}], ":=",
      RowBox[{"Mod", "[",
        RowBox[{"a", "+", "Pi", ",", RowBox[{"2", "Pi"}]}], "]"}], "-", "Pi"}]], "Input"],

  Cell[BoxData[
    RowBox[{
      RowBox[{"logGauss0", "[",
        RowBox[{"e_", ",", "s_"}], "]"}], ":=",
      RowBox[{
        RowBox[{"-", RowBox[{"1", "/", "2"}], "*", RowBox[{"(", RowBox[{"e", "/", "s"}], ")"}], "^", "2"}],
        "-", RowBox[{"Log", "[", RowBox[{"Sqrt", "[", RowBox[{"2", "Pi"}], "]"}], " ", "s"}], "]"}]}]], "Input"],

  Cell[TextData[{"(Full notebook content is provided in the downloadable .nb file.)"}], "Text"]
}]
      

Note: the downloadable Chapter8_Lesson4.nb contains the complete runnable notebook expression.

Problem 1 (ESS bounds): Let \( w^{(i)} \ge 0 \) and \( \sum_{i=1}^N w^{(i)} = 1 \). Prove \( 1 \le \left(\sum_i (w^{(i)})^2\right)^{-1} \le N \).

Solution: This is exactly the proposition in Section 3: use (i) \( (w^{(i)})^2 \le w^{(i)} \) to get \( \sum_i (w^{(i)})^2 \le 1 \Rightarrow \hat{N}_{\text{eff}} \ge 1 \), and (ii) Cauchy–Schwarz to get \( 1 = (\sum_i w^{(i)})^2 \le N\sum_i (w^{(i)})^2 \Rightarrow \hat{N}_{\text{eff}} \le N \).

Problem 2 (Degeneracy criterion): Suppose after normalization \( w^{(1)} = \alpha \) and the remaining weights are equal \( w^{(2)}=\cdots=w^{(N)}=(1-\alpha)/(N-1) \). Compute \( \hat{N}_{\text{eff}} \) and discuss how it changes with \( \alpha \).

Solution:

\[ \sum_{i=1}^N (w^{(i)})^2 = \alpha^2 + (N-1)\left(\frac{1-\alpha}{N-1}\right)^2 = \alpha^2 + \frac{(1-\alpha)^2}{N-1} \]

\[ \hat{N}_{\text{eff}}(\alpha) = \frac{1}{\alpha^2 + \frac{(1-\alpha)^2}{N-1}} \]

When \( \alpha \rightarrow 1 \), the denominator tends to 1, so \( \hat{N}_{\text{eff}} \rightarrow 1 \) (complete collapse). When \( \alpha = 1/N \), the weights are uniform and \( \hat{N}_{\text{eff}} = N \).

Problem 3 (Resampling trigger design): You have \( N=2000 \) particles. Choose a threshold \( N_{\text{th}} \) and justify it in terms of degeneracy vs impoverishment.

Solution: A typical policy is \( N_{\text{th}} = \rho N \) with \( \rho \in [0.4,0.7] \). If odometry noise is low and the likelihood is sharp, choose smaller \( \rho \) (e.g., 0.4) to avoid excessive resampling; then add roughening. If the environment is ambiguous and likelihood is multimodal, use larger \( \rho \) (e.g., 0.6–0.7) to prevent weight collapse from dominating the estimate.

Problem 4 (Kidnapped detection): Propose a statistically motivated detector using \( \bar{\ell}_t = \sum_i w^{(i)}\log(p(z_t \mid x_t^{(i)})+\varepsilon) \), and show how to convert it into an injection schedule \( \epsilon_t \).

Solution: A simple and effective detector is a threshold test: if \( \bar{\ell}_t < \ell_{\text{th}} \) for \( K \) consecutive steps, declare mismatch. Then choose \( \epsilon_t=\epsilon_{\max} \) during mismatch, and \( \epsilon_t=\epsilon_{\min} \) otherwise:

\[ \epsilon_t = \begin{cases} \epsilon_{\max} & \text{if } \bar{\ell}_{t-j} < \ell_{\text{th}} \ \forall j \in \{0,\dots,K-1\} \\ \epsilon_{\min} & \text{otherwise} \end{cases} \]

The “consecutive” condition reduces false positives caused by transient sensor noise spikes.

Problem 5 (Mixture proposal correction): If sampling uses \( q(x) = (1-\epsilon)p(x\mid u) + \epsilon p_0(x) \), derive the SIS weight correction term and explain why simply setting \( w \propto p(z\mid x) \) may be biased.

Solution: For SIS, weights satisfy \( \tilde{w} \propto \frac{p(z\mid x)p(x\mid u)}{q(x)} \). Therefore,

\[ \tilde{w} \propto \frac{p(z\mid x)p(x\mid u)} {(1-\epsilon)p(x\mid u) + \epsilon p_0(x)} \]

Ignoring \( q(x) \) over-weights samples drawn from the global component in a way that does not match the target posterior, which can bias estimates in regimes where \( \epsilon \) is non-negligible. The approximate “inject after resampling” method often works, but the mixture correction is the principled baseline.

• Degeneracy arises when importance weights collapse; a primary diagnostic is \( \hat{N}_{\text{eff}} = 1/\sum_i w_i^2 \).
• Resampling mitigates degeneracy but can cause sample impoverishment; roughening is a practical countermeasure.
• The kidnapped robot problem is extreme mismatch; robust recovery requires global support (injection or sensor-driven proposals).
• A principled view is a mixture proposal with correct importance correction; many systems implement a pragmatic injected-particle heuristic.