Kalman Filter

calendar Dec 12, 2024 · 3 min read · filters kalman state-estimation sensor-fusion robotics  ·
Share on: linkedin copy

Visual guide to the Kalman Filter - optimal recursive estimator for linear systems with Gaussian noise.

Intuition

The Kalman Filter combines two sources of information:

  1. Prediction from a model (physics/dynamics)
  2. Measurement from sensors (noisy observations)

It finds the optimal balance between trusting the model vs trusting the sensors.

graph LR
    A[Previous<br/>Estimate] --> B[Predict<br/>Forward]
    B --> C[Predicted<br/>State]
    D[Measurement] --> E[Update<br/>Correction]
    C --> E
    E --> F[Updated<br/>Estimate]
    F --> A
    
    style B fill:#ffcccc
    style E fill:#ccccff
    style F fill:#ccffcc

Two-Step Process

Step 1: Predict (Time Update)

Use physics/model to predict where we'll be next.

Step 2: Update (Measurement Update)

Correct prediction using sensor measurement.

Interactive Simulation: 1D Position Tracking

State Space Model

Prediction:

$$ \hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1} + B_k u_k $$$$ P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k $$

Update:

$$ K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1} $$$$ \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1}) $$$$ P_{k|k} = (I - K_k H_k) P_{k|k-1} $$

Python Implementation

 1import numpy as np
 2
 3class KalmanFilter:
 4    def __init__(self, F, H, Q, R, x0, P0, B=None):
 5        """
 6        F: State transition matrix
 7        H: Observation matrix
 8        Q: Process noise covariance
 9        R: Measurement noise covariance
10        x0: Initial state estimate
11        P0: Initial covariance estimate
12        B: Control input matrix (optional)
13        """
14        self.F = F
15        self.H = H
16        self.Q = Q
17        self.R = R
18        self.x = x0
19        self.P = P0
20        self.B = B
21    
22    def predict(self, u=None):
23        """Prediction step"""
24        self.x = self.F @ self.x
25        if u is not None:
26            self.x += self.B @ u
27        self.P = self.F @ self.P @ self.F.T + self.Q
28    
29    def update(self, z):
30        """Update step with measurement z"""
31        # Innovation
32        y = z - self.H @ self.x
33        
34        # Innovation covariance
35        S = self.H @ self.P @ self.H.T + self.R
36        
37        # Kalman gain
38        K = self.P @ self.H.T @ np.linalg.inv(S)
39        
40        # Update state
41        self.x = self.x + K @ y
42        
43        # Update covariance
44        self.P = (np.eye(len(self.x)) - K @ self.H) @ self.P
45        
46        return self.x
47
48# Example: 1D position tracking
49dt = 0.1
50F = np.array([[1, dt], [0, 1]])  # [position, velocity]
51H = np.array([[1, 0]])  # Measure position only
52Q = np.eye(2) * 0.01  # Process noise
53R = np.array([[1.0]])  # Measurement noise
54
55kf = KalmanFilter(F, H, Q, R, x0=np.array([0, 0]), P0=np.eye(2))
56
57# Process measurements
58for measurement in measurements:
59    kf.predict()
60    estimate = kf.update(measurement)

Applications

  • GPS/IMU sensor fusion
  • Object tracking
  • Robot localization
  • Economic forecasting

Further Reading

Related Snippets