You observe a system’s position every second with a noisy GPS sensor, but you need to know both position and velocity for control. You have a physics model (constant velocity) but the measurements are unreliable. How do you recover the true state?
The Kalman Filter answers this question: it estimates a system’s hidden state from noisy measurements, combining a prediction from a dynamic model with a correction from new measurements, weighted by their respective uncertainties.
In this post, we build a complete Kalman Filter from scratch in NumPy, verify it against a known ground-truth trajectory, and show exactly how much uncertainty it reduces compared to raw measurements.
The problem: tracking an object with noisy measurements #
Suppose an object moves in one dimension with (approximately) constant velocity. We can measure its position every dt seconds, but the sensor has standard deviation σ = 3.0 meters of noise.
We never measure velocity directly, yet we need it for control. Can we recover it from position-only observations?
import numpy as np
import matplotlib.pyplot as plt
np.random.seed(42) # for reproducibility
dt = 1.0 # sampling interval
T = 30.0 # total time
N = int(T / dt) + 1 # number of samples
# Ground truth: constant velocity motion
v_true = 1.5 # true velocity [m/s]
t = np.arange(N) * dt
x_true = v_true * t # x(t) = v * t
# Noisy position measurements (GPS)
sigma_gps = 3.0
noise = np.random.normal(0, sigma_gps, size=N)
z = x_true + noise # measurementsThe raw measurements are scattered far from the true trajectory. Can we do better?
The Kalman Filter model #
For a linear system with Gaussian noise, the Kalman Filter assumes:
State transition: x_k = F·x_{k-1} + w_k
Measurement: z_k = H·x_k + v_k
Where:
x_kis the state vector (position, velocity)Fis the state transition matrixHis the observation matrixw_k,v_kare process and measurement noise
For constant velocity motion:
x_k = [position_k, velocity_k]^T
F = [[1, dt],
[0, 1]] # state transition matrix
H = [[1, 0]] # observation matrix (measure position)
Q = [[q, 0],
[0, q]] # process noise covariance
R = [[r]] # measurement noise varianceLet’s implement the filter:
# Kalman Filter setup
# State: [position, velocity]^T
# Measurement: position only
F = np.array([[1, dt],
[0, 1]]) # state transition matrix
H = np.array([[1, 0]]) # observation matrix (measure position)
Q = np.diag([0.01, 0.1]) # process noise covariance
R = np.array([[sigma_gps**2]]) # measurement noise variance
# Initial state estimate and covariance
x0 = np.array([0.0, 0.0]) # initial guess: position 0, velocity 0
P0 = np.diag([100.0, 100.0]) # high uncertainty (we're guessing)
def kalman_predict(x, P, F, Q):
"""Predict the next state and covariance."""
x_pred = F @ x
P_pred = F @ P @ F.T + Q
return x_pred, P_pred
def kalman_update(x_pred, P_pred, z, H, R):
"""Update the state estimate with a measurement."""
# Innovation/Residual
y = z - H @ x_pred
# Innovation covariance
S = H @ P_pred @ H.T + R
# Kalman gain
K = P_pred @ H.T @ np.linalg.inv(S)
# State update
x_upd = x_pred + K @ y
# Covariance update
I = np.eye(len(x_pred))
P_upd = (I - K @ H) @ P_pred
return x_upd, P_updRunning the filter #
Now we run the filter over all measurements:
# Run the filter
x_est = np.zeros((N, 2)) # store position and velocity estimates
P_est = np.zeros((N, 2, 2)) # store covariance matrices
x_est[0] = x0
P_est[0] = P0
for k in range(1, N):
# Predict
x_pred, P_pred = kalman_predict(x_est[k-1], P_est[k-1], F, Q)
# Update with measurement
x_est[k], P_est[k] = kalman_update(x_pred, P_pred, z[k], H, R)Results: Position and Velocity Estimation #
Let’s compute the RMSE and compare against raw measurements:
Position RMSE — raw measurements: 3.12 m
Position RMSE — Kalman filter: 0.53 m
Velocity RMSE — Kalman filter: 0.18 mThe Kalman filter reduces position error by 83% compared to raw GPS measurements and successfully infers velocity with high accuracy despite never measuring it directly.
How the Kalman Filter works #
The filter operates in two phases:
1. Prediction (time update):
- Propagate the state forward using the motion model:
x_pred = F·x - Propagate the uncertainty:
P_pred = F·P·F^T + Q
2. Update (measurement correction):
- Compute the innovation (residual):
y = z - H·x_pred - Compute the Kalman gain:
K = P_pred·H^T·(H·P_pred·H^T + R)^{-1} - Correct the state:
x_upd = x_pred + K·y - Update uncertainty:
P_upd = (I - K·H)·P_pred
The Kalman gain K automatically balances trust between the prediction and measurement. When measurement noise R is high, K becomes small and we trust the prediction more.
Why variance reduction matters #
The key insight: by maintaining a covariance matrix P, the Kalman Filter knows how uncertain each state estimate is. This enables:
- Optimal weighting: The Kalman gain
Kis derived to minimize the posterior covariance - Consistency: The reported uncertainty
Pmatches actual estimation error - Sensor fusion: Multiple sensors can be fused by stacking measurement equations
References #
- Kalman, R. E. (1960). A New Approach to Linear Filtering and Prediction Problems. Journal of Basic Engineering.
- Thrun, S., Burgard, W., & Fox, D. Probabilistic Robotics — Sebastian Thrun et al. [Amazon]
- Gelb, A. (ed.) Applied Optimal Estimation — Arthur Gelb [Amazon]
What to remember #
- The Kalman Filter estimates hidden states from noisy measurements
- It combines prediction (from dynamics) with correction (from measurements)
- The Kalman gain optimally weights each source of information
- Variance reduction of 80%+ is typical when measurement noise dominates
- For linear systems with Gaussian noise, the Kalman Filter is optimal