M7-T2 — Deriving the Multivariate Kalman Filter

Learning objectives

After this lesson, you should be able to:

• Write the linear-Gaussian state-space model with F, Q, H and R, and say what each matrix means physically.
• Derive the matrix prediction and correction from the Bayes filter and from conditioning a joint Gaussian.
• Compute the innovation covariance S and the gain K = P Hᵀ S⁻¹, and explain K row by row.
• Explain how an unmeasured state is estimated through the cross-covariance.

The intuition

A robot drives along a corridor at about 1 m/s. Its state is two numbers: position and velocity. Its sensor reports position only, with 20 cm of noise. Velocity is never measured, yet the filter should know it. How?

The model supplies the link: over a step of 0.5 s the position changes by velocity times 0.5. A wrong velocity estimate therefore produces a growing position error, and a position error carries information about the velocity error. The filter keeps a covariance matrix whose off-diagonal entry records exactly this: after a prediction step, errors in position and velocity are correlated (here with a correlation of about 0.7). When a reading shows the robot further along than predicted, the correlation says that the velocity was probably underestimated too, and the filter raises it.

This is the main practical power of the Kalman filter: you measure what is easy to measure and estimate what is useful. Wheel speed from encoders, heading rate from the gyro, a bias in the gyro from the heading error: all are corrected through the model and the cross-covariances.

The scalar rule “move towards the reading by a fraction K” becomes a vector rule: each state variable moves by its own fraction of the innovation, and the fractions are set by how correlated that variable is with the measured one.

Definitions

State $\mathbf{x}\in\mathbb{R}^n$

Everything the filter estimates. Here position and velocity.

Transition $F$

How the state evolves without noise: $\mathbf{x}_t = F\mathbf{x}_{t-1} + B\mathbf{u}_t + \mathbf{w}_t$. For constant velocity $F = \begin{bmatrix}1 & \Delta t\\0 & 1\end{bmatrix}$.

Process noise $Q$

Covariance of $\mathbf{w}_t$: how much the state is disturbed per step. Positive semi-definite.

Measurement $H, R$

$\mathbf{z}_t = H\mathbf{x}_t + \mathbf{v}_t$, $\mathbf{v}_t\sim\mathcal{N}(0,R)$. $H$ selects what is measured: $H = [1;;0]$ for position only.

Innovation $\mathbf{y}, S$

$\mathbf{y} = \mathbf{z} - H\bar{\mathbf{x}}$ and its covariance $S = H\bar{P}H^\top + R$.

Gain $K$

An $n\times m$ matrix mapping the innovation into a correction of every state variable.

Derivation: conditioning a joint Gaussian

The prediction is the linear propagation of module 4. The correction can be derived by writing the joint distribution of the state and the measurement and conditioning on the measurement.

1. $$\begin{bmatrix}\mathbf{x}\\\mathbf{z}\end{bmatrix}\sim\mathcal{N}\!\left(\begin{bmatrix}\bar{\mathbf{x}}\\H\bar{\mathbf{x}}\end{bmatrix},\begin{bmatrix}\bar P & \bar PH^\top\\H\bar P & H\bar PH^\top + R\end{bmatrix}\right)$$

   The prior state is Gaussian, and $\mathbf{z} = H\mathbf{x} + \mathbf{v}$ is a linear function of it plus independent noise. Its covariance with the state is $\mathrm{Cov}(\mathbf{x},\mathbf{z}) = \bar P H^\top$.

2. $$\boldsymbol\mu_{x|z} = \boldsymbol\mu_x + \Sigma_{xz}\Sigma_{zz}^{-1}(\mathbf{z} - \boldsymbol\mu_z)$$

   The conditioning formula for a joint Gaussian (conditioning lesson of module 4).

3. $$\mathbf{x}_t = \bar{\mathbf{x}} + \underbrace{\bar PH^\top S^{-1}}_{K}\,(\mathbf{z} - H\bar{\mathbf{x}})$$

   Substituting the blocks, with $\Sigma_{zz} = S$. The gain is the cross-covariance divided by the innovation covariance: how much the measurement tells about each state variable, per unit of its uncertainty.

4. $$P_t = \bar P - \bar PH^\top S^{-1}H\bar P = (I - KH)\bar P$$

   The conditional covariance: the prior minus the information the reading carries. It never increases the uncertainty.

Why the unmeasured variable moves

Write $\bar P = \begin{bmatrix}p_{xx}&p_{xv}\\p_{xv}&p_{vv}\end{bmatrix}$ and $H = [1;0]$. Then $S = p_{xx} + r$ and the gain is

The velocity changes by $p_{xv}/(p_{xx}+r)$ times the position innovation. If $p_{xv} = 0$ the velocity would never be corrected. The model creates the correlation, because $F$ adds $\Delta t\cdot v$ to the position: in the prediction $\bar p_{xv} = p_{xv} + \Delta t\,p_{vv} + q_{xv}$, which is positive whenever the velocity is uncertain.

A state variable is observable through a sensor if the model correlates it with something the sensor sees. If the model does not link them, no filter can estimate it: that is the subject of the last lesson of the module.

A worked example

The robot of the figure: a state of position and velocity, $\Delta t = 0.5$ s, acceleration noise of standard deviation 0.05 m/s per √s, a position sensor with $r = 0.04$ m², and a poor start $\mathbf{x}_0 = (0, 0)$ with identity covariance. The true velocity is about 1 m/s.

1. Step 1. The reading is 0.7 m from a true position of 0.513 m. The gain is $(0.969, 0.388)$: the position takes 97% of the innovation; the velocity estimate moves from 0 to 0.28 m/s, with a standard deviation still large (0.90 m/s) because one reading says little about the velocity.
2. Step 2. One step later the correlation is 0.66 and the velocity gain jumps to 1.42 (in m/s per metre of innovation): with two positions the filter can see a slope. The velocity estimate is 0.67 m/s and its standard deviation 0.46.
3. Step 5 to 20. The velocity estimate reaches 0.85, then 1.01 and 1.11 m/s (truth 1.02, 1.12 and 1.09), with a standard deviation of 0.073 m/s. The position gain falls from 0.97 to 0.34 and the velocity gain from 0.39 to 0.14.
4. Honest uncertainty. Over 300 runs the root-mean-square position error at step 20 is 0.120 m (claimed 0.117), and the velocity error 0.068 m/s (claimed 0.073).

import math
from robotprob import Rng, kf_predict, kf_update, mean

dt, sq, r = 0.5, 0.05, 0.04
F = [[1.0, dt], [0.0, 1.0]]                     # position += velocity * dt
H = [[1.0, 0.0]]                                # only the position is measured
q = sq * sq
Q = [[q * dt**3 / 3, q * dt**2 / 2],            # white-noise acceleration: errors in position and velocity are correlated
     [q * dt**2 / 2, q * dt]]
R = [[r]]

def run(seed, steps=20):
    rng = Rng(seed)
    pos, vel = 0.0, 1.0                         # the truth
    x, P = [0.0, 0.0], [[1.0, 0.0], [0.0, 1.0]] # the belief: no idea of the velocity
    out = []
    for t in range(1, steps + 1):
        vel += rng.gauss(0.0, sq * math.sqrt(dt))
        pos += vel * dt
        z = pos + rng.gauss(0.0, math.sqrt(r))
        x, P = kf_predict(x, P, F, Q)
        x, P, y, S, K = kf_update(x, P, [z], H, R)
        out.append((t, pos, vel, z, x, P, K))
    return out

o = run(5)
print('step  true pos  est pos  true vel  est vel  std pos  std vel  corr  gain(pos,vel)')
for t, pos, vel, z, x, P, K in o:
    if t in (1, 2, 3, 5, 10, 20):
        c = P[0][1] / math.sqrt(P[0][0] * P[1][1])
        print(str(t).rjust(4), str(round(pos,3)).rjust(9), str(round(x[0],3)).rjust(8), str(round(vel,3)).rjust(9), str(round(x[1],3)).rjust(8), str(round(math.sqrt(P[0][0]),3)).rjust(8), str(round(math.sqrt(P[1][1]),3)).rjust(8), str(round(c,2)).rjust(5), (round(K[0][0],3), round(K[1][0],3)))
ev=[];ep=[]
for s in range(1,301):
    t,pos,vel,z,x,P,K=run(s)[-1]
    ev.append(x[1]-vel); ep.append(x[0]-pos)
print('300 runs: rms pos error',round(math.sqrt(mean([e*e for e in ep])),4),' claimed',round(math.sqrt(o[-1][5][0][0]),4))
print('rms vel error',round(math.sqrt(mean([e*e for e in ev])),4),' claimed',round(math.sqrt(o[-1][5][1][1]),4))

step  true pos  est pos  true vel  est vel  std pos  std vel  corr  gain(pos,vel)
   1     0.513      0.7     1.026     0.28    0.197    0.898  0.09 (0.969, 0.388)
   2     1.003    1.079      0.98     0.67    0.186    0.463  0.66 (0.865, 1.416)
   3     1.479    1.493     0.953     0.76    0.177    0.265  0.76 (0.784, 0.888)
   5     2.499    2.396     1.024    0.852    0.154     0.13  0.79 (0.591, 0.394)
  10     5.185     5.04     1.118    1.011    0.123    0.075  0.68 (0.38, 0.158)
  20    10.662   10.716     1.093    1.113    0.117    0.073  0.67 (0.343, 0.143)
300 runs: rms pos error 0.1201  claimed 0.1172
rms vel error 0.0681  claimed 0.0733

In robotics

Tracking with a motion model

A constant-velocity model with position readings is the standard tracker for moving objects: other cars, pedestrians, a ball. The Perception course builds the intuition in Simple Filtering and Kalman Filter Intuition, and the UAV course applies the same matrix filter to a vehicle in Kalman Filter.

Sensor fusion and biases

Adding a gyro-bias state to a heading filter lets a heading sensor correct the bias even though the bias is never observed directly: the same cross-covariance mechanism. The state grows, the filter does not change.

Choosing Q

The process noise matrix encodes how the state can deviate from the model. For a constant-velocity model the white-noise-acceleration form above is physically motivated: its off-diagonal entries say that acceleration noise perturbs both position and velocity together. A diagonal Q is simpler and often adequate, but it gets the correlations wrong. M7-T3: Innovation, Gain and Tuning Q and R gives the practical tuning recipe.

Cost

The filter costs about $O(n^3)$ per update, trivial for n of 2 to 20 on an embedded processor. It becomes expensive for the thousands of landmarks of SLAM, which is why mapping uses sparse or information forms (module 11).

Common misconceptions

Key takeaways

1. The matrix filter is the scalar one with matrices: predict with F and Q, correct with the gain K = P̄Hᵀ S⁻¹.
2. The correction is the conditional of a joint Gaussian: the prior minus the information carried by the measurement.
3. The off-diagonal terms of P are what let a sensor correct states it does not measure.
4. Unmeasured states are estimated only if the model correlates them with measured ones.
5. Use the Joseph form for the covariance update, and check claimed against real errors.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• T. D. Barfoot, State Estimation for Robotics, 2nd edition, 2024 , chapter 3 (Linear-Gaussian Estimation) derives the filter from a batch problem and from Bayes. Free to read as the author’s PDF; the book is copyrighted.
• S. Särkkä, Bayesian Filtering and Smoothing, Cambridge University Press, 2013, chapter 4 (Bayesian filtering equations and exact solutions: the Kalman filter). Paid book; the author also hosts a free PDF on his page.
• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, section 3.2. Paid book.
• C. Stachniss, “Kalman Filter & EKF” (video lecture) . Free to watch.

Knowledge check

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

What’s next

The filter is derived. The next lesson looks at what it computes at run time (the innovation, the gain) and at how to choose the two matrices Q and R that you must supply.

Related content

Nothing below is required to finish this lesson.

• Before
  M7-T1: The Kalman Filter in One Dimension

  Derive the scalar Kalman filter from the fusion of two Gaussians and explain the gain as a ratio of variances.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M3-T4: Kalman Filter
• UAV M3-L1: Implement a Kalman Filter in Python
• Perception P7-T4: Simple Filtering and Kalman Filter Intuition
• UAV M3-T15: Normalized Estimation Error Squared