Kalman Filter

calendarDec 12, 2024 · 2 min read · filters kalman state-estimation sensor-fusion robotics  ·
Share on: linkedincopy

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.

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

Applications

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

Further Reading

Related Snippets