LUNAROPS · OPERATIONAL UPLINK 100% UPTIME 1,247d POSTS 893 JEFF.MOON@LUNAROPS.DEV UTC --:--:--

Kalman Filters Explained

23 min read Robotics & Control Systems
kalman-filterstate-estimationsensor-fusionroboticscontrol-systemsnavigation
Contents

Every drone holding position in a gust, every phone that knows where you are when the GPS drops out under a bridge, every self-driving car tracking the vehicle ahead, and every rocket that knew its own attitude in 1969 is running some descendant of the same algorithm: the Kalman filter. The problem it solves is universal and unglamorous. You have a model that predicts where something will be in the next instant, and that model is wrong in small random ways. You have a sensor that measures where it actually is, and that sensor is also wrong in small random ways. Neither source alone is trustworthy enough. The Kalman filter is the provably optimal way to combine the two, weighting each by how much you trust it, and crucially it does this recursively — it keeps a single running estimate and folds in each new measurement as it arrives, never needing to store history. That recursive, constant-memory structure is why it ran on the Apollo Guidance Computer with a few kilowords of memory and why it runs today in the inner loop of an IMU sampling at a kilohertz. This post walks the core idea, the predict-and-update equations as they are actually implemented, the intuition for the Kalman gain as a trust dial between model and sensor, the linear-Gaussian assumptions that make it optimal, the EKF and UKF extensions that drag it into the nonlinear world where every real robot lives, and the honest reality that tuning the noise matrices Q and R is the part nobody warns you about.

The Core Idea: Optimal Fusion of Two Bad Estimates

Strip away the matrices and the Kalman filter is one idea: if you have two independent noisy estimates of the same quantity, the best combined estimate is a weighted average where each estimate is weighted by the inverse of its variance. Trust the more certain one more.

Concretely, suppose you think a robot is at position 10.0 meters with a standard deviation of 2.0 meters (variance 4.0), and a sensor reads 11.0 meters with a standard deviation of 1.0 meter (variance 1.0). The sensor is more precise, so the fused estimate should land closer to 11.0 than to 10.0. The math of fusing two Gaussians gives the fused mean as a variance-weighted blend, and the fused variance is smaller than either input — combining information always sharpens your estimate:

   fused_mean = (sigma_b^2 * mean_a + sigma_a^2 * mean_b) / (sigma_a^2 + sigma_b^2)
              = (1.0 * 10.0 + 4.0 * 11.0) / (4.0 + 1.0) = 10.8

   fused_var  = (sigma_a^2 * sigma_b^2) / (sigma_a^2 + sigma_b^2)
              = (4.0 * 1.0) / 5.0 = 0.8

The fused estimate is 10.8 with variance 0.8 — pulled toward the more trusted sensor, and more certain than either input. That single calculation, generalized to vectors and matrices and run recursively in time, is the Kalman filter. Everything else is bookkeeping: propagating the estimate forward in time between measurements, and tracking how uncertainty grows during prediction and shrinks during update.

The filter maintains two things at every step. The state vector x, your best current estimate of the quantities you care about — say position and velocity, x = [p, v]. And the covariance matrix P, which encodes how uncertain you are about that estimate and how the uncertainties in different state components correlate. A small P means a confident estimate; a large P means a vague one. The whole dance is about how x and P evolve as time passes and measurements arrive.

The Two Models You Have to Provide

The filter needs you to describe the world in two ways. First, how does the state evolve on its own, in the absence of measurements? This is the process model, a linear map F (the state transition matrix) that takes the state forward one timestep, plus an optional control input u mapped through a matrix B. For a constant-velocity point with timestep dt:

   x = [position, velocity]

   F = [ 1   dt ]      x_next = F x   means    position_next = position + velocity*dt
       [ 0    1 ]                              velocity_next = velocity

The process model is never exactly right — the robot accelerates, the wind gusts, the model omits dynamics. That modeling error is captured by the process noise covariance Q, which says how much uncertainty the world injects per timestep. Bigger Q means “I do not trust my prediction much; the world surprises me.”

Second, how does a measurement relate to the state? This is the measurement model, a linear map H (the observation matrix) that takes the state and produces what a perfect sensor would read. If your sensor reports only position, not velocity:

   H = [ 1   0 ]      z_predicted = H x   means   you expect to measure the position

The sensor is also imperfect, and its noise is captured by the measurement noise covariance R. Bigger R means “this sensor is junk; do not move my estimate far based on it.”

These five matrices — F, B, Q, H, R — plus the initial x and P fully specify a linear Kalman filter. F and H come from physics and from how your sensor works, and you usually know them. Q and R are the noise terms, and they are where the suffering happens, which we will get to.

The Predict-Update Loop

The filter runs a two-phase cycle, forever. Predict pushes the state forward in time using the process model and grows the uncertainty (you are less sure after coasting blind for a timestep). Update folds in a measurement and shrinks the uncertainty (you just learned something). Here is the data flow:

        +-------------------------------------------------+
        |                                                 |
        v                                                 |
   +---------+       +----------------+       +-----------+|
   | initial |  -->  |    PREDICT     |  -->  |  UPDATE   ||
   | x0, P0  |       |                |       |           ||
   +---------+       | x = F x + B u  |       | y = z-H x ||
                     | P = F P F^T +Q |       | K = ...   ||
                     +----------------+       | x = x+K y ||
                          ^                   | P = ...   ||
                          |                   +-----------+|
                     (every dt)                  ^   |     |
                                                 |   |     |
                                          measurement z    |
                                                     |     |
                                                     +-----+
                                              (loop forever)

The predict step is two lines:

   x_pred = F x + B u                  # propagate the state estimate forward
   P_pred = F P F^T + Q                # propagate (and grow) the covariance

The F P F^T term transforms the old uncertainty through the dynamics; the + Q adds the new uncertainty the world injected. Note P grows here with nothing to check it — if you never measured anything, P would balloon forever, which correctly says “I have no idea where it is anymore.”

The update step is where the fusion happens. First compute the innovation y, the surprise — the difference between what you measured and what you expected to measure:

   y = z - H x_pred                    # innovation (measurement residual)
   S = H P_pred H^T + R                # innovation covariance
   K = P_pred H^T S^-1                 # the Kalman gain
   x = x_pred + K y                    # corrected state
   P = (I - K H) P_pred                # corrected (shrunk) covariance

The innovation y is the heart of it. If the measurement matches the prediction, y is zero and the state does not move. If they disagree, the filter nudges the state toward the measurement — but only by K times the disagreement. The matrix K, the Kalman gain, decides how much of that surprise to believe.

The Kalman Gain Is a Trust Dial

Look at the gain again, but reason about it as a scalar to build intuition. In the one-dimensional case K = P_pred / (P_pred + R). Two limits tell you everything:

  • If your prediction is very uncertain (P_pred huge) relative to the sensor (R small), then K -> 1. The filter says “I don’t trust my model; just take the measurement.” x = x_pred + 1 * (z - x_pred) = z. You snap to the sensor.
  • If your prediction is very confident (P_pred tiny) relative to a noisy sensor (R huge), then K -> 0. The filter says “this sensor is garbage; ignore it.” x = x_pred + 0 = x_pred. You keep the prediction.

K is a continuous knob between those extremes, set automatically at every timestep by the current ratio of model uncertainty to sensor uncertainty. This is the elegant part: you never hand-tune the gain. You tell the filter how noisy your model is (Q) and how noisy your sensor is (R), and it computes the optimal blend at every step from the running covariance. When a sensor temporarily goes quiet, P grows, the next measurement gets a high gain, and the filter happily snaps back. When measurements are flowing steadily, P shrinks to an equilibrium and the gain settles.

The full matrix form does exactly this per state dimension, and the off-diagonal terms in P let a measurement of one quantity correct another. This is the genuinely clever bit: even though your sensor only measures position, the filter updates your velocity estimate too, because P has learned the correlation between position error and velocity error. That is how a GPS that only reports position can sharpen your velocity estimate, and how it all amounts to honest sensor fusion rather than a glorified low-pass filter.

The Assumptions, and Why They Matter

The Kalman filter is not merely a good estimator for linear-Gaussian systems — it is the optimal one, the minimum-mean-square-error estimator, and it is unbiased. That optimality is a theorem, and it rests on two assumptions you should know cold:

  1. Linearity. The process model F and measurement model H are linear maps. A Gaussian pushed through a linear transform stays Gaussian, so the entire belief stays a single Gaussian (a mean x and covariance P) at every step. That closure is what lets the whole filter be just a mean and a covariance.

  2. Gaussian, white, zero-mean noise. Process noise and measurement noise are Gaussian, uncorrelated across time, and have known covariances Q and R. Under this, the Gaussian fully describes the belief and the variance-weighted average is genuinely optimal.

When both hold, you cannot do better than the Kalman filter, full stop. When they do not — and in robotics they almost never fully do — the filter is no longer optimal and may not even be correct. A range sensor measuring distance to a landmark is a nonlinear function of robot pose (it involves a square root and an arctangent). A rotating body’s orientation does not evolve linearly. The instant your F or H stops being a matrix and becomes a nonlinear function f() or h(), the plain Kalman filter no longer applies, and you reach for one of its nonlinear cousins.

The Extended Kalman Filter: Linearize and Pray

The Extended Kalman Filter (EKF) is the workhorse of real robotics and was the navigation filter on Apollo. Its move is brutally simple: if the models are nonlinear, linearize them at the current best estimate using a first-order Taylor expansion, then run the ordinary Kalman equations on the linearized version.

Concretely, you provide nonlinear functions f(x, u) for the dynamics and h(x) for the measurement, and at each step you compute their Jacobians — the matrices of partial derivatives evaluated at the current state:

   F = df/dx  evaluated at x         # Jacobian of the dynamics
   H = dh/dx  evaluated at x         # Jacobian of the measurement

Then the predict step propagates the state through the real nonlinear f, but propagates the covariance through the Jacobian F; the update step uses the real nonlinear h for the innovation, but the Jacobian H for the gain:

   x_pred = f(x, u)                   # nonlinear propagation of the mean
   P_pred = F P F^T + Q               # linear propagation of covariance via Jacobian
   y      = z - h(x_pred)             # innovation using the real nonlinear measurement
   K      = P_pred H^T (H P_pred H^T + R)^-1
   x      = x_pred + K y
   P      = (I - K H) P_pred

The EKF is cheap, well understood, and good enough for an enormous amount of real navigation. But its failure modes are real and worth respecting:

  • It is biased and suboptimal. A first-order linearization throws away curvature. A Gaussian pushed through a nonlinear function is not Gaussian, but the EKF pretends it is. When the nonlinearity is mild over the spread of your uncertainty, this is fine. When it is sharp, the approximation is bad.
  • It can diverge. If the linearization point is far from the truth — because the filter got a bad measurement, or initialized poorly, or the process noise was set too small — the Jacobian is evaluated at the wrong place, the correction goes the wrong way, the estimate moves further from truth, the next Jacobian is even more wrong, and the filter walks off into nonsense. Once an EKF diverges it usually does not recover. Overconfidence (P too small, often from a Q set too small) is the classic divergence trigger.
  • You have to derive Jacobians. For a complicated state and measurement model this is error-prone algebra, and a sign error in a Jacobian produces a filter that mostly works and occasionally explodes, which is the worst kind of bug.

The Unscented Kalman Filter: Sample, Don’t Differentiate

The Unscented Kalman Filter (UKF) attacks the EKF’s core weakness — that linearizing the function loses information — with a different idea: don’t linearize the function at all. Instead, deterministically sample the distribution, push the samples through the exact nonlinear function, and reconstruct the resulting Gaussian from the transformed samples.

The samples are called sigma points. For an n-dimensional state, the UKF picks 2n + 1 carefully placed points: the mean, and a pair of points along each principal axis of the covariance ellipsoid, spread out by a factor derived from P. Each sigma point carries a weight. The procedure is:

  1. Generate sigma points from x and P (using a matrix square root of P, typically via Cholesky).
  2. Push every sigma point through the exact nonlinear f() (for predict) or h() (for update). No Jacobians.
  3. Compute the weighted mean and weighted covariance of the transformed points to recover the new Gaussian.

This unscented transform captures the mean and covariance of the transformed distribution to second order for any nonlinearity, versus the EKF’s first order — and it does so without ever computing a derivative. The practical wins are real: the UKF is typically more accurate than the EKF on strongly nonlinear problems, it is derivative-free so there is no Jacobian algebra to get wrong, and it is more robust to initialization. The costs are that it runs 2n+1 function evaluations per step instead of one (more expensive for large states), it has tuning parameters of its own (the spread parameters alpha, beta, kappa), and it still fundamentally assumes the posterior is well described by a single Gaussian — so it does not help when the true belief is multi-modal.

When the Gaussian Breaks: Particle Filters

Both EKF and UKF assume the belief is one Gaussian blob. Some problems break that assumption hard. The classic is the kidnapped robot: a robot wakes up somewhere in a building with a symmetric floor plan, and its sensor readings are consistent with several widely separated locations at once. The true belief is multi-modal — three distinct peaks, say — and no single Gaussian can represent “it is probably here, or here, or here.” Squashing that into one Gaussian gives you a confident estimate of the average of three rooms, which is a hallway nobody is in.

The particle filter represents the belief not as a mean and covariance but as a cloud of thousands of weighted samples (“particles”), each a hypothesis about the full state. Predict moves every particle through the (possibly wild) nonlinear dynamics with random noise; update reweights each particle by how well it explains the measurement; then a resampling step kills low-weight particles and clones high-weight ones, concentrating computation where the probability is. Because it never assumes a distribution shape, it handles arbitrary nonlinearity and arbitrary multi-modal, non-Gaussian beliefs — at the cost of needing many particles (and the count needed grows badly with state dimension, the curse of dimensionality). It is the right tool for global localization and other genuinely multi-modal problems, and overkill for the unimodal tracking problems where a Kalman variant suffices.

KF Family at a Glance

Filter Handles nonlinearity Belief shape Needs Jacobians Cost per step Typical use
Linear KF No (linear only) Single Gaussian No Cheapest Constant-velocity tracking, linear sensor fusion
EKF Yes, 1st-order linearize Single Gaussian Yes Cheap IMU+GPS fusion, classic SLAM back-ends, most flight controllers
UKF Yes, 2nd-order (sigma points) Single Gaussian No ~2n+1 evals Attitude estimation, strongly nonlinear unimodal problems
Particle filter Yes, arbitrary Arbitrary / multi-modal No Expensive (N particles) Global localization, kidnapped robot, non-Gaussian noise

And the symbol glossary, because the notation is half the battle:

Symbol Name What it is
x State vector Your estimate, e.g. position and velocity
P State covariance Uncertainty in x (and correlations)
F State transition Linear dynamics, or its Jacobian in the EKF
B, u Control model, input Known forces driving the state
Q Process noise covariance How much the model is wrong per step
H Measurement model Maps state to expected measurement
R Measurement noise covariance How noisy the sensor is
z Measurement What the sensor actually reported
y Innovation z - H x, the surprise
S Innovation covariance H P H^T + R
K Kalman gain The trust dial, P H^T S^-1

One Predict-Update Step in NumPy

A constant-velocity tracker fusing a position-only sensor, written out so you can see every matrix:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

import numpy as np

dt = 0.1
F = np.array([[1, dt],        # constant-velocity dynamics
              [0,  1]])
H = np.array([[1, 0]])        # we only measure position
Q = np.array([[0.001, 0],     # process noise (tuning knob)
              [0,   0.01]])
R = np.array([[0.5]])         # measurement noise (sensor spec)

x = np.array([[0.0],          # initial state: pos=0, vel=0
              [0.0]])
P = np.eye(2) * 500.0         # large initial uncertainty: we know nothing yet

def step(x, P, z):
    # --- predict ---
    x = F @ x
    P = F @ P @ F.T + Q
    # --- update ---
    y = z - H @ x                       # innovation
    S = H @ P @ H.T + R                 # innovation covariance
    K = P @ H.T @ np.linalg.inv(S)      # Kalman gain
    x = x + K @ y
    P = (np.eye(len(x)) - K @ H) @ P
    return x, P

# feed it a noisy measurement of position 1.0
x, P = step(x, P, np.array([[1.0]]))
print(x.ravel(), "\n", P)

On that first step, because P started huge, the gain is near 1 and the estimate snaps almost all the way to the measurement; run it over a stream of measurements and P settles, the gain drops, and the filter smooths. For production use a numerically stable form — the Joseph-form covariance update P = (I-KH) P (I-KH)^T + K R K^T keeps P symmetric positive-definite when the naive (I-KH)P drifts, and square-root or UD-factorized filters are standard in flight code where P must never go non-physical.

Where These Actually Live

The Kalman family is not academic — it is in the boring, load-bearing middle of nearly every autonomous system.

IMU + GPS fusion is the canonical example. An IMU (accelerometers and gyroscopes) gives you fast, smooth, high-rate motion but drifts without bound as you integrate its noise. GPS gives you an absolute position that does not drift but is slow, jumpy, and drops out under bridges and indoors. An EKF (or an error-state Kalman filter, the standard formulation in aerospace) runs the IMU in the predict step at high rate and folds in GPS in the update step whenever a fix arrives. The result is a position-and-velocity estimate that is both smooth and drift-free — the best of both sensors. This is what is running in your phone’s location stack, in drone autopilots like PX4 and ArduPilot, and in car navigation.

Attitude estimation — knowing which way a body is pointing — fuses gyroscopes (smooth, drifting) with accelerometers and magnetometers (noisy, absolute). Because orientation lives on a nonlinear manifold (rotations do not add like vectors), this is a natural EKF or UKF problem, and quaternion-based variants are the norm.

SLAM back-ends historically used the EKF to jointly estimate a robot’s pose and a map of landmarks in one giant state vector, the classic EKF-SLAM. Modern systems have largely moved to factor-graph optimization for the full problem, but Kalman-style filtering still lives in the front-end and in the inertial pre-integration. We went deep on the mapping side in SLAM in practice, and the Kalman filter is the estimation primitive sitting under a lot of it. If you are wiring any of this together on a robot, it almost certainly flows through the middleware we covered in ROS and ROS2 explained, where robot_localization is a ready-made EKF/UKF node.

A filtered state estimate is also what the controllers downstream consume — a PID loop holding altitude wants a clean, low-latency estimate of altitude and vertical velocity, and the Kalman filter is what gives the controller something smooth to act on instead of raw sensor jitter.

The Tuning Reality Nobody Warns You About

The equations are clean. Making a filter work on real hardware is not, and the gap is almost entirely about Q and R.

R you can often measure: leave the sensor still, log it, compute the variance of the noise, and there is your R. Sensor datasheets give you a starting figure. Q is the problem. Process noise represents everything your model leaves out — unmodeled accelerations, vibration, a dynamics equation that is only approximately true — and there is no datasheet for “how wrong is my model.” You set Q by trial and error against logged data, and the symptoms guide you:

  • Q too small (you trust the model too much): the filter becomes overconfident, P shrinks, the gain drops, and it stops listening to measurements. It tracks beautifully smoothly and lags reality badly, and at the extreme it diverges because it has decided the sensor is lying when actually the model is.
  • Q too large (you trust the model too little): the filter chases every measurement, P stays large, the gain stays high, and the output is nearly as noisy as the raw sensor. You built an expensive low-pass filter.

The right Q lives somewhere between, and there is no formula — it is a bias-variance trade-off you dial in on real data. This is the dirty secret of applied Kalman filtering: the elegant optimality theorem assumes you know Q and R exactly, and in practice you are guessing at Q, so the “optimal” filter is optimal for a noise model you made up. Practitioners watch the normalized innovation squared (NIS) to sanity-check: if your noise model is right, the innovations should be consistent with S, and a chi-squared test on the NIS tells you whether your Q/R are roughly honest. If innovations are persistently larger than S predicts, your covariance is too small and divergence looms.

The other hard-won lessons: initialization matters — a wildly wrong initial x with a small initial P tells the filter “I am very sure, and very wrong,” which is poison for an EKF. Start with a large P so the first measurements dominate. Divergence is the failure mode — add fault detection (gate measurements by their innovation; reject any z whose innovation is implausibly large given S) so one bad GPS fix does not poison the state. And numerical hygiene — covariances must stay symmetric and positive-definite, which is why the Joseph form and square-root filters exist.

Finally, the honest question every engineer should ask before reaching for a Kalman filter: do you actually need one? For the narrow but common job of fusing one fast-drifting sensor with one slow-absolute sensor — a gyro and an accelerometer for tilt, say — a complementary filter does it with a single constant: estimate = a * (estimate + gyro*dt) + (1-a) * accel. It is a high-pass on one sensor plus a low-pass on the other, it has one tuning parameter instead of two noise matrices, it never diverges, and on a small drone it is frequently indistinguishable from a Kalman filter in the output while being far easier to reason about. A Kalman filter earns its complexity when you have many sensors, real cross-correlations between states, time-varying uncertainty, or a need for a principled covariance estimate downstream. If you have two sensors and one axis, try the complementary filter first.

Verdict

The Kalman filter is the right tool when you need to fuse a noisy prediction with a noisy measurement into a single best estimate, and its genius is the recursive, constant-memory predict-update loop that grows uncertainty while coasting and shrinks it on every measurement, with the Kalman gain automatically setting the blend from the running ratio of model trust to sensor trust. For linear-Gaussian systems it is provably optimal and you cannot beat it. For the nonlinear systems that every real robot lives in, the EKF linearizes via Jacobians and is cheap, ubiquitous, and prone to divergence if you get overconfident; the UKF skips the Jacobians by pushing sigma points through the exact nonlinearity and buys accuracy and robustness for more compute; and the particle filter abandons the Gaussian entirely for the multi-modal problems like global localization where a single blob simply cannot represent the truth. The equations are not the hard part — predict is two lines, update is five — the hard part is Q and R, where the elegant optimality theorem quietly assumes you know noise statistics you are actually guessing at, and the difference between a filter that tracks reality and one that walks off a cliff is a few orders of magnitude in a matrix you tuned by hand against logged data. Learn the linear filter until the gain-as-trust-dial intuition is automatic, reach for the EKF as your default in robotics, keep the UKF in your pocket for the strongly nonlinear cases, know that particle filters exist for when the belief goes multi-modal, watch your innovations to catch divergence early, and never forget that for two sensors and one axis a complementary filter might do the whole job with a single constant and never blow up.

Sources

Comments