Kalman Filter

The Kalman filter is an optimal recursive estimator for linear systems with Gaussian noise. It maintains a state estimate and its uncertainty, alternating between predict (use the model) and update (use the measurement) steps.

Why It Matters

Kalman filters are used everywhere sensors are noisy: GPS navigation, drone flight control, financial modeling, robot localization. The filter automatically balances trust between the model prediction and the sensor measurement based on their respective uncertainties.

The Predict-Update Cycle

Predict (Time Update)

Use the system model to project the state forward:

x̂⁻ = A · x̂ + B · u          predict state
P⁻  = A · P · Aᵀ + Q         predict covariance (uncertainty grows)

Update (Measurement Update)

Correct the prediction with a new measurement:

K  = P⁻ · Cᵀ · (C · P⁻ · Cᵀ + R)⁻¹    Kalman gain
x̂  = x̂⁻ + K · (z - C · x̂⁻)             correct state
P   = (I - K · C) · P⁻                   correct covariance (uncertainty shrinks)

Symbol	Meaning
x̂	State estimate
P	State covariance (how uncertain is the estimate)
A	State transition matrix (system dynamics)
B	Input matrix
C	Measurement matrix (what the sensor sees)
Q	Process noise covariance (model uncertainty)
R	Measurement noise covariance (sensor uncertainty)
K	Kalman gain (how much to trust the measurement)
z	Actual measurement

Intuition: The Kalman Gain

K large (close to 1): trust measurement → R is small or P is large
  → "My model is uncertain, but my sensor is good"

K small (close to 0): trust prediction → R is large or P is small
  → "My model is confident, sensor is noisy"

The filter automatically computes the optimal blend at each step.

1D Example: Position Tracking

A sensor measures position with noise. The object is assumed to be roughly stationary.

import numpy as np
import matplotlib.pyplot as plt
 
np.random.seed(42)
 
# True position and noisy measurements
true_pos = 5.0
measurements = true_pos + np.random.randn(50) * 2.0  # noise std = 2
 
# Kalman filter (1D, constant model: x_next = x)
x = 0.0       # initial estimate
P = 10.0      # initial uncertainty (high = "I don't know")
Q = 0.01      # process noise (how much position changes per step)
R = 4.0       # measurement noise variance (2² = 4)
 
estimates = []
for z in measurements:
    # Predict
    P_pred = P + Q
 
    # Update
    K = P_pred / (P_pred + R)
    x = x + K * (z - x)
    P = (1 - K) * P_pred
    estimates.append(x)
 
plt.plot(measurements, 'x', alpha=0.5, label='Measurements')
plt.plot(estimates, '-', linewidth=2, label='Kalman estimate')
plt.axhline(y=true_pos, color='r', linestyle='--', label='True position')
plt.xlabel('Step'); plt.ylabel('Position'); plt.legend(); plt.grid(); plt.show()

Multidimensional Example: Position + Velocity

Track a moving object where we measure position but also want to estimate velocity:

import numpy as np
 
dt = 0.1  # 10 Hz
 
# State: [position, velocity]
A = np.array([[1, dt],   # pos = pos + vel*dt
              [0,  1]])   # vel = vel (constant velocity model)
 
B = np.array([[0], [0]])  # no control input
C = np.array([[1, 0]])    # only measure position
 
Q = np.array([[0.01, 0],  # process noise
              [0, 0.1]])
R = np.array([[4.0]])     # measurement noise variance
 
# Initialize
x = np.array([[0], [0]])  # initial state guess
P = np.eye(2) * 10        # high initial uncertainty
 
estimates = []
for z_val in measurements_2d:
    z = np.array([[z_val]])
 
    # Predict
    x = A @ x + B @ np.array([[0]])
    P = A @ P @ A.T + Q
 
    # Update
    S = C @ P @ C.T + R
    K = P @ C.T @ np.linalg.inv(S)
    x = x + K @ (z - C @ x)
    P = (np.eye(2) - K @ C) @ P
 
    estimates.append(x.flatten())

Now x[1] gives an estimated velocity even though we never measured it directly — the filter infers it from position changes.

Tuning Q and R

Parameter	Effect of Increasing
Q (process noise)	Filter trusts model less, tracks measurements faster, noisier output
R (measurement noise)	Filter trusts sensor less, smoother output, slower response

How to set them:

• R: measure sensor noise directly (record stationary data, compute variance)
• Q: harder — represents modeling uncertainty. Start small, increase until the filter tracks well
• Ratio Q/R is what matters. If you double both Q and R, the filter behavior is the same

Extended Kalman Filter (EKF)

For nonlinear systems (most real robots), the standard Kalman doesn’t apply. The EKF linearizes at each step:

1. Predict: x̂⁻ = f(x̂, u) (nonlinear model)
2. Compute Jacobian: F = ∂f/∂x at current estimate
3. Predict covariance: P⁻ = F·P·Fᵀ + Q
4. Update: standard Kalman equations (linearized around x̂⁻)

Limitation: linearization can be poor for highly nonlinear systems. Alternatives: Unscented Kalman Filter (UKF), particle filter.

Computational Cost

Dimension	Operations per step	Typical
1D	~10 multiplications	Trivial
6D (IMU)	~200 multiplications	No problem on any MCU
15D (GPS/INS)	~2000 multiplications	Fine on Cortex-M4+
100D+ (SLAM)	Matrix inversion dominates	Needs sparse methods

The bottleneck is the matrix inversion in the Kalman gain: (C·P·Cᵀ + R)⁻¹. For large state spaces, sparse or information-form Kalman filters are used.

Related

• State Space Representation — Kalman filter operates on state space models
• Sensor Fusion — practical application of Kalman filtering
• SLAM — Kalman filter for simultaneous localization and mapping
• Digital Control — discrete-time framework shared with digital Kalman
• Transfer Functions — Kalman filter is the dual of optimal control (LQR)

Cross-references

• State Estimation and Sensor Fusion — Kalman filter applied to autonomous systems
• Tutorial - Target Tracking with Kalman Filter — Kalman filter for radar tracking