Particle Filters & Non-Gaussian Estimation

Comprehensive guide to sequential Monte Carlo (SMC) methods for tracking non-Gaussian, nonlinear systems.

Particle filters represent probability distributions as weighted samples (particles), enabling estimation in highly nonlinear and non-Gaussian scenarios where Kalman filters fail. pytcl ships a complete functional particle filter toolkit in pytcl.dynamic_estimation.particle_filters.

Table of Contents:

• Particle Filter Fundamentals

• Bootstrap Particle Filter

• Resampling Strategies

• Degeneracy Detection & Solutions

• Sequential Importance Sampling

• Practical Implementation

• Performance & Efficiency

• Common Issues & Solutions

• Best Practices

Particle Filter Fundamentals

Why Particle Filters?

Kalman filters assume: - Linear systems (or EKF/UKF with differentiability) - Gaussian noise - Unimodal distributions

Particle filters work with: - Arbitrary nonlinear models - Non-Gaussian and multimodal distributions - Discontinuous measurement likelihoods - Heavy-tailed distributions and outliers

Particle Filter Concept:

Represent state uncertainty as a set of \(N_p\) weighted samples:

\[p(x_k \mid z_{1:k}) \approx \sum_{i=1}^{N_p} w_k^{(i)} \, \delta(x_k - x_k^{(i)})\]

Where:

• \(x_k^{(i)}\): i-th particle (state sample)

• \(w_k^{(i)}\): weight of i-th particle

• \(\delta(\cdot)\): Dirac delta function

• Weights normalize to 1: \(\sum_i w_k^{(i)} = 1\)

Key Equations:

1. Prediction: Sample from motion model

   \[x_k^{(i)} \sim p(x_k \mid x_{k-1}^{(i)})\]

2. Update: Compute importance weights

   \[w_k^{(i)} \propto p(z_k \mid x_k^{(i)}) \cdot w_{k-1}^{(i)}\]

3. Normalize:

   \[\hat{w}_k^{(i)} = \frac{w_k^{(i)}}{\sum_j w_k^{(j)}}\]

4. Resample (if needed): Generate new particles from weighted distribution

Bootstrap Particle Filter

Algorithm: Sequential Importance Resampling (SIR)

pytcl provides the complete SIR cycle as bootstrap_pf_step, which performs predict, update, and adaptive resampling in one call. Particles and weights travel together in a ParticleState named tuple.

import numpy as np
from pytcl.dynamic_estimation.particle_filters import (
    bootstrap_pf_step,
    initialize_particles,
    particle_covariance,
    particle_mean,
)

rng = np.random.default_rng(42)

# 1D constant-velocity state: [position, velocity]
dt = 0.1

def f(x):
    """Motion model: x_{k+1} = f(x_k)."""
    return np.array([x[0] + x[1] * dt, x[1]])

def h(x):
    """Measurement model: observe position only."""
    return np.array([x[0]])

def Q_sample(n, rng):
    """Draw n process-noise samples, shape (n, 2)."""
    return rng.normal(0.0, [0.02, 0.10], size=(n, 2))

R = np.array([[0.25]])  # measurement noise covariance

# Initialize 500 particles from a Gaussian prior
state = initialize_particles(
    x0=np.array([0.0, 1.0]), P0=np.diag([1.0, 0.25]), N=500, rng=rng
)

# Simulate and filter
true_x = np.array([0.0, 1.0])
for k in range(50):
    true_x = f(true_x)
    z = h(true_x) + rng.normal(0.0, 0.5, size=1)
    state = bootstrap_pf_step(
        state.particles, state.weights, z, f, h, Q_sample, R, rng=rng
    )

x_hat = particle_mean(state.particles, state.weights)
P_hat = particle_covariance(state.particles, state.weights)
print(f"true:     pos={true_x[0]:.3f} vel={true_x[1]:.3f}")
print(f"estimate: pos={x_hat[0]:.3f} vel={x_hat[1]:.3f}")
print(f"pos std:  {np.sqrt(P_hat[0, 0]):.3f}")
# true:     pos=5.000 vel=1.000
# estimate: pos=5.081 vel=1.137
# pos std:  0.213

Predict and Update as Separate Steps

When you need finer control (e.g. multiple measurements per scan, custom likelihoods), use the individual building blocks. bootstrap_pf_update takes any likelihood function likelihood_func(z, x) -> float; gaussian_likelihood covers the common Gaussian-measurement case.

from pytcl.dynamic_estimation.particle_filters import (
    bootstrap_pf_predict,
    bootstrap_pf_update,
    gaussian_likelihood,
)

# Predict: propagate particles through f and add process noise
particles = bootstrap_pf_predict(state.particles, f, Q_sample, rng=rng)

# Update: reweight particles by measurement likelihood
z = np.array([5.2])

def likelihood(z, x):
    return gaussian_likelihood(z, h(x), R)

weights, log_lik = bootstrap_pf_update(particles, state.weights, z, likelihood)
print(f"weights sum to {weights.sum():.6f}")
print(f"log marginal likelihood: {log_lik:.3f}")
# weights sum to 1.000000
# log marginal likelihood: -0.325

Resampling Strategies

Problem: Particle Degeneracy

As the filter runs, most particles get zero weight (weight collapse). Solution: resample. pytcl ships three resamplers; each takes (particles, weights) and returns a new equally-weighted particle set.

Set up a degenerate particle set to compare them:

from pytcl.dynamic_estimation.particle_filters import (
    effective_sample_size,
    resample_multinomial,
    resample_residual,
    resample_systematic,
)

# Highly non-uniform weights: most mass on a few particles
N = 500
demo_particles = rng.normal(0.0, 1.0, size=(N, 2))
raw = np.exp(-0.5 * (demo_particles[:, 0] - 2.0) ** 2 / 0.1)
demo_weights = raw / raw.sum()
print(f"ESS before resampling: {effective_sample_size(demo_weights):.1f}")
# ESS before resampling: 43.0

1. Multinomial Resampling (Simple)

Sample with replacement proportionally to weight. Highest variance.

resampled = resample_multinomial(demo_particles, demo_weights, rng=rng)
print(f"unique ancestors kept: {len(np.unique(resampled[:, 0]))}")
# unique ancestors kept: 71

2. Systematic Resampling (Best for Most Cases)

Deterministic positions with a single random offset. Low variance, O(N).

resampled = resample_systematic(demo_particles, demo_weights, rng=rng)
print(f"unique ancestors kept: {len(np.unique(resampled[:, 0]))}")
# unique ancestors kept: 70

3. Stratified Resampling

Note

pytcl does not ship a stratified resampler. The implementation below is a self-contained reference; in practice resample_systematic is the recommended low-variance choice.

def resample_stratified(particles, weights, rng):
    """Stratified resampling: one uniform draw per stratum."""
    N = len(weights)
    u = (np.arange(N) + rng.uniform(size=N)) / N
    indices = np.searchsorted(np.cumsum(weights), u)
    return particles[indices]

resampled = resample_stratified(demo_particles, demo_weights, rng)
print(f"unique ancestors kept: {len(np.unique(resampled[:, 0]))}")
# unique ancestors kept: 70

4. Residual Resampling (Semi-Deterministic)

Keep floor(N * w_i) copies of each particle deterministically, then fill the remaining slots stochastically from the fractional remainders.

resampled = resample_residual(demo_particles, demo_weights, rng=rng)
print(f"unique ancestors kept: {len(np.unique(resampled[:, 0]))}")
# unique ancestors kept: 72

Resampling Comparison:

Function	Complexity	Variance	Stability
resample_multinomial	O(N log N)	High	Unstable
resample_systematic	O(N)	Lowest	Robust
stratified (hand-rolled)	O(N)	Low	Very Good
resample_residual	O(N log N)	Very low	Excellent

Recommended: resample_systematic for most applications.

Degeneracy Detection & Solutions

Effective Sample Size (ESS):

\[N_{\text{eff}} = \frac{1}{\sum_i \left(w^{(i)}\right)^2}\]

ESS ranges from 1 (all weight on one particle) to \(N\) (uniform weights). Resample when \(N_{\text{eff}} < \tau N\) (typically \(\tau = 0.5\)). pytcl computes it with effective_sample_size:

def resample_if_needed(particles, weights, threshold=0.5, rng=None):
    """Adaptive resampling: resample only when degenerate."""
    N = len(weights)
    if effective_sample_size(weights) < threshold * N:
        particles = resample_systematic(particles, weights, rng=rng)
        weights = np.full(N, 1.0 / N)
    return particles, weights

particles, weights = resample_if_needed(
    demo_particles, demo_weights, rng=rng
)
print(f"ESS after adaptive resampling: {effective_sample_size(weights):.1f}")
# ESS after adaptive resampling: 500.0

Solutions to Degeneracy:

1. Adaptive Resampling: Resample only when needed (above). This is what bootstrap_pf_step does internally via its resample_threshold argument.

2. Regularization: Add jitter to particles after resampling

def resample_with_regularization(particles, weights, rng, bandwidth=1.0):
    """Resample and add regularization noise."""
    resampled = resample_systematic(particles, weights, rng=rng)

    # Adaptive bandwidth from particle spread
    h_bw = bandwidth * particles.std(axis=0)
    return resampled + rng.normal(0.0, h_bw, size=resampled.shape)

regularized = resample_with_regularization(
    demo_particles, demo_weights, rng
)
print(f"distinct particles: {len(np.unique(regularized[:, 0]))}")
# distinct particles: 500

Sequential Importance Sampling (SIS)

More General Particle Filter Framework:

The bootstrap filter is SIS with the prior \(p(x_k \mid x_{k-1})\) as proposal plus adaptive resampling. Composing the pytcl building blocks by hand exposes each SIS stage, which is useful when you need a custom proposal or likelihood:

def sis_step(particles, weights, z, likelihood, rng):
    """One SIS cycle: propose, reweight, adaptively resample."""
    # 1. Propose from the motion model (bootstrap proposal)
    particles = bootstrap_pf_predict(particles, f, Q_sample, rng=rng)

    # 2. Reweight by measurement likelihood
    weights, _ = bootstrap_pf_update(particles, weights, z, likelihood)

    # 3. Resample if degenerate
    return resample_if_needed(particles, weights, rng=rng)

state = initialize_particles(
    x0=np.array([0.0, 1.0]), P0=np.diag([1.0, 0.25]), N=500, rng=rng
)
particles, weights = state.particles, state.weights

true_x = np.array([0.0, 1.0])
for k in range(30):
    true_x = f(true_x)
    z = h(true_x) + rng.normal(0.0, 0.5, size=1)
    particles, weights = sis_step(particles, weights, z, likelihood, rng)

x_hat = particle_mean(particles, weights)
print(f"true pos={true_x[0]:.3f}  estimate pos={x_hat[0]:.3f}")
# true pos=3.000  estimate pos=2.954

Practical Implementation

Complete Tracking System:

Range-only tracking is a classic nonlinear problem where the measurement function makes EKF linearization fragile:

# 2D constant-velocity state: [x, y, vx, vy]
def f_cv(x):
    dt = 0.5
    return np.array([x[0] + x[2] * dt, x[1] + x[3] * dt, x[2], x[3]])

def h_range(x):
    """Range from origin: strongly nonlinear in state."""
    return np.array([np.hypot(x[0], x[1])])

def Q_sample_cv(n, rng):
    return rng.normal(0.0, [0.05, 0.05, 0.20, 0.20], size=(n, 4))

R_range = np.array([[4.0]])

rng = np.random.default_rng(7)
state = initialize_particles(
    x0=np.array([100.0, 50.0, -2.0, 1.0]),
    P0=np.diag([100.0, 100.0, 1.0, 1.0]),
    N=2000,
    rng=rng,
)

truth = np.array([105.0, 45.0, -2.5, 1.2])
pos_errors, range_errors = [], []
for k in range(60):
    truth = f_cv(truth)
    z = h_range(truth) + rng.normal(0.0, 2.0, size=1)
    state = bootstrap_pf_step(
        state.particles, state.weights, z, f_cv, h_range,
        Q_sample_cv, R_range, rng=rng,
    )
    x_hat = particle_mean(state.particles, state.weights)
    pos_errors.append(np.hypot(*(x_hat[:2] - truth[:2])))
    range_errors.append(abs(np.hypot(*x_hat[:2]) - np.hypot(*truth[:2])))

print(f"mean position error: {np.mean(pos_errors):.2f}")
print(f"mean range error:    {np.mean(range_errors):.2f}")
# mean position error: 7.39
# mean range error:    2.72

Range-only measurements observe bearing only weakly (through motion over time), so the position error stays several times larger than the range error, which converges to the measurement noise floor. The particle cloud spreads along the range arc: exactly the non-Gaussian behavior a Kalman filter cannot represent.

Multi-Target Extension:

Run one ParticleState per target and update each with its associated measurement:

targets = {
    tid: initialize_particles(x0, np.diag([25.0, 25.0, 1.0, 1.0]),
                              N=1000, rng=rng)
    for tid, x0 in {
        0: np.array([100.0, 50.0, -2.0, 1.0]),
        1: np.array([-40.0, 80.0, 1.5, -0.5]),
    }.items()
}

# associations: list of (measurement, target_id) pairs from a data
# association stage (e.g. GNN); see :doc:`assignment_association`
associations = [(np.array([111.0]), 0), (np.array([90.0]), 1)]

for z, tid in associations:
    s = targets[tid]
    targets[tid] = bootstrap_pf_step(
        s.particles, s.weights, z, f_cv, h_range,
        Q_sample_cv, R_range, rng=rng,
    )

for tid, s in targets.items():
    x_hat = particle_mean(s.particles, s.weights)
    print(f"target {tid}: range estimate {np.hypot(*x_hat[:2]):.1f}")
# target 0: range estimate 111.0
# target 1: range estimate 89.7

Performance & Efficiency

Computational Complexity:

• Prediction: O(N_p x d) where d = state dimension

• Update: O(N_p) for likelihood evaluation

• Resampling: O(N_p) to O(N_p log N_p) depending on method

• Overall: O(N_p) per cycle (most efficient SMC method)

Memory Considerations:

def estimate_memory_usage(num_particles, state_dim, num_timesteps):
    """Estimate memory for storing particle histories."""
    bytes_per_particle = state_dim * 8  # double precision
    bytes_per_weight = 8

    bytes_per_step = (bytes_per_particle + bytes_per_weight) * num_particles
    return bytes_per_step * num_timesteps / (1024**2)

mb = estimate_memory_usage(1000, 10, 1000)
print(f"1000 particles, 10 states, 1000 steps: {mb:.0f} MB")
# 1000 particles, 10 states, 1000 steps: 84 MB

Scaling with Particle Count:

Monte Carlo error shrinks like \(1/\sqrt{N_p}\). Measure it directly on the 1D tracking problem from the first section:

# One fixed measurement sequence shared by every trial, so that
# trial-to-trial spread isolates the Monte Carlo error
data_rng = np.random.default_rng(123)
true_x = np.array([0.0, 1.0])
truths, zs = [], []
for _ in range(30):
    true_x = f(true_x)
    truths.append(true_x.copy())
    zs.append(h(true_x) + data_rng.normal(0.0, 0.5, size=1))

def run_trial(n_particles, rng):
    """Run the 1D tracker once, return mean position error."""
    state = initialize_particles(
        np.array([0.0, 1.0]), np.diag([1.0, 0.25]), n_particles, rng=rng
    )
    errs = []
    for tx, z in zip(truths, zs):
        state = bootstrap_pf_step(
            state.particles, state.weights, z, f, h, Q_sample, R, rng=rng
        )
        x_hat = particle_mean(state.particles, state.weights)
        errs.append(abs(x_hat[0] - tx[0]))
    return np.mean(errs)

for n in [50, 200, 1000]:
    errors = [run_trial(n, np.random.default_rng(seed))
              for seed in range(20)]
    print(f"N={n:5d}: mean_error={np.mean(errors):.4f} "
          f"std={np.std(errors):.4f}")
# N=   50: mean_error=0.1592 std=0.0171
# N=  200: mean_error=0.1480 std=0.0121
# N= 1000: mean_error=0.1473 std=0.0050

The mean error approaches the noise-limited floor while the trial-to-trial standard deviation drops roughly as \(1/\sqrt{N_p}\).

Common Issues & Solutions

Problem: Particle Collapse (All Weight on Few Particles)

Symptom: ESS drops to < 10% of N

Solutions:

1. Increase resampling threshold (e.g., resample at 0.5*N instead of 0.2*N)

2. Use regularization/jitter

3. Increase number of particles

4. Check measurement likelihood (may be too informative)

def diagnose_collapse(weights):
    """Analyze weight distribution."""
    sorted_w = np.sort(weights)[::-1]
    top_10_percent = np.sum(sorted_w[: len(weights) // 10])

    print(f"Top 10% have {top_10_percent * 100:.1f}% of weight")
    if top_10_percent > 0.8:
        print("WARNING: Significant weight collapse")
        print("  - Consider more particles")
        print("  - Add regularization noise")
        print("  - Increase resampling threshold")

diagnose_collapse(demo_weights)
# Top 10% have 93.9% of weight
# WARNING: Significant weight collapse
#   - Consider more particles
#   - Add regularization noise
#   - Increase resampling threshold

Problem: Particle Impoverishment (After Heavy Resampling)

Symptom: Many identical particles, filter stuck

Solutions:

1. Use regularization with adaptive bandwidth

2. Mix in samples from proposal distribution

3. Lower resampling threshold (resample less often)

def regularized_resample(particles, weights, rng, scale=1.0):
    """Resample with adaptive regularization (Silverman's rule)."""
    new_particles = resample_systematic(particles, weights, rng=rng)

    n = len(particles)
    h_bw = scale * n ** (-1 / 5) * new_particles.std(axis=0)
    return new_particles + rng.normal(0.0, h_bw, size=new_particles.shape)

jittered = regularized_resample(demo_particles, demo_weights, rng)
print(f"distinct particles: {len(np.unique(jittered[:, 0]))}")
# distinct particles: 500

Problem: Filter Diverges (State Estimates Far from Reality)

Solutions:

1. Check likelihood calculation for bugs

2. Verify process noise is appropriate

3. Increase particles

4. Use likelihood annealing (temper a sharp likelihood in stages)

def annealed_update(particles, weights, z, likelihood_fn, rng,
                    num_stages=5):
    """Incorporate a sharp likelihood gradually (tempering).

    At each stage, apply the likelihood raised to an increment of
    beta so that the full likelihood is absorbed after all stages,
    resampling between stages to keep the particle set healthy.
    """
    beta_prev = 0.0
    for stage in range(1, num_stages + 1):
        beta = stage / num_stages
        lik = np.array([likelihood_fn(z, p) for p in particles])
        weights = weights * lik ** (beta - beta_prev)
        weights = weights / weights.sum()

        particles, weights = resample_if_needed(
            particles, weights, rng=rng
        )
        beta_prev = beta
    return particles, weights

def sharp_likelihood(z, x):
    return gaussian_likelihood(z, x[:1], np.array([[0.01]]))

z_sharp = np.array([2.0])
annealed, w = annealed_update(
    demo_particles, np.full(len(demo_particles), 1 / len(demo_particles)),
    z_sharp, sharp_likelihood, rng,
)
print(f"ESS after annealed update: {effective_sample_size(w):.1f}")
# ESS after annealed update: 284.4

Best Practices

1. Choose Particle Count Wisely

   • Start with N=500 for 3D problems

   • Rule of thumb: N ~ 10^(d/2) for quick estimates

   • Monitor ESS; if < 100 typically, increase N

2. Adaptive Parameters

   • Resampling threshold (resample_threshold in bootstrap_pf_step): 0.3-0.7 of N

   • Regularization bandwidth: use Silverman’s rule

   • Process noise: match actual motion uncertainty

3. Monitor Filter Health

   • Track effective_sample_size over time (should oscillate, not decay monotonically)

   • Check innovation sequences (should be white noise)

   • Visualize particle cloud (should spread over state space)

4. Use Systematic Resampling

   • Better variance than multinomial

   • Negligible overhead vs bootstrap

   • The default resample_method in bootstrap_pf_step

5. Combine with Other Methods

   • Use Kalman filter as proposal distribution (proposal-guided PF)

   • Use particle filter for initialization of EKF

   • Hybrid systems for mixed Gaussian/non-Gaussian

6. For Real-Time Systems

   • Use smallest N that maintains accuracy (test offline first)

   • Compute ESS during update, resample only when needed

   • Profile particle filter speed vs system requirements

Troubleshooting

Diagnostic Checklist:

def diagnose_particle_filter(particles, weights, spread_floor=0.01):
    """Basic particle filter health checks."""
    issues = []

    # Check 1: effective sample size
    ess = effective_sample_size(weights)
    if ess < 0.2 * len(weights):
        issues.append("Severe weight collapse - increase particles")

    # Check 2: particle spread
    spread = particles.std(axis=0)
    if np.any(spread < spread_floor):
        issues.append("Particles too concentrated - increase noise")

    return issues

print(diagnose_particle_filter(demo_particles, demo_weights))
# ['Severe weight collapse - increase particles']

print(diagnose_particle_filter(state.particles, state.weights))
# []

Beyond these structural checks, validate estimates against ground truth in simulation and check innovation whiteness; see Troubleshooting Guide.

See Also

• Common Use Cases & Recipes - Ready-to-use particle filter examples

• Troubleshooting Guide - Common particle filter issues

• Kalman Filter Tuning Guide - Parameter selection strategies

• API: pytcl.dynamic_estimation.particle_filters module for implementations