Let the robot pose/state at time \( t \) be \( \mathbf{x}_t \in \mathbb{R}^d \) (in mobile robotics typically \( d=3 \) for planar pose). The Bayes-filter belief is \( bel(\mathbf{x}_t) = p(\mathbf{x}_t \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) \), where \( \mathbf{z}_{1:t} \) are measurements and \( \mathbf{u}_{1:t} \) controls.

A particle representation uses a weighted empirical measure: \( \{(\mathbf{x}_t^{(i)}, w_t^{(i)})\}_{i=1}^N \) with \( w_t^{(i)} \ge 0 \) and \( \sum_{i=1}^N w_t^{(i)} = 1 \). The belief is approximated by

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

For any integrable test function \( f \), the posterior expectation is approximated as

\[ \mathbb{E}[f(\mathbf{x}_t) \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}] = \int f(\mathbf{x}_t)\, bel(\mathbf{x}_t)\, d\mathbf{x}_t \approx \sum_{i=1}^N w_t^{(i)} f(\mathbf{x}_t^{(i)}). \]

The core questions of this lesson are: (i) how to compute \( w_t^{(i)} \) when particles are sampled from a proposal distribution, and (ii) how resampling prevents weight degeneracy while preserving the represented belief in expectation.

Consider a target density \( \pi(\mathbf{x}) \) (known up to normalization) and a proposal density \( q(\mathbf{x}) \) from which we can sample. If \( \mathbf{x}^{(i)} \sim q(\mathbf{x}) \), define the unnormalized weight: \( \tilde{w}^{(i)} = \frac{\pi(\mathbf{x}^{(i)})}{q(\mathbf{x}^{(i)})} \).

The (self-normalized) importance sampling estimator of \( I = \mathbb{E}_{\pi}[f(\mathbf{x})] \) is:

\[ \hat{I}_N = \frac{\sum_{i=1}^N \tilde{w}^{(i)} f(\mathbf{x}^{(i)})}{\sum_{i=1}^N \tilde{w}^{(i)}} = \sum_{i=1}^N w^{(i)} f(\mathbf{x}^{(i)}), \quad w^{(i)} = \frac{\tilde{w}^{(i)}}{\sum_{j=1}^N \tilde{w}^{(j)}}. \]

Proposition (Unbiasedness of the unnormalized numerator).

Let \( Z = \int \pi(\mathbf{x}) d\mathbf{x} \) (unknown). Define \( \pi_Z(\mathbf{x}) = \pi(\mathbf{x})/Z \). Then the estimator \( \widehat{J}_N = \frac{1}{N}\sum_{i=1}^N \tilde{w}^{(i)} f(\mathbf{x}^{(i)}) \) satisfies \( \mathbb{E}[\widehat{J}_N] = Z \, \mathbb{E}_{\pi_Z}[f(\mathbf{x})] \).

Proof. Using \( \mathbf{x}^{(i)} \sim q \) i.i.d. and linearity of expectation:

\[ \mathbb{E}[\widehat{J}_N] = \mathbb{E}\left[\tilde{w} f(\mathbf{x})\right] = \int \frac{\pi(\mathbf{x})}{q(\mathbf{x})} f(\mathbf{x}) q(\mathbf{x}) d\mathbf{x} = \int \pi(\mathbf{x}) f(\mathbf{x}) d\mathbf{x} = Z \int f(\mathbf{x}) \pi_Z(\mathbf{x}) d\mathbf{x}. \]

The self-normalized estimator \( \hat{I}_N \) is generally biased for finite \( N \) (ratio of random variables), but it is consistent under standard regularity conditions.

Theorem (Consistency, sketch).

If \( \mathbb{E}_q[\tilde{w}] < \infty \) and \( \mathbb{E}_q[|\tilde{w} f(\mathbf{x})|] < \infty \), then \( \hat{I}_N \rightarrow \mathbb{E}_{\pi_Z}[f(\mathbf{x})] \) almost surely as \( N \rightarrow \infty \).

This static result is the backbone of particle filters; we next convert it into a time-recursive form.

Particle filters often operate on trajectories \( \mathbf{x}_{0:t} = (\mathbf{x}_0,\dots,\mathbf{x}_t) \). The (unnormalized) target density for trajectories is \( \pi_t(\mathbf{x}_{0:t}) \propto p(\mathbf{x}_{0:t}, \mathbf{z}_{1:t} \mid \mathbf{u}_{1:t}) \). Under the Markov motion model and conditional sensor independence:

\[ p(\mathbf{x}_{0:t}, \mathbf{z}_{1:t} \mid \mathbf{u}_{1:t}) = p(\mathbf{x}_0)\prod_{k=1}^{t} p(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{u}_k)\; \prod_{k=1}^{t} p(\mathbf{z}_k \mid \mathbf{x}_k). \]

Choose a sequential proposal (importance) distribution:

\[ q_t(\mathbf{x}_{0:t} \mid \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) = q_{t-1}(\mathbf{x}_{0:t-1} \mid \mathbf{z}_{1:t-1}, \mathbf{u}_{1:t-1})\; q(\mathbf{x}_t \mid \mathbf{x}_{0:t-1}, \mathbf{z}_{1:t}, \mathbf{u}_{1:t}). \]

Define the unnormalized importance weight for a trajectory sample \( \mathbf{x}_{0:t}^{(i)} \sim q_t \) by \( \tilde{w}_t^{(i)} = \frac{\pi_t(\mathbf{x}_{0:t}^{(i)})}{q_t(\mathbf{x}_{0:t}^{(i)})} \).

Proposition (Weight recursion).

\[ \tilde{w}_t^{(i)} = \tilde{w}_{t-1}^{(i)} \cdot \frac{ p(\mathbf{z}_t \mid \mathbf{x}_t^{(i)})\; p(\mathbf{x}_t^{(i)} \mid \mathbf{x}_{t-1}^{(i)}, \mathbf{u}_t) }{ q(\mathbf{x}_t^{(i)} \mid \mathbf{x}_{0:t-1}^{(i)}, \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) }. \]

Proof. Expand the ratio using the factorization above:

\[ \tilde{w}_t = \frac{ p(\mathbf{x}_0)\prod_{k=1}^{t} p(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{u}_k)\; \prod_{k=1}^{t} p(\mathbf{z}_k \mid \mathbf{x}_k) }{ q_{t-1}(\mathbf{x}_{0:t-1}\mid \mathbf{z}_{1:t-1},\mathbf{u}_{1:t-1}) \; q(\mathbf{x}_t \mid \mathbf{x}_{0:t-1},\mathbf{z}_{1:t},\mathbf{u}_{1:t}) }. \]

\[ \tilde{w}_t = \underbrace{ \frac{ p(\mathbf{x}_0)\prod_{k=1}^{t-1} p(\mathbf{x}_k \mid \mathbf{x}_{k-1}, \mathbf{u}_k)\; \prod_{k=1}^{t-1} p(\mathbf{z}_k \mid \mathbf{x}_k) }{ q_{t-1}(\mathbf{x}_{0:t-1}\mid \mathbf{z}_{1:t-1},\mathbf{u}_{1:t-1}) } }_{\tilde{w}_{t-1}} \cdot \frac{ p(\mathbf{z}_t \mid \mathbf{x}_t)\; p(\mathbf{x}_t \mid \mathbf{x}_{t-1}, \mathbf{u}_t) }{ q(\mathbf{x}_t \mid \mathbf{x}_{0:t-1},\mathbf{z}_{1:t},\mathbf{u}_{1:t}) }. \]

In Monte Carlo Localization, a common choice is \( q(\mathbf{x}_t \mid \mathbf{x}_{t-1},\mathbf{u}_t)=p(\mathbf{x}_t \mid \mathbf{x}_{t-1},\mathbf{u}_t) \). Then the update simplifies to \( \tilde{w}_t^{(i)} \propto \tilde{w}_{t-1}^{(i)} p(\mathbf{z}_t \mid \mathbf{x}_t^{(i)}) \).

In mobile-robot localization, sensor likelihoods can be extremely peaked. Directly computing \( \tilde{w}^{(i)} \) may underflow. Use log-weights: \( \ell^{(i)} = \log \tilde{w}^{(i)} \).

Normalization requires computing \( w^{(i)} = \frac{\exp(\ell^{(i)})}{\sum_j \exp(\ell^{(j)})} \). Use the stable identity:

\[ \log\!\left(\sum_{j=1}^N e^{\ell^{(j)}}\right) = m + \log\!\left(\sum_{j=1}^N e^{\ell^{(j)} - m}\right), \quad m = \max_j \ell^{(j)}. \]

Proof. Factor out \( e^m \) from the sum:

\[ \sum_{j=1}^N e^{\ell^{(j)}} = e^m \sum_{j=1}^N e^{\ell^{(j)}-m} \;\Rightarrow\; \log\!\left(\sum_{j=1}^N e^{\ell^{(j)}}\right) = m + \log\!\left(\sum_{j=1}^N e^{\ell^{(j)}-m}\right). \]

This is implemented in each programming language implementation later in the lesson.

Sequential importance sampling suffers from degeneracy: after several updates, most weights become nearly zero, and a few particles dominate. A standard diagnostic is the effective sample size: \( N_{\text{eff}} \) .

Definition (ESS approximation).

\[ N_{\text{eff}} \;\approx\; \frac{1}{\sum_{i=1}^N (w^{(i)})^2}. \]

Derivation via coefficient of variation.

Define the coefficient of variation of the unnormalized weights by \( \mathrm{CV}^2 = \frac{1}{N}\sum_{i=1}^N \left(\frac{\tilde{w}^{(i)}}{\bar{\tilde{w}}}-1\right)^2 \), where \( \bar{\tilde{w}} = \frac{1}{N}\sum_i \tilde{w}^{(i)} \). For normalized weights \( w^{(i)} = \tilde{w}^{(i)}/\sum_j \tilde{w}^{(j)} \), one obtains

\[ \mathrm{CV}^2 = N\sum_{i=1}^N (w^{(i)})^2 - 1 \quad\Rightarrow\quad \frac{N}{1+\mathrm{CV}^2} = \frac{1}{\sum_{i=1}^N (w^{(i)})^2} = N_{\text{eff}}. \]

Proposition (ESS bounds).

\[ 1 \le N_{\text{eff}} \le N. \]

Proof.

Since \( \sum_i w^{(i)} = 1 \) and \( w^{(i)} \ge 0 \), we have \( \sum_i (w^{(i)})^2 \le (\sum_i w^{(i)})^2 = 1 \), hence \( N_{\text{eff}} = 1/\sum_i (w^{(i)})^2 \ge 1 \). Also by Cauchy–Schwarz, \( (\sum_i w^{(i)})^2 \le N \sum_i (w^{(i)})^2 \Rightarrow 1 \le N \sum_i (w^{(i)})^2 \), so \( N_{\text{eff}} \le N \). Equality \( N_{\text{eff}} = N \) holds iff \( w^{(i)}=1/N \) for all \( i \).

In practice, resampling is triggered when \( N_{\text{eff}} \) drops below a threshold \( \gamma N \), where typical \( \gamma \in [0.3, 0.8] \).

Resampling replaces the weighted set \( \{(\mathbf{x}^{(i)}, w^{(i)})\} \) with an unweighted (uniform-weight) set \( \{\mathbf{x}'^{(i)}\} \) by sampling indices \( a_i \in \{1,\dots,N\} \) such that \( \mathbb{P}(a_i = j) = w^{(j)} \). Then \( \mathbf{x}'^{(i)} = \mathbf{x}^{(a_i)} \) and new weights are \( 1/N \).

Proposition (Resampling preserves expectations conditionally).

For any function \( f \), conditioned on the current weighted particles,

\[ \mathbb{E}\!\left[\frac{1}{N}\sum_{i=1}^N f(\mathbf{x}'^{(i)}) \;\middle|\; \{(\mathbf{x}^{(j)}, w^{(j)})\}_{j=1}^N \right] = \sum_{j=1}^N w^{(j)} f(\mathbf{x}^{(j)}). \]

Proof. Using conditional expectation and i.i.d. resampled indices:

\[ \mathbb{E}\!\left[f(\mathbf{x}'^{(i)}) \mid \{(\mathbf{x}^{(j)}, w^{(j)})\}\right] = \sum_{j=1}^N \mathbb{P}(a_i=j)\, f(\mathbf{x}^{(j)}) = \sum_{j=1}^N w^{(j)} f(\mathbf{x}^{(j)}). \]

Averaging over \( i \) yields the stated identity. Therefore, resampling does not bias empirical expectations (given the current particle approximation).

Resampling reduces weight degeneracy but can cause sample impoverishment (loss of diversity) when the motion noise is low. Later lessons address remedies (better proposals and rejuvenation), but for MCL it is common to rely on process noise and occasional random particle injection.

flowchart TD
  A["Inputs: particles x[i], weights w[i], control u_t, measurement z_t"] --> B["Propagate: sample x[i] ~ proposal q(x given prev, u_t, z_t)"]
  B --> C["Compute log-weights: l[i] = log p(z_t|x[i]) + log p(x[i]|prev,u_t) - log q(...)"]
  C --> D["Normalize: w[i] = exp(l[i] - logSumExp(l))"]
  D --> E["Compute ESS: Neff = 1 / sum(w[i]^2)"]
  E --> F{"Neff < gamma * N ?"}
  F -->|"yes"| G["Resample ancestors a[i] ~ Cat(w); set x_prime[i] = x[a[i]]"]
  G --> H["Reset weights: w[i] = 1/N"]
  F -->|"no"| I["Keep weighted set"]
  H --> J["Output belief at time t"]
  I --> J
  

The choice of resampling scheme matters because it changes the variance introduced by resampling. Multinomial resampling is simplest but highest variance. Stratified and systematic resampling are lower variance while remaining \( \mathcal{O}(N) \).

flowchart TD
  S["Need resampling"] --> M["Multinomial: \na[i] iid ~ Cat(w)"]
  S --> R["Residual: floor(N*w) deterministic + \nsample residual"]
  S --> T["Stratified: u_i in \n((i-1)/N, i/N)"]
  S --> Y["Systematic: one u0, \nu_i = u0 + (i-1)/N"]
  M --> VM["Highest variance; \nsimplest"]
  R --> VR["Lower variance when \nsome weights large"]
  T --> VT["Low variance; \nindependent strata"]
  Y --> VY["Very low variance; \nfast O(N)"]
        

A standard rule in localization is \( \gamma = 0.5 \) (resample when half the particles are effectively lost), but the optimal choice depends on sensor sharpness and computation budget.

The following implementations focus on: (i) log-weight normalization, (ii) ESS, and (iii) systematic resampling, with additional resampling schemes in Python. In real AMR systems, these are commonly embedded in ROS/ROS2 localization nodes (e.g., AMCL-style filters) and tuned via sensor models.

Chapter8_Lesson2.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Chapter 8 - Lesson 2: Importance Sampling and Resampling (Particle Filters)

This file is intentionally "from scratch" (NumPy-only) to make the mechanics clear.

Core topics:
  - log-weight normalization (log-sum-exp)
  - Effective Sample Size (ESS)
  - Multinomial, residual, stratified, and systematic resampling
"""

import numpy as np


def log_sum_exp(logw: np.ndarray) -> float:
    """Stable computation of log(sum(exp(logw)))."""
    m = np.max(logw)
    return float(m + np.log(np.sum(np.exp(logw - m))))


def normalize_log_weights(logw: np.ndarray) -> np.ndarray:
    """Return normalized weights from unnormalized log-weights."""
    lse = log_sum_exp(logw)
    w = np.exp(logw - lse)
    w = w / np.sum(w)
    return w


def effective_sample_size(w: np.ndarray) -> float:
    """ESS approximation: N_eff = 1 / sum_i w_i^2 (w normalized)."""
    return float(1.0 / np.sum(np.square(w)))


def multinomial_resample(w: np.ndarray, rng: np.random.Generator) -> np.ndarray:
    """Multinomial resampling. Returns ancestor indices."""
    N = w.size
    return rng.choice(N, size=N, replace=True, p=w)


def systematic_resample(w: np.ndarray, rng: np.random.Generator) -> np.ndarray:
    """Systematic resampling (low variance, O(N)). Returns ancestor indices."""
    N = w.size
    positions = (rng.random() + np.arange(N)) / N
    cdf = np.cumsum(w)
    a = np.zeros(N, dtype=int)
    i, j = 0, 0
    while i < N:
        if positions[i] < cdf[j]:
            a[i] = j
            i += 1
        else:
            j += 1
    return a


def stratified_resample(w: np.ndarray, rng: np.random.Generator) -> np.ndarray:
    """Stratified resampling."""
    N = w.size
    positions = (np.arange(N) + rng.random(N)) / N
    cdf = np.cumsum(w)
    a = np.zeros(N, dtype=int)
    i, j = 0, 0
    while i < N:
        if positions[i] < cdf[j]:
            a[i] = j
            i += 1
        else:
            j += 1
    return a


def residual_resample(w: np.ndarray, rng: np.random.Generator) -> np.ndarray:
    """Residual resampling: floor(N*w_i) deterministic + sample residual."""
    N = w.size
    Ns = np.floor(N * w).astype(int)
    R = N - np.sum(Ns)
    a = np.empty(N, dtype=int)
    k = 0
    for i in range(N):
        if Ns[i] > 0:
            a[k:k + Ns[i]] = i
            k += Ns[i]
    if R > 0:
        residual = (N * w - Ns)
        residual = residual / np.sum(residual)
        a[k:] = rng.choice(N, size=R, replace=True, p=residual)
    rng.shuffle(a)
    return a


def demo_1d_tracking(seed: int = 7) -> None:
    """
    Minimal 1D demo:
      x_t = x_{t-1} + u + v, v ~ N(0,q^2)
      z_t = x_t + n,         n ~ N(0,r^2)
    """
    rng = np.random.default_rng(seed)

    N = 2000
    q = 0.4
    r = 0.7

    x = rng.normal(0.0, 4.0, size=N)
    logw = np.zeros(N)

    x_true = 2.0
    for t in range(1, 9):
        u = 0.3
        x_true = x_true + u + rng.normal(0.0, q)

        x = x + u + rng.normal(0.0, q, size=N)
        z = x_true + rng.normal(0.0, r)

        logw = -0.5 * np.square((z - x) / r)
        w = normalize_log_weights(logw)

        Neff = effective_sample_size(w)
        x_hat = float(np.sum(w * x))
        print(f"t={t:02d}, z={z:+.3f}, true={x_true:+.3f}, est={x_hat:+.3f}, ESS={Neff:.1f}")

        if Neff < 0.5 * N:
            a = systematic_resample(w, rng)
            x = x[a]
            logw[:] = 0.0
    print("Done.")


if __name__ == "__main__":
    demo_1d_tracking()
      

Chapter8_Lesson2.cpp

/*
Chapter 8 - Lesson 2: Importance Sampling and Resampling (Particle Filters)
Build:
  g++ -O2 -std=c++17 Chapter8_Lesson2.cpp -o pf_resample
*/

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

static double log_sum_exp(const std::vector<double>& logw) {
    double m = *std::max_element(logw.begin(), logw.end());
    double s = 0.0;
    for (double lw : logw) s += std::exp(lw - m);
    return m + std::log(s);
}

static std::vector<double> normalize_log_weights(const std::vector<double>& logw) {
    const double lse = log_sum_exp(logw);
    std::vector<double> w(logw.size());
    double sumw = 0.0;
    for (size_t i = 0; i < logw.size(); ++i) {
        w[i] = std::exp(logw[i] - lse);
        sumw += w[i];
    }
    for (double& wi : w) wi /= sumw;
    return w;
}

static double effective_sample_size(const std::vector<double>& w) {
    double s2 = 0.0;
    for (double wi : w) s2 += wi * wi;
    return 1.0 / s2;
}

static std::vector<int> systematic_resample(const std::vector<double>& w, std::mt19937& gen) {
    const int N = static_cast<int>(w.size());
    std::uniform_real_distribution<double> unif(0.0, 1.0);
    const double u0 = unif(gen) / N;

    std::vector<double> cdf(N);
    std::partial_sum(w.begin(), w.end(), cdf.begin());

    std::vector<int> a(N);
    int j = 0;
    for (int i = 0; i < N; ++i) {
        double u = u0 + static_cast<double>(i) / N;
        while (u > cdf[j] && j < N - 1) j++;
        a[i] = j;
    }
    return a;
}

static void demo_1d_tracking(unsigned seed = 7) {
    std::mt19937 gen(seed);
    std::normal_distribution<double> n01(0.0, 1.0);

    const int N = 2000;
    const double q = 0.4;
    const double r = 0.7;
    const double u = 0.3;

    std::vector<double> x(N);
    for (int i = 0; i < N; ++i) x[i] = 4.0 * n01(gen);

    double x_true = 2.0;
    for (int t = 1; t <= 8; ++t) {
        x_true = x_true + u + q * n01(gen);

        for (int i = 0; i < N; ++i) x[i] = x[i] + u + q * n01(gen);

        const double z = x_true + r * n01(gen);

        std::vector<double> logw(N);
        for (int i = 0; i < N; ++i) {
            const double e = (z - x[i]) / r;
            logw[i] = -0.5 * e * e;
        }
        std::vector<double> w = normalize_log_weights(logw);

        const double Neff = effective_sample_size(w);
        double x_hat = 0.0;
        for (int i = 0; i < N; ++i) x_hat += w[i] * x[i];

        std::cout << "t=" << t
                  << ", z=" << z
                  << ", true=" << x_true
                  << ", est=" << x_hat
                  << ", ESS=" << Neff << "\n";

        if (Neff < 0.5 * N) {
            std::vector<int> a = systematic_resample(w, gen);
            std::vector<double> x_new(N);
            for (int i = 0; i < N; ++i) x_new[i] = x[a[i]];
            x.swap(x_new);
        }
    }
}

int main() {
    demo_1d_tracking();
    return 0;
}
      

Chapter8_Lesson2.java

/*
Chapter 8 - Lesson 2: Importance Sampling and Resampling (Particle Filters)

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

import java.util.Random;

public class Chapter8_Lesson2 {

    static double logSumExp(double[] logw) {
        double m = Double.NEGATIVE_INFINITY;
        for (double v : logw) m = Math.max(m, v);
        double s = 0.0;
        for (double v : logw) s += Math.exp(v - m);
        return m + Math.log(s);
    }

    static double[] normalizeLogWeights(double[] logw) {
        double lse = logSumExp(logw);
        double[] w = new double[logw.length];
        double sum = 0.0;
        for (int i = 0; i < logw.length; i++) {
            w[i] = Math.exp(logw[i] - lse);
            sum += w[i];
        }
        for (int i = 0; i < w.length; i++) w[i] /= sum;
        return w;
    }

    static double effectiveSampleSize(double[] w) {
        double s2 = 0.0;
        for (double wi : w) s2 += wi * wi;
        return 1.0 / s2;
    }

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

        double u0 = rng.nextDouble() / N;
        int[] a = new int[N];
        int j = 0;
        for (int i = 0; i < N; i++) {
            double u = u0 + ((double) i) / N;
            while (u > cdf[j] && j < N - 1) j++;
            a[i] = j;
        }
        return a;
    }

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

    static void demo1DTracking(long seed) {
        Random rng = new Random(seed);

        int N = 2000;
        double q = 0.4;
        double r = 0.7;
        double u = 0.3;

        double[] x = new double[N];
        for (int i = 0; i < N; i++) x[i] = 4.0 * randn(rng);

        double xTrue = 2.0;

        for (int t = 1; t <= 8; t++) {
            xTrue = xTrue + u + q * randn(rng);

            for (int i = 0; i < N; i++) x[i] = x[i] + u + q * randn(rng);

            double z = xTrue + r * randn(rng);

            double[] logw = new double[N];
            for (int i = 0; i < N; i++) {
                double e = (z - x[i]) / r;
                logw[i] = -0.5 * e * e;
            }
            double[] w = normalizeLogWeights(logw);

            double Neff = effectiveSampleSize(w);
            double xHat = 0.0;
            for (int i = 0; i < N; i++) xHat += w[i] * x[i];

            System.out.printf("t=%02d, z=%+.3f, true=%+.3f, est=%+.3f, ESS=%.1f%n",
                    t, z, xTrue, xHat, Neff);

            if (Neff < 0.5 * N) {
                int[] a = systematicResample(w, rng);
                double[] xNew = new double[N];
                for (int i = 0; i < N; i++) xNew[i] = x[a[i]];
                x = xNew;
            }
        }
    }

    public static void main(String[] args) {
        demo1DTracking(7L);
    }
}
      

Chapter8_Lesson2.m (MATLAB + optional Simulink skeleton)

% Chapter 8 - Lesson 2: Importance Sampling and Resampling (Particle Filters)
%
% Run:
%   Chapter8_Lesson2

function Chapter8_Lesson2()
    rng(7);

    N = 2000;
    q = 0.4;
    r = 0.7;
    u = 0.3;

    x = 4.0 * randn(N,1);
    logw = zeros(N,1);

    x_true = 2.0;

    for t = 1:8
        x_true = x_true + u + q*randn();
        x = x + u + q*randn(N,1);

        z = x_true + r*randn();

        logw = -0.5 * ((z - x)/r).^2;

        w = normalize_log_weights(logw);
        Neff = effective_sample_size(w);
        x_hat = sum(w .* x);

        fprintf('t=%02d, z=%+.3f, true=%+.3f, est=%+.3f, ESS=%.1f\n', ...
            t, z, x_true, x_hat, Neff);

        if Neff < 0.5*N
            a = systematic_resample(w);
            x = x(a);
            logw(:) = 0.0;
        end
    end

    % Optional Simulink creation (requires Simulink):
    % create_simulink_skeleton();
end

function lse = log_sum_exp(logw)
    m = max(logw);
    lse = m + log(sum(exp(logw - m)));
end

function w = normalize_log_weights(logw)
    lse = log_sum_exp(logw);
    w = exp(logw - lse);
    w = w / sum(w);
end

function Neff = effective_sample_size(w)
    Neff = 1.0 / sum(w.^2);
end

function a = systematic_resample(w)
    N = length(w);
    cdf = cumsum(w);
    u0 = rand() / N;
    a = zeros(N,1);
    j = 1;
    for i = 1:N
        u = u0 + (i-1)/N;
        while u > cdf(j) && j < N
            j = j + 1;
        end
        a(i) = j;
    end
end
      

Chapter8_Lesson2.nb (Wolfram Mathematica)

(* Chapter 8 - Lesson 2: Importance Sampling and Resampling (Particle Filters) *)

logSumExp[logw_List] := Module[{m = Max[logw]},
  m + Log[Total[Exp[logw - m]]]
];

normalizeLogWeights[logw_List] := Module[{lse = logSumExp[logw], w},
  w = Exp[logw - lse];
  w/Total[w]
];

ess[w_List] := 1/Total[w^2];

systematicResample[w_List] := Module[{n = Length[w], cdf, u0, u, j = 1, a},
  cdf = Accumulate[w];
  u0 = RandomReal[]/n;
  a = ConstantArray[0, n];
  Do[
    u = u0 + (i - 1)/n;
    While[u > cdf[[j]] && j < n, j++];
    a[[i]] = j;
  , {i, 1, n}];
  a
];

(* Demo: 1D tracking-like update *)
Module[{n = 2000, q = 0.4, r = 0.7, u = 0.3, x, xTrue = 2.0, z, logw, w, neff, xHat, a},
  SeedRandom[7];
  x = 4.0 RandomVariate[NormalDistribution[0, 1], n];
  Do[
    xTrue = xTrue + u + q RandomVariate[NormalDistribution[0, 1]];
    x = x + u + q RandomVariate[NormalDistribution[0, 1], n];
    z = xTrue + r RandomVariate[NormalDistribution[0, 1]];
    logw = -0.5 ((z - x)/r)^2;
    w = normalizeLogWeights[logw];
    neff = ess[w];
    xHat = Total[w x];
    Print["t=", IntegerString[t, 10, 2], ", est=", NumberForm[xHat, {Infinity, 3}], ", ESS=", NumberForm[neff, {Infinity, 1}]];
    If[neff < 0.5 n,
      a = systematicResample[w];
      x = x[[a]];
    ];
  , {t, 1, 8}];
]
      

Problem 1 (Derive the sequential weight update):

Starting from \( \tilde{w}_t = \frac{\pi_t(\mathbf{x}_{0:t})}{q_t(\mathbf{x}_{0:t})} \) and the factorization of \( \pi_t \) and \( q_t \), derive the recursion: \( \tilde{w}_t = \tilde{w}_{t-1}\cdot \frac{p(\mathbf{z}_t\mid \mathbf{x}_t)p(\mathbf{x}_t\mid \mathbf{x}_{t-1},\mathbf{u}_t)}{q(\mathbf{x}_t\mid \mathbf{x}_{0:t-1},\mathbf{z}_{1:t},\mathbf{u}_{1:t})} \).

Solution:

Use the decomposition \( \pi_t(\mathbf{x}_{0:t}) = \pi_{t-1}(\mathbf{x}_{0:t-1})\; p(\mathbf{x}_t\mid \mathbf{x}_{t-1},\mathbf{u}_t)\; p(\mathbf{z}_t\mid \mathbf{x}_t) \) and \( q_t = q_{t-1}\; q(\mathbf{x}_t \mid \mathbf{x}_{0:t-1}, \mathbf{z}_{1:t}, \mathbf{u}_{1:t}) \). Then

\[ \tilde{w}_t = \frac{\pi_{t-1}}{q_{t-1}} \cdot \frac{p(\mathbf{z}_t\mid \mathbf{x}_t)p(\mathbf{x}_t\mid \mathbf{x}_{t-1},\mathbf{u}_t)} {q(\mathbf{x}_t\mid \mathbf{x}_{0:t-1},\mathbf{z}_{1:t},\mathbf{u}_{1:t})} = \tilde{w}_{t-1}\cdot \frac{p(\mathbf{z}_t\mid \mathbf{x}_t)p(\mathbf{x}_t\mid \mathbf{x}_{t-1},\mathbf{u}_t)} {q(\mathbf{x}_t\mid \cdot)}. \]

Problem 2 (ESS bounds and equality cases):

For normalized weights \( w^{(i)} \ge 0 \), \( \sum_i w^{(i)}=1 \), prove \( 1 \le \frac{1}{\sum_i (w^{(i)})^2} \le N \). Identify when equality holds.

Solution:

Since \( \sum_i (w^{(i)})^2 \le (\sum_i w^{(i)})^2 = 1 \), we get \( N_{\text{eff}} \ge 1 \), with equality when one weight is 1 and others 0. By Cauchy–Schwarz, \( (\sum_i w^{(i)})^2 \le N\sum_i (w^{(i)})^2 \), hence \( 1 \le N\sum_i (w^{(i)})^2 \Rightarrow N_{\text{eff}} \le N \), with equality when \( w^{(i)}=1/N \) for all \( i \).

Problem 3 (Unbiasedness of resampling for expectations):

Let resampled particles be \( \mathbf{x}'^{(i)} = \mathbf{x}^{(a_i)} \) with \( \mathbb{P}(a_i=j)=w^{(j)} \). Prove that conditioned on the current set, the resampled average is unbiased: \( \mathbb{E}[\frac{1}{N}\sum_i f(\mathbf{x}'^{(i)}) \mid \cdot ] = \sum_j w^{(j)} f(\mathbf{x}^{(j)}) \).

Solution:

For each \( i \), compute conditional expectation: \( \mathbb{E}[f(\mathbf{x}'^{(i)})\mid \cdot] = \sum_j \mathbb{P}(a_i=j) f(\mathbf{x}^{(j)}) = \sum_j w^{(j)} f(\mathbf{x}^{(j)}) \). Average over \( i \) and use linearity.

Problem 4 (Log-sum-exp stability):

Show that if \( m=\max_j \ell^{(j)} \), then \( \log(\sum_j e^{\ell^{(j)}}) = m + \log(\sum_j e^{\ell^{(j)}-m}) \), and explain why this reduces underflow.

Solution:

Factor \( e^m \) out of the sum. Underflow is avoided because all exponents \( \ell^{(j)}-m \le 0 \), so \( e^{\ell^{(j)}-m} \in (0,1] \), which is numerically representable even when \( e^{\ell^{(j)}} \) is not.

Problem 5 (Concrete resampling computation):

Let \( N=5 \) and normalized weights \( \mathbf{w} = (0.50, 0.20, 0.15, 0.10, 0.05) \). Compute \( N_{\text{eff}} \). If systematic resampling uses \( u_0 = 0.06 \), list the five positions \( u_i = u_0 + (i-1)/N \) and determine the selected ancestor indices.

Solution:

\[ N_{\text{eff}} = \frac{1}{0.50^2 + 0.20^2 + 0.15^2 + 0.10^2 + 0.05^2} = \frac{1}{0.25 + 0.04 + 0.0225 + 0.01 + 0.0025} = \\ \frac{1}{0.325} \approx 3.0769. \]

Positions are \( (0.06,\; 0.26,\; 0.46,\; 0.66,\; 0.86) \). With CDF \( (0.50,\; 0.70,\; 0.85,\; 0.95,\; 1.00) \): \( 0.06,0.26,0.46 \) fall in bin 1; \( 0.66 \) in bin 2; \( 0.86 \) in bin 4. Ancestors: \( (1,1,1,2,4) \) (1-indexed).

We derived particle-filter weighting as sequential importance sampling, proved practical stability tools (log-sum-exp), introduced ESS with rigorous bounds, and formalized resampling as an operator that preserves empirical expectations while mitigating weight degeneracy. These results are the mathematical core of Monte Carlo Localization; the next lesson specializes the likelihood term \( p(\mathbf{z}_t \mid \mathbf{x}_t) \) for LiDAR and vision sensing.