The particle filter

September 10, 2026

The Kalman filter carries its belief as a mean and a covariance: one best guess, and an ellipse of doubt around it. That is the right shape for a target that is somewhere near where you last saw it. It is the wrong shape for a robot switched on in a corridor with no idea which end, because the honest belief is two places at once, and no single ellipse can be in two places.

A particle filter carries belief as a population of guesses instead. Each particle is a complete pose, position and heading, and each carries a weight saying how well it has explained the sensor so far. The population can take any shape the evidence gives it: spread over a whole building, split into two clusters at opposite corners, or gathered into a knot a few centimetres across.

Belief as samples

The state is a pose $x = (p_x, p_y, \theta)$. Belief about it is $N$ particles $x^{[i]}$ with weights $w^{[i]}$ summing to one, and any question about the state is answered by a weighted sum over them:

Put $f(x) = x$ and that is the mean pose the crosshair marks. Put $f$ equal to one inside a region and zero outside and it is the probability the robot is in that region, which is how the tool counts hypotheses: weight binned onto a grid, and each run of heavy cells is one place the population believes in.

Nothing here assumes the belief is Gaussian, or that the motion is linear, or that the sensor is. The cost is that $N$ has to be large enough for the population to have a particle near wherever the truth is, and the number needed grows with every dimension of state. Three dimensions is comfortable. Twelve, the state a quadcopter carries, is not, and there a Kalman filter or one of its relatives is the right tool.

Three moves, once per scan

Predict. Every particle is pushed through the same motion model the robot was told to execute, speed $v$ and turn rate $\omega$ for $\Delta t$, each with its own draw of noise:

The noise is not a nuisance to be minimised. After a resample the population is copies of the same few poses, and this is the only step that spreads them apart again so the next scan has distinct hypotheses to choose between. Drag motion noise to zero and the copies stay copies: the cloud stops refining, and follows the odometry off the truth with nothing to pull it back.

Weigh. Each particle says what the lidar would read from its pose, by casting each beam against the map until it hits a wall. The weight is the likelihood of the actual scan $z$ given that prediction $\hat z^{[i]}$, independent Gaussian noise of standard deviation $\sigma$ on each of the $K$ beams, multiplied into whatever weight the particle already had:

Computed in log space, with the best particle’s log-weight subtracted from every one before the exponential is taken. Eight beams from a pose a few metres off give an exponent in the hundreds, and $e^{-400}$ is exactly zero in a double. A population in which every weight is zero has nowhere to divide, and a scan taken from anywhere but right next to the current cloud would kill the filter outright. After the shift the best particle has weight one before normalising, whatever its absolute likelihood was. A particle standing inside a wall gets zero without a ray being cast: the map is known, and no scan can argue it back.

Resample. Weights on their own degenerate. A few particles come to carry nearly all of it and the rest are carried along dead, still predicted and still weighed, contributing nothing. The measure is the effective sample size,

which is $N$ when all weights are equal and 1 when one particle has everything. Set resampling to never and watch the readout: it is a handful of particles within three seconds and 1.0 by five, whatever the sliders say. Resampling draws a new population of $N$ from the old one in proportion to weight, so a particle at three times the average weight becomes about three copies and one at a hundredth usually vanishes, then resets every weight to $1/N$. The tool does it only when $N_{\text{eff}}$ falls below $N/2$, the usual rule: resampling every step throws away diversity to fix a problem that has not arisen yet.

The tolerance is not the sensor’s noise

The lidar here has five centimetres of noise. The filter runs with $\sigma = 2$ metres, and the gap is the single most important number in the tool.

$\sigma$ is not a description of the sensor. It is how far a beam is allowed to disagree before the particle is discounted, and with two thousand guesses spread over seventy square metres and a full turn of heading, the particle that fits the first scan best is typically three-quarters of a metre and fifteen degrees from the truth. At $\sigma = 0.3$ the first scan leaves an effective sample size of about two: one particle, wherever it happened to land, takes the entire population, and the filter has committed before it has seen anything. At $\sigma = 2$ the same scan on the same population leaves around 270 effective particles, spread over every pose that is roughly consistent, and the filter keeps its options open until the map closes them.

Drag beam tolerance down to 0.3 and press reset the cloud to watch the collapse. The population gathers into one tight knot in the first second, sometimes on the robot, as often on the far corner, and there is no way to tell the two runs apart from the inside. Drag it up to 3 and the knot never gets tight: the settled spread grows with $\sigma$, because a scan cannot pin a pose finer than the tolerance it is judged at. Two metres is the trade this building sets: wide enough that the first scan does not decide alone, narrow enough that the settled spread is about a third of a metre and the mean within ten centimetres or so of the robot.

Two places at once

The building is a ring corridor around a block, and turned half a turn it lands on itself: stand anywhere in it, take the spot diagonally opposite facing the other way, and the eight beams read the same numbers. A filter switched on with no prior is entitled to believe both, and does. Within a couple of seconds the cloud is two clusters at opposite corners, ringed and labelled with their share, each facing the other’s way.

The split between them means nothing. Both clusters explain every scan equally well, so whichever holds more weight is the one whose particles happened to land a little closer, and the proportion wanders from step to step on Monte Carlo noise alone. Seventy-thirty is not evidence. Across eight runs at the tool’s settings the weaker cluster averaged thirty percent of the belief through the ambiguous stretch and never fell below one percent, which is the property that matters: it survives to be right if it turns out to be.

Look at the crosshair while this lasts. The weighted mean of two clusters is the point between them, which here is inside the central block, and the readout puts it a metre or more from the robot. That is not a defect in the estimate. It is the honest answer to a question with two answers, and a Kalman filter, which can only report a mean and an ellipse, would have to report exactly that point and call it the estimate.

What ends it is the one recess in the top wall. The robot itself does not have to reach the recess; its ghost does. As the robot drives along the bottom corridor, the ghost cluster drives along the top, and the moment a beam from the ghost’s position should see the recess and the real scan does not show it, that cluster starts to lose weight, a factor of $e^{-0.5\,(1.1/2)^2}$ or so per scan on the beams that disagree. It fades over about three seconds and is gone. From then on there is one cluster, and it is on the robot.

Which resampling

Multinomial resampling, the textbook version, draws $N$ times independently from the weights. It gets the proportions right on average and wastes diversity doing it: with every weight equal, each particle is missed by all $N$ draws with probability $(1 - 1/N)^N \approx 1/e$, so about a third of a perfectly healthy population is dropped and replaced by copies of the rest for no reason at all.

Low-variance, or systematic, resampling uses one uniform draw $u_0$ and $N$ evenly spaced pointers at $(u_0 + m)/N$ along the cumulative weight. A particle of weight $w$ is copied either $\lfloor Nw \rfloor$ or $\lceil Nw \rceil$ times, never anything else. Equal weights come back untouched. It is what every localisation implementation uses, and what the tool defaults to.

In this building it makes less visible difference than the textbook suggests. The two clusters are thinned by weighting far more than by the resample draw, so their split wanders about as much either way; the test that shows the difference is the one above, on a population with nothing to choose between.

Confidently wrong

Press kidnap the robot. It is picked up and put down somewhere else, and the cloud stays exactly where it was. Every particle now disagrees with the scan by metres, and every particle disagrees by about the same amount, so after the shift by the best of them the weights come out nearly uniform, $N_{\text{eff}}$ stays high, and nothing resamples. The filter is as confident as it was a second ago and entirely wrong, and there is no quantity inside it that says so: the best of a wrong population is normalised to certainty like any other best.

Nothing in predict or weigh can fix this. A resampled population only ever holds descendants of poses it already had, and none of them is anywhere near the robot. The cloud will follow the odometry from where it is, slide onto a corner that fits when the robot turns one, and in a building this symmetric that is the truth about half the time and its ghost the rest. In eight runs without help, three found the robot again inside forty seconds, by that luck.

The repair is to admit particles from outside the population. Fresh particles replaces one percent of the population every step with poses drawn uniformly over the free space, each at the mean weight. While the filter is right they score nothing against a cluster that is fitting to centimetres and vanish at the next resample, so the cost is one percent, permanently. When the filter is wrong, the first one to land anywhere near the robot outscores the entire cluster by hundreds of orders of magnitude, $N_{\text{eff}}$ drops to one, and the next resample copies it across the population. Twenty scouts a step found the kidnapped robot again in seven of eight runs, after anywhere from a moment to twenty-four seconds depending on where it had been put; the holdout was still searching at forty. Adaptive versions inject in proportion to how far the average likelihood has fallen below its long-run value, so the scouts arrive only when the population has actually lost the plot, and that is the version to use when the one percent starts to matter.

The code

The filter as the tool runs it. Rand is any source of uniforms in $[0, 1)$; std::mt19937 behind a lambda will do. Every function that draws takes it explicitly rather than reaching for a global, so two runs from the same seed are the same run, which is the only way to tell a change in the filter from a change in the dice.

struct Pose { double x, y, theta; };
struct Segment { double ax, ay, bx, by; };
struct Block { double x0, y0, x1, y1; };
struct Sensor { std::vector<double> angles; double maxRange; };
struct Command { double v, w; };
struct Noise { double v, w; };

const double PI = 3.14159265358979323846;
const double TAU = 2 * PI;

using Rand = std::function<double()>;

// Wrap to [-pi, pi). Arithmetic rather than atan2, so it is exact.
double wrapAngle(double a) { return a - TAU * std::floor((a + PI) / TAU); }

// Box-Muller, one value per call, second discarded. Two draws.
double gaussian(Rand& rand) {
    const double u = std::max(rand(), 1e-12);
    return std::sqrt(-2 * std::log(u)) * std::cos(TAU * rand());
}

// A polygon boundary and rectangular blocks inside it. The wall
// segments a ray can hit are derived once, so the filter never has to
// know which kind of edge it is looking at.
struct Map {
    std::vector<std::array<double, 2>> outer;
    std::vector<Block> blocks;
    std::vector<Segment> segments;
    double minX, maxX, minY, maxY;
};

Map makeMap(std::vector<std::array<double, 2>> outer, std::vector<Block> blocks) {
    Map m;
    m.outer = outer;
    m.blocks = blocks;
    for (size_t i = 0; i < outer.size(); i++) {
        const auto& a = outer[i];
        const auto& b = outer[(i + 1) % outer.size()];
        m.segments.push_back({a[0], a[1], b[0], b[1]});
    }
    for (const Block& b : blocks) {
        m.segments.push_back({b.x0, b.y0, b.x1, b.y0});
        m.segments.push_back({b.x1, b.y0, b.x1, b.y1});
        m.segments.push_back({b.x1, b.y1, b.x0, b.y1});
        m.segments.push_back({b.x0, b.y1, b.x0, b.y0});
    }
    m.minX = m.maxX = outer[0][0];
    m.minY = m.maxY = outer[0][1];
    for (const auto& p : outer) {
        m.minX = std::min(m.minX, p[0]); m.maxX = std::max(m.maxX, p[0]);
        m.minY = std::min(m.minY, p[1]); m.maxY = std::max(m.maxY, p[1]);
    }
    return m;
}

// Even-odd rule. Points on an edge are not worth deciding either way.
bool pointInPolygon(const std::vector<std::array<double, 2>>& poly, double x, double y) {
    bool inside = false;
    for (size_t i = 0, j = poly.size() - 1; i < poly.size(); j = i++) {
        const double xi = poly[i][0], yi = poly[i][1];
        const double xj = poly[j][0], yj = poly[j][1];
        if ((yi > y) != (yj > y) && x < (xj - xi) * (y - yi) / (yj - yi) + xi) inside = !inside;
    }
    return inside;
}

// Inside the boundary and strictly outside every block.
bool isFree(const Map& map, double x, double y) {
    if (!pointInPolygon(map.outer, x, y)) return false;
    for (const Block& b : map.blocks) {
        if (x > b.x0 && x < b.x1 && y > b.y0 && y < b.y1) return false;
    }
    return true;
}

// Uniform over free space by rejection: x, then y, and only once the
// point is free a third draw for the heading.
Pose sampleFree(const Map& map, Rand& rand) {
    double x, y;
    do {
        x = map.minX + rand() * (map.maxX - map.minX);
        y = map.minY + rand() * (map.maxY - map.minY);
    } while (!isFree(map, x, y));
    const double theta = rand() * TAU - PI;
    return {x, y, theta};
}

// Distance along a ray to the first wall, or maxRange. A sensor clamps
// the same way, so a beam that reaches nothing agrees with a prediction
// that reaches nothing.
double rayCast(const Map& map, double px, double py, double angle, double maxRange) {
    const double dx = std::cos(angle);
    const double dy = std::sin(angle);
    double best = maxRange;
    for (const Segment& s : map.segments) {
        const double ex = s.bx - s.ax;
        const double ey = s.by - s.ay;
        const double denom = dx * ey - dy * ex;
        if (std::abs(denom) < 1e-12) continue;
        const double qx = s.ax - px;
        const double qy = s.ay - py;
        const double t = (qx * ey - qy * ex) / denom;
        const double u = (qx * dy - qy * dx) / denom;
        if (t >= 0 && u >= 0 && u <= 1 && t < best) best = t;
    }
    return best;
}

std::vector<double> expectedRanges(const Map& map, const Pose& pose, const Sensor& sensor) {
    std::vector<double> out(sensor.angles.size());
    for (size_t k = 0; k < sensor.angles.size(); k++) {
        out[k] = rayCast(map, pose.x, pose.y, pose.theta + sensor.angles[k], sensor.maxRange);
    }
    return out;
}

std::vector<Pose> initUniform(const Map& map, int n, Rand& rand) {
    std::vector<Pose> particles(n);
    for (int i = 0; i < n; i++) particles[i] = sampleFree(map, rand);
    return particles;
}

std::vector<double> uniformWeights(int n) { return std::vector<double>(n, 1.0 / n); }

Pose move(const Pose& p, double v, double w, double dt) {
    return {p.x + v * std::cos(p.theta) * dt, p.y + v * std::sin(p.theta) * dt,
            wrapAngle(p.theta + w * dt)};
}

// Every particle through the motion model, each with its own draw of
// noise on the commanded speed and turn rate. The noise is what spreads
// copies apart again after a resample.
std::vector<Pose> predict(const std::vector<Pose>& particles, Command u, double dt,
                          Noise noise, Rand& rand) {
    std::vector<Pose> out(particles.size());
    for (size_t i = 0; i < particles.size(); i++) {
        const double v = u.v + noise.v * gaussian(rand);
        const double w = u.w + noise.w * gaussian(rand);
        out[i] = move(particles[i], v, w, dt);
    }
    return out;
}

double logLikelihood(const std::vector<double>& z, const std::vector<double>& zHat, double sigma) {
    double sum = 0;
    for (size_t k = 0; k < z.size(); k++) {
        const double d = (z[k] - zHat[k]) / sigma;
        sum += d * d;
    }
    return -0.5 * sum;
}

struct Weighed { std::vector<double> weights; double neff; };

// Score every particle against the scan, folding the previous weights
// in, and normalise. Log space, shifted by the best particle, or eight
// beams from a pose a few metres off underflow to a population of
// zeros. A particle inside a wall gets zero without a ray cast.
Weighed weigh(const std::vector<Pose>& particles, const std::vector<double>& prior,
              const std::vector<double>& z, const Map& map, const Sensor& sensor, double sigma) {
    const int n = static_cast<int>(particles.size());
    std::vector<double> logw(n);
    double maxLog = -std::numeric_limits<double>::infinity();
    for (int i = 0; i < n; i++) {
        const Pose& p = particles[i];
        if (!isFree(map, p.x, p.y) || prior[i] <= 0) {
            logw[i] = -std::numeric_limits<double>::infinity();
            continue;
        }
        logw[i] = std::log(prior[i]) + logLikelihood(z, expectedRanges(map, p, sensor), sigma);
        if (logw[i] > maxLog) maxLog = logw[i];
    }
    // Nothing in free space: the scan says nothing about this population.
    if (maxLog == -std::numeric_limits<double>::infinity()) return {uniformWeights(n), double(n)};

    std::vector<double> weights(n);
    double sum = 0;
    for (int i = 0; i < n; i++) {
        weights[i] = std::exp(logw[i] - maxLog);
        sum += weights[i];
    }
    double sumSq = 0;
    for (int i = 0; i < n; i++) {
        weights[i] /= sum;
        sumSq += weights[i] * weights[i];
    }
    return {weights, 1 / sumSq};
}

// Low-variance resampling: one draw, n evenly spaced pointers along the
// cumulative weight. Each particle is copied floor(n w) or ceil(n w)
// times and nothing else, so equal weights come back untouched.
std::vector<Pose> resampleSystematic(const std::vector<Pose>& particles,
                                     const std::vector<double>& weights, double u0) {
    const int n = static_cast<int>(particles.size());
    std::vector<Pose> out(n);
    int i = 0;
    double c = weights[0];
    for (int m = 0; m < n; m++) {
        const double u = (u0 + m) / n;
        while (u > c && i < n - 1) {
            i++;
            c += weights[i];
        }
        out[m] = particles[i];
    }
    return out;
}

// Multinomial: n independent draws. Right on average, and it drops a
// fraction 1/e of an equally weighted population for nothing.
std::vector<Pose> resampleMultinomial(const std::vector<Pose>& particles,
                                      const std::vector<double>& weights, Rand& rand) {
    const int n = static_cast<int>(particles.size());
    std::vector<double> cumulative(n);
    double c = 0;
    for (int i = 0; i < n; i++) {
        c += weights[i];
        cumulative[i] = c;
    }
    std::vector<Pose> out(n);
    for (int m = 0; m < n; m++) {
        const double u = rand() * c;
        int lo = 0, hi = n - 1;
        while (lo < hi) {
            const int mid = (lo + hi) / 2;
            if (cumulative[mid] < u) lo = mid + 1;
            else hi = mid;
        }
        out[m] = particles[lo];
    }
    return out;
}

// Replace count particles, spaced evenly through the population, with
// fresh uniform draws at the mean weight. Every step, not only on
// resample: a population that is wrong the same way everywhere has
// near-uniform weights and never resamples.
void inject(std::vector<Pose>& particles, std::vector<double>& weights, int count,
            const Map& map, Rand& rand) {
    const int n = static_cast<int>(particles.size());
    for (int i = 0; i < count; i++) {
        const int k = static_cast<int>(std::floor((i + 0.5) * n / count));
        particles[k] = sampleFree(map, rand);
        weights[k] = 1.0 / n;
    }
}

struct Estimate { double x, y, theta, spread; };

// Weighted mean pose, heading averaged on the circle, and the weighted
// RMS distance of the population from that mean.
Estimate estimate(const std::vector<Pose>& particles, const std::vector<double>& weights) {
    double x = 0, y = 0, c = 0, s = 0;
    for (size_t i = 0; i < particles.size(); i++) {
        x += weights[i] * particles[i].x;
        y += weights[i] * particles[i].y;
        c += weights[i] * std::cos(particles[i].theta);
        s += weights[i] * std::sin(particles[i].theta);
    }
    double spread = 0;
    for (size_t i = 0; i < particles.size(); i++) {
        const double dx = particles[i].x - x;
        const double dy = particles[i].y - y;
        spread += weights[i] * (dx * dx + dy * dy);
    }
    return {x, y, std::atan2(s, c), std::sqrt(spread)};
}

enum class Resample { Systematic, Multinomial, Never };

struct FilterState {
    std::vector<Pose> particles;
    std::vector<double> weights;
    double neff = 0;
    bool resampled = false;
};

struct Options {
    const Map* map;
    const Sensor* sensor;
    double sigma;
    Noise motionNoise;
    Resample resample = Resample::Systematic;
    double neffThreshold = 0.5;
    double inject = 0;
};

// Predict, weigh, and resample only once the population has degenerated
// to half its size or less.
FilterState step(const FilterState& state, Command u, const std::vector<double>& z, double dt,
                 const Options& opts, Rand& rand) {
    const int n = static_cast<int>(state.particles.size());

    std::vector<Pose> predicted = predict(state.particles, u, dt, opts.motionNoise, rand);
    std::vector<double> prior = state.weights;
    if (opts.inject > 0) {
        inject(predicted, prior, static_cast<int>(std::lround(opts.inject * n)), *opts.map, rand);
    }
    Weighed w = weigh(predicted, prior, z, *opts.map, *opts.sensor, opts.sigma);

    if (opts.resample == Resample::Never || w.neff >= opts.neffThreshold * n) {
        return {predicted, w.weights, w.neff, false};
    }

    std::vector<Pose> particles =
        opts.resample == Resample::Multinomial
            ? resampleMultinomial(predicted, w.weights, rand)
            : resampleSystematic(predicted, w.weights, rand());
    return {particles, uniformWeights(n), w.neff, true};
}

Explore further

• Watch the first two seconds after a reset. Four clusters, one per corner, then two: the side corridors are a metre wider than the top and bottom, and the beam straight across is what tells a corner from its neighbours.
• While there are two clusters, watch their percentages against each other for a few seconds. They drift with no scan to drive them, because no scan can. Reset and watch which one starts ahead this time.
• Pause during the two-cluster phase and find the crosshair. It is inside the block. Read the mean error in the readout and imagine it as the output of a filter that could only report a mean.
• Set beam tolerance to 0.3 and reset several times. Count how often the knot forms on the robot and how often on the far corner, and note that the readout gives no warning either way.
• Set beam tolerance to 3 and let it settle. The cloud never gets tight, a spread of well over a metre against a third at the default, because the scan is not being allowed to pin anything.
• Set resampling to never and watch effective N. It is a handful of particles within a few seconds and 1.0 not long after, and from then on the estimate is one particle’s opinion carried by two thousand.
• Set motion noise to zero after the cloud has settled. The knot stops refining, and over the next lap slides half a metre or more off the robot as the odometry drifts and nothing corrects it.
• Kidnap the robot with fresh particles off. Watch effective N stay high while the mean error reads metres: nothing inside the filter can see the problem. Turn fresh particles on and watch the scouts land; the readout says when one of them wins.
• Kidnap the robot while the cloud is still two clusters. The population has to re-find both the truth and its ghost, and the recess has to come round again before it can pick one.
• Switch to multinomial resampling and watch a settled cloud through a few resamples. Nothing obvious changes here, which is the honest finding: the weighting thins the population far more than the draw does in this building. The property that separates the two is exact and small, and is the one above about $1/e$.
• Push particles down to 500 with the default tolerance and reset. The first scan now leaves about seventy effective particles rather than 270, too few for both corners to be well represented, and one cluster is often thin or missing; in two of eight runs the filter ended the lap on the wrong corner. The number of particles needed is set by how sparse the population is where the truth might be, not by how much compute is available.