M3-T4: Kalman Filter

Learning objectives

After this lesson, you should be able to:

• Write the predict and update equations of the Kalman filter and say what each term means.
• Derive the Kalman gain as the minimiser of posterior variance in the scalar case.
• Explain how $Q$ and $R$ set the trade-off between model and measurement.
• Implement a constant-velocity filter and check its consistency.

Conceptual explanation

The Kalman filter keeps a Gaussian belief over the state: a mean $\hat{x}$ and a covariance $P$. It alternates two steps. Predict: push the belief through the motion model; uncertainty grows because the model is imperfect (process noise $Q$). Update: when a measurement arrives, blend it with the prediction; uncertainty shrinks. The blend weight is the Kalman gain $K$.

For linear models with Gaussian noise, it is the exact Bayesian posterior and the minimum-variance linear estimator. For non-linear ones (lesson M3-T5) it is an approximation.

Engineering intuition

In one dimension, the update is a weighted average of the prediction and the measurement, weighted by the inverse of their variances: whichever is more certain gets more weight. $K=P$⁻/($P$⁻ + $R$) is between 0 and 1: near 0 when the measurement is noisy ($R$ large) and near 1 when the prediction is poor ($P$⁻ large). Tuning $Q$ is therefore a statement about how much you trust your model: a large $Q$ makes the filter nervous and responsive, a small $Q$ makes it smooth and slow to react to real changes.

Mathematical formulation

Model: $x_{k}=F\,x_{k-1}+B\,u_{k}+w_{k}$, $w$ ~ $N$(0, $Q$); measurement $z_{k}=H\,x_{k}+v_{k}$, $v$ ~ $N$(0, $R$).

$x$, $P$

state estimate and its covariance.

$F$, $B$, $H$

state-transition, input and measurement matrices.

$Q$, $R$

process and measurement noise covariances.

$y$, $S$

innovation (residual) and its covariance: used for gating and consistency (lessons M3-T13, M3-T14).

$K$

Kalman gain.

Scalar derivation

Combine a prior $N$($x$⁻, $P$⁻) and a measurement $z=x+v$ with variance $R$. The posterior mean $x$⁻ + $K$($z$ − $x$⁻) has variance $(1-K)^{2}\,P$⁻ + $K^{2}\,R$. Setting the derivative with respect to $K$ to zero:

The posterior is always more certain than either source. The Joseph form shown above is algebraically equal to ($I$ − KH)$P$⁻ but keeps $P$ symmetric positive definite under rounding error, so use it in real code.

Constant-velocity model

Architecture and data flow

Sensor sample → timestamp and validity check → predict to the measurement time (never to “now”) → update → publish state and covariance. Getting the time of the prediction right is the most common implementation detail missed in practice: a measurement that is 80 ms old must be applied to the state as it was 80 ms ago, or an out-of-sequence strategy must be used.

Real-world UAV applications

• GNSS-aided position and velocity tracking.
• Altitude estimation from barometer and accelerometer.
• Target tracking for follow-me and landing.
• The core of the EKF inside PX4 and of every VIO and LIO system.

Implementation example

This runs as shown: a constant-velocity filter tracks a vehicle from noisy position measurements; it checks accuracy and that the filter’s covariance matches the actual error (statistical consistency).

import numpy as np
rng = np.random.default_rng(3)
dt, n = 0.1, 400
F = np.array([[1, dt], [0, 1]]); H = np.array([[1.0, 0.0]])
q = 0.5                                            # white acceleration spectral density
Q = q * np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]])
R = np.array([[2.0**2]])                           # 2 m measurement sigma

# truth: accelerations are white noise with the SAME spectral density, so the model is correct
x_true = np.zeros((n, 2)); x = np.array([0.0, 5.0])
L = np.linalg.cholesky(Q)
for k in range(n):
    x = F @ x + L @ rng.standard_normal(2); x_true[k] = x
z = x_true[:, 0] + 2.0 * rng.standard_normal(n)

xh = np.array([0.0, 0.0]); P = np.diag([25.0, 25.0])
nees, err = [], []
for k in range(n):
    xh = F @ xh; P = F @ P @ F.T + Q                       # predict
    y = z[k] - H @ xh; S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    xh = xh + (K @ y).ravel()
    I_KH = np.eye(2) - K @ H
    P = I_KH @ P @ I_KH.T + K @ R @ K.T                      # Joseph form
    e = xh - x_true[k]
    if k > 50: nees.append(e @ np.linalg.inv(P) @ e); err.append(e)
rms = np.sqrt(np.mean(np.array(err)[:, 0] ** 2))
print(f"position RMS error {rms:.2f} m (measurement sigma 2.0), mean NEES {np.mean(nees):.2f} (expected 2)")
assert rms < 2.0                                         # filtered estimate beats the raw measurement
assert 1.0 < np.mean(nees) < 3.2                         # consistent within sampling variability: 2 degrees of freedom

Common engineering mistakes

Failure modes

Debugging approach

1. Plot the innovation $y$ and ±$2\sqrt{S}$ bounds: about 95% of samples should fall inside and the sequence should look white.
2. Compute NIS and NEES (simulation) and compare to their expected chi-square means.
3. Verify the filter against a simulated truth where $Q$ and $R$ are known exactly, before trying real data.
4. Check the sign and units of every matrix with a unit test of predict-only behaviour.

Practical exercises

1. Derive the scalar gain and show posterior variance is below both prior variance and $R$.
2. Change $q$ in the example to 5 and then 0.05 while keeping the true $q$ at 0.5. Report NEES in both cases.
3. Add acceleration to the state (three-state model) and compare the lag on a step in acceleration.
4. Show that the filter reduces to a running average when $F=1$, $Q=0$ and $R$ is constant.

Interview questions

Write an answer in your own words, then reveal the model answer. Nothing is sent anywhere.

References and further reading

Primary papers, official documentation and standards. Each claim above that is specific to a tool or version points here.

• R. E. Kalman, “A new approach to linear filtering and prediction problems”, J. Basic Engineering, 1960
• $T$. $D$. Barfoot, State Estimation for Robotics, Cambridge University Press, 2017, chapter 3.
• $Y$. Bar-Shalom, $X$. $R$. Li, $T$. Kirubarajan, Estimation with Applications to Tracking and Navigation, Wiley, 2001.

What’s next