The Kalman Filter: State Estimation Under Noise

The Problem

You are running a system and you want to know its true state $x_k$. The problem: your sensors produce noisy measurements, and your model of how the state evolves also has uncertainty. The Kalman filter combines both sources of information (the physical model and the measurement) to produce the best possible estimate of the true state.

Variables

Symbol	Meaning
$x_k$, $\hat{x}_k$	True state / predicted state
$P_k$, $\hat{P}_k$	Process covariance matrices
$A$	State transition matrix
$B$	Control input matrix
$C$	Measurement conversion matrix
$D$	Noise input matrix
$H$	Measurement matrix (shapes the calculation)
$K$	Kalman gain
$U_k$	Control input (external variable)
$y_k$	Measured state
$\varepsilon_k$	Process noise
$\theta_k$	Measurement noise
$Q$	Process noise covariance
$R$	Measurement noise covariance

The Two-Phase Loop

The Kalman filter alternates between predict and correct at every time step.

Phase 1: Predict

Use the physical model to project the state forward:

Project the process covariance forward:

This gives a predicted state and predicted uncertainty, both based on the model alone, before seeing the new measurement.

Phase 2: Correct

Take the new measurement:

Compute the Kalman gain, which determines how much to trust the measurement vs the prediction:

Update the state estimate by blending the prediction with the measurement residual $[y_k - H\hat{x}_{k|k-1}]$:

Update the covariance:

Understanding the Kalman Gain

The term $[y_k - H\hat{x}_{k|k-1}]$ is the innovation, the difference between what the sensor measured and what the model predicted. The Kalman gain $K$ controls how much of this innovation to absorb:

• If $R$ is large (sensor is noisy), $K$ is small. Trust the model more, ignore most of the measurement
• If $R$ is small (sensor is accurate), $K$ is large. Trust the measurement more, correct the model aggressively

The covariance update $(I - KH)\hat{P}$ shows the same logic: a larger $K$ (more trust in measurement) shrinks the uncertainty $P_k$ faster.

Why This Matters for Control

The Kalman filter is commonly embedded inside MPC and other control systems when the full state $x$ cannot be measured directly. It provides a clean, statistically optimal estimate of the state at each time step, which the controller then uses to compute the next action.

What You Can Do Now

The code below runs a 1D Kalman filter on a constant signal buried in sensor noise, showing the predict-correct loop and how the estimate converges to the true value.

import numpy as np

np.random.seed(42)
n_steps = 40
true_state = 5.0     # constant true value we are trying to estimate

# Noise parameters
Q = 0.01   # process noise variance (model uncertainty)
R = 2.0    # measurement noise variance (sensor uncertainty)

# Noisy measurements
measurements = true_state + np.random.normal(0, np.sqrt(R), n_steps)

# Initial estimates
x_est = 0.0    # prior state estimate
P = 1.0        # prior uncertainty

print(f"{'Step':>4}  {'Measured':>10}  {'Estimated':>10}  {'Kalman Gain':>12}")
for k, z in enumerate(measurements):
    # --- Predict ---
    x_pred = x_est          # state doesn't change (constant signal model)
    P_pred = P + Q          # uncertainty grows slightly each step

    # --- Correct ---
    K = P_pred / (P_pred + R)           # Kalman gain
    x_est = x_pred + K * (z - x_pred)  # blend prediction with measurement
    P = (1 - K) * P_pred               # update uncertainty

    if k < 5 or k == n_steps - 1:
        print(f"{k+1:>4}  {z:>10.3f}  {x_est:>10.3f}  {K:>12.4f}")

print(f"\nTrue value: {true_state:.3f}, Final estimate: {x_est:.3f}")

Change R to a larger value (noisy sensor) and watch the Kalman gain stay small. The filter leans on the model. Reduce R and the gain grows, trusting measurements more. Setting Q = 0 models a perfectly known system with no drift.