Learning the Theory

What is a Kalman Filter?

#

The Kalman filter is an optimal state estimator for linear dynamic systems with Gaussian noise. It’s used everywhere from GPS navigation to robotics.

The Problem

#

We want to estimate the state of a system (position, velocity, etc.) given:

• Noisy measurements
• Uncertain dynamics
• Prior knowledge

Key Concepts

#

State Space Representation

#

A system is described by:

• State vector x: The true state we want to estimate
• Measurement vector z: What we can observe
• Control input u: Actions we apply

The Two-Step Process

#

1. Predict: Use the system model to predict the next state
2. Update: Incorporate new measurements to correct the prediction

Mathematical Framework

#

State Transition

#

x_k = F * x_{k-1} + B * u_k + w_k

Where:

• F = State transition matrix
• B = Control input matrix
• w_k = Process noise (zero-mean Gaussian)

Measurement Model

#

z_k = H * x_k + v_k

Where:

• H = Measurement matrix
• v_k = Measurement noise (zero-mean Gaussian)

Noise Covariances

#

Q = Process noise covariance
R = Measurement noise covariance

The Kalman Filter Equations

#

Predict Step

#

x̂_k|k-1 = F * 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}
x̂_k|k = x̂_k|k-1 + K_k * (z_k - H * x̂_k|k-1)
P_k|k = (I - K_k * H) * P_k|k-1

Where:

• x̂ = State estimate
• P = Error covariance
• K = Kalman gain

Intuition

#

The Kalman gain K determines how much we trust:

• Our prediction (low K) vs.
• New measurements (high K)

When measurements are noisy (high R), K is small → trust the prediction When process is uncertain (high Q), K is large → trust the measurements

Simple Example: Estimating Position

#

Imagine tracking a car:

• State: [position, velocity]
• Measurement: GPS position (noisy)
• Dynamics: position += velocity * dt

The Kalman filter optimally fuses:

• Physics model (predict where car should be)
• GPS measurement (where we observe it)

Why It’s Optimal

#

For linear systems with Gaussian noise, the Kalman filter is provably optimal - it minimizes the mean squared error of the estimate.

Next Steps

#

Now that we understand the theory, let’s implement it in code!