M9-T7 — Linear Kalman Filter Mathematics

Learning objectives

After this lesson, you should be able to:

• Write the Kalman predict and update steps with all matrices and say what each variable means.
• Derive the gain K = P⁻Hᵀ(HP⁻Hᵀ + R)⁻¹ and interpret it as a ratio of covariances.
• Use the Joseph form and symmetrisation to keep P positive definite, and check consistency with the innovation test.
• Run one full cycle by hand on a position-velocity model and read the result.

Introduction and motivation

A robot predicts its position from wheel speeds, which are noisy, then receives a GPS fix, which is also noisy. The Kalman filter says exactly how much of the correction to apply: if the prediction is shaky and the GPS is sharp, trust GPS; if the reverse, trust the prediction. It also updates the uncertainty, and it extends to dozens of coupled states (position, velocity, attitude error, sensor biases). It is the algorithmic form of the Gaussian fusion of the first lesson and the linear algebra of the whole course.

The intuition

Predict: move the belief forward with the dynamics; since the dynamics are imperfect, spread it a little (add Q). Update: look at the measurement. If it differs from what the prediction expects (the innovation), shift the estimate toward it by a fraction K of the difference. The fraction depends on how large the prediction covariance is compared with the measurement covariance, and it is a matrix, so it can correct correlated states (a position measurement also corrects velocity) through the off-diagonal terms of P.

Geometric and physical interpretation

Geometric view. The belief is an ellipse. Prediction pushes it through F (rotating and shearing it, Module 6) and inflates it with Q. The update multiplies the prior Gaussian by the measurement likelihood Gaussian; the product of two Gaussians is a Gaussian whose ellipse is the intersection region, smaller than either. Physical view. A GPS fix tightens position strongly, but with a position-velocity covariance that couples the two, it also pulls the velocity estimate: the filter infers speed from the pattern of fixes.

The mathematics

x, P

the state estimate and its covariance (the belief N(x, P)).

F, B, u

state-transition matrix, control matrix and control input.

Q

process-noise covariance: how much the model is wrong per step.

z, H, R

the measurement, the measurement matrix (z ≈ Hx) and measurement-noise covariance.

v, S, K

innovation z − Hx⁻, its covariance S = HP⁻Hᵀ + R, and the Kalman gain.

Predict

x⁻ = F x + B u        P⁻ = F P Fᵀ + Q

What it means. The mean follows the model; the covariance is the propagation rule of the previous lessons plus Q (positive semi-definite), so it never shrinks in this step.

Update

v = z − H x⁻,   S = H P⁻ Hᵀ + R,   K = P⁻ Hᵀ S⁻¹
x = x⁻ + K v          P = (I − K H) P⁻

What it means. The innovation v is the surprise; S is its covariance; K scales the surprise into a state correction. P shrinks because information was added.

Joseph form (numerically safe)

P = (I − K H) P⁻ (I − K H)ᵀ + K R Kᵀ

What it means. Algebraically equal to the short form for the optimal K, but a sum of positive semi-definite terms, so rounding cannot make it indefinite or asymmetric. Use it, then symmetrise.

Predict: x⁻ = Fx, P⁻ = FPFᵀ + Q. Update: S = HP⁻Hᵀ + R, K = P⁻HᵀS⁻¹, x = x⁻ + K(z − Hx⁻), P = (I − KH)P⁻ (Joseph form in code).

Step-by-step derivation: the gain from Gaussian conditioning and from minimum variance

1. Prediction. If x ~ N(x̂, P) and x⁺ = Fx + w, w ~ N(0, Q) independent, then x⁺ is Gaussian with mean Fx̂ and covariance FPFᵀ + Q (covariance propagation and the sum of independent errors).
2. Joint Gaussian of state and measurement. With prior N(x⁻, P⁻) and z = Hx + n, n ~ N(0, R): z has mean Hx⁻ and covariance S = HP⁻Hᵀ + R; the cross-covariance of x and z is Cov(x, z) = P⁻Hᵀ.
3. Condition on z. By the Gaussian conditioning formula (lesson 3), E[x | z] = x⁻ + Cov(x, z) S⁻¹ (z − Hx⁻) = x⁻ + P⁻HᵀS⁻¹ v, so K = P⁻HᵀS⁻¹.
4. Posterior covariance. Again by conditioning, Cov(x | z) = P⁻ − P⁻HᵀS⁻¹HP⁻ = P⁻ − KSKᵀ = (I − KH)P⁻ (using KS = P⁻Hᵀ).
5. Same gain from least squares. Choose K to minimise the trace of the posterior covariance of x⁻ + K(z − Hx⁻). That covariance is (I − KH)P⁻(I − KH)ᵀ + KRKᵀ; setting the derivative with respect to K to zero gives −2(I − KH)P⁻Hᵀ + 2KR = 0, i.e. K(HP⁻Hᵀ + R) = P⁻Hᵀ: the same K. This is the Joseph form, valid for any K and minimal at the optimum.
6. Scalar check. With H = 1: K = P/(P + R), x = x⁻ + K(z − x⁻), P⁺ = (1 − K)P⁻ = PR/(P + R), the inverse-variance fusion of lesson 1.
7. Information form. Equivalently P⁻¹ = (P⁻)⁻¹ + HᵀR⁻¹H: information (inverse covariance) adds. The update is whitened least squares (previous lesson) of the prior plus the measurement, solved recursively.
8. Consistency. If the model is right, the innovation is zero-mean Gaussian with covariance S, uncorrelated in time, and vᵀS⁻¹v ~ χ² (NIS, Mahalanobis lesson). Systematic departures mean the model, Q or R is wrong.
9. Observability (rank). The filter can estimate a state component only if the model and measurements make it observable: the observability matrix [H; HF; HF²; …] must have full column rank (Module 3). A velocity is observable from position fixes, a constant bias seen only through an unobservable direction is not.

Steady state. For a constant linear system the covariance converges to a fixed point (the algebraic Riccati equation) and the gain to a constant; a steady-state filter then just multiplies by constant matrices.

Extended and error-state versions. For nonlinear models, F and H are the Jacobians of the model at the current estimate (Module 8): that is the EKF. The error-state filter estimates small deviations (using the Lie-algebra tools of Module 10 for rotations).

A worked example: one cycle of a position-velocity filter

State x = (position, velocity), time step 0.1 s, F = [[1, 0.1], [0, 1]], Q = diag(0.0005, 0.01), H = [1, 0], R = 0.04. Prior x = (0, 1), P = diag(0.04, 0.25).

• Predict mean. x⁻ = F x = (0 + 0.1·1, 1) = (0.1, 1.0).
• Predict covariance. FPFᵀ = [[0.04 + 0.1²·0.25, 0.1·0.25], [0.025, 0.25]] = [[0.0425, 0.025], [0.025, 0.25]]; adding Q: P⁻ = [[0.043, 0.025], [0.025, 0.26]]. Note the position and velocity errors have become correlated (0.025).
• Innovation covariance. S = H P⁻ Hᵀ + R = 0.043 + 0.04 = 0.083.
• Gain. K = P⁻Hᵀ/S = (0.043, 0.025)/0.083 = (0.518, 0.301): the measurement of position corrects the position by 52 % of the innovation and the velocity by 0.30 per metre of innovation, through the correlation.
• Measurement. z = 0.13, innovation v = 0.13 − 0.1 = 0.03. Normalised innovation squared v²/S = 0.0108: a tiny surprise.
• State update. x = (0.1 + 0.518·0.03, 1.0 + 0.301·0.03) = (0.1155, 1.0090).
• Covariance update. P = (I − KH)P⁻ = [[0.0207, 0.0120], [0.0120, 0.2525]]. The position variance fell from 0.043 to 0.0207 (about halved), the velocity variance only from 0.26 to 0.2525 (one position fix says little about velocity); the Joseph form gives the same numbers, and the eigenvalues 0.0201 and 0.2531 are positive.

Robotics application

Localisation and SLAM. The EKF-SLAM filter is this algorithm with F and H replaced by Jacobians and the state augmented by landmarks; the covariance blocks carry the correlations that let one loop closure correct the whole map; see EKF-SLAM, Bayesian estimation and probability and uncertainty.

UAV navigation. IMU prediction at hundreds of hertz and GNSS, barometer and magnetometer updates at lower rates; the error-state Kalman filter propagates small attitude and bias errors; see the extended Kalman filter, the error-state Kalman filter, inertial navigation systems and IMU, barometer, magnetometer and GNSS.

LiDAR and mapping. LiDAR odometry results enter as measurements with covariances from scan matching; the filter fuses them with IMU prediction; see LiDAR point clouds for SLAM and scan registration.

Mobile robots and manipulators. Wheel-odometry plus visual-odometry fusion and joint-state estimation from noisy encoders both use linear Kalman steps; see robot motion models for navigation.

Engineering implications and limitations

Practical rules

• Use the Joseph form and symmetrise. P = (I − KH)P⁻(I − KH)ᵀ + KRKᵀ, then (P + Pᵀ)/2. The short form (I − KH)P⁻ loses positive definiteness over long runs.
• Solve, do not invert. Compute K by solving S Kᵀ = H P⁻ (Cholesky of S), not with an explicit S⁻¹; it also gives the NIS cheaply.
• Tune Q and R from data. R from sensor characterisation; Q so that the innovations are white with the predicted covariance (NIS near the measurement dimension).
• Gate the innovation. Reject measurements with vᵀS⁻¹v above the chi-square limit (previous lessons) before the update.
• Handle asynchronous sensors. Predict to the measurement timestamp, then update; never apply a measurement at the wrong time.
• Check observability. If a state is unobservable its covariance never shrinks: add a measurement or accept a drifting estimate there.
• Guard the numbers. Large and small variances in one matrix (metres and radians, or squared velocity) hurt conditioning: scale states, or use a square-root filter.

Limits

The linear Kalman filter is optimal only for linear models with Gaussian, independent, correctly specified noise. It is not robust to outliers, modelling errors or non-Gaussian noise; nonlinear models need EKF, UKF or smoothing, and multi-modal beliefs need particle filters.

Common misconceptions

Engineering takeaways

1. Predict: x⁻ = Fx + Bu, P⁻ = FPFᵀ + Q (uncertainty grows). Update: S = HP⁻Hᵀ + R, K = P⁻HᵀS⁻¹, x = x⁻ + K(z − Hx⁻), P = (I − KH)P⁻ (uncertainty shrinks).
2. The gain is the cross-covariance of state and measurement times S⁻¹, from Gaussian conditioning or from minimising trace P.
3. Correlations in P let a measurement of one component correct others (position fixes correct velocity).
4. Use the Joseph form, symmetrise, solve with Cholesky and gate the innovation; check NIS and observability.
5. EKF, UKF and the error-state filter are this algorithm with Jacobians and Lie-algebra updates (Modules 8 and 10).

References and further reading

Primary papers and authoritative documentation.

• Barfoot, State Estimation for Robotics (author-hosted first edition, free to read): chapter 3, linear-Gaussian estimation and chapter 4 nonlinear extensions
• UCSD ECE276A: Sensing and Estimation in Robotics: Kalman filter derivation and EKF for SLAM
• MIT 18.06SC Linear Algebra (OCW): lecture on least squares, weighted least squares and the Kalman filter (18.085 or 18.06 supplement)
• Mathematics for Machine Learning: chapter 6.5, conditioning of Gaussians underlying the Kalman filter

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Module 9 closes the loop on uncertainty in vector spaces. Module 10 asks what happens when the state is not a vector at all: rotations and poses live on curved spaces, where adding a small error to an orientation is not even defined. Lie groups and algebras give the replacement.

Related content

Nothing below is required to finish this lesson.

• Before
  M9-T6: Whitening Transformations

  Transform correlated uncertain quantities into independent unit-variance ones.

• Lab
  M9-L1: Robot Localization with Uncertainty

  Localise a robot with range measurements: update a position estimate and its covariance, and find why an overconfident filter fails.