Kalman Filter

The Kalman filter estimates the hidden state of a dynamic system, such as the position and velocity of a moving object, from a sequence of noisy measurements. At every time step it predicts the state forward with a motion model, then corrects the prediction with the new measurement, weighting the two by their uncertainties. Introduced by Rudolf Kalman in 1960, it is one of the most widely used estimation algorithms in engineering. In computer vision it is the standard way to smooth noisy detections, predict where an object will appear in the next frame, and bridge short gaps in detection, which makes it a core component of object tracking systems.

Problem

A system’s state cannot be observed directly; at each time step $k$ a sensor produces a noisy measurement that depends on it. The goal is to estimate the current state from all measurements so far, online, without reprocessing the history. In tracking, the state might be a bounding-box center and its velocity, and the measurement the center reported by a detector, which jitters, occasionally goes missing, and carries no velocity information.

Inputs and Outputs

Inputs: a state-space model (transition matrix $F$, measurement matrix $H$, noise covariances $Q$ and $R$); an initial estimate $\hat{\mathbf{x}}_{0 \mid 0}$ with covariance $P_{0 \mid 0}$; and a measurement $\mathbf{z}_k$ at each step, or none when it is missing.

Outputs, at each step: the state estimate $\hat{\mathbf{x}}_{k \mid k}$ and its error covariance $P_{k \mid k}$, plus the one-step predictions of state and measurement used for gating and data association.

Intuition

The filter maintains a belief: a best guess and an uncertainty around it.

• Predict. Move the belief forward with the motion model: an object at $p$ with velocity $v$ should now be near $p + v \Delta t$. Because the model is imperfect, uncertainty grows.
• Update. Move the estimate part of the way toward the new measurement. Uncertainty shrinks.

How far the estimate moves is set by the Kalman gain. In the scalar case, with prediction $\hat{x}^-$ of variance $P$ and a measurement $z$ of variance $R$,

$$K = \frac{P}{P + R}, \qquad \hat{x} = \hat{x}^- + K (z - \hat{x}^-).$$

A confident prediction and a noisy sensor give a small gain; an uncertain prediction and a precise sensor give a gain near 1. The matrix version does the same per direction in state space and also corrects components that are never measured: velocity is inferred from how successive position measurements deviate from the predictions.

Algorithm

1. Initialize $\hat{\mathbf{x}}_{0 \mid 0}$ and $P_{0 \mid 0}$, with large variance for poorly known components.
2. For each time step $k = 1, 2, \ldots$:
   1. Predict the state and covariance with the motion model.
   2. If a measurement $\mathbf{z}_k$ is available, compute the innovation, the Kalman gain, and the updated state and covariance.
   3. Otherwise, keep the prediction as the estimate; its covariance keeps growing until a measurement arrives.

Mathematical Formulation

State-space model

The Kalman filter assumes a linear-Gaussian state-space model:

$$\mathbf{x}_k = F \mathbf{x}_{k-1} + \mathbf{w}_k, \qquad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, Q)$$ $$\mathbf{z}_k = H \mathbf{x}_k + \mathbf{v}_k, \qquad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, R)$$

Here $\mathbf{x}_k \in \mathbb{R}^n$ is the state and $\mathbf{z}_k \in \mathbb{R}^m$ the measurement; the process noise $\mathbf{w}_k$ and measurement noise $\mathbf{v}_k$ are zero-mean, white, and mutually independent. (A known control input $B \mathbf{u}_k$ can be added to the transition; vision trackers rarely have one.) $\hat{\mathbf{x}}_{k \mid k-1}$ denotes the estimate of $\mathbf{x}_k$ given measurements up to step $k-1$ (the prediction), $\hat{\mathbf{x}}_{k \mid k}$ the estimate given measurements up to step $k$, and $P_{k \mid k-1}$, $P_{k \mid k}$ the corresponding error covariances.

Predict

$$\hat{\mathbf{x}}_{k \mid k-1} = F \hat{\mathbf{x}}_{k-1 \mid k-1}$$ $$P_{k \mid k-1} = F P_{k-1 \mid k-1} F^\top + Q$$

Update

$$\mathbf{y}_k = \mathbf{z}_k - H \hat{\mathbf{x}}_{k \mid k-1}$$ $$S_k = H P_{k \mid k-1} H^\top + R$$ $$K_k = P_{k \mid k-1} H^\top S_k^{-1}$$ $$\hat{\mathbf{x}}_{k \mid k} = \hat{\mathbf{x}}_{k \mid k-1} + K_k \mathbf{y}_k$$ $$P_{k \mid k} = (I - K_k H) P_{k \mid k-1}$$

$\mathbf{y}_k$ is the innovation and $S_k$ its covariance. When the model is correct, the squared Mahalanobis distance $\mathbf{y}_k^\top S_k^{-1} \mathbf{y}_k$ follows a chi-squared distribution with $m$ degrees of freedom. Trackers use it to gate measurements: a detection whose distance exceeds a chi-squared threshold is considered too unlikely to belong to the track.

Running example: constant-velocity model

To track the center $(p_x, p_y)$ of a point or bounding box, a common choice is the state $\mathbf{x} = (p_x, p_y, v_x, v_y)^\top$, measurement $\mathbf{z} = (p_x, p_y)^\top$, and frame interval $\Delta t$:

$$F = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}, \qquad H = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix}$$

Velocity stays constant and position integrates it; process noise accounts for unmodeled acceleration. Modeling the acceleration as constant within each frame interval and independent between intervals, with variance $q$ per axis (the discrete white-noise acceleration model), gives

$$Q = q \, G G^\top, \qquad G = \begin{bmatrix} \Delta t^2 / 2 & 0 \\ 0 & \Delta t^2 / 2 \\ \Delta t & 0 \\ 0 & \Delta t \end{bmatrix}$$

and isotropic detector noise gives $R = r I_2$. Bounding-box trackers often extend the state with box size. SORT, for example, uses the box center, area, and aspect ratio, plus velocities of the center and area; the aspect ratio is assumed constant.

Parameters

• Measurement noise $R$. Detector noise; it can be estimated against ground truth and for box detectors often scales with object size.
• Process noise $Q$. How much true motion departs from the model; usually tuned. Small $Q$ gives smooth estimates that lag behind maneuvers; large $Q$ responds quickly but passes more noise.
• Ratio, not scale. Multiplying $Q$, $R$, and $P_{0 \mid 0}$ by the same factor leaves the gain and state estimates unchanged; only the reported covariance scales.
• Initial covariance $P_{0 \mid 0}$. Large for components unknown at initialization, such as velocity for a track created from one detection.
• Motion model. Constant position, velocity, or acceleration trade responsiveness against smoothness; constant velocity is the usual default in video.

A useful consistency check: in a well-tuned filter, the normalized innovation squared $\mathbf{y}_k^\top S_k^{-1} \mathbf{y}_k$ averages about $m$. Consistently larger values mean the filter is overconfident, usually because $Q$ or $R$ is too small.

Complexity

For state dimension $n$ and measurement dimension $m$, each step costs $O(n^3)$ for the covariance prediction and $O(n^2 m + n m^2 + m^3)$ for the update, including the inversion of $S_k$. Memory is $O(n^2)$. The cost per step does not grow with sequence length, because the estimate and covariance summarize all past measurements; for the four-dimensional constant-velocity model it is negligible even with hundreds of tracked objects.

Implementation

A constant-velocity Kalman filter tracking a point that moves in a straight line, observed with Gaussian noise of standard deviation 2 pixels:

import numpy as np

dt = 1.0                     # time between frames
q = 0.01                     # acceleration variance (unmodeled motion)
r = 4.0                      # measurement noise variance, in pixels^2

# State x = [px, py, vx, vy]; measurement z = [px, py].
F = np.array([[1, 0, dt, 0],
              [0, 1, 0, dt],
              [0, 0, 1,  0],
              [0, 0, 0,  1]], dtype=float)
H = np.array([[1, 0, 0, 0],
              [0, 1, 0, 0]], dtype=float)
G = np.array([[dt**2 / 2, 0], [0, dt**2 / 2], [dt, 0], [0, dt]])
Q = q * G @ G.T              # white-noise acceleration model
R = r * np.eye(2)

def predict(x, P):
    x = F @ x
    P = F @ P @ F.T + Q
    return x, P

def update(x, P, z):
    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

# Simulate a point moving at constant velocity, observed with noise.
rng = np.random.default_rng(0)
true_x = np.array([0.0, 0.0, 2.0, 1.0])
x = np.array([0.0, 0.0, 0.0, 0.0])       # initial guess: unknown velocity
P = np.diag([10.0, 10.0, 100.0, 100.0])  # large initial uncertainty

raw_err, filt_err = [], []
for t in range(50):
    true_x = F @ true_x
    z = H @ true_x + rng.normal(0.0, np.sqrt(r), size=2)
    x, P = predict(x, P)
    x, P = update(x, P, z)
    raw_err.append(np.linalg.norm(z - true_x[:2]))
    filt_err.append(np.linalg.norm(x[:2] - true_x[:2]))

print(f"mean position error, raw measurements: {np.mean(raw_err[10:]):.2f} px")
print(f"mean position error, Kalman filter:    {np.mean(filt_err[10:]):.2f} px")
print("estimated velocity:", np.round(x[2:], 2))

Output:

mean position error, raw measurements: 2.54 px
mean position error, Kalman filter:    1.37 px
estimated velocity: [1.85 0.87]

After a short warm-up, the filtered positions are nearly twice as accurate as the raw measurements, and the filter has recovered the velocity, true value $(2, 1)$, although velocity is never measured. When a detection is missing, a tracker calls predict without update. Production code typically solves a linear system instead of forming $S_k^{-1}$, and uses the Joseph form $P = (I - KH) P (I - KH)^\top + K R K^\top$ to keep the covariance symmetric and positive semi-definite under rounding.

Properties and Behavior

• Optimality. For a linear model with Gaussian noise and Gaussian initial state, the posterior of the state given all measurements is Gaussian with mean $\hat{\mathbf{x}}_{k \mid k}$ and covariance $P_{k \mid k}$. The Kalman estimate, the closed-form recursive Bayesian solution, is then the minimum mean squared error (MMSE) estimate and also the most probable state. Without the Gaussian assumption, but with the same linear model and known second-order noise statistics, it is the best linear unbiased estimator: minimum error variance among estimators linear in the measurements. A nonlinear estimator may do better.
• Data-independent covariance. $P_{k \mid k}$ and $K_k$ depend only on $F$, $H$, $Q$, $R$, and $P_{0 \mid 0}$, not on the measured values. For a time-invariant model that is detectable and stabilizable (with respect to the process noise), they converge to steady-state values, so a fixed-gain filter can be precomputed.
• Missing data. Skipping the update is correct under the model; the growing uncertainty widens the gate for re-associating the object.
• Sensitivity to modeling errors. With a wrong motion model or misestimated $Q$ and $R$, the filter can be biased, lag behind maneuvers, or become overconfident and ignore measurements.

Limitations

• Linearity. Perspective projection, rotation, and bearing-only sensors are nonlinear; the basic filter does not apply directly.
• Unimodal Gaussian belief. A single Gaussian cannot represent an object that may be behind either of two occluders, and one outlier detection can pull the estimate far off. Gating and robust variants mitigate this.
• No data association. With several objects, a separate step decides which detection updates which filter, for example the Hungarian algorithm on predicted positions or Mahalanobis distances. The filter cannot detect wrong associations.
• Model dependence. Constant velocity describes erratic motion, such as people in crowds, poorly, and uncompensated camera motion is treated as object motion.

Variants

The extended Kalman filter linearizes nonlinear dynamics and measurement functions around the current estimate using their Jacobians. It is widely used, for example in visual-inertial odometry and SLAM, but can diverge when the nonlinearity is strong relative to the uncertainty.

The unscented Kalman filter passes a small, deterministically chosen set of sigma points through the nonlinear functions and fits a Gaussian to the result. It needs no Jacobians and typically captures the transformed mean and covariance more accurately than linearization, at comparable cost.

The information filter propagates the inverse covariance and an information vector; updates become additive, which suits multi-sensor and decentralized fusion.

The Rauch–Tung–Striebel smoother refines past estimates offline with later measurements: a backward pass after the forward filter gives error covariance no larger than the filter’s.

Particle filters represent the belief with weighted samples and handle nonlinear models and multimodal distributions at much higher cost. Multiple-model filters run Kalman filters with different motion models in parallel and mix their estimates, which suits targets that alternate between, for example, moving straight and turning.