Sensor Fusion & Particle Filters for Indoor Positioning

A single indoor sensor never gives you a usable track. Radio fixes from a fingerprint estimator jump a metre or two between consecutive samples; an inertial dead-reckoning stream is smooth but drifts without bound. This guide, part of the Indoor Positioning & Fingerprinting reference, shows how a particle filter fuses those two failure modes into one continuous estimate that is both stable and geometrically legal — a position that stays inside walkable space, keeps its heading, and hands a clean fix to the wayfinding engine. The technique sits between the raw estimators upstream and the routing graph downstream: it consumes fixes and inertial steps, and it emits a single smoothed pose per frame.

The Problem: Two Sensors, Two Complementary Ways to Be Wrong #

Indoor positioning has no equivalent of a clean outdoor GNSS lock. Every input is degraded, and each is degraded differently, which is exactly what makes fusion worth doing.

A radio estimator — whether it is WiFi RSSI fingerprinting or BLE beacon positioning — is unbiased but jittery. Its per-sample error is roughly zero-mean, so it does not walk away over time, but any single fix can land two or three metres from truth because RSSI is noisy and multipath is savage indoors. Feed those fixes straight to a map and the marker teleports.

An inertial stream — step events and a heading from a pedestrian-dead-reckoning module reading the phone’s accelerometer, gyroscope, and magnetometer — is smooth but biased. Consecutive samples are tightly correlated, so the track looks beautiful, but small errors in step length and heading integrate into unbounded drift. After thirty seconds of pure dead reckoning you may be a whole corridor off.

Fuse them and the errors cancel: the inertial motion model carries the estimate smoothly between radio fixes, and each radio fix pins the accumulating inertial drift back to truth. A particle filter is the natural fusion engine because indoor belief is not a tidy Gaussian blob. It is multimodal — you might be in one of two parallel corridors — and it is hard-constrained by walls, which a linear filter cannot represent.

That is the core reason to reach past a Kalman filter. A Kalman filter (and its extended variant, the EKF) assumes a single unimodal Gaussian belief and linear-Gaussian dynamics; it is cheap and excellent when those hold. Indoors they do not: the wall-constraint likelihood is a hard non-linear cut, and the belief splits around obstacles. A particle filter represents the belief as a cloud of weighted samples, so it carries multiple hypotheses and applies an arbitrary likelihood — including “this particle just walked through a wall, kill it” — with no linearisation at all.

Prerequisites & Dependencies #

This guide assumes the surrounding pipeline is already producing the two inputs the filter fuses, plus the floor geometry it constrains against:

• A radio fix source. A function that returns a (x, y, sigma) estimate per sample from your fingerprint or beacon layer — the output of WiFi RSSI fingerprinting or trilateration over BLE. The filter treats it as an unbiased measurement with a known standard deviation.
• An inertial motion stream. A pedestrian-dead-reckoning module emitting a step displacement (metres) and heading (radians) per event. The filter uses this as its motion model; it does not need raw IMU samples.
• Floor geometry as walls. The walkable-space boundary from your parsed plan, expressed as shapely LineString segments. This is the same geometry produced by wall and door detection, reused here as a hard motion constraint.
• Libraries. numpy for the particle arrays and vectorised weighting, shapely (2.x) for the wall-crossing test. A scipy.spatial KD-tree is optional for large wall sets.

Everything below operates in the building’s local Cartesian frame — metres, origin on a survey control point — so no re-projection happens inside the filter.

How It Works: Predict, Update, Constrain, Resample #

The filter maintains N particles, each a hypothesis (x, y, heading, weight). Every frame runs one cycle of four stages, and the order matters: motion first, evidence second, geometry third, renormalisation last.

1. Predict. Advance every particle by the latest inertial step and heading, adding process noise so the cloud spreads to cover the true motion uncertainty. This is the motion model; it is where the smoothness comes from.
2. Update. Weight each particle by how well its position agrees with the current radio fix, using a Gaussian likelihood centred on the fix. Particles near the fix gain weight; distant ones lose it. This is where drift gets corrected.
3. Constrain. Zero the weight of any particle whose predicted step crossed a wall segment. A hypothesis that teleported through a partition is not physically reachable and must not survive.
4. Resample. When the effective sample size collapses, draw a fresh particle set in proportion to weight, so computation concentrates on plausible hypotheses. The weighted mean of the surviving cloud is the reported pose.

Step-by-Step Implementation #

The three code blocks below implement the cycle end to end. Particles are held as a single numpy array of shape (N, 4) — columns x, y, heading, weight — so every stage is vectorised.

Step 1: Predict from the inertial step with process noise #

The predict step is the motion model. Each particle takes the reported step length and heading, but with independent Gaussian noise added to both, so the cloud spreads to cover the real uncertainty in stride length and turn rate. Too little noise and the filter grows overconfident and ignores the radio fix; too much and the estimate goes slack.

import logging

import numpy as np

logger = logging.getLogger(__name__)


def predict(
    particles: np.ndarray,
    step_len: float,
    heading: float,
    sigma_step: float = 0.15,
    sigma_head: float = 0.10,
    rng: np.random.Generator | None = None,
) -> np.ndarray:
    """Advance particles (N,4: x,y,heading,weight) by one inertial step with process noise."""
    if particles.ndim != 2 or particles.shape[1] != 4:
        raise ValueError(f"particles must be (N,4), got {particles.shape}")
    rng = rng or np.random.default_rng()
    n = particles.shape[0]

    noisy_head = particles[:, 2] + heading + rng.normal(0.0, sigma_head, n)
    noisy_len = step_len + rng.normal(0.0, sigma_step, n)
    out = particles.copy()
    out[:, 0] += noisy_len * np.cos(noisy_head)
    out[:, 1] += noisy_len * np.sin(noisy_head)
    out[:, 2] = np.arctan2(np.sin(noisy_head), np.cos(noisy_head))  # wrap to [-pi, pi]
    logger.debug("predict: step=%.2fm heading=%.3frad n=%d", step_len, heading, n)
    return out

Step 2: Weight particles by the radio-fix likelihood #

The update step folds in the measurement. Each particle’s weight is multiplied by a 2D isotropic Gaussian evaluated at its distance from the radio fix, with the fix’s own sigma as the spread. Weights are computed in log-space and re-exponentiated after subtracting the max, which keeps the arithmetic from underflowing when the cloud has drifted far from the fix.

import logging

import numpy as np

logger = logging.getLogger(__name__)


def update_weights(
    particles: np.ndarray,
    fix_xy: tuple[float, float],
    sigma_meas: float = 2.0,
) -> np.ndarray:
    """Reweight particles by a Gaussian likelihood around a radio fix; returns normalised weights."""
    if sigma_meas <= 0:
        raise ValueError("sigma_meas must be positive")
    dx = particles[:, 0] - fix_xy[0]
    dy = particles[:, 1] - fix_xy[1]
    sq_dist = dx * dx + dy * dy

    log_lik = -0.5 * sq_dist / (sigma_meas * sigma_meas)          # Gaussian, in log-space
    log_w = np.log(np.clip(particles[:, 3], 1e-300, None)) + log_lik
    log_w -= log_w.max()                                          # stabilise before exp
    w = np.exp(log_w)
    total = w.sum()
    if total <= 0 or not np.isfinite(total):
        logger.warning("degenerate weights (total=%.3e); resetting to uniform", total)
        w = np.full(particles.shape[0], 1.0 / particles.shape[0])
    else:
        w /= total
    return w

Step 3: Apply the wall constraint, resample, and estimate #

The final stage enforces geometry, then renormalises the population. Any particle whose motion segment from its previous position crossed a wall is zeroed — that hypothesis walked through a partition and is dead. Then systematic resampling redraws the cloud in proportion to weight (low variance, one random draw), and the weighted mean of the survivors is the pose you report.

import logging

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

logger = logging.getLogger(__name__)


def constrain_and_resample(
    prev: np.ndarray,
    curr: np.ndarray,
    weights: np.ndarray,
    walls: STRtree,
    rng: np.random.Generator | None = None,
) -> tuple[np.ndarray, np.ndarray]:
    """Zero wall-crossing particles, systematically resample, and return (particles, mean_pose)."""
    rng = rng or np.random.default_rng()
    n = curr.shape[0]

    for i in range(n):
        seg = LineString([(prev[i, 0], prev[i, 1]), (curr[i, 0], curr[i, 1])])
        if walls.query(seg).size and any(seg.crosses(walls.geometries[j]) for j in walls.query(seg)):
            weights[i] = 0.0

    total = weights.sum()
    if total <= 0:
        logger.error("all particles died on wall constraint; reseeding around last mean")
        weights = np.full(n, 1.0 / n)            # recover rather than crash
    else:
        weights /= total

    positions = np.cumsum(weights)
    step = 1.0 / n
    start = rng.uniform(0.0, step)
    picks = np.searchsorted(positions, start + step * np.arange(n))
    resampled = curr[np.clip(picks, 0, n - 1)].copy()
    resampled[:, 3] = 1.0 / n

    mean_pose = np.average(curr[:, :3], axis=0, weights=weights)
    logger.info("resampled n=%d mean=(%.2f, %.2f)", n, mean_pose[0], mean_pose[1])
    return resampled, mean_pose

Edge Cases & Gotchas #

The recurring failure is treating resampling as a per-frame reflex. Resample on evidence — the effective sample size — not on the clock.

Validation Output #

Fusion is worth it only if it beats the raw radio fixes, so validate against a surveyed ground-truth track and assert the root-mean-square error shrinks. Replay a logged walk through the filter and compare per-frame error to the raw estimator:

import numpy as np

def rms_error(track: np.ndarray, truth: np.ndarray) -> float:
    return float(np.sqrt(np.mean(np.sum((track - truth) ** 2, axis=1))))

raw_rms = rms_error(raw_fixes, ground_truth)        # jittery radio-only track
fused_rms = rms_error(filtered_track, ground_truth)  # particle-filter output

assert fused_rms < raw_rms, "fusion must beat raw fixes"
assert fused_rms < 2.5, f"fused RMS {fused_rms:.2f}m exceeds the 2.5m target"
# A well-tuned filter typically cuts RMS error by 30-50% versus raw fixes and
# removes the frame-to-frame teleporting entirely.
print(f"raw={raw_rms:.2f}m  fused={fused_rms:.2f}m  improvement={100*(1-fused_rms/raw_rms):.0f}%")

The other check is legality: assert that no reported pose lies outside walkable space. If the weighted mean ever lands inside a wall polygon, the constraint stage is not catching a crossing — usually because the wall set has a gap where a door was removed.

Performance & Scale Notes #

• Particle count is the master dial. Cost is linear in N. A single-floor corridor network tracks well with 300–500 particles; a large open concourse with genuine multimodality may need 1,000–2,000. Below ~200 you risk depletion; above a few thousand you are burning cycles for no accuracy gain.
• The wall test dominates runtime. Naive per-particle crosses() against every wall is O(N·W). Put walls in an shapely STR-tree (or a scipy.spatial KD-tree of segment midpoints) so each particle only tests its handful of local candidates — that turns the per-frame cost from tens of milliseconds to sub-millisecond.
• Stay inside the frame budget. At a 1–2 Hz radio-fix rate and 50 Hz inertial events, the filter has tens of milliseconds per cycle on a phone. Vectorise predict and update fully; only the wall test needs a Python loop, and the tree keeps it short.
• Resampling is cheap, roughening is free. Systematic resampling is O(N) with one random draw. Adding a small roughening jitter after resampling costs one more numpy call and is the single most effective guard against impoverishment.
• Downstream cost is bounded. The filter emits exactly one pose per frame, so snapping that pose onto the routing graph sees a clean, single-point input rather than a cloud.

Frequently Asked Questions #

Related #

• Implementing a Particle Filter for Indoor Positioning — the concrete, copy-ready ParticleFilter class and parameter tuning.
• WiFi RSSI Fingerprinting — one of the radio-fix sources the filter fuses.
• BLE Beacon Positioning — the other radio-fix source, useful where WiFi coverage is thin.
• Snapping Positioning Fixes onto the Routing Graph — where the filter’s smoothed pose goes next.
• Wall & Door Detection Algorithms — the source of the wall geometry the constraint stage uses.

This page is part of the Indoor Positioning & Fingerprinting reference.