Kalman filter cheat sheet

This sheet is for anyone writing or debugging a Kalman filter for odometry, IMU fusion or tracking. It lists the three common variants side by side, how to set the noise matrices, and how to tell from the data whether the filter is consistent. Background is in sensors and noise and the state estimation hub. Glossary: Kalman filter, extended Kalman filter, particle filter.

Notation

Symbol	Size	Meaning
x, P	n, n x n	State estimate and its covariance
F, B, u	n x n, n x m, m	Linear motion model and control input
f(x, u)	n	Nonlinear motion model (EKF, UKF)
Q	n x n	Process noise covariance: what the model gets wrong per step
z, H, h(x)	p, p x n, p	Measurement, linear model, nonlinear model
R	p x p	Measurement noise covariance
y, S, K	p, p x p, n x p	Innovation, its covariance, Kalman gain

KF, EKF and UKF side by side

Step	Kalman filter (KF)	Extended KF (EKF)	Unscented KF (UKF)
Model	Linear F, H, Gaussian noise	Nonlinear f, h, linearised at the estimate	Nonlinear f, h, no derivatives
Predict mean	x = F x + B u	x = f(x, u)	Push 2n+1 sigma points through f, take the weighted mean
Predict covariance	P = F P F^T + Q	P = Fj P Fj^T + Q, Fj = df/dx at x	Weighted outer products of the propagated points, plus Q
Predicted measurement	H x	h(x)	Weighted mean of h applied to the sigma points
Innovation	y = z - H x	y = z - h(x)	y = z - z_mean
Innovation covariance	S = H P H^T + R	S = Hj P Hj^T + R, Hj = dh/dx at x	S = weighted spread of h(sigma points), plus R
Gain	K = P H^T S^-1	K = P Hj^T S^-1	K = Pxz S^-1 (Pxz is the state-measurement cross-covariance)
Update	x = x + K y, P = (I - K H) P	Same, with Hj	x = x + K y, P = P - K S K^T
Cost per step	Matrix products, one p x p inverse	KF cost plus two Jacobians	2n+1 calls each to f and h, one matrix square root
Fails when	Model is not linear	Strong nonlinearity within one standard deviation, wrong Jacobian	Very non-Gaussian or multimodal belief (use a particle filter)

Write the covariance update in Joseph form. It keeps P symmetric and positive definite under rounding:

P = (I - K H) P (I - K H)^T + K R K^T

Sigma points for the UKF

The scaled sigma points used in most UKF code (for example MerweScaledSigmaPoints in FilterPy):

lambda   = alpha^2 (n + kappa) - n
X_0      = x
X_i      = x + column_i( sqrt((n + lambda) P) )        i = 1..n
X_(n+i)  = x - column_i( sqrt((n + lambda) P) )
Wm_0     = lambda / (n + lambda)
Wc_0     = lambda / (n + lambda) + (1 - alpha^2 + beta)
Wm_i     = Wc_i = 1 / (2 (n + lambda))                 i = 1..2n
alpha small (FilterPy suggests 1e-3), beta = 2 for Gaussian priors, kappa often 0

Angles need care in the UKF. Average sigma-point headings with a circular mean, and wrap every difference, or a spread around plus or minus pi averages to zero.

Choosing Q and R

Matrix	Where to get it	Check
R	Log the sensor with the robot still and take the variance per channel. Or derive it from the datasheet noise density and the sensor bandwidth	NIS mean near p on real data
Q	Derive it from a noise model. For a constant-velocity state driven by white acceleration of standard deviation sigma_a, use Q = G G^T sigma_a^2 with G = (dt^2 / 2, dt). Pick sigma_a from the accelerations the platform can actually produce	NEES in simulation, NIS on real data
P0	Honest initial uncertainty. Too small and the filter ignores early measurements	NEES over the first steps

The ratio of Q to R sets the behaviour. A large Q trusts measurements and gives a noisy estimate. A small Q trusts the model and lags or diverges when the model is wrong. Scaling Q, R and P0 by the same factor changes P but not the estimate.

Consistency checks

Test	Statistic	Needs	Consistent if
NEES	e^T P^-1 e, with e = truth - estimate	Ground truth (simulation, motion capture)	Average over N runs lies in the chi-square band with N n degrees of freedom, divided by N
NIS	y^T S^-1 y	Only the filter’s own innovations, so it works on real data	Average lies in the chi-square band with N p degrees of freedom, divided by N
Whiteness	Autocorrelation of the innovations	Logged innovations	Near zero at every non-zero lag. A correlated innovation means a missing state or a wrong model

Runnable Kalman filter with NEES and NIS

A constant-velocity target, position measured at 10 Hz. The block runs 100 Monte Carlo runs with the true Q, then with Q 100 times too small and 100 times too large, and reports the average NEES and NIS against their 95 % bands.

import numpy as np
from scipy.stats import chi2

rng = np.random.default_rng(42)
DT, STEPS, RUNS = 0.1, 100, 100
F = np.array([[1.0, DT], [0.0, 1.0]])                 # constant velocity: state (position, velocity)
H = np.array([[1.0, 0.0]])                            # we measure position only
SIGMA_A, SIGMA_Z = 0.5, 0.2                           # accel noise (m/s^2), sensor noise (m)
G = np.array([[0.5 * DT**2], [DT]])
Q_TRUE = G @ G.T * SIGMA_A**2                         # discrete white-noise acceleration model
R = np.array([[SIGMA_Z**2]])


def kf_step(x, P, z, Q):
    x = F @ x                                         # predict
    P = F @ P @ F.T + Q
    y = z - H @ x                                     # innovation
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    x = x + K @ y                                     # update
    I_KH = np.eye(2) - K @ H
    P = I_KH @ P @ I_KH.T + K @ R @ K.T               # Joseph form keeps P symmetric
    return x, P, (y.T @ np.linalg.solve(S, y)).item()


def monte_carlo(q_scale: float):
    nees, nis, sq_err = np.zeros((RUNS, STEPS)), np.zeros((RUNS, STEPS)), np.zeros((RUNS, STEPS))
    for i in range(RUNS):
        truth = np.array([[0.0], [1.0]])
        x, P = np.array([[0.0], [0.0]]), np.diag([1.0, 1.0])
        for k in range(STEPS):
            truth = F @ truth + G * rng.normal(0, SIGMA_A)
            z = H @ truth + rng.normal(0, SIGMA_Z)
            x, P, nis[i, k] = kf_step(x, P, z, Q_TRUE * q_scale)
            e = truth - x
            nees[i, k] = (e.T @ np.linalg.solve(P, e)).item()
            sq_err[i, k] = e[0, 0] ** 2
    return nees.mean(axis=0), nis.mean(axis=0), np.sqrt(sq_err.mean())


nees_band = chi2.ppf([0.025, 0.975], 2 * RUNS) / RUNS   # run-averaged NEES, state dim 2
nis_band = chi2.ppf([0.025, 0.975], 1 * RUNS) / RUNS    # run-averaged NIS, measurement dim 1
print(f"95 % bands: NEES {np.round(nees_band, 2)}, NIS {np.round(nis_band, 2)}")
for q_scale in (0.01, 1.0, 100.0):
    a_nees, a_nis, rmse = monte_carlo(q_scale)
    in_nees = np.mean((a_nees[10:] >= nees_band[0]) & (a_nees[10:] <= nees_band[1]))   # skip 10 warm-up steps
    in_nis = np.mean((a_nis[10:] >= nis_band[0]) & (a_nis[10:] <= nis_band[1]))
    print(f"Q x {q_scale:6.2f}: mean NEES {a_nees.mean():6.2f} ({100 * in_nees:3.0f} % of steps in band), "
          f"mean NIS {a_nis.mean():5.2f} ({100 * in_nis:3.0f} %), position RMSE {rmse:.3f} m")

Output:

95 % bands: NEES [1.63 2.41], NIS [0.74 1.3 ]
Q x   0.01: mean NEES  68.65 (  0 % of steps in band), mean NIS  1.97 ( 28 %), position RMSE 0.201 m
Q x   1.00: mean NEES   2.02 ( 94 % of steps in band), mean NIS  1.02 ( 93 %), position RMSE 0.097 m
Q x 100.00: mean NEES   1.13 (  0 % of steps in band), mean NIS  0.81 ( 77 %), position RMSE 0.132 m

With the true Q, the average NEES sits at 2.02 for a 2-state filter, close to the expected value of 2. A Q that is too small makes the filter overconfident. NEES jumps to about 69 and the position error doubles. A Q that is too large makes it underconfident. NEES drops below the band and the estimate gets noisier. NIS moves the same way, less sharply, and it is the one you can compute on a real robot.

EKF: check the Jacobian and wrap the angles

Most EKF bugs are a wrong Jacobian or an unwrapped angle in the innovation. This block checks a range-bearing Jacobian against central differences and shows the wrapping problem.

import numpy as np

LANDMARK = np.array([4.0, 3.0])                      # known landmark position (m)


def h(x: np.ndarray) -> np.ndarray:
    """Range and bearing from a robot pose (x, y, theta) to the landmark."""
    dx, dy = LANDMARK - x[:2]
    return np.array([np.hypot(dx, dy), np.arctan2(dy, dx) - x[2]])


def h_jacobian(x: np.ndarray) -> np.ndarray:
    dx, dy = LANDMARK - x[:2]
    q = dx**2 + dy**2
    r = np.sqrt(q)
    return np.array([[-dx / r, -dy / r, 0.0],
                     [dy / q, -dx / q, -1.0]])


def numeric_jacobian(f, x: np.ndarray, eps: float = 1e-6) -> np.ndarray:
    cols = []
    for i in range(len(x)):
        d = np.zeros_like(x)
        d[i] = eps
        cols.append((f(x + d) - f(x - d)) / (2 * eps))   # central difference
    return np.array(cols).T


def wrap(a: float) -> float:
    return (a + np.pi) % (2 * np.pi) - np.pi


x = np.array([1.0, 0.5, 0.3])
err = np.abs(h_jacobian(x) - numeric_jacobian(h, x)).max()
print(f"analytic vs numeric Jacobian, max difference: {err:.1e}")

z_pred, z_meas = np.array([5.0, 3.10]), np.array([5.1, -3.12])   # bearing near +pi and -pi
naive = z_meas - z_pred
fixed = np.array([naive[0], wrap(naive[1])])
print("innovation without wrapping:", np.round(naive, 3))
print("innovation with wrapping:   ", np.round(fixed, 3))

Without wrapping, a bearing measured just past minus pi against a prediction just short of plus pi gives an innovation of minus 6.22 rad. The real difference is 0.063 rad. Wrap every angle difference, in the innovation and in the state update.

Pitfalls

Use np.linalg.solve(S, y) rather than forming S^-1 when you only need S^-1 y.
Process the measurements in timestamp order. Late measurements need a buffer and a re-run, not an update with stale data.
A filter that stays consistent only after you inflate Q by large factors usually has a modelling error: a missing bias state, a wrong frame, or a unit mismatch between degrees and radians.
Gate outliers with the NIS: reject a measurement whose y^T S^-1 y exceeds a chi-square threshold for p degrees of freedom.
For orientation, estimate a small error angle and keep the full attitude as a quaternion (an error-state filter). See rotation representations.