Implementing a Particle Filter for Indoor Positioning

This page is the hands-on companion to Sensor Fusion & Particle Filters for Indoor Positioning: it gives you one compact, typed ParticleFilter you can drop into a positioning service, plus the parameter values that make it behave.

What the Filter State Actually Holds

A particle filter for pedestrian indoor positioning represents belief as a population of hypotheses about where the user is. Each particle is four numbers: x and y in the building’s local metric frame, a heading in radians, and a weight that says how plausible that hypothesis is given every measurement so far. The population as a whole approximates the posterior distribution over position — its spread is your uncertainty, its weighted mean is your best estimate, and the fact that it can be bimodal is what lets it survive parallel corridors that a single-point estimate would average into a wall.

The filter never stores raw sensor history. It carries only the current cloud, and each frame folds in exactly two things: a motion input (a step displacement and heading from a pedestrian-dead-reckoning module) and a measurement (a radio (x, y) fix from a fingerprint or beacon estimator). One frame is: move every particle by the step, reweight by the fix, kill particles that crossed a wall, and resample when the cloud has collapsed onto too few survivors.

The four-step particle filter cycle and what advances each step Four states in a cycle. Predict moves every particle by the inertial step plus process noise, and runs on each IMU sample. Update weights each particle by the likelihood of the observed radio fix given that particle's position. Constrain kills any particle whose step crossed a wall polygon, which is what makes the estimate physically plausible. Resample runs only when the effective sample size falls below half the particle count, and produces a fresh particle set that feeds the next predict step. Predict, update, constrain — and resample only when you must each IMU step weights assigned survivors fresh particle set Predict move every particle by the IMU step + noise Update weight by p(radio fix | particle position) Constrain kill particles whose step crossed a wall Resample only when N_eff < N/2

Constrain is the step that earns the filter. A Kalman filter can smooth a noisy fix; only a particle set can enforce “you did not walk through that wall”, because the constraint is not expressible as a Gaussian.

Minimal Working Example

The class below is a complete single-floor filter. Particles are a numpy array of shape (N, 4); every stage is vectorised except the wall test. Motion in, one pose out.

import logging

import numpy as np
from shapely.geometry import LineString
from shapely.strtree import STRtree

logger = logging.getLogger(__name__)


class ParticleFilter:
    def __init__(self, n: int, origin: tuple[float, float], walls: STRtree,
                 sigma_meas: float = 2.0, seed: int = 0) -> None:
        self.rng = np.random.default_rng(seed)
        self.walls = walls
        self.sigma_meas = sigma_meas
        self.p = np.zeros((n, 4))
        self.p[:, 0] = origin[0] + self.rng.normal(0, 1.0, n)
        self.p[:, 1] = origin[1] + self.rng.normal(0, 1.0, n)
        self.p[:, 3] = 1.0 / n

    def step(self, step_len: float, heading: float, fix: tuple[float, float]) -> np.ndarray:
        prev = self.p[:, :2].copy()
        h = self.p[:, 2] + heading + self.rng.normal(0, 0.10, len(self.p))
        d = step_len + self.rng.normal(0, 0.15, len(self.p))
        self.p[:, 0] += d * np.cos(h)
        self.p[:, 1] += d * np.sin(h)
        self.p[:, 2] = np.arctan2(np.sin(h), np.cos(h))

        sq = (self.p[:, 0] - fix[0]) ** 2 + (self.p[:, 1] - fix[1]) ** 2
        logw = np.log(np.clip(self.p[:, 3], 1e-300, None)) - 0.5 * sq / self.sigma_meas ** 2
        for i, seg in enumerate(LineString([prev[i], self.p[i, :2]]) for i in range(len(self.p))):
            if any(seg.crosses(self.walls.geometries[j]) for j in self.walls.query(seg)):
                logw[i] = -np.inf                      # zero weight: crossed a wall
        w = np.exp(logw - logw.max())
        total = w.sum()
        if not np.isfinite(total) or total <= 0:
            logger.warning("degenerate weights; reseeding uniform")
            w = np.full(len(self.p), 1.0 / len(self.p))
        self.p[:, 3] = w / w.sum()
        self._maybe_resample()
        return np.average(self.p[:, :3], axis=0, weights=self.p[:, 3])

    def _maybe_resample(self) -> None:
        neff = 1.0 / np.sum(self.p[:, 3] ** 2)
        if neff < len(self.p) / 2:                     # resample on evidence, not the clock
            pos = np.cumsum(self.p[:, 3])
            picks = np.searchsorted(pos, (self.rng.uniform(0, 1 / len(self.p))
                                          + np.arange(len(self.p)) / len(self.p)))
            self.p = self.p[np.clip(picks, 0, len(self.p) - 1)].copy()
            self.p[:, :2] += self.rng.normal(0, 0.05, (len(self.p), 2))  # roughening
            self.p[:, 3] = 1.0 / len(self.p)

Driving it is one call per frame: pose = pf.step(step_len, heading_delta, radio_fix). The returned pose is (x, y, heading), ready to hand downstream.

Parameter Reference

Parameter Symbol Typical value Effect of raising it
Particle count n 300–500 (corridors), up to 2000 (open space) More hypotheses, less depletion, linear cost increase
Step-length noise sigma_step 0.15 m Cloud spreads faster along motion; more tolerant of stride error, less confident
Heading noise sigma_head 0.10 rad Radio fixes regain authority over direction; cures wrong-corridor drift
Measurement spread sigma_meas 2.0 m Fix trusted less; smoother but slower to correct drift
Resample threshold Neff < N/2 half the particle count Higher threshold resamples more often (risking impoverishment); lower resamples less
Roughening jitter — 0.05 m post-resample Restores diversity after resampling; too much re-adds noise
Particle filter parameters and the trade-off each one sets A table of six parameters. A particle count of 500 to 2,000 costs linearly, and below about 300 the fix visibly rattles. Process noise of about 0.25 metres per step controls how much the filter trusts its motion model; set too low, the filter ignores the radio entirely. Measurement noise of 2.5 to 4 metres controls trust in the radio; set too low, the fix chases every scan. Resampling at half the particle count is standard; resampling too eagerly causes particle impoverishment. The wall-crossing test — intersecting each particle's step segment with the wall geometry — is the constraint that justifies the filter. The initial spread should be about twice the radio accuracy; too tight and the filter cannot recover from a bad start. Six knobs, and two of them are really one Parameter Typical Trade-off particle count N 500 - 2,000 linear cost; below ~300 the fix rattles process noise sigma_v 0.25 m/step △ too low: filter ignores the radio measurement sigma_z 2.5 - 4 m △ too low: fix chases every scan resample threshold N / 2 too eager: particle impoverishment wall-crossing test segment x wall ● the constraint worth its cost init spread radio accuracy x 2 too tight: filter cannot recover The two sigmas are a ratio: what matters is how much the filter trusts motion vs radio.

Only the ratio of the two sigmas matters. Tuning them independently produces a filter that is either a dead-reckoner that ignores the radio, or a smoother that adds latency and nothing else.

sigma_meas should track your estimator’s real error. If your radio layer reports a per-fix sigma, pass it in per frame rather than fixing it in the constructor — the filter then automatically distrusts fixes taken in poorly covered areas.

Common Errors & Fixes

RuntimeWarning: invalid value encountered in divide from the normalisation. Every weight underflowed to zero, so w.sum() is 0 and the division produces nan. This happens when the whole cloud has drifted far from the fix and the raw Gaussian likelihoods are astronomically small. The fix is already in the example — compute weights in log-space and subtract the max before exponentiating:

Effective sample size with and without resampling Two curves of effective sample size against filter step, out of one thousand particles. Without resampling the effective size decays exponentially, falling below one hundred within about twenty steps and reaching single figures by step forty-two, at which point one particle carries essentially all the weight and the filter has degenerated. With resampling triggered whenever the effective size falls below five hundred, the curve oscillates between roughly six hundred and nine hundred and never degenerates. Weight concentrates on one particle unless you intervene 0 250 500 750 1000 0 20 40 60 filter step effective sample size (of 1,000 particles) no resampling resample when N_eff < 500 degenerate: one particle carries all the weight

Degeneracy is silent. A degenerate filter still returns a position every step — it has simply stopped considering alternatives, so it tracks the motion model and ignores the radio entirely.

logw = np.log(np.clip(self.p[:, 3], 1e-300, None)) - 0.5 * sq / self.sigma_meas ** 2
w = np.exp(logw - logw.max())          # largest weight becomes 1.0, never underflows

The estimate freezes and stops tracking new fixes. This is particle impoverishment: after aggressive resampling, nearly every particle is a copy of one ancestor, so the cloud has no diversity left to explore. Watch Neff; if it sits near 1, you are resampling too hard. Add roughening — a small Gaussian jitter to positions right after resampling — which is the self.p[:, :2] += self.rng.normal(0, 0.05, ...) line above. It re-injects spread without discarding the concentration you earned.

The marker moves at the wrong speed — half or double the real pace. The motion scale is wrong: step_len is being passed in the wrong unit (centimetres instead of metres) or the pedestrian-dead-reckoning module is emitting a raw sensor magnitude rather than a calibrated stride. Assert your units at the boundary — a human step is ~0.6–0.8 m — and calibrate the step-length model before it reaches the filter, because no amount of process noise fixes a systematic scale error.

Integration Point

This filter is the middle stage of the positioning pipeline. Its measurement input is the (x, y) fix produced by WiFi RSSI fingerprinting or BLE beacon positioning, and its motion input is the step-and-heading stream from a dead-reckoning module — the design rationale for fusing exactly these two is laid out in the parent guide, Sensor Fusion & Particle Filters. The single pose it returns each frame is not the end of the line: it is still a free-floating point that must be bound to a routable edge before the wayfinding engine can use it, which is the job of snapping noisy positions to the routing graph. Keep the filter and the snapper separate — the filter smooths, the snapper constrains to the network.

Frequently Asked Questions

Should I resample every frame?

No — resample only when the effective sample size Neff = 1 / Σwᵢ² falls below a threshold, typically half the particle count. Resampling every frame throws away the particle diversity you need to represent uncertainty and quickly leads to impoverishment, where the whole cloud collapses onto one ancestor and the estimate freezes. Resample on evidence that the cloud has concentrated, not on the clock.

What do I feed the filter when there is no radio fix this frame?

Run predict only and skip the weighting step. Inertial events arrive far faster than radio fixes, so most frames are motion-only: advance the particles by the step, leave the weights untouched, and do not resample. The cloud simply spreads a little more each dead-reckoning step until the next fix arrives and pulls it back. This is the fusion working as intended — inertial carries you between fixes, radio corrects the accumulated drift.

How do I stop the wall test from dominating my frame time?

Keep the walls in a shapely STR-tree, as the example does, and query it per particle so each one only tests the handful of segments near its own motion line rather than every wall on the floor. That turns the constraint from O(N·W) into roughly O(N·log W). If it is still too slow at high particle counts, precompute a rasterised walkable mask and test particle cells against it, trading memory for a constant-time lookup.

This page is part of the Sensor Fusion & Particle Filters guide within the Indoor Positioning & Fingerprinting reference.