Gaussian belief propagation

October 18, 2026

Polytrees ended on the condition the whole algorithm rested on: one path between any two nodes. Evidence that can reach a node by two routes arrives twice and is believed twice, so the product over children stops being a product over independent things and the sweep stops being exact.

Robots cannot honour that condition. Three robots that can all range to each other are a triangle, and a triangle is a loop. So either message passing is the wrong tool for a group of robots locating each other, or the condition is softer than it looks. It is softer, in a specific and measurable way, and this page is about exactly how far it bends.

Inference / Gaussian belief propagation

A web of robots locating each other

Every robot has a poor idea of where it is and an accurate range to its neighbours. Nobody solves the whole problem; each one passes messages to the robots it can reach. Drag the sweep slider to watch it settle, then prune the web to a tree and compare the uncertainty it reports against the uncertainty it is entitled to.

Range edges—

Position error—

Overconfident by—

Settled after—

—

Sweep 120 of 120

Robots 14

Sensor range 50 m

Messages dropped 0%

Prune the web to a spanning tree Show the uncertainty a direct solve gives

Each robot has a poor absolute fix and accurate ranges to whatever is close enough to measure. Nobody holds the whole problem. Each one updates from the messages its neighbours send and sends messages back, and the estimate you see is what that settles to.

Gaussians, in the form that makes messages add

A Gaussian written the usual way does not combine conveniently. Written in information form it does:

\mathcal{N}^{-1}(x;\, \eta,\, \Lambda), \qquad \Lambda = \Sigma^{-1}, \qquad \eta = \Lambda \mu

The product of two Gaussians is the sum of their information. So the belief at a variable is the sum of its incoming messages, and the message a variable sends back to one factor is that sum minus what the factor last said to it:

b(v_i) = \sum_{f \in n(v_i)} m_{f \to v_i}, \qquad m_{v_i \to f_j} = b(v_i) - m_{f_j \to v_i}

Pearl’s “leave out what the recipient already told you” is a subtraction. That is the whole variable side of the algorithm.

A factor is a measurement, linearised

A factor is a Gaussian constraint on the variables it touches. An anchor is unary and already linear. A range is binary and is not, so it gets a first-order expansion about the current estimate. With $d = p_a - p_b$ and $r = \lVert d \rVert$ ,

h(x) = r, \qquad J = \begin{bmatrix} \dfrac{d^\top}{r} & -\dfrac{d^\top}{r} \end{bmatrix}

and the general linearised factor is

\Lambda_f = J^\top \Lambda_s J, \qquad \eta_f = J^\top \Lambda_s \left(z - h(x_0) + J x_0\right)

The bracket collapses here. $J x_0$ is $d \cdot (p_a - p_b) / r$ , which is $r$ , and $h(x_0)$ is also $r$ , so the two cancel and what is left is the measurement itself. The linearisation point enters only through $J$ , which is worth knowing before porting it.

The factor then sends each variable a message by conditioning on what the other variables said and integrating them out, which in information form is a Schur complement:

\eta_A' = \eta_A - \Lambda_{AB}\Lambda_{BB}^{-1}\eta_B, \qquad \Lambda_A' = \Lambda_{AA} - \Lambda_{AB}\Lambda_{BB}^{-1}\Lambda_{BA}

A range factor’s $\Lambda_f$ is $J^\top \Lambda_s J$ , an outer product, so it has rank one. On the first sweep, before any neighbour has said anything, $\Lambda_{BB}$ is singular and the limit of that expression is zero information, which is the right answer: a distance to a point nobody has located tells you nothing about where you are.

Prune it to a tree and the answer is exact

Tick prune the web to a spanning tree. The same robots, the same priors, the same measurements, with every edge that would close a loop removed.

Message passing now reproduces a direct solve of the whole system to the last bit. Not approximately: the means agree to around $10^{-12}$ , and so do the covariances. Every bar in the right-hand panel sits on 1.

It settles in two sweeps, and the position error comes down from 5.07 m to 4.74 m.

Put the loops back

Untick it. The web has 36 range edges instead of 13, and two things happen at once.

The estimate gets better. 3.08 m against the tree’s 4.74 m, because 23 measurements that a tree had to discard are now being used.

The uncertainty gets worse, and not slightly. The ellipses come out 2.17 times too tight on average and 3.07 times too tight at the worst robot. The means are still exact, agreeing with the direct solve to around $10^{-11}$ . It is only the second moment that breaks.

sensor range	range edges	position error	uncertainty too tight by
30 m	16	4.67 m	1.15x
40 m	25	4.64 m	1.62x
50 m	36	3.08 m	2.17x
65 m	63	2.83 m	3.83x
80 m	78	2.85 m	4.40x

Accuracy buys out. Past about 50 m the error stops improving, because the configuration is already rigid and more measurements of it add little. Overconfidence does not buy out. It keeps climbing with every edge added, because every edge is another route for the same evidence to come back round.

That asymmetry is the practical content of the page. A denser web gives a better answer and a worse account of how good that answer is.

What is actually guaranteed

The claim worth carrying away is narrower than “loopy belief propagation works” and narrower than “loopy belief propagation is overconfident”.

On convergence, the means are exact. That is a theorem for Gaussian models and it is why the method is usable at all: the thing you steer on is right.

The covariances are not exact, and the direction is not guaranteed either. Three robots in a single triangle come out within three percent, some axes high and some low. It takes an overlapping web before the error becomes large and one-sided, and in this arena every axis is overconfident once there are 36 edges over 14 robots.

What to do about it

Not more sweeps. The covariances are not converging slowly to the right answer, they are converging to a different one: the marginals of the computation tree, the infinite unrolling of the graph, rather than of the graph. Damping and scheduling decide whether the loop settles, never what it settles on.

Solve the system directly. The dashed ellipses in the tool come from exactly that, one factorisation of the whole $2N \times 2N$ information matrix. Exact, and it needs the entire graph in one place, which is the one thing message passing was for.

Reweight the messages. Tree-reweighted belief propagation decomposes the graph into spanning trees and weights each edge by how often it appears across them. It stays distributed and is built to err the other way, giving variances that are too large rather than too small, which is the useful direction to be wrong in. It costs the edge appearance probabilities, which are not free.

Or stop asking the covariance for something it cannot give. This is Murai et al.’s answer and it is sharper than it first sounds. The belief at a variable is the product of the messages arriving from its factors, so its mean is those messages’ means weighted by their precisions. The mean is exact. So the precisions, relative to each other, must be exact too, whatever has happened to their absolute scale: inflating every incoming precision by the same factor moves no mean.

Relative precision is what the work usually needs. Judging which of several measurements to believe, which is what their robust kernels do by reading a factor’s Mahalanobis distance, is a comparison between factors meeting at one variable. It never asks what the absolute uncertainty is. Gating a measurement against an absolute covariance is the use that genuinely breaks, and it is the use the loops took away.

Dropping messages costs sweeps, not accuracy

Drag messages dropped to 90%. Nine out of ten messages never arrive, and after 120 sweeps the error is 3.02 m against 3.08 m with perfect delivery.

What it costs is time. At sweep 20 the error is 4.11 m with nine in ten dropped against 2.88 m with none. A message that goes missing leaves the previous one standing, and the next sweep carries the same information again. Nothing waits for anything, so there is no deadlock to recover from, only a slower approach.

That property is the point of Murai, Ortiz, Saeedi, Kelly and Davison, A Robot Web for Distributed Many-Device Localization, IEEE Transactions on Robotics 40 (2024), which this page follows. Their system has each robot publish its outgoing messages for its neighbours to read whenever they can, with no synchronisation and no shared state, and they show it holding up to around 50% message loss while matching a centralised solver across a thousand robots. The tool here simulates a schedule rather than distributing anything, so it demonstrates the algorithm and not the system.

Two further things in that paper this page leaves out. Their pose variables live on $SE(2)$ rather than $\mathbb{R}^2$ , with messages carried in the tangent space at a point estimate, which is what practical robot states require and what most of their §IV is about. And their interrobot factors carry robust kernels, which let the same machinery survive 70 to 80 percent of range measurements being corrupted, by letting a factor weaken itself in proportion to how badly it disagrees with everything else.

Where the measurements come from

Every factor on this page arrives with its measurement $z$ and its precision $\Lambda_s$ already settled, which is a convenience the page has not earned. Both come from a front end, and for a robot carrying a laser that front end is scan matching: register this scan against the last one, or against the map, and the rigid transform that lines them up is the relative pose a factor records.

The method is iterative closest point, arrived at twice in the same year by Besl and McKay, A Method for Registration of 3-D Shapes, IEEE TPAMI 14(2), 1992, and by Chen and Medioni, Object Modelling by Registration of Multiple Range Images, Image and Vision Computing 10(3), 1992, the first matching point to point and the second point to plane. Rusinkiewicz and Levoy, Efficient Variants of the ICP Algorithm, 3DIM 2001, take the algorithm apart into the six choices every variant is making. For a planar laser the usual one is Censi, An ICP Variant Using a Point-to-Line Metric, ICRA 2008.

Two alternatives drop the correspondence step rather than refining it. Biber and Strasser, The Normal Distributions Transform, IROS 2003, fit a Gaussian per cell and match against a smooth surface instead of against points. Olson, Real-Time Correlative Scan Matching, ICRA 2009, searches the transform space exhaustively. Pomerleau, Colas and Siegwart, A Review of Point Cloud Registration Algorithms for Mobile Robotics, Foundations and Trends in Robotics 4(1), 2015, surveys the field.

The reference that matters most here is Lu and Milios, Robot Pose Estimation in Unknown Environments by Matching 2D Range Scans, Autonomous Robots 4, 1997. They match scans pairwise and then solve the whole network of relative pose constraints at once, which is the structure this page runs messages over. The factor graph a modern system optimises is their network, and a Robot Web is that network with the solve distributed.

And there is a sting in it for this page. $\Lambda_s$ is exactly what scan matching is worst at. A registration returns a transform readily and an honest covariance for that transform reluctantly, which is its own literature: Censi, An Accurate Closed-Form Estimate of ICP’s Covariance, ICRA 2007; Segal, Haehnel and Thrun, Generalized-ICP, RSS 2009, which folds surface structure into the metric; and Olson’s correlative matching above, which gets a covariance for free by having searched the space. So the overconfidence the loops introduce here is sitting on top of an input precision that was already an estimate, and the two errors do not cancel.

The code

The variable side is addition and subtraction. The factor side is one linearisation and one Schur complement.

// A Gaussian in information form: Lambda = Sigma^-1, eta = Lambda mu.
// Carried this way because the product of two Gaussians is the sum of
// their information, which makes a belief update an addition.
struct Info {
    std::vector<double> eta;
    std::vector<std::vector<double>> lambda;
};

Info emptyInfo(int n) {
    return {std::vector<double>(n, 0.0), std::vector<std::vector<double>>(n, std::vector<double>(n, 0.0))};
}

// A variable's belief is the sum of everything its factors told it.
void addInto(Info& a, const Info& b) {
    for (std::size_t i = 0; i < a.eta.size(); i++) {
        a.eta[i] += b.eta[i];
        for (std::size_t j = 0; j < a.eta.size(); j++) a.lambda[i][j] += b.lambda[i][j];
    }
}

// And the message back out to one factor is that sum without the factor's
// own contribution. Pearl's "leave out what the recipient already said",
// as a subtraction.
Info subtract(const Info& a, const Info& b) {
    Info out = emptyInfo((int)a.eta.size());
    for (std::size_t i = 0; i < a.eta.size(); i++) {
        out.eta[i] = a.eta[i] - b.eta[i];
        for (std::size_t j = 0; j < a.eta.size(); j++) out.lambda[i][j] = a.lambda[i][j] - b.lambda[i][j];
    }
    return out;
}

// Gauss-Jordan against several right-hand sides, so that solving against
// the identity gives an inverse. Returns an empty vector when singular.
std::vector<std::vector<double>> solveMatrix(std::vector<std::vector<double>> A,
                                             std::vector<std::vector<double>> B) {
    const int n = (int)A.size();
    const int m = (int)B[0].size();
    for (int i = 0; i < n; i++) A[i].insert(A[i].end(), B[i].begin(), B[i].end());

    for (int col = 0; col < n; col++) {
        int pivot = col;
        for (int r = col + 1; r < n; r++) {
            if (std::fabs(A[r][col]) > std::fabs(A[pivot][col])) pivot = r;
        }
        if (std::fabs(A[pivot][col]) < 1e-14) return {};
        std::swap(A[col], A[pivot]);

        const double d = A[col][col];
        for (int c = col; c < n + m; c++) A[col][c] /= d;
        for (int r = 0; r < n; r++) {
            if (r == col) continue;
            const double f = A[r][col];
            if (f == 0.0) continue;
            for (int c = col; c < n + m; c++) A[r][c] -= f * A[col][c];
        }
    }

    std::vector<std::vector<double>> out(n, std::vector<double>(m));
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < m; j++) out[i][j] = A[i][n + j];
    }
    return out;
}

// A range measurement as a Gaussian over the two positions it relates.
// The (z - h(x0) + J x0) of the general form collapses: J x0 and h(x0)
// are both the current distance, so what is left is the measurement.
Info rangeFactor(const double* pa, const double* pb, double z, double precision) {
    const double dx = pa[0] - pb[0];
    const double dy = pa[1] - pb[1];
    // Two robots on the same spot have no direction between them. Hold
    // the denominator at a floor rather than filling the graph with NaN.
    const double r = std::max(std::hypot(dx, dy), 1e-6);
    const double J[4] = {dx / r, dy / r, -dx / r, -dy / r};

    Info out = emptyInfo(4);
    for (int i = 0; i < 4; i++) {
        out.eta[i] = precision * z * J[i];
        for (int j = 0; j < 4; j++) out.lambda[i][j] = precision * J[i] * J[j];
    }
    return out;
}

// Integrate out every block but `keep`, by the Schur complement.
//
// Lambda_BB is singular whenever what is being integrated out carries no
// information, which is every range factor on the first sweep. The limit
// is zero information out, and a ridge gets there; returning Lambda_AA
// instead has the factor assert a constraint it has not earned.
Info marginalise(const Info& joint, const std::vector<int>& blocks, int keep) {
    if (blocks.size() == 1) return joint;

    std::vector<int> keepIdx, dropIdx;
    int at = 0;
    for (std::size_t b = 0; b < blocks.size(); b++) {
        for (int i = 0; i < blocks[b]; i++) ((int)b == keep ? keepIdx : dropIdx).push_back(at + i);
        at += blocks[b];
    }

    const std::size_t ka = keepIdx.size(), kb = dropIdx.size();
    std::vector<std::vector<double>> Laa(ka, std::vector<double>(ka));
    std::vector<std::vector<double>> Lab(ka, std::vector<double>(kb));
    std::vector<std::vector<double>> Lbb(kb, std::vector<double>(kb));
    for (std::size_t i = 0; i < ka; i++) {
        for (std::size_t j = 0; j < ka; j++) Laa[i][j] = joint.lambda[keepIdx[i]][keepIdx[j]];
        for (std::size_t j = 0; j < kb; j++) Lab[i][j] = joint.lambda[keepIdx[i]][dropIdx[j]];
    }
    for (std::size_t i = 0; i < kb; i++) {
        for (std::size_t j = 0; j < kb; j++) Lbb[i][j] = joint.lambda[dropIdx[i]][dropIdx[j]];
    }

    std::vector<std::vector<double>> I(kb, std::vector<double>(kb, 0.0));
    for (std::size_t i = 0; i < kb; i++) I[i][i] = 1.0;
    std::vector<std::vector<double>> LbbInv = solveMatrix(Lbb, I);
    if (LbbInv.empty()) {
        for (std::size_t i = 0; i < kb; i++) Lbb[i][i] += 1e-6;
        LbbInv = solveMatrix(Lbb, I);
    }

    // Lab * Lbb^-1, used for both corrections.
    std::vector<std::vector<double>> M(ka, std::vector<double>(kb, 0.0));
    for (std::size_t i = 0; i < ka; i++) {
        for (std::size_t c = 0; c < kb; c++) {
            for (std::size_t k = 0; k < kb; k++) M[i][c] += Lab[i][k] * LbbInv[k][c];
        }
    }

    Info out = emptyInfo((int)ka);
    for (std::size_t i = 0; i < ka; i++) {
        out.eta[i] = joint.eta[keepIdx[i]];
        for (std::size_t k = 0; k < kb; k++) out.eta[i] -= M[i][k] * joint.eta[dropIdx[k]];
        for (std::size_t j = 0; j < ka; j++) {
            out.lambda[i][j] = Laa[i][j];
            for (std::size_t k = 0; k < kb; k++) out.lambda[i][j] -= M[i][k] * Lab[j][k];
        }
    }
    return out;
}

Explore further

Drag the sweep slider from 0 with the web intact. Sweep 0 is every robot knowing only its own fix. The error falls fastest in the first ten sweeps and then crawls, which is the difference between information reaching a robot and the loops settling what to make of it.
Tick the tree and watch every bar in the right-hand panel snap to 1. Untick it and watch them spread out to the right.
Push the sensor range to 80 m. The error barely improves past 50 m, and the bars keep marching right, reaching 4.4 times too tight.
Pull the sensor range down to 30 m. 16 edges over 14 robots is barely more than a tree, and the overconfidence falls to 1.15x with it.
Set messages dropped to 90% and scrub to sweep 20, then to 120. The first is much worse than a clean run and the second is the same. Losing messages costs sweeps.
Click new web a few times with the tree on. The bars sit on 1 every time, whatever the layout, because that is a theorem rather than a tendency.
Watch a robot near the edge of the arena against one in the middle. The middle one has more neighbours, a smaller ellipse, and a worse bar.