In mobile robot localization we estimate the robot pose \( \mathbf{x}_t = [x_t, y_t, \theta_t]^\top \in \mathbb{R}^2 \times \mathbb{S}^1 \) given controls \( \mathbf{u}_{1:t} \), observations \( \mathbf{z}_{1:t} \), and a known environment representation (e.g., landmarks, grid map). The central object is the belief: \( \mathrm{bel}(\mathbf{x}_t) \), a probability density (or distribution) over pose.

MCL replaces a continuous distribution with a finite set of samples (“particles”) that can represent multi-modal and highly non-Gaussian beliefs—situations where Kalman-type methods can be brittle due to linearization and unimodality assumptions. Intuitively:

• \( \mathrm{bel}(\mathbf{x}_t) \) is a “cloud” of plausible poses, not a single pose.
• Motion spreads the cloud (uncertainty grows); sensing contracts it (uncertainty shrinks).
• A particle set is an operational way to carry this cloud through nonlinear transformations.

flowchart TD
  A["Initialize particles from prior over pose"] --> B["Predict: sample motion model using control u_t"]
  B --> C["Score: compute sensor likelihood for each particle using z_t"]
  C --> D["Normalize weights"]
  D --> E["Resample: replicate high-weight particles, discard low-weight particles"]
  E --> F["Estimate pose (mean / MAP / best particle)"]
  F --> B
        

The loop above is the “shape” of MCL. The formal justification comes from the Bayes filter and from the theory of empirical measures and Monte Carlo estimators.

We adopt the standard Markov assumptions already introduced in Chapter 6:

• State Markov: \( p(\mathbf{x}_t \mid \mathbf{x}_{0:t-1}, \mathbf{u}_{1:t}) = p(\mathbf{x}_t \mid \mathbf{x}_{t-1}, \mathbf{u}_t) \).
• Conditional observation independence: \( p(\mathbf{z}_t \mid \mathbf{x}_{0:t}, \mathbf{u}_{1:t}, \mathbf{z}_{1:t-1}) = p(\mathbf{z}_t \mid \mathbf{x}_t) \).

The Bayes filter update for localization is:

\[ \mathrm{bel}(\mathbf{x}_t) = \eta \, p(\mathbf{z}_t \mid \mathbf{x}_t) \int p(\mathbf{x}_t \mid \mathbf{u}_t, \mathbf{x}_{t-1}) \, \mathrm{bel}(\mathbf{x}_{t-1}) \, d\mathbf{x}_{t-1} \]

where \( \eta \) is the normalization constant ensuring the belief integrates to one:

\[ \eta^{-1} = \int p(\mathbf{z}_t \mid \mathbf{x}_t) \int p(\mathbf{x}_t \mid \mathbf{u}_t, \mathbf{x}_{t-1}) \, \mathrm{bel}(\mathbf{x}_{t-1}) \, d\mathbf{x}_{t-1} \, d\mathbf{x}_t \]

MCL’s key idea is: keep the recursion, but approximate the integrals by Monte Carlo (sampling) operations.

A particle set is a weighted empirical distribution: \( \{(\mathbf{x}_t^{(i)}, w_t^{(i)})\}_{i=1}^N \) , where \( \mathbf{x}_t^{(i)} \) is a pose sample and \( w_t^{(i)} \ge 0 \) with \( \sum_{i=1}^N w_t^{(i)} = 1 \). The particle approximation of the belief can be written using Dirac deltas:

\[ \mathrm{bel}_N(\mathbf{x}_t) = \sum_{i=1}^N w_t^{(i)} \, \delta\!\big(\mathbf{x}_t - \mathbf{x}_t^{(i)}\big). \]

For any integrable test function \( \varphi(\mathbf{x}) \), the belief expectation is:

\[ \mathbb{E}_{\mathrm{bel} }[\varphi(\mathbf{x}_t)] = \int \varphi(\mathbf{x}_t)\, \mathrm{bel}(\mathbf{x}_t)\, d\mathbf{x}_t \approx \sum_{i=1}^N w_t^{(i)}\, \varphi\!\big(\mathbf{x}_t^{(i)}\big) = \mathbb{E}_{\mathrm{bel}_N}[\varphi(\mathbf{x}_t)]. \]

Proposition (Monte Carlo consistency, unweighted case). Suppose \( \mathbf{X}^{(i)} \) are i.i.d. samples from a distribution with density \( \mathrm{bel}(\mathbf{x}) \), and define the unweighted estimator \( \hat{I}_N = \frac{1}{N}\sum_{i=1}^N \varphi(\mathbf{X}^{(i)}) \). If \( \mathbb{E}[|\varphi(\mathbf{X})|] < \infty \), then \( \hat{I}_N \rightarrow \mathbb{E}[\varphi(\mathbf{X})] \) almost surely.

Proof. By the Strong Law of Large Numbers, for i.i.d. random variables \( Y_i = \varphi(\mathbf{X}^{(i)}) \) with \( \mathbb{E}[|Y_1|] < \infty \), we have \( \frac{1}{N}\sum_{i=1}^N Y_i \rightarrow \mathbb{E}[Y_1] \) almost surely. Substituting \( Y_i \) yields the claim. ■

Variance scaling (intuition that drives particle counts). If \( \mathbb{V}\mathrm{ar}(\varphi(\mathbf{X})) < \infty \), then

\[ \mathbb{V}\mathrm{ar}(\hat{I}_N) = \mathbb{V}\mathrm{ar}\!\left(\frac{1}{N}\sum_{i=1}^N \varphi(\mathbf{X}^{(i)})\right) = \frac{1}{N}\mathbb{V}\mathrm{ar}\big(\varphi(\mathbf{X})\big). \]

This \( 1/N \) scaling explains why particle filters trade compute for accuracy: doubling accuracy typically costs substantially more than doubling particles due to model evaluation costs.

The prediction integral \( \int p(\mathbf{x}_t \mid \mathbf{u}_t, \mathbf{x}_{t-1})\,\mathrm{bel}(\mathbf{x}_{t-1})\,d\mathbf{x}_{t-1} \) is approximated by sampling:

\[ \mathbf{x}_t^{(i)} \sim p(\mathbf{x}_t \mid \mathbf{u}_t, \mathbf{x}_{t-1}^{(i)}), \quad i=1,\dots,N. \]

For a common planar kinematic update (differential-drive style) with control \( \mathbf{u}_t = [v_t,\omega_t]^\top \) and time step \( \Delta t \), one (noise-free) discretization is:

\[ \mathbf{x}_t = \begin{cases} \begin{bmatrix} x_{t-1} + v_t \Delta t \cos\theta_{t-1} \\ y_{t-1} + v_t \Delta t \sin\theta_{t-1} \\ \theta_{t-1} \end{bmatrix} & \text{if } |\omega_t| \approx 0 \\ \\ \begin{bmatrix} x_{t-1} + \frac{v_t}{\omega_t}\left(\sin(\theta_{t-1}+\omega_t\Delta t)-\sin\theta_{t-1}\right) \\ y_{t-1} + \frac{v_t}{\omega_t}\left(-\cos(\theta_{t-1}+\omega_t\Delta t)+\cos\theta_{t-1}\right) \\ \theta_{t-1} + \omega_t\Delta t \end{bmatrix} & \text{otherwise.} \end{cases} \]

In MCL, we explicitly model motion uncertainty (slip, discretization, actuator noise) by sampling noisy controls (one simple choice): \( v_t' = v_t + \epsilon_v \), \( \omega_t' = \omega_t + \epsilon_\omega \), with \( \epsilon_v \sim \mathcal{N}(0,\sigma_v^2) \), \( \epsilon_\omega \sim \mathcal{N}(0,\sigma_\omega^2) \).

After prediction we incorporate the new sensor reading via \( p(\mathbf{z}_t \mid \mathbf{x}_t) \). MCL encodes this as particle weights:

\[ \tilde{w}_t^{(i)} \propto p(\mathbf{z}_t \mid \mathbf{x}_t^{(i)}), \qquad w_t^{(i)} = \frac{\tilde{w}_t^{(i)} }{\sum_{j=1}^N \tilde{w}_t^{(j)} }. \]

Concrete likelihood (range-to-landmarks). Suppose we have known landmark positions \( \{\mathbf{m}_k\}_{k=1}^K \) and measure ranges \( \mathbf{z}_t = [r_{t,1},\dots,r_{t,K}]^\top \). The predicted range from pose \( \mathbf{x} = [x,y,\theta]^\top \) to landmark \( \mathbf{m}_k = [m_{k,x},m_{k,y}]^\top \) is: \( \hat{r}_k(\mathbf{x}) = \sqrt{(m_{k,x}-x)^2 + (m_{k,y}-y)^2} \).

With independent Gaussian range noise, \( r_{t,k} = \hat{r}_k(\mathbf{x}_t) + \nu_k \), \( \nu_k \sim \mathcal{N}(0,\sigma_r^2) \), the likelihood becomes:

\[ p(\mathbf{z}_t \mid \mathbf{x}_t) = \prod_{k=1}^K \mathcal{N}\!\big(r_{t,k}; \hat{r}_k(\mathbf{x}_t), \sigma_r^2\big) \propto \exp\!\left(-\frac{1}{2\sigma_r^2}\sum_{k=1}^K \big(r_{t,k} - \hat{r}_k(\mathbf{x}_t)\big)^2\right). \]

Operationally, MCL is a repeated cycle of: sample motion, score measurement consistency, and manage weight collapse by resampling (formal mechanisms are the focus of Lesson 2).

After repeated weighting, most particles often carry negligible weight (a form of degeneracy). Resampling replaces the weighted set with an (approximately) unweighted set drawn from the categorical distribution over indices: \( \mathbb{P}(I=i) = w_t^{(i)} \).

A common point estimate is the empirical mean pose:

\[ \hat{x}_t = \sum_{i=1}^N w_t^{(i)} x_t^{(i)}, \qquad \hat{y}_t = \sum_{i=1}^N w_t^{(i)} y_t^{(i)}. \]

For heading, naïvely averaging angles fails near wrap-around. Use the circular mean: \( \hat{\theta}_t = \mathrm{atan2}\!\left(\sum_i w_t^{(i)}\sin\theta_t^{(i)}, \sum_i w_t^{(i)}\cos\theta_t^{(i)}\right) \).

\[ \hat{\theta}_t = \mathrm{atan2}\!\left(\sum_{i=1}^N w_t^{(i)}\sin\theta_t^{(i)}, \sum_{i=1}^N w_t^{(i)}\cos\theta_t^{(i)}\right). \]

• Initialization: global localization uses a broad prior; tracking uses a tight prior around last estimate.
• Particle count: increases with map ambiguity and sensor sparsity; decreases when sensor likelihood is sharp and distinctive.
• Noise parameters: \( \sigma_v,\sigma_\omega \) capture motion uncertainty; \( \sigma_r \) captures sensor noise and unmodeled effects.
• Real stacks: ROS/ROS2 typically deploy “AMCL” variants (adaptive particle counts, robust sensor models). This lesson focuses on the base mechanism that those systems extend.

The remainder of this lesson provides minimal, from-scratch implementations in multiple languages that directly realize the predict–weight–resample loop using a landmark-range sensor model.

Typical robotics-adjacent Python tooling includes NumPy/SciPy, Matplotlib, and (in full systems) ROS2 (rclpy) plus navigation packages. Here we keep a stand-alone scientific stack.

File: Chapter8_Lesson1.py

"""
Chapter 8 - Particle-Filter Localization
Lesson 1: Monte Carlo Localization (MCL) Intuition

Minimal from-scratch MCL in a known 2D landmark map.
- State: x = [x, y, theta]
- Control: u = [v, omega] (differential-drive style)
- Measurement: ranges to known landmarks

Dependencies:
  pip install numpy matplotlib
"""

import numpy as np
import matplotlib.pyplot as plt


def wrap_angle(a: float) -> float:
    return (a + np.pi) % (2 * np.pi) - np.pi


def motion_model(x: np.ndarray, u: np.ndarray, dt: float,
                 sigma_v: float, sigma_omega: float, rng: np.random.Generator) -> np.ndarray:
    """Sample x_t ~ p(x_t | u_t, x_{t-1})."""
    v = u[0] + rng.normal(0.0, sigma_v)
    w = u[1] + rng.normal(0.0, sigma_omega)

    x_new = x.copy()
    th = x[2]
    if abs(w) < 1e-9:
        x_new[0] += v * dt * np.cos(th)
        x_new[1] += v * dt * np.sin(th)
    else:
        x_new[0] += (v / w) * (np.sin(th + w * dt) - np.sin(th))
        x_new[1] += (v / w) * (-np.cos(th + w * dt) + np.cos(th))
        x_new[2] = wrap_angle(th + w * dt)
    return x_new


def expected_ranges(x: np.ndarray, landmarks: np.ndarray) -> np.ndarray:
    dx = landmarks[:, 0] - x[0]
    dy = landmarks[:, 1] - x[1]
    return np.sqrt(dx * dx + dy * dy)


def gaussian_logpdf(e: np.ndarray, sigma: float) -> float:
    # log N(e; 0, sigma^2 I) up to constant terms included for numerical stability
    return -0.5 * np.sum((e / sigma) ** 2) - e.size * np.log(sigma * np.sqrt(2 * np.pi))


def systematic_resample(particles: np.ndarray, weights: np.ndarray, rng: np.random.Generator) -> np.ndarray:
    """Systematic resampling: O(N), low variance."""
    n = particles.shape[0]
    positions = (rng.random() + np.arange(n)) / n
    cdf = np.cumsum(weights)
    cdf[-1] = 1.0
    idx = np.searchsorted(cdf, positions)
    return particles[idx].copy()


def mcl_step(particles: np.ndarray, weights: np.ndarray, u: np.ndarray, z: np.ndarray,
             landmarks: np.ndarray, dt: float,
             sigma_v: float, sigma_omega: float, sigma_r: float,
             rng: np.random.Generator):
    # 1) Predict
    for i in range(particles.shape[0]):
        particles[i] = motion_model(particles[i], u, dt, sigma_v, sigma_omega, rng)

    # 2) Update weights via range likelihood p(z | x)
    logw = np.empty(particles.shape[0])
    for i in range(particles.shape[0]):
        zhat = expected_ranges(particles[i], landmarks)
        logw[i] = gaussian_logpdf(z - zhat, sigma_r)
    logw -= np.max(logw)  # stabilize
    weights = np.exp(logw)
    weights /= np.sum(weights)

    # 3) Resample
    particles = systematic_resample(particles, weights, rng)
    weights.fill(1.0 / particles.shape[0])

    # 4) Point estimate (weighted mean after resample is simply average)
    mu = particles.mean(axis=0)
    mu[2] = np.arctan2(np.mean(np.sin(particles[:, 2])), np.mean(np.cos(particles[:, 2])))
    return particles, weights, mu


def simulate_true_robot(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
    """Noise-free kinematics for 'ground truth' (for demo)."""
    v, w = u
    x_new = x.copy()
    th = x[2]
    if abs(w) < 1e-9:
        x_new[0] += v * dt * np.cos(th)
        x_new[1] += v * dt * np.sin(th)
    else:
        x_new[0] += (v / w) * (np.sin(th + w * dt) - np.sin(th))
        x_new[1] += (v / w) * (-np.cos(th + w * dt) + np.cos(th))
        x_new[2] = wrap_angle(th + w * dt)
    return x_new


def main():
    rng = np.random.default_rng(0)

    # Map: four landmarks at the corners of a square
    landmarks = np.array([[-5.0, -5.0], [5.0, -5.0], [5.0, 5.0], [-5.0, 5.0]])

    # True pose
    x_true = np.array([-3.0, -2.0, 0.3])

    # MCL parameters
    N = 800
    sigma_v = 0.10
    sigma_omega = 0.05
    sigma_r = 0.35
    dt = 0.1

    # Initial belief: broad Gaussian around origin
    particles = np.zeros((N, 3))
    particles[:, 0] = rng.normal(0.0, 3.0, size=N)
    particles[:, 1] = rng.normal(0.0, 3.0, size=N)
    particles[:, 2] = rng.uniform(-np.pi, np.pi, size=N)
    weights = np.full(N, 1.0 / N)

    # Controls: gentle curve
    T = 120
    traj_true = [x_true.copy()]
    traj_est = []

    for t in range(T):
        u = np.array([0.6, 0.25])  # v, omega
        x_true = simulate_true_robot(x_true, u, dt)
        traj_true.append(x_true.copy())

        # Range measurement (noisy)
        z = expected_ranges(x_true, landmarks) + rng.normal(0.0, sigma_r, size=landmarks.shape[0])

        particles, weights, mu = mcl_step(
            particles, weights, u, z, landmarks, dt,
            sigma_v, sigma_omega, sigma_r, rng
        )
        traj_est.append(mu)

    traj_true = np.array(traj_true)
    traj_est = np.array(traj_est)

    # Plot
    plt.figure()
    plt.scatter(particles[:, 0], particles[:, 1], s=5, alpha=0.25, label="particles")
    plt.plot(traj_true[:, 0], traj_true[:, 1], label="true")
    plt.plot(traj_est[:, 0], traj_est[:, 1], label="estimate")
    plt.scatter(landmarks[:, 0], landmarks[:, 1], marker="x", s=80, label="landmarks")
    plt.axis("equal")
    plt.grid(True)
    plt.title("Monte Carlo Localization (MCL) — Intuition Demo")
    plt.legend()
    plt.show()


if __name__ == "__main__":
    main()
      

Common C++ robotics ecosystems: Eigen (linear algebra), ROS2 (rclcpp), and full localization stacks (e.g., AMCL in Nav2). This demo is a compact stand-alone program using Eigen and STL random sampling.

File: Chapter8_Lesson1.cpp

/*
Chapter 8 - Particle-Filter Localization
Lesson 1: Monte Carlo Localization (MCL) Intuition

Minimal from-scratch MCL in a known 2D landmark map (ranges).
Dependencies: Eigen (header-only)

Compile (example):
  g++ -O2 -std=c++17 Chapter8_Lesson1.cpp -I/usr/include/eigen3 -o mcl_demo
*/

#include <iostream>
#include <vector>
#include <random>
#include <cmath>
#include <numeric>
#include <algorithm>

#include <Eigen/Dense>

struct Particle {
  Eigen::Vector3d x;  // [x, y, theta]
  double w;
};

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

static Eigen::Vector3d motion_sample(
    const Eigen::Vector3d& x, const Eigen::Vector2d& u, double dt,
    double sigma_v, double sigma_omega,
    std::mt19937& gen)
{
  std::normal_distribution<double> nv(0.0, sigma_v);
  std::normal_distribution<double> nw(0.0, sigma_omega);

  const double v = u(0) + nv(gen);
  const double w = u(1) + nw(gen);

  Eigen::Vector3d xn = x;
  const double th = x(2);

  if (std::abs(w) < 1e-9) {
    xn(0) += v * dt * std::cos(th);
    xn(1) += v * dt * std::sin(th);
  } else {
    xn(0) += (v / w) * (std::sin(th + w * dt) - std::sin(th));
    xn(1) += (v / w) * (-std::cos(th + w * dt) + std::cos(th));
    xn(2) = wrap_angle(th + w * dt);
  }
  return xn;
}

static Eigen::VectorXd expected_ranges(
    const Eigen::Vector3d& x,
    const std::vector<Eigen::Vector2d>& landmarks)
{
  Eigen::VectorXd r(landmarks.size());
  for (size_t i = 0; i < landmarks.size(); ++i) {
    const double dx = landmarks - x(0);
    const double dy = landmarks - x(1);
    r(i) = std::sqrt(dx*dx + dy*dy);
  }
  return r;
}

static double gaussian_logpdf(const Eigen::VectorXd& e, double sigma) {
  const double pi = 3.14159265358979323846;
  const double n = static_cast<double>(e.size());
  return -0.5 * (e.array() / sigma).square().sum() - n * std::log(sigma * std::sqrt(2.0 * pi));
}

static std::vector<Particle> systematic_resample(
    const std::vector<Particle>& P,
    std::mt19937& gen)
{
  const int N = static_cast<int>(P.size());
  std::uniform_real_distribution<double> uni(0.0, 1.0);

  std::vector<double> cdf(N);
  cdf[0] = P[0].w;
  for (int i = 1; i < N; ++i) cdf[i] = cdf[i-1] + P[i].w;
  cdf.back() = 1.0;

  const double u0 = uni(gen) / static_cast<double>(N);
  std::vector<Particle> out;
  out.reserve(N);

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

static Eigen::Vector3d mean_pose(const std::vector<Particle>& P) {
  Eigen::Vector3d mu = Eigen::Vector3d::Zero();
  double s = 0.0;
  double c = 0.0;
  for (const auto& p : P) {
    mu(0) += p.x(0);
    mu(1) += p.x(1);
    s += std::sin(p.x(2));
    c += std::cos(p.x(2));
  }
  mu(0) /= static_cast<double>(P.size());
  mu(1) /= static_cast<double>(P.size());
  mu(2) = std::atan2(s / static_cast<double>(P.size()), c / static_cast<double>(P.size()));
  return mu;
}

int main() {
  std::mt19937 gen(0);
  std::normal_distribution<double> n0(0.0, 1.0);
  std::uniform_real_distribution<double> unif(-3.14159265358979323846, 3.14159265358979323846);

  // Landmarks: square corners
  std::vector<Eigen::Vector2d> lm = {
    {-5.0, -5.0}, {5.0, -5.0}, {5.0, 5.0}, {-5.0, 5.0}
  };

  Eigen::Vector3d x_true(-3.0, -2.0, 0.3);

  const int N = 800;
  const double sigma_v = 0.10, sigma_omega = 0.05, sigma_r = 0.35;
  const double dt = 0.1;

  // Initialize particles
  std::vector<Particle> P(N);
  for (int i = 0; i < N; ++i) {
    P[i].x(0) = 3.0 * n0(gen);
    P[i].x(1) = 3.0 * n0(gen);
    P[i].x(2) = unif(gen);
    P[i].w = 1.0 / static_cast<double>(N);
  }

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

  // Run
  const int T = 120;
  for (int t = 0; t < T; ++t) {
    const Eigen::Vector2d u(0.6, 0.25);  // v, omega (commanded)

    // True pose (noise-free for demo)
    x_true = motion_sample(x_true, u, dt, 0.0, 0.0, gen);

    // Range measurement (noisy)
    Eigen::VectorXd z = expected_ranges(x_true, lm);
    for (int i = 0; i < z.size(); ++i) z(i) += nr(gen);

    // Predict
    for (auto& p : P) p.x = motion_sample(p.x, u, dt, sigma_v, sigma_omega, gen);

    // Weight update
    std::vector<double> logw(N);
    double maxlogw = -1e300;
    for (int i = 0; i < N; ++i) {
      const Eigen::VectorXd zhat = expected_ranges(P[i].x, lm);
      const Eigen::VectorXd e = z - zhat;
      logw[i] = gaussian_logpdf(e, sigma_r);
      if (logw[i] > maxlogw) maxlogw = logw[i];
    }

    double sumw = 0.0;
    for (int i = 0; i < N; ++i) {
      P[i].w = std::exp(logw[i] - maxlogw);
      sumw += P[i].w;
    }
    for (auto& p : P) p.w /= sumw;

    // Resample
    P = systematic_resample(P, gen);

    // Report estimate
    const Eigen::Vector3d mu = mean_pose(P);
    std::cout << "t=" << t
              << " true=(" << x_true(0) << "," << x_true(1) << "," << x_true(2) << ")"
              << " est=("  << mu(0)     << "," << mu(1)     << "," << mu(2)     << ")"
              << "\n";
  }

  std::cout << "Done.\n";
  return 0;
}
      

Java in robotics often appears in middleware tooling, simulation, and legacy ROSJava systems. For numerics, common libraries include EJML or Apache Commons Math; here we implement sampling and likelihood from scratch.

File: Chapter8_Lesson1.java

/*
Chapter 8 - Particle-Filter Localization
Lesson 1: Monte Carlo Localization (MCL) Intuition

Minimal from-scratch MCL in a known 2D landmark map (ranges).
Output: prints true and estimated pose each step.

Compile:
  javac Chapter8_Lesson1.java
Run:
  java Chapter8_Lesson1
*/

import java.util.Arrays;
import java.util.Random;

public class Chapter8_Lesson1 {

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

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

    // Box-Muller
    static double randn(Random rng) {
        double u1 = Math.max(1e-12, rng.nextDouble());
        double u2 = rng.nextDouble();
        return Math.sqrt(-2.0 * Math.log(u1)) * Math.cos(2.0 * Math.PI * u2);
    }

    static void motionSample(double[] out, double x, double y, double th,
                             double vCmd, double wCmd, double dt,
                             double sigmaV, double sigmaW, Random rng) {
        double v = vCmd + sigmaV * randn(rng);
        double w = wCmd + sigmaW * randn(rng);

        if (Math.abs(w) < 1e-9) {
            out[0] = x + v * dt * Math.cos(th);
            out[1] = y + v * dt * Math.sin(th);
            out[2] = th;
        } else {
            out[0] = x + (v / w) * (Math.sin(th + w * dt) - Math.sin(th));
            out[1] = y + (v / w) * (-Math.cos(th + w * dt) + Math.cos(th));
            out[2] = wrapAngle(th + w * dt);
        }
    }

    static double[] expectedRanges(double x, double y, double[][] landmarks) {
        double[] r = new double[landmarks.length];
        for (int i = 0; i < landmarks.length; i++) {
            double dx = landmarks[i][0] - x;
            double dy = landmarks[i][1] - y;
            r[i] = Math.sqrt(dx*dx + dy*dy);
        }
        return r;
    }

    static double gaussianLogPdf(double[] e, double sigma) {
        double logNorm = -Math.log(sigma * Math.sqrt(2.0 * Math.PI));
        double s = 0.0;
        for (double v : e) s += -0.5 * (v / sigma) * (v / sigma) + logNorm;
        return s;
    }

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

        double u0 = rng.nextDouble() / (double)N;
        Particle[] out = new Particle[N];
        int i = 0;
        for (int m = 0; m < N; m++) {
            double u = u0 + (double)m / (double)N;
            while (u > cdf[i] && i < N-1) i++;
            out[m] = new Particle(P[i].x, P[i].y, P[i].th, 1.0 / (double)N);
        }
        return out;
    }

    static double[] meanPose(Particle[] P) {
        int N = P.length;
        double mx = 0.0, my = 0.0;
        double s = 0.0, c = 0.0;
        for (Particle p : P) {
            mx += p.x; my += p.y;
            s += Math.sin(p.th);
            c += Math.cos(p.th);
        }
        mx /= (double)N;
        my /= (double)N;
        double mth = Math.atan2(s / (double)N, c / (double)N);
        return new double[]{mx, my, mth};
    }

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

        double[][] landmarks = new double[][]{
                {-5.0, -5.0}, {5.0, -5.0}, {5.0, 5.0}, {-5.0, 5.0}
        };

        double[] xTrue = new double[]{-3.0, -2.0, 0.3};

        int N = 800;
        double sigmaV = 0.10, sigmaW = 0.05, sigmaR = 0.35;
        double dt = 0.1;

        Particle[] P = new Particle[N];
        for (int i = 0; i < N; i++) {
            double x = 3.0 * randn(rng);
            double y = 3.0 * randn(rng);
            double th = -Math.PI + 2.0 * Math.PI * rng.nextDouble();
            P[i] = new Particle(x, y, th, 1.0 / (double)N);
        }

        int T = 120;
        for (int t = 0; t < T; t++) {
            double vCmd = 0.6, wCmd = 0.25;

            // True robot (noise-free for demo)
            double[] tmp = new double[3];
            motionSample(tmp, xTrue[0], xTrue[1], xTrue[2], vCmd, wCmd, dt, 0.0, 0.0, rng);
            xTrue = tmp;

            // Measurement: ranges with noise
            double[] z = expectedRanges(xTrue[0], xTrue[1], landmarks);
            for (int i = 0; i < z.length; i++) z[i] += sigmaR * randn(rng);

            // Predict
            for (Particle p : P) {
                motionSample(tmp, p.x, p.y, p.th, vCmd, wCmd, dt, sigmaV, sigmaW, rng);
                p.x = tmp[0]; p.y = tmp[1]; p.th = tmp[2];
            }

            // Weight update
            double[] logw = new double[N];
            double maxLogw = -1e300;
            for (int i = 0; i < N; i++) {
                double[] zhat = expectedRanges(P[i].x, P[i].y, landmarks);
                double[] e = new double[zhat.length];
                for (int k = 0; k < e.length; k++) e[k] = z[k] - zhat[k];
                logw[i] = gaussianLogPdf(e, sigmaR);
                if (logw[i] > maxLogw) maxLogw = logw[i];
            }
            double sumw = 0.0;
            for (int i = 0; i < N; i++) {
                P[i].w = Math.exp(logw[i] - maxLogw);
                sumw += P[i].w;
            }
            for (int i = 0; i < N; i++) P[i].w /= sumw;

            // Resample
            P = systematicResample(P, rng);

            double[] mu = meanPose(P);
            System.out.printf("t=%d true=(%.3f,%.3f,%.3f) est=(%.3f,%.3f,%.3f)%n",
                    t, xTrue[0], xTrue[1], xTrue[2], mu[0], mu[1], mu[2]);
        }

        System.out.println("Done.");
    }
}
      

MATLAB is widely used in control and robotics prototyping. If you have Robotics System Toolbox, you can later compare this script with toolbox-based localization workflows; however, the code below is fully stand-alone and can be integrated into Simulink via MATLAB Function blocks (e.g., implementing mclStep as a function).

File: Chapter8_Lesson1.m

% Chapter 8 - Particle-Filter Localization
% Lesson 1: Monte Carlo Localization (MCL) Intuition
%
% Minimal from-scratch MCL in a known 2D landmark map (range-only).
% State: x = [x; y; theta]
%
% Tested with base MATLAB. If you have Robotics System Toolbox, you can
% later compare against built-in localization utilities, but this script
% is fully stand-alone.

clear; clc; close all;
rng(0);

% Landmarks (square corners)
landmarks = [-5 -5;
              5 -5;
              5  5;
             -5  5];

% True pose
xTrue = [-3; -2; 0.3];

% Parameters
N = 800;
dt = 0.1;

sigmaV = 0.10;
sigmaW = 0.05;
sigmaR = 0.35;

% Initialize particles (broad prior)
P = zeros(3, N);
P(1,:) = 3.0 * randn(1,N);
P(2,:) = 3.0 * randn(1,N);
P(3,:) = -pi + 2*pi*rand(1,N);
w = ones(1,N) / N;

T = 120;
trajTrue = zeros(3, T+1);
trajEst  = zeros(3, T);
trajTrue(:,1) = xTrue;

for t = 1:T
    u = [0.6; 0.25]; % [v; omega]

    % True robot (noise-free for demo)
    xTrue = motionDeterministic(xTrue, u, dt);
    trajTrue(:,t+1) = xTrue;

    % Measurement: ranges with noise
    z = expectedRanges(xTrue, landmarks) + sigmaR*randn(size(landmarks,1),1);

    % MCL step
    [P, w, mu] = mclStep(P, w, u, z, landmarks, dt, sigmaV, sigmaW, sigmaR);
    trajEst(:,t) = mu;
end

figure; hold on; grid on; axis equal;
scatter(P(1,:), P(2,:), 8, 'filled', 'MarkerFaceAlpha', 0.15);
plot(trajTrue(1,:), trajTrue(2,:), 'LineWidth', 1.5);
plot(trajEst(1,:), trajEst(2,:), 'LineWidth', 1.5);
scatter(landmarks(:,1), landmarks(:,2), 80, 'x', 'LineWidth', 2);
legend('particles', 'true', 'estimate', 'landmarks');
title('Monte Carlo Localization (MCL) — Intuition Demo');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function x = motionDeterministic(x, u, dt)
    v = u(1); w = u(2);
    th = x(3);
    if abs(w) < 1e-9
        x(1) = x(1) + v*dt*cos(th);
        x(2) = x(2) + v*dt*sin(th);
    else
        x(1) = x(1) + (v/w)*(sin(th+w*dt) - sin(th));
        x(2) = x(2) + (v/w)*(-cos(th+w*dt) + cos(th));
        x(3) = wrapAngle(th + w*dt);
    end
end

function x = motionSample(x, u, dt, sigmaV, sigmaW)
    v = u(1) + sigmaV*randn();
    w = u(2) + sigmaW*randn();
    th = x(3);
    if abs(w) < 1e-9
        x(1) = x(1) + v*dt*cos(th);
        x(2) = x(2) + v*dt*sin(th);
    else
        x(1) = x(1) + (v/w)*(sin(th+w*dt) - sin(th));
        x(2) = x(2) + (v/w)*(-cos(th+w*dt) + cos(th));
        x(3) = wrapAngle(th + w*dt);
    end
end

function r = expectedRanges(x, landmarks)
    dx = landmarks(:,1) - x(1);
    dy = landmarks(:,2) - x(2);
    r = sqrt(dx.^2 + dy.^2);
end

function lw = gaussianLogPdf(e, sigma)
    lw = -0.5*sum((e./sigma).^2) - numel(e)*log(sigma*sqrt(2*pi));
end

function idx = systematicResample(w)
    N = numel(w);
    cdf = cumsum(w);
    cdf(end) = 1.0;
    u0 = rand()/N;
    u = u0 + (0:(N-1))/N;
    idx = zeros(1,N);
    i = 1;
    for m = 1:N
        while u(m) > cdf(i) && i < N
            i = i + 1;
        end
        idx(m) = i;
    end
end

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

function [P, w, mu] = mclStep(P, w, u, z, landmarks, dt, sigmaV, sigmaW, sigmaR)
    N = size(P,2);

    % Predict
    for i = 1:N
        P(:,i) = motionSample(P(:,i), u, dt, sigmaV, sigmaW);
    end

    % Update weights
    logw = zeros(1,N);
    for i = 1:N
        zhat = expectedRanges(P(:,i), landmarks);
        logw(i) = gaussianLogPdf(z - zhat, sigmaR);
    end
    logw = logw - max(logw);
    w = exp(logw);
    w = w / sum(w);

    % Resample
    idx = systematicResample(w);
    P = P(:,idx);
    w = ones(1,N)/N;

    % Mean pose (angle via circular mean)
    mu = mean(P,2);
    mu(3) = atan2(mean(sin(P(3,:))), mean(cos(P(3,:))));
end
      

Mathematica is effective for symbolic inspection of probabilistic models and for rapid prototyping with strong visualization. The notebook below implements the same landmark-range MCL loop.

File: Chapter8_Lesson1.nb

Notebook[{
Cell["Chapter 8: Particle-Filter Localization", "Title"],
Cell["Lesson 1: Monte Carlo Localization (MCL) Intuition", "Subtitle"],
Cell["Minimal MCL in a known 2D landmark map (range-only). Evaluate cells top-to-bottom.", "Text"],
Cell["(* Chapter 8 - Particle-Filter Localization\n   Lesson 1: Monte Carlo Localization (MCL) Intuition\n\n   Minimal MCL in a known 2D landmark map (range-only).\n   Evaluate cells top-to-bottom.\n*)\n\nClearAll[\"Global`*\"];\nSeedRandom[0];\n\nwrapAngle[a_] := Module[{pi = Pi, b},\n  b = Mod[a + pi, 2 pi];\n  If[b < 0, b = b + 2 pi];\n  b - pi\n];\n\nmotionSample[{x_, y_, th_}, {vCmd_, wCmd_}, dt_, sigV_, sigW_] := Module[\n  {v = vCmd + sigV RandomVariate[NormalDistribution[0, 1]],\n   w = wCmd + sigW RandomVariate[NormalDistribution[0, 1]]},\n  If[Abs[w] < 10^-9,\n    {x + v dt Cos[th], y + v dt Sin[th], th},\n    {\n      x + (v/w) (Sin[th + w dt] - Sin[th]),\n      y + (v/w) (-Cos[th + w dt] + Cos[th]),\n      wrapAngle[th + w dt]\n    }\n  ]\n];\n\nexpectedRanges[{x_, y_, th_}, landmarks_] := Sqrt[(landmarks[[All, 1]] - x)^2 + (landmarks[[All, 2]] - y)^2];\n\ngaussianLogPdf[e_, sigma_] := -1/2 Total[(e/sigma)^2] - Length[e] Log[sigma Sqrt[2 Pi]];\n\nsystematicResample[particles_, weights_] := Module[\n  {n = Length[particles], cdf = Accumulate[weights], u0, u, idx},\n  cdf[[-1]] = 1.0;\n  u0 = RandomReal[]/n;\n  u = u0 + Range[0, n - 1]/n;\n  idx = 1 + UnitStep[u - #] & /@ cdf // Total /@ Transpose[{ConstantArray[1, n]}]; (* robust but verbose *)\n  particles[[Clip[idx, {1, n}]]]\n];\n\n(* Map *)\nlandmarks = { {-5., -5.}, {5., -5.}, {5., 5.}, {-5., 5.} };\n\n(* True pose *)\nxTrue = {-3., -2., 0.3};\n\n(* Parameters *)\nnP = 800;\ndt = 0.1;\nsigV = 0.10;\nsigW = 0.05;\nsigR = 0.35;\n\n(* Initialize particles (broad prior) *)\nparticles = Transpose@{\n  RandomVariate[NormalDistribution[0, 3], nP],\n  RandomVariate[NormalDistribution[0, 3], nP],\n  RandomReal[{-Pi, Pi}, nP]\n};\nweights = ConstantArray[1./nP, nP];\n\nmeanPose[parts_] := Module[{mx, my, mth},\n  mx = Mean[parts[[All, 1]]];\n  my = Mean[parts[[All, 2]]];\n  mth = ArcTan[Mean[Sin[parts[[All, 3]]]], Mean[Cos[parts[[All, 3]]]]];\n  {mx, my, mth}\n];\n\nT = 120;\ntrajTrue = ConstantArray[0., {T + 1, 3}];\ntrajEst  = ConstantArray[0., {T, 3}];\ntrajTrue[[1]] = xTrue;\n\nDo[\n  u = {0.6, 0.25};\n\n  (* True robot (noise-free for demo) *)\n  xTrue = motionSample[xTrue, u, dt, 0., 0.];\n  trajTrue[[t + 1]] = xTrue;\n\n  (* Measurement (ranges with noise) *)\n  z = expectedRanges[xTrue, landmarks] + RandomVariate[NormalDistribution[0, sigR], Length[landmarks]];\n\n  (* Predict *)\n  particles = motionSample[#, u, dt, sigV, sigW] & /@ particles;\n\n  (* Update weights *)\n  logw = gaussianLogPdf[z - expectedRanges[#, landmarks], sigR] & /@ particles;\n  logw = logw - Max[logw];\n  weights = Exp[logw];\n  weights = weights/Total[weights];\n\n  (* Resample *)\n  particles = systematicResample[particles, weights];\n  weights = ConstantArray[1./nP, nP];\n\n  mu = meanPose[particles];\n  trajEst[[t]] = mu;\n, {t, 1, T}];\n\nShow[\n  Graphics[\n    {\n      PointSize[0.008], Opacity[0.25], Point[particles[[All, {1, 2}]]],\n      Thick, Line[trajTrue[[All, {1, 2}]]],\n      Thick, Line[trajEst[[All, {1, 2}]]],\n      Red, PointSize[0.02], Point[landmarks]\n    },\n    Axes -> True, AspectRatio -> 1, GridLines -> Automatic,\n    PlotRange -> { {-7, 7}, {-7, 7} }\n  ],\n  PlotLabel -> \"Monte Carlo Localization (MCL) — Intuition Demo\"\n]\n", "Input"]
}, WindowTitle->"Chapter8_Lesson1"]
      

Problem 1 (Derive the Bayes filter recursion): Starting from \( \mathrm{bel}(\mathbf{x}_t) = p(\mathbf{x}_t \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) \), derive the recursion \( \mathrm{bel}(\mathbf{x}_t) = \eta \, p(\mathbf{z}_t \mid \mathbf{x}_t) \int p(\mathbf{x}_t \mid \mathbf{u}_t,\mathbf{x}_{t-1}) \mathrm{bel}(\mathbf{x}_{t-1}) d\mathbf{x}_{t-1} \) under the Markov assumptions stated in Section 2.

Solution: Start with Bayes: \( p(\mathbf{x}_t \mid \mathbf{z}_{1:t},\mathbf{u}_{1:t}) \propto p(\mathbf{z}_t \mid \mathbf{x}_t,\mathbf{z}_{1:t-1},\mathbf{u}_{1:t}) \, p(\mathbf{x}_t \mid \mathbf{z}_{1:t-1},\mathbf{u}_{1:t}) \). By conditional observation independence, \( p(\mathbf{z}_t \mid \mathbf{x}_t,\mathbf{z}_{1:t-1},\mathbf{u}_{1:t}) = p(\mathbf{z}_t \mid \mathbf{x}_t) \), so \( \mathrm{bel}(\mathbf{x}_t) = \eta \, p(\mathbf{z}_t \mid \mathbf{x}_t)\, p(\mathbf{x}_t \mid \mathbf{z}_{1:t-1},\mathbf{u}_{1:t}) \). Next expand the prediction term by marginalizing \( \mathbf{x}_{t-1} \):

\[ p(\mathbf{x}_t \mid \mathbf{z}_{1:t-1},\mathbf{u}_{1:t}) = \int p(\mathbf{x}_t,\mathbf{x}_{t-1} \mid \mathbf{z}_{1:t-1},\mathbf{u}_{1:t})\, d\mathbf{x}_{t-1}. \]

Apply the chain rule: \( p(\mathbf{x}_t,\mathbf{x}_{t-1} \mid \cdot) = p(\mathbf{x}_t \mid \mathbf{x}_{t-1}, \cdot)\, p(\mathbf{x}_{t-1}\mid \cdot) \). By the state Markov property, \( p(\mathbf{x}_t \mid \mathbf{x}_{t-1}, \mathbf{z}_{1:t-1},\mathbf{u}_{1:t}) = p(\mathbf{x}_t \mid \mathbf{x}_{t-1},\mathbf{u}_t) \), and \( p(\mathbf{x}_{t-1}\mid \mathbf{z}_{1:t-1},\mathbf{u}_{1:t}) = p(\mathbf{x}_{t-1}\mid \mathbf{z}_{1:t-1},\mathbf{u}_{1:t-1}) = \mathrm{bel}(\mathbf{x}_{t-1}) \). Substituting yields the recursion. ■

Problem 2 (Unbiasedness and variance of Monte Carlo expectation): Let \( \mathbf{X}^{(1)},\dots,\mathbf{X}^{(N)} \) be i.i.d. from \( \mathrm{bel} \), and define \( \hat{I}_N = \frac{1}{N}\sum_{i=1}^N \varphi(\mathbf{X}^{(i)}) \). Show that \( \mathbb{E}[\hat{I}_N] = \mathbb{E}[\varphi(\mathbf{X})] \) and \( \mathbb{V}\mathrm{ar}(\hat{I}_N) = \mathbb{V}\mathrm{ar}(\varphi(\mathbf{X}))/N \).

Solution: Linearity of expectation gives: \( \mathbb{E}[\hat{I}_N] = \frac{1}{N}\sum_{i=1}^N \mathbb{E}[\varphi(\mathbf{X}^{(i)})] = \mathbb{E}[\varphi(\mathbf{X})] \). For variance, independence implies \( \mathbb{V}\mathrm{ar}\left(\sum_i Y_i\right) = \sum_i \mathbb{V}\mathrm{ar}(Y_i) \). With \( Y_i=\varphi(\mathbf{X}^{(i)}) \): \( \mathbb{V}\mathrm{ar}(\hat{I}_N) = \frac{1}{N^2}\sum_{i=1}^N \mathbb{V}\mathrm{ar}(Y_i) = \mathbb{V}\mathrm{ar}(Y_1)/N \). ■

Problem 3 (Derive the range-only likelihood weight): Assume independent Gaussian range noise: \( r_{t,k} = \hat{r}_k(\mathbf{x}_t) + \nu_k \), \( \nu_k \sim \mathcal{N}(0,\sigma_r^2) \). Derive a formula for particle weights \( w_t^{(i)} \) up to proportionality.

Solution: Independence gives \( p(\mathbf{z}_t \mid \mathbf{x}) = \prod_{k=1}^K \mathcal{N}(r_{t,k}; \hat{r}_k(\mathbf{x}),\sigma_r^2) \). Dropping constants common to all particles:

\[ w_t^{(i)} \propto p(\mathbf{z}_t \mid \mathbf{x}_t^{(i)}) \propto \exp\!\left(-\frac{1}{2\sigma_r^2}\sum_{k=1}^K (r_{t,k} - \hat{r}_k(\mathbf{x}_t^{(i)}))^2\right). \]

Normalize by dividing by \( \sum_{j=1}^N w_t^{(j)} \). ■

Problem 4 (Resampling moments for a single particle): Consider multinomial resampling: draw \( N \) i.i.d. indices with \( \mathbb{P}(I=i)=w^{(i)} \). Let \( C_i \) be the number of times particle \( i \) is selected. Show \( \mathbb{E}[C_i] = N w^{(i)} \) and \( \mathbb{V}\mathrm{ar}(C_i) = N w^{(i)}(1-w^{(i)}) \).

Solution: Write \( C_i = \sum_{m=1}^N \mathbf{1}\{I_m=i\} \), where \( I_m \) are i.i.d. categorical draws. Then \( \mathbf{1}\{I_m=i\} \) are i.i.d. Bernoulli with success probability \( w^{(i)} \). Therefore \( \mathbb{E}[C_i] = \sum_{m=1}^N w^{(i)} = N w^{(i)} \), and \( \mathbb{V}\mathrm{ar}(C_i)=\sum_{m=1}^N w^{(i)}(1-w^{(i)}) = N w^{(i)}(1-w^{(i)}) \). ■

Problem 5 (Circular mean for heading): Explain why \( \frac{1}{N}\sum_{i=1}^N \theta^{(i)} \) is not a reliable estimator of heading, and justify \( \hat{\theta}=\mathrm{atan2}\!\left(\sum_i w^{(i)}\sin\theta^{(i)}, \sum_i w^{(i)}\cos\theta^{(i)}\right) \).

Solution: Angles live on a circle: values near \( +\pi \) and \( -\pi \) are close geometrically but far numerically. Example: \( \theta_1=\pi-\epsilon \), \( \theta_2=-\pi+\epsilon \) have mean near zero if averaged linearly, which is incorrect (the cluster is near \( \pm \pi \)). Represent each angle as a unit vector \( [\cos\theta,\sin\theta]^\top \) and average vectors; the resulting mean direction is the argument of the averaged vector, yielding the stated circular mean. ■

We formulated localization as a Bayes filtering problem over pose and introduced MCL as a particle (empirical) approximation to the belief. The core intuition is operational: propagate pose uncertainty by sampling from the motion model, score particles by sensor consistency, and manage weight collapse with resampling. Lesson 2 will formalize importance sampling and resampling strategies, including their statistical consequences and practical design criteria.