Tuning and limits

The equations of the Kalman filter are exact, but they are exact about a model that you wrote down. If the noise numbers or the motion model are wrong, the filter does not warn you. It keeps producing confident estimates, smoothly and politely, that happen to be off. This final lesson is about the two tuning knobs, Q and R, about how to check whether your filter is honest, about what to do when the world is not linear, and about how all of this appears inside a real flight controller.

What Q and R actually mean

R is the measurement noise variance: the square of the standard deviation of your sensor error. It is the easier one, because you can measure it. Log the sensor while the robot is still, compute the standard deviation as in lesson 1, and square it.

Q is the process noise: how much you distrust your motion model between steps. It is harder, because there is no instrument that reads it. It is not a physical noise at all but a confession of everything the model leaves out: unmodelled acceleration, wheel slip, a slope, a payload change.

Only the ratio matters. Multiply Q and R by the same factor and the gain K, and therefore the estimate, does not change. A high Q / R ratio means "trust the sensor, my model is shaky": the gain is high and the output is responsive but noisy. A low ratio means "trust the model, the sensor is bad": the output is smooth but slow to react to real changes.

What happens when they are wrong

We use the scalar filter from lesson 3 on a hard case: the true value is not constant but follows a slow sine wave around 10 with an amplitude of 5. The sensor noise has a true standard deviation of 2, so the correct R is 4, and we keep that fixed. The filter model says the value stays put between steps, which is wrong, so Q has to absorb the difference. We sweep Q across six orders of magnitude and measure the RMS error against the truth.

tune_q.py

ResetRun

import math import random random.seed(8) N = 400 truth = [10 + 5 * math.sin(2 * math.pi * k / 200) for k in range(N)] meas = [t + random.gauss(0, 2.0) for t in truth] # real noise std 2, so R = 4 def run(Q, R): x, P = meas[0], 4.0 sq = 0.0 for k in range(1, N): P = P + Q # predict: model says the value stays put K = P / (P + R) x = x + K * (meas[k] - x) P = (1 - K) * P sq += (x - truth[k]) ** 2 return math.sqrt(sq / (N - 1)), K print("raw measurement RMS error: %.2f" % math.sqrt(sum((m - t) ** 2 for m, t in zip(meas, truth)) / N)) print(" Q gain K RMS error") for Q in (0.0001, 0.01, 0.1, 1.0, 100.0): err, K = run(Q, 4.0) print("%8g %.3f %.2f" % (Q, K, err))

The raw measurements have an RMS error of about 2.00. Now read the table from the top. With Q = 0.0001 the steady gain is about 0.005 and the error is about 3.48, worse than not filtering at all: the filter is so sure that the value is constant that it ignores the sensor while the truth walks away from it. With Q = 0.01 the error is about 1.72, with Q = 0.1 it reaches the best value, about 0.82, and with Q = 1 it rises to about 0.98. With Q = 100 the gain is about 0.96, the filter basically copies each measurement, and the error is about 1.92, nearly the raw error. The curve is a U: too little Q gives lag, too much gives noise, and the sweet spot sits between them.

The innovation check

How do you know you are honest without ground truth? Use the innovation, the difference between the measurement and the prediction nu = z - x_pred. The filter predicts how large this should be: its variance is S = P_pred + R. If the model is consistent, the innovations are zero-mean, uncorrelated and have the variance S. The normalised innovation squared, NIS = nu^2 / S, should then average about 1 for a single measurement. A value far above 1 means the filter is overconfident, because surprises are larger than it expected. A value far below 1 means it is too cautious.

The same number gives a gate for bad data. A normalised innovation above 9 is a three-sigma surprise, which happens by chance only about once in 370 readings. In the test below we inject one faulty reading of +25 and check three settings of R against the true value of 4.

innovation_check.py

ResetRun

import math import random random.seed(21) N, FAULT = 300, 150 truth = [10 + 5 * math.sin(2 * math.pi * k / 200) for k in range(N)] meas = [t + random.gauss(0, 2.0) for t in truth] # real noise std 2 meas[FAULT] += 25.0 # one faulty reading def run(Q, R, label): x, P = meas[0], 4.0 nis, flagged = [], [] for k in range(1, N): P = P + Q S = P + R # predicted variance of the innovation nu = meas[k] - x # innovation: measurement minus prediction score = nu * nu / S # normalised innovation squared (NIS) if score > 9.0: # beyond 3 sigma flagged.append(k) if k != FAULT: nis.append(score) K = P / S x = x + K * nu P = (1 - K) * P print("%-22s mean NIS %5.2f | flagged %3d | fault caught: %s" % (label, sum(nis) / len(nis), len(flagged), FAULT in flagged)) run(0.1, 4.0, "honest R = 4") run(0.1, 0.25, "overconfident R = 0.25") run(0.1, 40.0, "too cautious R = 40")

With the honest R = 4 the mean NIS (leaving out the fault) is about 1.1, close to the ideal of 1, and only 2 steps are flagged: the real fault and one false alarm, as expected from a three-sigma gate over 300 samples. With the overconfident R = 0.25 the mean NIS jumps to about 13.5 and more than 100 steps are flagged, so the gate would throw away more than a third of perfectly good data. With the too cautious R = 40 the mean NIS drops to about 0.17. A rule of thumb is to tune Q and R until the average NIS is close to the number of measurement dimensions, here 1.

When the world is not linear

Everything so far assumed that the model and the sensor are linear: x_new = F * x and z = H * x. Many real problems are not. A robot that moves at speed v with heading theta has x_new = x + v * cos(theta) * dt, which is nonlinear in theta. A radar or a lidar that reports range and bearing sees range = sqrt(x^2 + y^2), nonlinear in the position. A matrix F cannot express these.

The extended Kalman filter (EKF) handles this by linearising. At each step it takes the nonlinear functions, computes their derivatives (the Jacobian) at the current estimate, and uses those matrices where the linear filter used F and H. The structure is otherwise unchanged: predict, innovation, gain, update.

The EKF is an approximation and has limits. It works when the estimate is close enough to the truth that the function is nearly straight in the neighbourhood. If the initial error is large, or the function bends sharply, the linearisation is poor and the filter can diverge while still reporting a small P. Unscented Kalman filters and particle filters are the usual alternatives.

Sensor fusion in a flight controller

A multicopter flight controller is a Kalman filter at industrial scale. Its state includes attitude, velocity and position, but also the gyro bias and the accelerometer bias, so the drift from lesson 1 is estimated and removed instead of just tolerated. The gyroscope and accelerometer, running at several hundred hertz, drive the predict step. Slower sensors arrive as updates: the magnetometer corrects heading, the barometer corrects height, and GPS corrects horizontal position and velocity at a few hertz. Each has its own R, and each is checked by an innovation gate. Open-source autopilots such as PX4 and ArduPilot use extended Kalman filters of this kind, and when GPS innovations stay too large, they stop trusting GPS and fall back to the remaining sensors. That is the whole course in one sentence: each sensor's weakness is covered by another, and the variances decide how much each one counts.