Kalman Filter Equations Cheat Sheet¶
The Five Core Equations¶
Prediction (Time Update)¶
1. State Prediction
2. Covariance Prediction
Update (Measurement Update)¶
3. Kalman Gain
4. State Update
5. Covariance Update
Variables¶
| Symbol | Name | Dimensions | Description |
|---|---|---|---|
| x̂ | State estimate | n×1 | Best guess of system state |
| P | Covariance | n×n | Uncertainty in state estimate |
| F | State transition | n×n | How state evolves |
| H | Measurement matrix | m×n | Maps state to measurements |
| Q | Process noise | n×n | Model uncertainty |
| R | Measurement noise | m×m | Sensor uncertainty |
| K | Kalman gain | n×m | Optimal weighting |
| z | Measurement | m×1 | Sensor reading |
| I | Identity | n×n | Identity matrix |
Python Template¶
Run from this course directory to reuse the shape-checked implementation in kalman_6d.py, without importing plotting dependencies:
import numpy as np
from kalman_6d import KalmanFilter
kf = KalmanFilter(F=[[1,1],[0,1]], H=[[1,0]], Q=np.zeros((2,2)),
R=[[2]], x0=[[0],[1]], P0=np.eye(2))
kf.predict()
kf.update([3]) # explicit flat-to-column conversion
print(kf.x[:, 0]) # [2.0, 1.5]
print(kf.P) # [[1.0, .5], [.5, .75]]
Common Models¶
Constant Velocity (1D)¶
dt = 0.1
F = np.array([[1, dt],
[0, 1 ]])
H = np.array([[1, 0]])
Q = q * np.array([[dt**4/4, dt**3/2],
[dt**3/2, dt**2 ]])
R = np.array([[sigma_position**2]]) # sigma_position is in metres
Constant Velocity (2D)¶
F = np.array([[1, 0, dt, 0 ],
[0, 1, 0, dt],
[0, 0, 1, 0 ],
[0, 0, 0, 1 ]])
H = np.array([[1, 0, 0, 0],
[0, 1, 0, 0]])
Constant Acceleration (1D)¶
Quick Reference¶
When to Use What¶
| Scenario | Model | State |
|---|---|---|
| Stationary object | Constant position | [x] |
| Moving at steady speed | Constant velocity | [x, v] |
| Accelerating object | Constant acceleration | [x, v, a] |
| 2D tracking | 2D constant velocity | [x, y, vx, vy] |
| 3D tracking | 3D constant velocity | [x, y, z, vx, vy, vz] |
Tuning Guidelines¶
Process Noise (Q):
- Too small → Filter ignores measurements, slow to adapt
- Too large → Filter jumps around, noisy estimates
- Derive Q(dt) from a named motion-noise model and units; see lesson 12.
Measurement Noise (R): - Should match actual sensor noise - Measure from sensor datasheet or experiments - R = σ² where σ is standard deviation
Initial Covariance (P₀): - Represents initial uncertainty - Large values → Trust first measurements more - Set each variance and cross-covariance from initial information; position and velocity entries have different units.
Common Mistakes¶
❌ Wrong: P = F @ P @ F + Q
✅ Right: P = F @ P @ F.T + Q
For one scalar measurement, dividing by scalar S is valid. For a matrix innovation covariance, elementwise division is wrong; use K = np.linalg.solve(S.T, (P @ H.T).T).T.
Match the implementation's shape contract: the scalar class accepts 5.0; the general class accepts [5.0] or [[5.0]]. A flat two-element vector and a (2,1) column must not broadcast into a (2,2) residual.
Diagnostics¶
Check Filter Health¶
# General m-dimensional innovation: compare NIS to a declared chi-square gate.
innovation = z - H @ x_pred # z and x_pred are explicit column vectors
nis = float((innovation.T @ np.linalg.solve(S, innovation)).item())
# For m=1, a threshold of 9 is the squared three-standard-deviation gate.
# For m>1 choose the corresponding chi-square quantile, not per-entry sqrt(S).
# Symmetry and semidefiniteness, allowing small floating-point roundoff.
tolerance = 1e-12 * max(1., np.abs(P).max())
if not np.allclose(P, P.T, rtol=0, atol=tolerance):
print("Inspect covariance asymmetry")
if np.linalg.eigvalsh((P + P.T) / 2).min() < -tolerance:
print("Inspect negative covariance eigenvalue")
# There is no universal [0,1] elementwise constraint on a matrix Kalman gain.
Extended Kalman Filter (EKF)¶
For non-linear systems:
def predict_ekf(x, P, f, F_jacobian, Q):
"""
f: non-linear state transition function
F_jacobian: Jacobian of f
"""
x_pred = f(x)
F = F_jacobian(x)
P_pred = F @ P @ F.T + Q
return x_pred, P_pred
def update_ekf(x_pred, P_pred, z, h, H_jacobian, R):
"""
h: non-linear measurement function
H_jacobian: Jacobian of h
"""
H = H_jacobian(x_pred)
y = z - h(x_pred)
S = H @ P_pred @ H.T + R
K = np.linalg.solve(S.T, (P_pred @ H.T).T).T
x = x_pred + K @ y
a = np.eye(P_pred.shape[0]) - K @ H
P = a @ P_pred @ a.T + K @ R @ K.T
return x, P
Useful Formulas¶
Innovation Covariance¶
Posterior Covariance (Joseph Form)¶
Mahalanobis Distance¶
Log Likelihood¶
Matrix Dimensions Check¶
For n states and m measurements:
x: n × 1
P: n × n
F: n × n
Q: n × n
H: m × n
R: m × m
K: n × m
z: m × 1
y: m × 1 (innovation)
S: m × m (innovation covariance)
Performance Tips¶
- Use Joseph form for covariance update (more stable)
- Enforce symmetry:
P = (P + P.T) / 2 - Check covariance symmetry and semidefiniteness; a large defect needs diagnosis
- Use Cholesky decomposition instead of matrix inverse when possible
- Use a consistent orientation-error model; quaternion normalization alone does not propagate or reset its covariance
Further Reading¶
- Chapter 8: 1D Kalman Filter Implementation
- Chapter 10: Matrix Form Details
- Chapter 14: Extended Kalman Filter
- Chapter 16: Error State Kalman Filter
- Chapter 20: Implementation Tricks