The Kalman filter

August 29, 2026

Every filter on the previous page does one job: it takes a noisy stream of some quantity and hands back a smoother version of the same quantity. None of them can report velocity from a position sensor, because velocity was never in the data. There is nothing to smooth into it.

A Kalman filter can, because it carries something the others do not: a model of how the state moves. Position and velocity are both tracked, both measurements away, and the model connects them. A position reading updates the velocity estimate too, by exactly as much as the model says the two are coupled.

The state and the model

The state is two numbers, position and velocity, and belief about them is a covariance rather than a single spread:

$p_{pp}$ and $p_{vv}$ are how uncertain position and velocity are on their own. $p_{pv}$ is new: how much an error in one predicts an error in the other. It starts at zero and stops being zero the moment a model connects them, which is exactly what happens next.

Between measurements, the state moves by the constant-velocity model, $p \mathrel{+}= v\,\Delta t$, and the covariance moves with it:

$F P F^{\mathsf T}$ is the old uncertainty carried forward through the same model as the state. $Q$ is what the model does not know: the target might not hold a constant velocity, and $Q$ says how much. Modelling the unknown part as a random acceleration of power spectral density $q$ gives

Most of it lands on $p_{vv}$ directly, an unknown acceleration acting for $\Delta t$. A smaller, correlated share leaks into $p_{pp}$ and into $p_{pv}$, because that unknown acceleration also moved the position a little through the $\Delta t$ it had to act over. $q$ is the one knob that says how much the model is trusted: small $q$ says the target moves at a very steady velocity, large $q$ admits it might not.

Folding in a measurement

A position reading $z$ arrives, and the update has to decide how much of the disagreement between $z$ and the predicted $p$ to believe. The general form is

with $H = \begin{bmatrix}1 & 0\end{bmatrix}$ picking out position, the only thing the sensor reports, and $R$ its measurement variance. Specialised to that one-row $H$, every matrix in it collapses to three numbers:

$K_p$ is the gain on position: what fraction of the disagreement $z - p$ to add to the position estimate. $K_v$ is the same disagreement applied to velocity, scaled by $p_{pv}$, the coupling the model has already built up between the two. A position measurement moves the velocity estimate only because, and only by as much as, the model has linked them. Cut $p_{pv}$ to zero and $K_v$ goes with it: without a model to move the coupling stays zero forever, which is the same as running two independent filters and is exactly what these four numbers rule out.

$S$ is the total disagreement expected between a measurement and the prediction, sensor noise and model uncertainty added together. Read the three extremes off it directly. $R \to 0$: $S \to p_{pp}$, so $K_p \to 1$ and the sensor is trusted completely. $R \to \infty$: $K_p \to 0$ and the sensor is ignored, which is correct, since a sensor that is all noise carries no information to add. And $p_{pp}' \le p_{pp}$ always, whatever $R$ is: a measurement, however noisy, never makes the filter less sure than it already was.

Fusing several sensors

Nothing above assumed there is only one sensor. The update takes whatever measurement arrives and folds it in; if another independent sensor reports something about the same state, folding that in too is calling update again. No new algorithm, no new equations: sensor fusion, in its entirety, is running the same update once per measurement, however many there are.

That is easy to agree with and easy to underrate, because the version with two sensors, one of them clearly better, feels like it’s mostly about trusting the better one. The version worth seeing is the one where every sensor is mediocre.

Each thin curve is one sensor’s belief, and none of them is good: every one carries the noise the each sensor’s noise slider sets, and no sensor here is the reliable one propping up the rest. The bold curve is all of them fused, and it comes out narrower than any single input, because variance falls by a factor of $n$ for $n$ equally noisy, independent sensors. Push sensors up and watch it keep shrinking with no floor in sight, past the point where any one sensor’s own reading is close to believable on its own.

The comparison that matters is against the best sensor of the batch, not the average one, because “average the readings” is the easy claim. Click new readings a few times and read the two error numbers against each other. Fused beats the best individual sensor more often than not once there are four or five of them, and it is not cheating to do it: picking out the best sensor after the fact requires already knowing the true value, which is exactly the thing being estimated. Fusion is the strategy that wins without needing to know that answer in advance. The one claim that never needs a caveat is the uncertainty readout: fused uncertainty is below every individual sensor’s own noise floor on every single draw, not just the lucky ones.

Chain $n$ updates with variances $R_1, \dots, R_n$ and the position term generalises to

exactly, whatever $p_{pv}$ is and in whatever order the measurements are folded in. Precision, the reciprocal of variance, adds. That is a stronger claim than “averaging helps”: another measurement can only ever raise the total precision, however much noise it carries on its own, and any of them can arrive in any order and land on the same posterior, because each update is a fusion of the current belief with one more independent Gaussian measurement and fusing several things one at a time reaches the same place however they are ordered.

The fuse a beacon fix toggle on the tool adds a second sensor along exactly these lines: a fix once a second, far less often than the continuous sensor’s every 50 ms, and usually a good deal more accurate, the way a wheel encoder runs constantly while a GPS fix or a lidar localisation arrives occasionally. Watch the uncertainty band and the readout when it lands: both drop, on a purely position-only correction, because the filter has been carrying a velocity estimate the whole time and the same update that corrects position also pulls that coupled velocity estimate along with it. The two sensors’ noises have to be independent for the variances to add this cleanly; correlated sensor errors, two GPS receivers on one antenna for instance, need the covariance between them carried explicitly rather than assumed away.

The gain settles on its own

Run predict and update back to back with no measurement attached and the gain does not depend on any $z$, only on $P$. Iterate the covariance alone and it converges to a fixed point regardless of where it started: an unsure prior takes a few more steps to get there, but the destination is the same. That fixed point is the steady-state gain, the number the filter spends almost all its life sitting at.

It is set entirely by the ratio of $q$ to $R$, not by either one alone. Raise $q$ and every prediction grows $p_{pp}$ further before the next measurement arrives, which raises $K_p$: a model admitting more uncertainty leans harder on the sensor. Raise $R$ and the opposite happens. The Gain Kp readout above is this fixed point, recomputed for whatever the sliders currently say, and it is the number that ties directly to the trade the sliders are making.

What q actually trades

Turn the maneuver on and watch what q does to it. A small q tells the filter constant velocity is nearly certain, so the turn at six seconds reads as an outlier the model doubts, and the velocity estimate lags behind it before slowly admitting the model was wrong. A large q keeps the filter ready to believe every sample might be the start of a new maneuver, which catches the turn fast and costs a jittery estimate the rest of the time, including the six seconds before the turn where nothing was happening at all. There is no q that is both instantly responsive and perfectly steady; picking one is picking which kind of wrong is cheaper for what the estimate feeds. A PID loop closed on a jittery velocity term chases the jitter, so the honest fix when a loop rings on a Kalman-filtered rate is usually a smaller q, not a bigger derivative gain.

The code

The state stays two numbers and the covariance stays three, which is what makes this port straightforward: no matrix library, no dimensions to get wrong.

struct Cov { double pp, pv, vv; };
struct State { double p, v; Cov P; };

// F P F^T + Q, specialised to the constant-velocity F and the
// white-noise-acceleration Q. Most of q lands on vv directly; the pp and
// pv terms are the smaller, correlated share that leaks in through the
// dt of travel at the (unknown) new speed.
Cov predictCov(const Cov& P, double dt, double q) {
    return {
        P.pp + 2 * dt * P.pv + dt * dt * P.vv + q * dt * dt * dt / 3.0,
        P.pv + dt * P.vv + q * dt * dt / 2.0,
        P.vv + q * dt,
    };
}

struct Gain { double p, v; };
struct UpdateResult { Cov P; Gain K; double s; };

// (I - K H) P with H = [1, 0], collapsed to three numbers. s is how much
// a measurement was expected to disagree with the prediction, sensor
// noise and model uncertainty added together.
UpdateResult updateCov(const Cov& P, double r) {
    const double s = P.pp + r;
    const Gain K{P.pp / s, P.pv / s};
    return {
        {P.pp * r / s, P.pv * r / s, P.vv - P.pv * P.pv / s},
        K,
        s,
    };
}

State predict(const State& state, double dt, double q) {
    return {state.p + state.v * dt, state.v, predictCov(state.P, dt, q)};
}

State update(const State& state, double z, double r) {
    const UpdateResult u = updateCov(state.P, r);
    const double innovation = z - state.p;
    return {
        state.p + u.K.p * innovation,
        state.v + u.K.v * innovation,
        u.P,
    };
}

State step(const State& state, double z, double dt, double q, double r) {
    return update(predict(state, dt, q), z, r);
}

// The gain depends only on P, never on z, so the recursion can be run on
// the covariance alone to find where it settles. Starting far from
// certain (pp, vv both large) is the ordinary and safe way to begin,
// since the fixed point does not depend on it.
Gain steadyGain(double dt, double q, double r, int iterations = 2000) {
    Cov P{1e8, 0.0, 1e8};
    Gain K{0.0, 0.0};
    for (int i = 0; i < iterations; i++) {
        P = predictCov(P, dt, q);
        UpdateResult u = updateCov(P, r);
        P = u.P;
        K = u.K;
    }
    return K;
}

Explore further

• Turn the maneuver off and read the velocity error against a plain frame-to-frame difference of the raw measurements: the readout, and the gap between the two traces below, is the whole argument for carrying a model instead of differencing.
• Drop q to its minimum with the maneuver on. The velocity estimate takes several seconds to admit the turn happened, all of it spent with the gain still computed for a target the model insists is not accelerating.
• Push q to its maximum and watch the velocity trace jitter before the maneuver even arrives, when the true velocity has not moved at all.
• Widen the sensor noise slider without touching q, and watch the gain readout fall: the same model, trusting a worse sensor less.
• Watch the shaded band. It narrows every update and widens every predict, and between maneuvers it settles to the same width step after step, which is the steady state the gain has also reached.
• Turn on the beacon fix and watch the band step down once a second rather than continuously: each fix is one more update call, not a different kind of correction.
• Drag the beacon noise slider from far worse than the continuous sensor to far better. The uncertainty readout keeps falling either way, just by less when the fix is a poor one, which is the precision-adds law directly.
• Set the beacon noise very low and the continuous sensor noise very high. The position trace starts tracking the once-a-second fix almost exactly and drifting on the model between fixes, which is what a GPS-corrected dead-reckoning estimate actually looks like.
• In the belief-fusion tool, set sensors to 2 and click new readings repeatedly. The fused estimate loses to the best individual sensor a good fraction of the time. Push sensors up to 8 and keep clicking: losing becomes the rare outcome, not the usual one.
• Push each sensor’s noise up until a single reading would be useless on its own, wide enough that its curve barely rises above the axis. Fused uncertainty still comes down to a curve worth trusting, just more slowly, because $\sigma/\sqrt{n}$ falls whatever $\sigma$ started at.
• Watch the ringed dot, the individual sensor closest to the true value. It moves to a different sensor almost every time you click new readings. Nothing about a single mediocre sensor says in advance whether it will be this run’s lucky one.
• Drag the true value back and forth. All the individual curves and the fused curve slide together, and the fused curve’s width never changes, because the noise is set by the sliders, not by where the true value happens to sit.