What is the Kalman Filter?
The Kalman Filter is an optimal recursive state estimation algorithm for linear Gaussian systems. It sequentially estimates the internal state of a system from noisy observations. Since its introduction by Rudolf E. Kalman in 1960, it has been widely used in navigation, control, signal processing, economics, and many other fields.
The Kalman Filter serves as the foundation for the following extended methods:
- Extended Kalman Filter (EKF) for nonlinear systems
- Unscented Kalman Filter (UKF) based on the Unscented Transform
- Cubature Kalman Filter (CKF)
- Particle Filter
An overview of these methods is available in Basics of Filtering Methods. This article explains the theory and implementation of the most fundamental Kalman Filter.
State-Space Model
The Kalman Filter assumes the following linear state-space model.
State transition model:
\[ \mathbf{x}_k = A \mathbf{x}_{k-1} + B \mathbf{u}_{k-1} + \mathbf{w}_{k-1}, \quad \mathbf{w}_{k-1} \sim \mathcal{N}(\mathbf{0}, Q) \tag{1} \]Observation model:
\[ \mathbf{z}_k = H \mathbf{x}_k + \mathbf{v}_k, \quad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, R) \tag{2} \]The variables are defined as follows:
| Symbol | Meaning |
|---|---|
| \(\mathbf{x}_k\) | State vector at time \(k\) |
| \(A\) | State transition matrix |
| \(B\) | Control input matrix |
| \(\mathbf{u}_{k-1}\) | Control input |
| \(\mathbf{w}_{k-1}\) | Process noise (covariance \(Q\)) |
| \(\mathbf{z}_k\) | Observation vector |
| \(H\) | Observation matrix |
| \(\mathbf{v}_k\) | Observation noise (covariance \(R\)) |
Algorithm
The Kalman Filter alternates between a prediction step and an update step.
Prediction Step
Predict the current state from the previous estimate.
Predicted state:
\[ \hat{\mathbf{x}}_{k|k-1} = A \hat{\mathbf{x}}_{k-1|k-1} + B \mathbf{u}_{k-1} \tag{3} \]Predicted covariance:
\[ P_{k|k-1} = A P_{k-1|k-1} A^T + Q \tag{4} \]Update Step
Incorporate the new observation to correct the prediction.
Innovation (measurement residual):
\[ \mathbf{y}_k = \mathbf{z}_k - H \hat{\mathbf{x}}_{k|k-1} \tag{5} \]Innovation covariance:
\[ S_k = H P_{k|k-1} H^T + R \tag{6} \]Kalman gain:
\[ K_k = P_{k|k-1} H^T S_k^{-1} \tag{7} \]Updated state:
\[ \hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + K_k \mathbf{y}_k \tag{8} \]Updated covariance:
\[ P_{k|k} = (I - K_k H) P_{k|k-1} \tag{9} \]The Kalman gain \(K_k\) automatically adjusts the balance between the prediction and the observation based on their respective uncertainties. When the observation noise is small (\(R\) is small), the filter trusts the observation more. When the process noise is small (\(Q\) is small), the filter trusts the model prediction more.
Python Implementation
We implement the Kalman Filter using a 1D constant-velocity model.
State-Space Model Definition
The state vector is \(\mathbf{x} = [p, v]^T\) (position, velocity), with the following model:
\[ A = \begin{bmatrix} 1 & \Delta t \\\ 0 & 1 \end{bmatrix}, \quad H = \begin{bmatrix} 1 & 0 \end{bmatrix} \]Only position is observed; velocity is estimated by the filter.
import numpy as np
import matplotlib.pyplot as plt
# ---- Parameter settings ----
dt = 1.0 # time step [s]
# State transition matrix (constant velocity model)
A = np.array([[1, dt],
[0, 1]])
# Observation matrix (position only)
H = np.array([[1, 0]])
# Process noise covariance (derived from acceleration noise)
q = 0.1 # acceleration noise variance
Q = q * np.array([[dt**3 / 3, dt**2 / 2],
[dt**2 / 2, dt]])
# Observation noise covariance
R = np.array([[1.0]])
Kalman Filter Implementation
class KalmanFilter:
"""Linear Kalman Filter"""
def __init__(self, A, H, Q, R, x0, P0):
self.A = A
self.H = H
self.Q = Q
self.R = R
self.x = x0.copy()
self.P = P0.copy()
def predict(self, u=None, B=None):
"""Prediction step (Eq. 3, 4)"""
self.x = self.A @ self.x
if u is not None and B is not None:
self.x += B @ u
self.P = self.A @ self.P @ self.A.T + self.Q
return self.x.copy(), self.P.copy()
def update(self, z):
"""Update step (Eq. 5-9)"""
# Innovation
y = z - self.H @ self.x
S = self.H @ self.P @ self.H.T + self.R
# Kalman gain
K = self.P @ self.H.T @ np.linalg.inv(S)
# State and covariance update
self.x = self.x + K @ y
I = np.eye(len(self.x))
self.P = (I - K @ self.H) @ self.P
return self.x.copy(), self.P.copy()
Simulation
np.random.seed(42)
T = 50 # number of time steps
# True initial state
x_true = np.array([0.0, 1.0]) # position=0, velocity=1
# Kalman Filter initialization
x0 = np.array([0.0, 0.0]) # initial estimate (velocity unknown)
P0 = np.diag([1.0, 1.0]) # initial covariance
kf = KalmanFilter(A, H, Q, R, x0, P0)
# Storage arrays
true_positions = [x_true[0]]
true_velocities = [x_true[1]]
measurements = []
est_positions = [x0[0]]
est_velocities = [x0[1]]
P_history = [P0.copy()]
for k in range(T):
# True state update
x_true = A @ x_true + np.random.multivariate_normal([0, 0], Q)
true_positions.append(x_true[0])
true_velocities.append(x_true[1])
# Generate observation
z = H @ x_true + np.random.multivariate_normal([0], R)
measurements.append(z[0])
# Kalman Filter
kf.predict()
x_est, P_est = kf.update(z)
est_positions.append(x_est[0])
est_velocities.append(x_est[1])
P_history.append(P_est.copy())
true_positions = np.array(true_positions)
true_velocities = np.array(true_velocities)
measurements = np.array(measurements)
est_positions = np.array(est_positions)
est_velocities = np.array(est_velocities)
P_history = np.array(P_history)
# ---- Plot results ----
time = np.arange(T + 1)
sigma_pos = np.sqrt(P_history[:, 0, 0])
fig, axes = plt.subplots(2, 1, figsize=(12, 8), sharex=True)
# Position estimate
axes[0].plot(time, true_positions, "b-", linewidth=1.5, label="True position")
axes[0].scatter(time[1:], measurements, c="gray", s=15, alpha=0.5,
label="Measurements", zorder=3)
axes[0].plot(time, est_positions, "r--", linewidth=1.2, label="KF estimate")
axes[0].fill_between(time,
est_positions - 2 * sigma_pos,
est_positions + 2 * sigma_pos,
color="red", alpha=0.15, label="$\pm 2\sigma$")
axes[0].set_ylabel("Position")
axes[0].set_title("Kalman Filter - 1D Constant Velocity Tracking")
axes[0].legend(loc="upper left", fontsize=9)
axes[0].grid(True, alpha=0.3)
# Velocity estimate
sigma_vel = np.sqrt(P_history[:, 1, 1])
axes[1].plot(time, true_velocities, "b-", linewidth=1.5, label="True velocity")
axes[1].plot(time, est_velocities, "r--", linewidth=1.2, label="KF estimate")
axes[1].fill_between(time,
est_velocities - 2 * sigma_vel,
est_velocities + 2 * sigma_vel,
color="red", alpha=0.15, label="$\pm 2\sigma$")
axes[1].set_xlabel("Time step $k$")
axes[1].set_ylabel("Velocity")
axes[1].legend(loc="upper left", fontsize=9)
axes[1].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig("kalman_filter_result.png", dpi=150)
plt.show()
# RMSE calculation
rmse_pos = np.sqrt(np.mean((true_positions[1:] - est_positions[1:]) ** 2))
rmse_vel = np.sqrt(np.mean((true_velocities[1:] - est_velocities[1:]) ** 2))
print(f"Position RMSE: {rmse_pos:.4f}")
print(f"Velocity RMSE: {rmse_vel:.4f}")
Discussion
The Kalman Filter converges within a few time steps and accurately estimates the true state from noisy observations. The \(\pm 2\sigma\) confidence band covers the true state well, indicating that the filter properly quantifies its estimation uncertainty.
Notably, velocity is not directly observed, yet the filter successfully estimates it indirectly from position observations alone. This is a key strength of the Kalman Filter.
Regarding parameter tuning:
- Increasing \(Q\) (process noise): Reduces trust in the model, causing the filter to rely more on observations. This yields faster response but makes estimates noisier.
- Increasing \(R\) (observation noise): Reduces trust in observations, causing the filter to rely more on model predictions. This produces smoother estimates but delays response to actual changes.
The Kalman Filter is limited to linear Gaussian systems. For nonlinear systems, one can use the EKF (Jacobian-based linearization), the UKF or CKF (sigma-point methods), or Particle Filters (Monte Carlo sampling).
Related Articles
- Extended Kalman Filter (EKF): Theory and Python Implementation - Extends KF to nonlinear systems using Jacobian-based linearization.
- Basics of Filtering Methods in Signal Processing - Overview of Kalman Filter, EKF, UKF, and Particle Filter.
- Unscented Transformation: Python Implementation - Sigma-point approach to handle nonlinear transformations without linearization.
- Cubature Kalman Filter (CKF): Theory and Python Implementation - Cubature Kalman Filter that resolves the weight issues of UKF.
- Particle Filter: Python Implementation with Resampling Comparison - Monte Carlo-based filtering for non-Gaussian distributions.
- RTS Smoother: Theory and Python Implementation - Smooths Kalman Filter results using observations from all time steps.
References
- Welch, G., & Bishop, G. (2006). “An Introduction to the Kalman Filter.” UNC Chapel Hill TR 95-041.
- Thrun, S., Burgard, W., & Fox, D. (2005). “Probabilistic Robotics.” MIT Press.