Kalman Filter for Noisy Sensor Data
Goal: Implement a 1D and 2D Kalman filter from scratch in Python. Track a moving object from noisy position measurements. Visualize how the filter outperforms raw measurements.
Prerequisites: Kalman Filter, State Space Representation, Sensor Fusion
The Problem
A GPS gives noisy position readings (±5 meters). We want a smooth, accurate position estimate. The Kalman filter combines a motion model (“the object moves at roughly constant velocity”) with measurements to produce an optimal estimate.
True position: ─────────────────── smooth line
Measurements: ○ ○ ○ ○ ○ ○ noisy dots
Kalman output: ── ── ── ── ── ── smooth, tracks truth closely
Part 1: 1D Kalman Filter (Position Only)
Step 1: The Simplest Case
State: position x. Model: position doesn’t change much (x_next ≈ x). Measurement: noisy position z.
import numpy as np
import matplotlib.pyplot as plt
np.random.seed(42)
# True position (constant at 5.0)
true_pos = 5.0
n = 50
# Noisy measurements
R = 4.0 # measurement noise variance (std=2)
measurements = true_pos + np.random.randn(n) * np.sqrt(R)
# Kalman filter
x = 0.0 # initial estimate (wrong on purpose)
P = 10.0 # initial uncertainty (high = "I don't know")
Q = 0.01 # process noise (how much we expect position to change)
estimates = []
uncertainties = []
for z in measurements:
# Predict (constant model: x doesn't change)
P_pred = P + Q
# Update
K = P_pred / (P_pred + R) # Kalman gain
x = x + K * (z - x) # correct estimate
P = (1 - K) * P_pred # correct uncertainty
estimates.append(x)
uncertainties.append(P)
plt.figure(figsize=(12, 5))
plt.plot(measurements, 'x', alpha=0.5, label='Measurements (noisy)')
plt.plot(estimates, '-', linewidth=2, label='Kalman estimate')
plt.axhline(y=true_pos, color='r', linestyle='--', label='True position')
plt.fill_between(range(n),
[e - 2*np.sqrt(u) for e, u in zip(estimates, uncertainties)],
[e + 2*np.sqrt(u) for e, u in zip(estimates, uncertainties)],
alpha=0.2, label='2σ uncertainty')
plt.xlabel('Step'); plt.ylabel('Position (m)'); plt.legend(); plt.grid()
plt.title('1D Kalman Filter: static position estimation')
plt.show()What to observe
- The estimate starts at 0 (wrong) and quickly converges to ~5.0
- The uncertainty (shaded band) starts wide and narrows rapidly
- After convergence, the Kalman estimate is much less noisy than raw measurements
Part 2: 2D Kalman Filter (Position + Velocity)
Now track a moving object. State: [position, velocity]. We measure position but also want to estimate velocity.
Step 2: Define the Model
dt = 1.0 # sample period (1 Hz GPS)
# State transition: x = [pos, vel]
# pos_new = pos + vel * dt
# vel_new = vel (constant velocity model)
A = np.array([[1, dt],
[0, 1]])
# Measurement matrix: we only measure position
C = np.array([[1, 0]])
# Process noise: accounts for acceleration we don't model
q = 0.1 # process noise parameter
Q = q * np.array([[dt**3/3, dt**2/2],
[dt**2/2, dt ]])
# Measurement noise
R = np.array([[25.0]]) # std = 5 metersStep 3: Generate Truth and Measurements
n = 100
true_pos = np.zeros(n)
true_vel = np.zeros(n)
# True motion: constant velocity with a turn at t=50
for i in range(1, n):
if i < 50:
true_vel[i] = 2.0 # 2 m/s
else:
true_vel[i] = -1.0 # reverse direction
true_pos[i] = true_pos[i-1] + true_vel[i] * dt
# Noisy measurements
measurements = true_pos + np.random.randn(n) * 5.0Step 4: Run the Filter
# Initialize
x = np.array([[0], # estimated position
[0]]) # estimated velocity
P = np.eye(2) * 100 # high initial uncertainty
est_pos = []
est_vel = []
for i in range(n):
z = np.array([[measurements[i]]])
# ── Predict ──
x = A @ x # state prediction
P = A @ P @ A.T + Q # covariance prediction
# ── Update ──
S = C @ P @ C.T + R # innovation covariance
K = P @ C.T @ np.linalg.inv(S) # Kalman gain
y = z - C @ x # innovation (measurement residual)
x = x + K @ y # state correction
P = (np.eye(2) - K @ C) @ P # covariance correction
est_pos.append(x[0, 0])
est_vel.append(x[1, 0])Step 5: Plot Results
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 8))
ax1.plot(true_pos, 'g-', linewidth=2, label='True position')
ax1.plot(measurements, 'rx', alpha=0.3, markersize=3, label='GPS measurements')
ax1.plot(est_pos, 'b-', linewidth=2, label='Kalman estimate')
ax1.set_ylabel('Position (m)'); ax1.legend(); ax1.grid()
ax1.set_title('Kalman Filter: position tracking')
ax2.plot(true_vel, 'g-', linewidth=2, label='True velocity')
ax2.plot(est_vel, 'b-', linewidth=2, label='Estimated velocity')
ax2.set_xlabel('Time (s)'); ax2.set_ylabel('Velocity (m/s)')
ax2.legend(); ax2.grid()
ax2.set_title('Kalman Filter: velocity estimation (never measured directly!)')
plt.tight_layout(); plt.show()What to observe
- Position estimate is much smoother than raw GPS
- Velocity is estimated even though we never measured it — the filter infers it from position changes
- At t=50 when velocity changes, the filter adapts (with some lag)
Step 6: Tuning Q and R
fig, axes = plt.subplots(1, 3, figsize=(15, 4))
for ax, q_val, title in zip(axes,
[0.001, 0.1, 10.0],
['Q=0.001 (trust model)', 'Q=0.1 (balanced)', 'Q=10 (trust measurements)']):
Q_test = q_val * np.array([[dt**3/3, dt**2/2], [dt**2/2, dt]])
x = np.array([[0], [0]])
P = np.eye(2) * 100
est = []
for i in range(n):
z = np.array([[measurements[i]]])
x = A @ x; P = A @ P @ A.T + Q_test
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
est.append(x[0, 0])
ax.plot(true_pos, 'g-', linewidth=2)
ax.plot(measurements, 'rx', alpha=0.2, markersize=2)
ax.plot(est, 'b-', linewidth=2)
ax.set_title(title); ax.grid()
plt.tight_layout(); plt.show()- Small Q: trusts the model, smooth but slow to react to velocity change
- Large Q: trusts measurements, responsive but noisy
- Just right: balances smoothness and responsiveness
Exercises
-
Add acceleration: Extend the state to
[position, velocity, acceleration](3×3 matrices). Generate data with varying acceleration and compare 2D vs 3D filter. -
Missing measurements: Simulate GPS dropouts (skip the update step for random intervals). The filter should coast on the model prediction. Plot the uncertainty growing during dropouts.
-
Two sensors: Add a second sensor (e.g., a speedometer measuring velocity with different noise). Extend C to
[[1, 0], [0, 1]]and R todiag([25, 1]). Compare with single-sensor results. -
IMU fusion: Implement a complementary filter and Kalman filter for combining accelerometer + gyroscope data. Compare smoothness and drift rejection.
Next: 12 - Simulate a 2-Link Robot Arm — forward and inverse kinematics with PID joint control.