The Kalman Filter

Every robot sensor lies — a little. GPS has 3 m noise. An IMU accumulates drift. Wheel odometry slips. No single reading is trustworthy in isolation, but a sequence of noisy readings, combined with a model of how the robot moves, yields an estimate far better than any individual measurement.

The Kalman filter (Rudolf Kálmán, 1960) solves exactly this: given a linear dynamical system driven by Gaussian noise and observed through a linear measurement model corrupted by Gaussian noise, it computes the minimum-variance unbiased estimate of the state at each timestep. It is not a heuristic — it is provably optimal under these assumptions. Outside linear-Gaussian systems, it becomes an approximation, but a remarkably robust one. The Apollo Guidance Computer used a Kalman filter for lunar navigation in 1969; your phone uses a variant for GPS-IMU fusion today.

Define the state vector $\mathbf{x}_t \in \mathbb{R}^n$ — whatever you want to estimate (position, velocity, orientation, bias). The process model says how the state evolves:

$\mathbf{x}_t = F_t \mathbf{x}_{t-1} + B_t \mathbf{u}_t + \mathbf{w}_t, \qquad \mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, Q_t)$

where $F_t$ is the state transition matrix, $B_t \mathbf{u}_t$ is the effect of control input $\mathbf{u}_t$ (e.g. motor commands), and $\mathbf{w}_t$ is process noise with covariance $Q_t$.

The measurement model relates observations to the state:

$\mathbf{z}_t = H_t \mathbf{x}_t + \mathbf{v}_t, \qquad \mathbf{v}_t \sim \mathcal{N}(\mathbf{0}, R_t)$

where $H_t$ is the measurement matrix and $R_t$ is measurement noise covariance. Tuning $Q$ and $R$ is the engineering art of the KF: $Q$ too small $\Rightarrow$ the filter is overconfident in its model; $R$ too small $\Rightarrow$ the filter over-trusts noisy sensors.

The Kalman filter maintains a Gaussian belief over the state: a mean $\hat{\mathbf{x}}_t$ and a covariance $\Sigma_t$ (also written $P_t$ in many texts). The filter alternates between predict and update.

Predict (propagate the prior through the process model):

$\hat{\mathbf{x}}_{t|t-1} = F_t \hat{\mathbf{x}}_{t-1|t-1} + B_t \mathbf{u}_t$

$\Sigma_{t|t-1} = F_t \Sigma_{t-1|t-1} F_t^\top + Q_t$

The mean propagates linearly. The covariance grows: $F_t \Sigma F_t^\top$ rotates and scales the uncertainty ellipsoid; $+ Q_t$ inflates it with process noise. Uncertainty only grows in the predict step — you can never become more certain without a measurement. This is why a robot navigating without sensors quickly becomes lost: each prediction inflates $\Sigma$.

When a measurement $\mathbf{z}_t$ arrives, compute the innovation (surprise):

$\tilde{\mathbf{y}}_t = \mathbf{z}_t - H_t \hat{\mathbf{x}}_{t|t-1}$

The Kalman gain $K_t$ determines how much to trust the measurement vs the prediction:

$K_t = \Sigma_{t|t-1} H_t^\top \left( H_t \Sigma_{t|t-1} H_t^\top + R_t \right)^{-1}$

Then update:

$\hat{\mathbf{x}}_{t|t} = \hat{\mathbf{x}}_{t|t-1} + K_t \tilde{\mathbf{y}}_t$

$\Sigma_{t|t} = (I - K_t H_t) \Sigma_{t|t-1}$

Intuition: if $R_t \to 0$ (perfect sensor), $K_t \to H_t^{-1}$ and the update fully trusts the measurement. If $R_t \to \infty$ (useless sensor), $K_t \to 0$ and the state is unchanged. The filter interpolates between these extremes based on the relative magnitudes of predicted uncertainty and sensor noise.

A clean implementation of one predict-update cycle:

import numpy as np

def kalman_predict(x, P, F, B, u, Q):
    """Predict step. Returns (x_pred, P_pred)."""
    x_pred = F @ x + B @ u
    P_pred = F @ P @ F.T + Q
    return x_pred, P_pred

def kalman_update(x_pred, P_pred, z, H, R):
    """Update step. Returns (x_upd, P_upd, innovation)."""
    y = z - H @ x_pred                         # innovation
    S = H @ P_pred @ H.T + R                   # innovation covariance
    K = P_pred @ H.T @ np.linalg.inv(S)        # Kalman gain
    x_upd = x_pred + K @ y
    P_upd = (np.eye(len(x_pred)) - K @ H) @ P_pred
    return x_upd, P_upd, y

# 1-D constant-velocity example
dt = 0.1
F = np.array([[1, dt], [0, 1]])   # [pos, vel] transition
H = np.array([[1, 0]])            # observe position only
Q = np.diag([0.01, 0.1])
R = np.array([[1.0]])
B = np.zeros((2, 1))
u = np.zeros((1,))

x = np.array([0.0, 1.0])          # initial state
P = np.eye(2) * 1.0               # initial covariance

For numerical stability on embedded systems, use the Joseph form for the covariance update: $P = (I - KH)P(I - KH)^\top + KRK^\top$, which remains positive-semidefinite even with floating-point rounding.

The Kalman filter is the BLUE — Best Linear Unbiased Estimator. Formally, it minimises the trace of the posterior covariance $\Sigma_{t|t}$ over all linear estimators, without assuming Gaussianity. Under the additional Gaussian assumptions, it is the MAP (maximum a posteriori) and minimum mean-squared-error estimator over all estimators — not just linear ones.

What breaks this optimality?

• Non-Gaussian noise: heavy tails (outlier measurements) can yank the mean far off. Robust variants (Huber loss, particle filters) handle this.
• Nonlinearity: a robot turning in a circle has a nonlinear motion model. Applying $F$ from a linearised system accumulates error.
• Unmodelled correlations: if two sensors share a common error source (e.g. both use the same clock), treating them as independent inflates confidence.

Knowing these failure modes is as important as knowing the equations.

The two-step recursion that runs at every timestep.

Most real robots have nonlinear process and measurement models. The EKF linearises them at the current estimate using first-order Taylor expansion:

$\mathbf{x}_t = f(\mathbf{x}_{t-1}, \mathbf{u}_t) + \mathbf{w}_t \qquad \Rightarrow \qquad F_t \approx \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{t-1|t-1}}$

$\mathbf{z}_t = h(\mathbf{x}_t) + \mathbf{v}_t \qquad \Rightarrow \qquad H_t \approx \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{t|t-1}}$

The predict and update equations are otherwise identical, but using the Jacobians $F_t$ and $H_t$. For a ground robot with state $[x, y, \theta, v]$, the motion Jacobian $\partial f / \partial \mathbf{x}$ is straightforward but tedious to derive by hand — tools like autodiff (JAX, PyTorch) or symbolic differentiation (SymPy) automate this. The EKF works well when the nonlinearity is mild over one timestep; for large steps or sharp turns it loses accuracy. The Unscented KF (UKF) uses deterministic sigma points to capture second-order statistics without computing Jacobians at all — better accuracy at similar cost.

A classic fusion task: combine a 1 Hz GPS (high noise, low rate, absolute) with a 200 Hz IMU (low noise, high rate, drifts).

State: $\mathbf{x} = [p_x, p_y, p_z, v_x, v_y, v_z, b_{ax}, b_{ay}, b_{az}]^\top$ — position, velocity, and accelerometer bias.

Process model: integrate IMU at 200 Hz, including bias as a random walk: $\dot{\mathbf{b}}_a = \mathbf{n}_b$. The predict step runs 200× per second.

Measurement model: GPS provides $[p_x, p_y, p_z]^\top$. $H = [I_3 | \mathbf{0}_{3 \times 6}]$, $R = \text{diag}(\sigma_{\text{GPS}}^2)$. Update runs only when a GPS fix arrives.

Result: the IMU gives smooth, high-rate pose between GPS fixes; GPS corrects the drift. The covariance blooms when GPS is lost (tunnels, urban canyons) and collapses after a good fix — exactly what the filter equations predict. This architecture, called loosely coupled GPS-IMU fusion, is the default in commercial autopilots.