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.

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

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:

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.