Kalman Filter

To control any dynamical system as accurately as possible, you need two things at once. The first is its mathematical description: the law by which the system evolves in time. The second is monitoring its state: measurements that tell where the system has actually ended up at the given moment. The trouble is that neither the model nor the measurements can be trusted absolutely.

A model always lies — if only a little. It doesn't know all the quirks of the particular object: friction, backlash, wear, random external nudges. It predicts an ideal system, while the one we actually control is real. Measurements lie too. A sensor is subject to noise, it jitters and errs.

It would be great to cross a bulldog with a rhino — take the model's predictive power and the sensors' grip on reality — and get an estimate better than either source alone. That is exactly what the Kalman filter does: at each step it weighs how much to trust the model's prediction against the fresh measurement and returns the state estimate that is optimal in the mean-square sense.

Let's start with a general description of a linear dynamical system:

where $A_c$ is the system matrix, $x$ is the state vector, $B_c$ is the input matrix, and $u$ is the control input.

Let's write system (1) in discrete form: split time into steps $i$ $= 0, 1, 2, \dots$ with interval $h$ and approximate the derivative by the Euler method — the ratio of increments over one step

solving this for $x_{i+1}$, we get a recurrence — the next state through the current one

that is, the continuous matrices collapse into a discrete transition matrix $A$ and a discrete control matrix $B$

and the formula takes its final form

here $u_i$ is the control input at step $i$.

A real system doesn't follow equation (3) exactly. Let's add a random vector $w_i$ — everything the model fails to describe

the process noise $w_i$ is taken to be random with zero mean; the measure of its spread — the covariance matrix $Q$, the model-error matrix — will be defined below, when we derive the covariance prediction.

One filter cycle is two beats: prediction from the model and correction from the measurement. First the prediction. By formula (3) we push the previous estimate $\hat{x}_{i-1}$ one step forward and obtain the a priori estimate $\hat{x}_i^{-}$ (the "minus" sign means "before the measurement is taken into account")

together with the estimate we also predict its uncertainty. Introduce the estimation error — the difference between the true state and the estimate — and let the measure of uncertainty be its covariance matrix $P$ (on the diagonal — the variances of the errors in each state component)

here $\mathbb{E}[\,\cdot\,]$ is the averaging operator (the expected value): it takes a random quantity and returns its mean over all possible realizations. A single error $e_i$ is random and unpredictable, but averaging the matrix $e_i e_i^{\mathsf{T}}$ over the ensemble smooths out the randomness and leaves a stable measure of spread — the covariance.

Let's see what this error turns into over one prediction step. The true state follows equation (4) with a shifted index, $x_i$ $= A x_{i-1}$ $+ B u_{i-1}$ $+ w_{i-1}$, while our estimate follows formula (5); subtract the second from the first. The identical $B u_{i-1}$ terms cancel, and what remains is

that is, the a priori error is the previous-step error $e_{i-1}$ pushed through the dynamics $A$, plus the fresh model error $w_{i-1}$. Take the covariance of both sides. The past error and the new noise are independent, so the cross terms $\mathbb{E}[e_{i-1} w_{i-1}^{\mathsf{T}}]$ vanish, and the two that remain are $P_{i-1}$ and $Q$ by their definitions

substituting $\mathbb{E}[e_{i-1} e_{i-1}^{\mathsf{T}}]$ $= P_{i-1}$ and $\mathbb{E}[w_{i-1} w_{i-1}^{\mathsf{T}}]$ $= Q$, we get the covariance-prediction formula: the old uncertainty is carried through $A$, and the model error $Q$ is added to it

on the diagonal of $Q$ are the variances of the error in each state component, off the diagonal — their mutual covariances. A large $Q$ means "I don't trust the model, it's crude"; a small one — "the model is accurate". Deriving $Q$ analytically from first principles almost never works, so it is chosen empirically — by tuning against the filter's behavior on real or simulated data. It is the main tuning knob, and $Q$ is usually taken diagonal, just to avoid guessing the covariances.

A word about $P$ itself: a small one means "I trust the estimate", a large one — "the estimate is unreliable". The filter drives it on its own — during prediction it grows (formula (6) adds $Q$), and at the correction step it will shrink once the measurement brings in information. The starting $P_0$ is taken deliberately large: the initial state is poorly known, so the filter distrusts the first steps — it quickly pulls toward the measurements and "warms up".

Usually we don't observe the whole state vector $x$. Only some quantities are observed — the measurement vector $y$, whose dimension is smaller than that of the state. The link between the measurement and the state is given by the observation matrix $H$

the role of $H$ is to reconcile the dimensions: it maps the $n$-dimensional state $x$ to the $m$-dimensional measurement $y$ (the number of rows $m$ $\le n$), selecting and linearly combining the state components that are actually measured. This is the whole point of an observer: to reconstruct what is not measured.

The sensor doesn't output $H x_i$ exactly — the measurement is distorted by noise $v_i$

for the noise $v_i$ it is convenient to assume a normal (Gaussian) distribution with zero mean — then the Kalman filter turns out to be the optimal Bayesian estimate as well. But this is not a requirement: for any other distribution with finite variance the filter remains the best linear estimate in the mean-square sense. The spread of the noise is set by the measurement-noise covariance matrix

it is built from the sensors' variances. If the noises of different sensors are independent, $R$ is diagonal: on the diagonal — the squared standard deviations (the instrument's rated accuracy), off the diagonal — zeros

the more accurate the sensor, the smaller its $\sigma^2$ and the more the filter will trust that measurement channel.

Now everything is ready to formulate the filter itself. At step $i$ we have two independent sources of information about the sought state $x$. The first is the model's prediction $\hat{x}_i^{-}$ with error covariance $P_i^{-}$: the true $x$ lies somewhere around $\hat{x}_i^{-}$ with spread $P_i^{-}$. The second is the measurement $y_i$ $= H x$ $+ v$ with noise covariance $R$: it points to $x$ with accuracy $R$. Both are inexact — we look for the state that is most likely for both at once.

We'll assume the errors of both sources are Gaussian (for the measurement noise we've already assumed this). Then each source defines a probability density of the form $\exp\!\left(-\tfrac12\, e^{\mathsf{T}} \Sigma^{-1} e\right)$, where $e$ is its error and $\Sigma$ its covariance. The inverse covariance $\Sigma^{-1}$ plays the role of a weight: the more reliable the source (the smaller its spread), the harder it pulls the estimate toward itself. The likelihood of a state $x$ is the product of two such densities

the most likely $x$ maximizes this product. The logarithm turns the product into a sum of exponents, and the maximum of $p(x)$ corresponds to the minimum of $-2\ln p(x)$; dropping the constants, we get exactly a sum of two quadratic forms — and that is our functional

the optimal estimate minimizes $J$. Take the derivative with respect to $x$ and set it to zero

expanding the brackets and collecting $x$ on the left-hand side, we get the normal equation

the matrix in square brackets is exactly the inverse a posteriori covariance $(P_i)^{-1}$ (information form: the prediction's information plus the measurement's information). From here the rule for updating the uncertainty follows at once

Inverting matrices the size of the state is expensive. Applying the matrix inversion lemma, we rewrite solution (13)–(14) in a computationally convenient form through the Kalman gain $K_i$

then the correction of the estimate is the prediction plus a term proportional to the residual $(y_i - H \hat{x}_i^{-})$ — the discrepancy between what the model predicted and what the sensor showed

and the error covariance after the correction decreases

the meaning of $K$ is a trust regulator. If the measurement is accurate ($R$ is small), $K$ is large and the estimate leans toward the sensor; if the prediction is reliable ($P^{-}$ is small), $K$ is small and the filter sticks to the model. The very same bulldog-and-rhino compromise, only computed optimally.