UAV Systems Engineering · M3-L1 · Guided lab

Implement a Kalman Filter in Python

Write predict and update yourself, then prove your filter is consistent.

Sensor Fusion and State Estimation
Established engineering
About 60 minutes

What you will do

Fill in the TODOs of the starter, run the checks, then explore what happens when $Q$ and $R$ are wrong. The reference solution is below the starter: try first.

Before you start

Lesson M3-T4 (Kalman filter) and M3-T14 (NIS).
Python 3 with NumPy.

Starter

import numpy as np
rng = np.random.default_rng(0)
dt, n = 0.1, 400
F = np.array([[1, dt], [0, 1]]); H = np.array([[1.0, 0.0]])
q, r_std = 0.5, 2.0
Q = q * np.array([[dt**3/3, dt**2/2], [dt**2/2, dt]]); R = np.array([[r_std**2]])

# simulate truth and measurements
Lq = np.linalg.cholesky(Q); x = np.array([0.0, 5.0]); truth = []
for _ in range(n):
    x = F @ x + Lq @ rng.standard_normal(2); truth.append(x.copy())
truth = np.array(truth); z = truth[:, 0] + r_std * rng.standard_normal(n)

def kf_step(xh, P, z):
    # TODO 1: predict
    # TODO 2: innovation y and its covariance S
    # TODO 3: Kalman gain K
    # TODO 4: update state and covariance (Joseph form)
    raise NotImplementedError
    return xh, P, y, S

Starter. Return the updated state, covariance, the innovation and its covariance.

Reference solution

Show the solution (try first)

import numpy as np
rng = np.random.default_rng(0)
dt, n = 0.1, 400
F = np.array([[1, dt], [0, 1]]); H = np.array([[1.0, 0.0]])
q, r_std = 0.5, 2.0
Q = q * np.array([[dt**3/3, dt**2/2], [dt**2/2, dt]]); R = np.array([[r_std**2]])
Lq = np.linalg.cholesky(Q); x = np.array([0.0, 5.0]); truth = []
for _ in range(n):
    x = F @ x + Lq @ rng.standard_normal(2); truth.append(x.copy())
truth = np.array(truth); z = truth[:, 0] + r_std * rng.standard_normal(n)

def kf_step(xh, P, z):
    xh = F @ xh; P = F @ P @ F.T + Q                                # predict
    y = np.array([z]) - H @ xh; S = H @ P @ H.T + R                  # innovation and covariance
    K = P @ H.T @ np.linalg.inv(S)                                   # gain
    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
    return xh, P, y, S

xh, P = np.array([0.0, 0.0]), np.diag([25.0, 25.0]); est = []; nis = []; nees = []
for k in range(n):
    xh, P, y, S = kf_step(xh, P, z[k])
    est.append(xh.copy()); nis.append(float(y @ np.linalg.inv(S) @ y))
    if k > 50: e = xh - truth[k]; nees.append(float(e @ np.linalg.inv(P) @ e))
est = np.array(est)
rms_f = np.sqrt(np.mean((est[50:, 0] - truth[50:, 0]) ** 2)); rms_z = np.sqrt(np.mean((z[50:] - truth[50:, 0]) ** 2))
print(f"RMS: raw measurement {rms_z:.2f} m, filter {rms_f:.2f} m | mean NIS {np.mean(nis[50:]):.2f} (1 expected), mean NEES {np.mean(nees):.2f} (2 expected)")
assert rms_f < rms_z and 0.7 < np.mean(nis[50:]) < 1.4 and 1.0 < np.mean(nees) < 3.2

Solution with consistency checks.

Experiments

Set the filter’s $Q$ to 0.01× and 100× the true value (change Q only in the filter, not in the simulation). Report RMS, mean NIS and NEES for each.
Set the filter’s $R$ to 0.1× and 10× the truth. Which direction gives overconfidence and how do the NIS and NEES show it?
Add a measurement outlier every 50 steps (+15 m) and add an NIS gate at the 99% chi-square value 6.63. How does RMS change?
Reduce the measurement rate by 10× (update only every 10th step) and plot the covariance sawtooth.

How to know it worked

The solution runs and its assertions pass.
You have a table of RMS, NIS and NEES for each mistuning.
You can explain each mistuning from the statistics alone.

If it does not work

Troubleshooting
Symptom	Likely cause	What to check
NEES far above 2	$Q$ or $R$ too small, or model error	Check units and that the simulation noise matches the filter model
LinAlgError	Covariance not positive definite	Use the Joseph form and check $Q$ is symmetric positive semi-definite
Estimate lags	$Q$ too small for the real accelerations	Increase $Q$ and watch NIS

References

T. D. Barfoot, State Estimation for Robotics (author’s page)
$Y$ . Bar-Shalom et al., Estimation with Applications to Tracking and Navigation, Wiley, 2001.

Record your progress

Saved in this browser. This lab runs on your own machine, so the site cannot check it for you.