P7-T4 — Simple Filtering and Kalman Filter Intuition

Learning objectives

After this lesson, you should be able to:

• Fuse a prediction and a measurement of one quantity by inverse-variance weighting and compute the Kalman gain.
• Write the predict and update equations of the constant-velocity Kalman filter and say what each term does.
• Explain the roles of the process noise Q and the measurement noise R.
• Use the normalised innovation squared to check whether the filter’s noise settings fit the data.

The idea: two noisy opinions, one better opinion

A prediction and a measurement are two opinions about where the object is now. Each is uncertain, and the uncertainty can be written as a variance: small for confident, large for doubtful. If you trust the one with the smaller variance more, you get an estimate that sits nearer to it, and, as the figure shows, the combined opinion is less uncertain than either.

Predict, update, repeat

The filter is a loop of two steps.

1. Predict. Move the state forward with the motion model. The uncertainty grows, because the future is less certain than the present: the prediction adds the process noise Q, which represents the random changes of velocity that the model does not capture, and the uncertainty about the velocity leaks into the position.
2. Update. Compare the new measurement with what was predicted. The difference is the innovation. Correct the state by a fraction of the innovation, the Kalman gain, and shrink the uncertainty, because a new piece of information has been absorbed.

The gain is large when the prediction is uncertain and the measurement is precise, and small in the opposite case. It starts high, when the filter knows little, and settles. That is the difference from the alpha-beta filter, whose gains are fixed.

The two knobs

Measurement noise R is how much to trust each measurement. It can often be taken from the detector’s known accuracy. Process noise Q is how much to trust the motion model, and it is harder: an object that can accelerate sharply needs a large Q, which makes the filter follow the measurements; a very predictable one allows a small Q and heavy smoothing. Too small a Q makes the filter over-confident: it stops listening, lags behind manoeuvres, and the gate based on its uncertainty becomes too narrow. Too large a Q passes the noise through.

The mathematics: from one number to the full filter

One quantity, two estimates. A prediction has mean x_p and variance P_p; a measurement has value z and variance R. The fused estimate weights each by the inverse of its variance:

K

the Kalman gain, between 0 and 1: the share of the innovation (z − x_p) that is believed. It tends to 1 when R is much smaller than P_p and to 0 when R is much larger.

x, P

the fused mean and variance. P is always smaller than both P_p and R: information has been added.

The constant-velocity filter. For the state x = (position, velocity)ᵀ with covariance P (a 2 × 2 matrix), transition F = [[1, Δt], [0, 1]] and observation H = [1, 0]:

P

the state covariance: its diagonal holds the variances of position and velocity, and the off-diagonal term couples them (a position error and a velocity error are related).

Q

the process-noise covariance. For random acceleration of variance q, Q = q · [[Δt⁴/4, Δt³/2], [Δt³/2, Δt²]].

R

the measurement variance (a single number for a scalar position).

y

the innovation: the measurement minus the predicted measurement.

S

the innovation covariance: how large an innovation to expect. It is also used for the gate: d² = y² / S is the squared statistical distance of Lesson P7-T2.

K

the Kalman gain, here a 2 × 1 vector: one component corrects the position and the other the velocity.

Consistency check. If the filter’s model and noise settings are right, the innovations are zero-mean with variance S, so the normalised innovation squared NIS = y² / S has an average of 1 for a scalar measurement. An average well above 1 means the filter is over-confident (Q or R too small); well below 1, under-confident. This test needs no ground truth (Lab P7-L3, stage 3).

Worked examples: one number, then one full step

One number. The predicted position is 12 with variance P_p = 4. The detector reports 13.5 with variance R = 1. Then

This is the picture in the figure. The estimate is close to the measurement because the measurement was four times as certain, and the variance 0.8 is below both 4 and 1.

One full two-state step. The state is x = (10, 2) with P = [[1, 0], [0, 1]]; Δt = 1, q = 0.4, R = 1, and the measurement is z = 13.5.

The position moved by 1.02 of the 1.5 innovation. The velocity rose from 2 to 2.58 although only the position was measured: the coupling term in P (1.2) lets a position surprise change the velocity. The position variance dropped from 2.1 to 0.68, and the velocity variance only from 1.4 to 0.94, because a single position measurement says little about velocity. After several steps the velocity variance falls further.

The check. The NIS for this step is y² / S = 1.5² / 3.1 = 0.73, near 1, as expected for a consistent filter (one value says little; the average over a run is what matters).

Engineering implications and limitations

Set R from the detector, Q from the motion. Measurement noise can be measured on a still target. Process noise is a judgement about how much the object can accelerate in one step: choose q = σ_a², with σ_a about half of the largest change of velocity per step you expect, then tune with the NIS.

Tune with the innovations. Plot the NIS over time on logged data. Persistently above 1: raise Q or R. Persistently below: lower them. This is the tool that works on a real robot, where truth is absent.

Use the covariance in the gate. The innovation covariance S already combines the prediction and the measurement uncertainty; gate on yᵀS⁻¹y and the gate widens when a track coasts (Lesson P7-T2).

Keep P well behaved. In long runs, round-off can make P lose symmetry or positive definiteness. Use the symmetric (Joseph) form of the covariance update or enforce symmetry, and clip variances to a sane range.

Skip the update, not the predict. On a frame with no detection, run the predict step only; the covariance grows and the gate widens, which is the right behaviour for a coasting track (Lesson P7-T5).

Know the filter’s assumptions. It is optimal for linear models and Gaussian noise. Detector errors that are heavy-tailed (an occasional wild box) break the Gaussian assumption; use the gate to reject outliers, or a robust update.

Common misconceptions

Engineering takeaways

1. Fusing a prediction and a measurement weights them by inverse variance: K = P_p/(P_p + R), P = (1 − K)P_p; in the example 12 and 13.5 give 13.2 with variance 0.8.
2. The filter alternates predict (x ← Fx, P ← FPFᵀ + Q: uncertainty grows) and update (gain from P and R: uncertainty shrinks).
3. The innovation covariance S = HPHᵀ + R is used both for the gain and for the statistical gate.
4. Q says how much to trust the motion model, R how much to trust the measurement; too small a Q makes the filter over-confident and slow to follow manoeuvres.
5. The mean NIS (y²/S) should be near 1; use it to tune on real data without ground truth.

References and further reading

The original paper, textbooks and a standard tutorial. The numbers in the examples were computed by hand and checked by running the equations.

• Kalman, R. E. (1960). A New Approach to Linear Filtering and Prediction Problems. Journal of Basic Engineering.
• Bar-Shalom, Y., Li, X.-R. and Kirubarajan, T. (2001). Estimation with Applications to Tracking and Navigation. Wiley: the filter, noise models and consistency tests (NIS).
• Thrun, S., Burgard, W. and Fox, D. (2005). Probabilistic Robotics. MIT Press: Gaussian filters.
• Welch, G. and Bishop, G. (2006). An Introduction to the Kalman Filter. UNC Chapel Hill technical report TR 95-041.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Next: the life of a track: how it starts, how it persists through gaps, and when it ends.

Related content

Nothing below is required to finish this lesson.

• Before
  P7-T3: Track State and Motion Estimation

  Define a track state and predict motion with a constant-velocity model.

• Lab
  P7-L3: Investigate How Noisy Detections Affect Track Quality

  Add a motion model and a filter and measure track error against noise.