Kalman Filter

🔥 Vibe Prompt

"Track a vehicle's position and velocity from noisy GPS (σ=30m) and IMU (σ=20m). Compare raw vs filtered."

import numpy as np

class KalmanFilter:
    def __init__(self):
        self.x = np.array([[0], [0], [0], [0]])  # pos_x, pos_y, vel_x, vel_y
        self.P = np.eye(4) * 100
        self.F = np.array([[1,0,1,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]])
        self.H = np.array([[1,0,0,0],[0,1,0,0]])
        self.R = np.eye(2) * 30  # GPS noise
        self.Q = np.eye(4) * 0.1
    
    def predict(self):
        self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q
    
    def update(self, z):
        y = z - self.H @ self.x
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        self.P = (np.eye(4) - K @ self.H) @ self.P

kf = KalmanFilter()
true_pos = np.array([100, 100])
true_vel = np.array([5, 3])

for t in range(20):
    true_pos = true_pos + true_vel  # True movement
    gps = true_pos + np.random.normal(0, 30, 2)  # Noisy GPS
    
    kf.predict()
    kf.update(gps)
    
    error_raw = np.linalg.norm(gps - true_pos)
    error_filt = np.linalg.norm(kf.x[:2].flatten() - true_pos)
    
    print(f"t={t}: raw_error={error_raw:.1f}, filt_error={error_filt:.1f}")

Summary

| Aspect | Detail | |--------|--------| | State | What we're tracking (position, velocity, etc.) | | Predict | Motion model (F) + process noise (Q) | | Update | Measurement (z) + measurement noise (R) | | Kalman Gain (K) | Balances trust between prediction and measurement | | Extensions | EKF (non-linear), UKF (unscented), particle filter (non-Gaussian) |

Applications

| Domain | Application | |--------|-------------| | 🛰️ GPS/IMU | Drone, self-driving car, smartphone position | | 🚀 Aerospace | Rocket guidance, satellite orbit determination | | 🤖 Robotics | SLAM (Simultaneous Localization And Mapping) | | 📈 Finance | Hidden Markov models for regime detection | | 🫀 Medical | ECG denoising, blood glucose monitoring |

Gradient Descent Course Complete! 🎉

• ✅ Gradient Descent Basics
• ✅ Momentum & Adam
• ✅ FFT / DFT
• ✅ PID Control
• ✅ Kalmanum & Adam\n- ✅ FFT / DFT\n- ✅ PID Control\n- ✅ Kalman Filter"}]}

Key Points

• Understand the core concepts thoroughly
• Practice with hands-on code examples
• Apply knowledge to real-world problems
• Review and reinforce through exercises

Further Learning

• Official documentation
• Open source projects on GitHub
• Community forums and discussions
• Related courses and tutorials

What Is a Kalman Filter?

The Kalman filter estimates the state of a dynamic system from noisy measurements. It's widely used in navigation, tracking, and control systems.

The Two Steps

PREDICT: Estimate next state from current state
    ↓
UPDATE: Correct estimate using new measurement
    ↓
(repeat)

Why Not Just Use the Measurement?

| Issue | Raw Measurement | Kalman Filter | |-------|----------------|---------------| | Noise | Every measurement has error | Filters out noise | | Dropouts | Sensor may miss readings | Predicts during gaps | | Lag | Measurement is outdated | Fuses prediction + measurement | | Multiple sources | Hard to combine | Naturally fuses multi-sensor |

The Kalman Filter Equations

Predict Step

$$\hat{x}{k|k-1} = F \hat{x}{k-1|k-1} + B u_k$$ $$P_{k|k-1} = F P_{k-1|k-1} F^T + Q$$

Update Step

$$K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}$$ $$\hat{x}{k|k} = \hat{x}{k|k-1} + K_k (z_k - H \hat{x}{k|k-1})$$ $$P{k|k} = (I - K_k H) P_{k|k-1}$$

Variable Meanings

| Variable | Name | Meaning | |----------|------|---------| | $\hat{x}$ | State estimate | Position, velocity, etc. | | $F$ | State transition | How state evolves over time | | $P$ | Covariance | Uncertainty in the estimate | | $Q$ | Process noise | How much the system random-walks | | $R$ | Measurement noise | How noisy the sensor is | | $K$ | Kalman gain | How much to trust the measurement | | $z$ | Measurement | What the sensor reads | | $H$ | Measurement function | Maps state to measurement space |

Python Implementation

import numpy as np

class KalmanFilter:
    def __init__(self, dt=0.1):
        # State: [position, velocity]
        self.x = np.array([[0], [0]])  # Initial state
        
        # State transition matrix (constant velocity model)
        self.F = np.array([[1, dt], [0, 1]])
        
        # Measurement matrix (we measure position only)
        self.H = np.array([[1, 0]])
        
        # Covariance matrices
        self.P = np.eye(2) * 100   # High initial uncertainty
        self.Q = np.eye(2) * 0.01  # Low process noise
        self.R = np.array([[1]])    # Measurement noise
    
    def predict(self):
        self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q
        return self.x
    
    def update(self, z):
        # Innovation
        y = z - self.H @ self.x
        
        # Innovation covariance
        S = self.H @ self.P @ self.H.T + self.R
        
        # Kalman gain
        K = self.P @ self.H.T @ np.linalg.inv(S)
        
        # Update state
        self.x = self.x + K @ y
        
        # Update covariance
        self.P = (np.eye(2) - K @ self.H) @ self.P
        
        return self.x

# Example: Tracking a moving object
kf = KalmanFilter(dt=0.1)

# Simulate noisy measurements
true_position = 0
measurements = []
for t in range(50):
    true_position += 0.5  # Moving at 5 units/sec
    noisy_measurement = true_position + np.random.randn() * 2
    measurements.append(noisy_measurement)
    
    kf.predict()
    kf.update(np.array([[noisy_measurement]]))
    
    if t % 10 == 0:
        print(f"Time {t}: True={true_position:.1f}, Measured={noisy_measurement:.1f}, Filtered={kf.x[0,0]:.1f}")

Applications

| Domain | Use Case | What It Tracks | |--------|----------|---------------| | GPS | Car navigation | Position + velocity | | Robotics | Robot arm control | Joint angles + velocities | | Finance | Portfolio tracking | Asset values + trends | | Weather | Temperature forecasting | Temp + pressure + wind | | Audio | Noise cancellation | Signal + noise components |

Summary

The Kalman filter optimally estimates system state from noisy measurements by fusing predictions with sensor data.

Key takeaways: | Kalman filter: predict (model) → update (measurement) → repeat | | Predict: estimate next state using system model (F matrix) | | Update: correct estimate using measurement (z) weighted by Kalman gain (K) | | Kalman gain: trust measurement when noise (R) is low, trust prediction when R is high | | Covariance (P): tracks uncertainty — increases during predict, decreases during update | | Q (process noise): how much the system can change unpredictably | | R (measurement noise): how noisy the sensor readings are | | Applications: GPS, robotics, finance, weather, audio |

Next Chapter: Linear Regression

The next chapter covers linear regression as a gradient descent application.