The extended Kalman filter

October 18, 2026

The Kalman filter is exact for a linear model: the state moves by a matrix, the sensor reads it through a matrix, and a Gaussian belief stays Gaussian through both. A robot’s pose is not like that. It drives along its heading, so where it ends up depends on the cosine and sine of an angle the filter is unsure of, and a Gaussian pushed through a cosine is no longer a Gaussian.

The extended Kalman filter keeps the Kalman machinery and linearises at every step. The mean goes through the real model. The covariance goes through the model’s Jacobian, evaluated at the current estimate, as if the model were linear for as far as the uncertainty reaches. When the uncertainty is small that is very nearly true, and the filter is as good as the linear one.

The measurement here is a lidar scan, and the filter does not read it beam by beam the way the particle filter does. It aligns the whole scan to the map first, and treats the pose that alignment lands on as a reading of the pose. Scan matching is then a sensor in its own right, and the interesting thing about it is how its noise changes with what the scan can see.

Predict through a linearisation

The state is the pose $x = (p_x, p_y, \theta)$, belief about it a mean and a $3 \times 3$ covariance $P$. Between scans the robot is commanded a speed $v$ and turn rate $\omega$, and the motion model carries the mean forward exactly:

The covariance cannot be carried through $f$ itself, so it is carried through the best linear stand-in for $f$ near the estimate, its Jacobians with respect to the state and to the command:

$M$ is how far the wheels can be trusted to do what they were told, the odometry noise slider, and $L$ turns it from command units into pose units. The third column of $F$ is the whole of the nonlinearity. A doubt in heading becomes a doubt in position across the direction of travel, growing with every metre driven, and this is where the covariance picks up the correlation between heading and sideways position that a later scan will use.

What the linearisation costs

The cost is the gap between the linear stand-in and the real model across the width of the uncertainty. Drive two metres straight with an uncertain heading, push twenty thousand sampled poses through $f$, and compare their spread with the one $F P F^{\mathsf T}$ predicts:

A robot driving with a spread of headings ends up on an arc, not a line, and the arc bends back toward the start. The linearised filter has no way to represent the bend. It puts the mean too far forward and believes it knows the distance travelled far better than it does. At a fraction of a degree, which is what a scan matcher holds the heading to, the two agree to the millimetre, and that is why this filter works. It is also why it should not be started on a guess.

A scan matcher as a sensor

The lidar here has ninety beams, one every four degrees, two centimetres of range noise and three metres of reach. A beam that reaches nothing reports no return rather than a reading at its limit. What comes back is a set of points $p_k$ in the robot’s own frame.

Iterative closest point aligns them to the map. Place the scan at a guess, the filter’s predicted pose. Pair each point with the nearest wall. Then solve for the small rigid motion that brings the points onto those walls, measuring only the distance across each wall, along its normal $n_k$, to its nearest point $c_k$:

One Gauss-Newton step on $\delta = (\Delta p_x, \Delta p_y, \Delta\theta)$, then pair again from the new pose and repeat until the step is negligible. From a prediction a centimetre or two out, that takes two or three iterations.

This is point-to-line rather than point-to-point. A lidar samples a wall at points that coincide with nothing on the map, so a residual measured to the nearest map point would penalise a scan for being correctly placed a few centimetres along a wall. Measured across the wall, a scan slides along it for free, which is the truth: a scan of one wall says how far away the wall is and nothing about where along it the robot stands. Points more than half a metre from every wall are dropped as something the map does not have.

The covariance of a match

A least-squares fit comes with its own covariance. With every residual carrying noise of standard deviation $\sigma$,

$\Lambda$ is the information the match carries: large along directions many points constrain, small along directions few do. The update is the ordinary Kalman update with the whole pose measured, $H = I$, and $R$ as the measurement noise. The innovation is the matched pose minus the predicted one, with the heading difference wrapped to the short way round, or a robot facing due west would see an innovation of a full turn.

The textbook gain $K = P (P + R)^{-1}$ needs $R$, and $R$ does not always exist. Rewritten around $\Lambda$ instead, it never needs inverting:

Wherever $R$ exists this is the same filter, to rounding. $I + P\Lambda$ is never singular, because $P$ is positive definite and $\Lambda$ is at worst semi-definite. A direction $\Lambda$ knows nothing about gets no gain at all, and the prediction carries through it untouched.

The bare corridor

The top corridor has a recess in its outer wall every four metres. Their side walls face along the corridor, there is always one within reach, and the match pins all three directions everywhere up there: the ellipse stays small, under two centimetres at 2σ.

The bottom corridor has no recesses. Its only walls that face along it are the two at its far ends, sixteen metres apart, and with three metres of reach there are ten metres in the middle where neither is in range. Every return there is on one of two parallel walls, every normal $n_k$ points across the corridor, and $\sum_k J_k^{\mathsf T} J_k$ has a null direction pointing along it. The match knows to a millimetre or two how far the robot is from each wall, and its heading from the walls’ direction. It says nothing about distance travelled.

The filter handles this without a special case. $\Lambda$ has nothing along the corridor, so the gain there is zero, and the covariance in that one direction grows on odometry alone while the other two stay pinned. The ellipse becomes a needle, about a quarter of a metre long at 2σ by the time a far wall comes into reach, against a millimetre or two across. The chart on the block plots the error against that bound: the error wanders upward through the corridor and the bound grows ahead of it. Then the end wall appears, the null direction closes, and the bound falls to a few centimetres in a single scan, the error with it.

Drag lidar range up to 8.5 m and the far ends are always in reach. The needle never forms, the bound never leaves a couple of centimetres, and the corridor is no different from any other stretch. Down at 2 m the blind stretch is longer still. Over six laps at the defaults the filter’s normalised error, $e^{\mathsf T} P^{-1} e$ against the truth, averaged between 1.4 and 2.4. An exactly honest filter averages 3, the number of dimensions; under it is the cautious side.

A fixed covariance is confidently wrong

The shortcut is to treat a scan matcher as a pose sensor of known accuracy: measure how well it does somewhere it does well, and use that number as $R$ every time. Set measurement covariance to fixed, one centimetre and a quarter of a degree, which is about what this matcher manages in the top corridor.

Up there nothing changes. In the bottom corridor everything does. The matcher still has nothing to say about distance along the corridor, so it returns the predicted value along it unchanged, plus whatever a few grazing returns off a distant wall nudge it by. The fixed $R$ says that value is good to a centimetre. The filter believes it, shrinks the covariance to match, and holds the ellipse at a centimetre while the robot’s real position drifts away from it on the same odometry that drove the estimate. The error line on the chart climbs through the corridor and the bound stays flat underneath it.

Nothing in the update could have noticed. It was handed a measurement and told how good it was. Over six laps, the normalised error in the middle of the corridor averaged between 20 and 215 with the fixed covariance, against 1 to 3 with the matcher’s own. The information matrix is not a refinement on top of the match. It is the part of the match that says which directions the scan actually saw.

The gate, and what it costs

An EKF accepts every measurement it is given, in proportion to how much it trusts it, and a scan matcher can be wrong: a door left open that the map has as a wall, people, glass, a fit that slid into the wrong corner. The usual defence is a gate. The squared Mahalanobis distance of the innovation,

is chi-squared with three degrees of freedom for a filter whose covariance is right, and anything past its 99% point, 11.3, is refused. The prediction stands for that step. In this building the map is exact and there is nothing for the gate to keep out, so what the tool can show is its cost.

Press knock it 23°. The robot is turned on the spot and the filter is not told. With no gate, the next scan from the old heading falls inside ICP’s basin, the match finds the true pose, and the update takes it whole: back within a couple of scans, heading and all. That is the EKF doing its job exactly, a huge innovation and a covariance small enough that the gain is nearly one.

Tick gate at χ² 99% and knock it again. The same match arrives with a $d^2$ around 1500, against a gate of 11.3, and is refused. The robot drives on at its new heading while the filter predicts it along the old one, and the next scan disagrees by more, and is refused too. The covariance grows on odometry alone, but the error grows faster, and the gate never reopens. Over six runs the gate refused nearly every one of the next 250 scans, and each run ended three to six metres from the robot. The gate that would have thrown out a bad match has thrown out every good one, because a single Gaussian has no way to hold “probably here, possibly somewhere else” for long enough to find out which.

Even with nothing knocked, a gate at 99% refuses about one honest scan in a hundred by design, and each refusal is a step on odometry alone. In one lap of six at the defaults, a refusal at a recess let the error grow past what the gate would accept, and the next twenty-six were refused before the covariance caught up. A gate is worth having where a sensor can lie. It needs a way out: reinitialising from a global method such as the particle filter after a run of refusals, which is what real localisation stacks do.

The code

The filter as the tool runs it: the matcher, predict, the update in information form, and one step that strings them together. The map is a list of wall segments, and a scan arrives as points in the robot frame.

struct Pose { double x, y, theta; };
struct Segment { double ax, ay, bx, by; };
struct Command { double v, w; };
struct Noise { double v, w; };

using Vec3 = std::array<double, 3>;
using Mat3 = std::array<Vec3, 3>;
using Point = std::array<double, 2>;
using Map = std::vector<Segment>;

const double PI = 3.14159265358979323846;
const double TAU = 2 * PI;
const double INF = std::numeric_limits<double>::infinity();

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

Mat3 matMul(const Mat3& A, const Mat3& B) {
    Mat3 out{};
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            for (int t = 0; t < 3; t++) out[i][j] += A[i][t] * B[t][j];
    return out;
}

Mat3 transpose(const Mat3& A) {
    Mat3 out{};
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++) out[i][j] = A[j][i];
    return out;
}

// Rounding leaves a covariance a hair off symmetric.
Mat3 symmetrise(const Mat3& A) {
    Mat3 out{};
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++) out[i][j] = 0.5 * (A[i][j] + A[j][i]);
    return out;
}

// Inverse by the adjugate, or nothing when singular to working precision.
std::optional<Mat3> inv3(const Mat3& A) {
    const double a = A[0][0], b = A[0][1], c = A[0][2];
    const double d = A[1][0], e = A[1][1], f = A[1][2];
    const double g = A[2][0], h = A[2][1], i = A[2][2];
    const double C00 = e * i - f * h;
    const double C01 = f * g - d * i;
    const double C02 = d * h - e * g;
    const double det = a * C00 + b * C01 + c * C02;
    double scale = 0;
    for (const Vec3& row : A)
        for (double v : row) scale = std::max(scale, std::abs(v));
    if (scale == 0) scale = 1;
    if (std::abs(det) < 1e-14 * scale * scale * scale) return std::nullopt;
    const double s = 1 / det;
    return Mat3{{{C00 * s, (c * h - b * i) * s, (b * f - c * e) * s},
                 {C01 * s, (a * i - c * g) * s, (c * d - a * f) * s},
                 {C02 * s, (b * g - a * h) * s, (a * e - b * d) * s}}};
}

struct Nearest { double x, y, nx, ny, d; };

// The nearest point to (qx, qy) on any wall, and that wall's normal.
// Nearest to the end of a wall rather than its face, the normal is the
// direction to the end instead: the wall's own normal would measure the
// distance to its line, which runs on past the end.
Nearest closestOnMap(const Map& map, double qx, double qy) {
    Nearest best{0, 0, 0, 0, 0};
    bool end = false;
    double bestD2 = INF;
    for (const Segment& s : map) {
        const double ex = s.bx - s.ax;
        const double ey = s.by - s.ay;
        const double len2 = ex * ex + ey * ey;
        const double t = std::max(0.0, std::min(1.0, ((qx - s.ax) * ex + (qy - s.ay) * ey) / len2));
        const double cx = s.ax + t * ex;
        const double cy = s.ay + t * ey;
        const double d2 = (qx - cx) * (qx - cx) + (qy - cy) * (qy - cy);
        if (d2 < bestD2) {
            bestD2 = d2;
            const double len = std::sqrt(len2);
            best = {cx, cy, -ey / len, ex / len, 0};
            end = t == 0 || t == 1;
        }
    }
    best.d = std::sqrt(bestD2);
    if (end && best.d > 1e-9) {
        best.nx = (qx - best.x) / best.d;
        best.ny = (qy - best.y) / best.d;
    }
    return best;
}

struct IcpOptions { int maxIter = 30; double maxDist = 0.5; double sigma = 0.02; double tol = 1e-5; };
struct Match { Pose pose; Mat3 info; int iterations; int inliers; double rms; bool ok; };

// Point-to-line ICP by Gauss-Newton. A millionth of the trace as damping
// keeps the solve posed where the scan is blind in some direction, and
// leaves the guess alone along it.
Match icp(const Map& map, const std::vector<Point>& points, Pose guess, const IcpOptions& o = {}) {
    double x = guess.x, y = guess.y, theta = guess.theta;
    Mat3 A{};
    int inliers = 0, iterations = 0;
    double rss = 0;

    for (int it = 0; it < o.maxIter; it++) {
        iterations = it + 1;
        const double c = std::cos(theta);
        const double s = std::sin(theta);
        A = Mat3{};
        Vec3 g{};
        inliers = 0;
        rss = 0;
        for (const Point& p : points) {
            const double rx = c * p[0] - s * p[1];
            const double ry = s * p[0] + c * p[1];
            const double qx = x + rx;
            const double qy = y + ry;
            const Nearest m = closestOnMap(map, qx, qy);
            if (m.d > o.maxDist) continue;
            const double r = m.nx * (qx - m.x) + m.ny * (qy - m.y);
            // d(R p)/dtheta is R p turned a quarter turn.
            const Vec3 J{m.nx, m.ny, m.nx * -ry + m.ny * rx};
            for (int i = 0; i < 3; i++) {
                g[i] += J[i] * r;
                for (int j = 0; j < 3; j++) A[i][j] += J[i] * J[j];
            }
            inliers++;
            rss += r * r;
        }
        if (inliers < 3) return {guess, Mat3{}, iterations, inliers, NAN, false};

        const double damping = 1e-6 * (A[0][0] + A[1][1] + A[2][2]);
        Mat3 D = A;
        for (int i = 0; i < 3; i++) D[i][i] += damping;
        const Mat3 Dinv = *inv3(D);
        Vec3 delta;
        for (int i = 0; i < 3; i++) delta[i] = -(Dinv[i][0] * g[0] + Dinv[i][1] * g[1] + Dinv[i][2] * g[2]);
        x += delta[0];
        y += delta[1];
        theta = wrapAngle(theta + delta[2]);
        if (std::hypot(delta[0], delta[1]) < o.tol && std::abs(delta[2]) < o.tol) break;
    }

    const double k = 1 / (o.sigma * o.sigma);
    Mat3 info = A;
    for (Vec3& row : info)
        for (double& v : row) v *= k;
    return {{x, y, theta}, info, iterations, inliers, std::sqrt(rss / inliers), true};
}

struct State { Pose pose; Mat3 P; };

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)};
}

// The mean through the model, the covariance through its Jacobians:
// P' = F P F^T + L M L^T.
State predict(const State& s, Command u, double dt, Noise noise) {
    const double c = std::cos(s.pose.theta);
    const double sn = std::sin(s.pose.theta);
    const Mat3 F{{{1, 0, -u.v * sn * dt}, {0, 1, u.v * c * dt}, {0, 0, 1}}};
    const double L[3][2] = {{c * dt, 0}, {sn * dt, 0}, {0, dt}};
    const double M[2][2] = {{noise.v * noise.v, 0}, {0, noise.w * noise.w}};

    double LM[3][2] = {};
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 2; j++)
            for (int t = 0; t < 2; t++) LM[i][j] += L[i][t] * M[t][j];
    Mat3 Q{};
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            for (int t = 0; t < 2; t++) Q[i][j] += LM[i][t] * L[j][t];

    const Mat3 FPF = matMul(matMul(F, s.P), transpose(F));
    Mat3 P;
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++) P[i][j] = FPF[i][j] + Q[i][j];
    return {move(s.pose, u.v, u.w, dt), symmetrise(P)};
}

// Measured pose minus predicted, heading the short way round.
Vec3 innovation(const Pose& z, const Pose& p) {
    return {z.x - p.x, z.y - p.y, wrapAngle(z.theta - p.theta)};
}

struct Update { State state; double d2; bool accepted; };

// The Kalman update with H = I, written around the information matrix
// so a match that is blind in some direction needs no inverse of it:
// P+ = (I + P info)^-1 P, K = P+ info, d2 = nu^T info (I + P info)^-1 nu.
Update update(const State& s, const Pose& z, const Mat3& info, double gate = INF) {
    const Vec3 nu = innovation(z, s.pose);
    Mat3 IPL = matMul(s.P, info);
    for (int i = 0; i < 3; i++) IPL[i][i] += 1;
    const Mat3 IPLinv = *inv3(IPL);
    const Mat3 Sinv = matMul(info, IPLinv);
    double d2 = 0;
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++) d2 += nu[i] * Sinv[i][j] * nu[j];
    if (d2 > gate) return {s, d2, false};

    const Mat3 P2 = symmetrise(matMul(IPLinv, s.P));
    const Mat3 K = matMul(P2, info);
    Vec3 dx;
    for (int i = 0; i < 3; i++) dx[i] = K[i][0] * nu[0] + K[i][1] * nu[1] + K[i][2] * nu[2];
    return {{{s.pose.x + dx[0], s.pose.y + dx[1], wrapAngle(s.pose.theta + dx[2])}, P2}, d2, true};
}

enum class Covariance { Scan, Fixed };

struct Options {
    const Map* map;
    Noise motionNoise;
    Covariance covariance = Covariance::Scan;
    double fixedXY = 0.01, fixedTheta = 0.0044;
    double gate = INF;
    IcpOptions icp;
};

struct StepResult { State state; State predicted; Match match; double d2; bool accepted; };

// Predict, match the scan from the predicted pose, update with the match.
StepResult step(const State& s, Command u, const std::vector<Point>& points, double dt, const Options& o) {
    const State predicted = predict(s, u, dt, o.motionNoise);
    const Match match = icp(*o.map, points, predicted.pose, o.icp);
    if (!match.ok) return {predicted, predicted, match, NAN, false};

    Mat3 info = match.info;
    if (o.covariance == Covariance::Fixed) {
        info = Mat3{{{1 / (o.fixedXY * o.fixedXY), 0, 0},
                     {0, 1 / (o.fixedXY * o.fixedXY), 0},
                     {0, 0, 1 / (o.fixedTheta * o.fixedTheta)}}};
    }
    const Update up = update(predicted, match.pose, info, o.gate);
    return {up.state, predicted, match, up.d2, up.accepted};
}

Explore further

• Watch the chart through the bottom corridor. The grey bound climbs and the error climbs under it, then both fall to a few centimetres in one scan as the far wall comes into reach. Read the uncertainty readout at that moment.
• Drag lidar range to 8.5 m and reset. The needle never forms. Drag it to 2 m and watch the side corridors go blind too, which they do at 3 m for a moment in their middles.
• Switch measurement covariance to fixed and watch the bottom corridor. The ellipse stays a dot while the error line climbs out of the bound. Then watch the scan dots as the far wall comes into range: they snap from the walls they were sitting on to where the robot really is.
• Knock the robot with no gate, anywhere on the loop. Its heading is back to a fraction of a degree within a couple of scans, because ICP from a pose 23° off still lands in the right basin here.
• Tick the gate and knock it. Watch the refused ticks fill the floor of the chart, the ellipse balloon, and the scan dots part company with the walls. Reset to recover; nothing inside the filter will.
• With the gate on and fixed covariance, drive through the bottom corridor. The error the fixed covariance let build up is refused at the far end as too surprising to be true, and the filter has to wait for odometry to inflate its doubt enough to accept the correction.
• Push odometry noise to zero. The ellipse in the corridor stops growing, because the only thing the filter has to go on there is the wheels, and they are now perfect. Push it to 0.3 m/s and the needle runs half a metre and more.