Kalman Filters

Steps

A Kalman filter moves through the same two steps every cycle, and the whole thing is easier to follow if we keep the chain in mind before any of the notation arrives:

$$\text{previous state}\stackrel{\text{predict}}{\to }\text{prior}\stackrel{\text{measurement}}{\to }\text{posterior}$$

As discussed in the discrete filter writeup, the action we take depends entirely on whether we are predicting or updating, and a Kalman filter is one specific choice about how to represent the belief while doing so.

Gaussians

The key idea is that a Kalman filter models the state as a Gaussian distribution, which means that representing a state requires only two things: an expected value and a variance. That is a strong assumption, but it buys us an enormous amount, because a distribution that would otherwise need an unbounded number of samples collapses into a handful of numbers we can carry forward cheaply.

Because the variables in a Kalman filter state depend on one another, a single variance per variable is not enough and we use a covariance matrix instead. It holds the variance of each variable along the diagonal and the covariance of every pair of variables in the off-diagonal entries:

$$P=\left[\begin{array}{ccc}{\sigma }_x^2& {\sigma }_{xy}& {\sigma }_{xv}\\ {\sigma }_{yx}& {\sigma }_y^2& {\sigma }_{yv}\\ {\sigma }_{vx}& {\sigma }_{vy}& {\sigma }_v^2\end{array}\right]$$

The off-diagonal terms are what let the filter learn that two quantities move together, so that correcting one of them also drags the other in the right direction.

Trust

Recalling that a discrete filter is essentially a way of blending our prediction with the measurement we just received, the question of which one we trust more is answered entirely by the covariances. The filter is not making a judgement call, it is weighing two distributions against each other and landing wherever the arithmetic puts it.

If the measurement covariance is large, meaning the sensor is telling us it could be a long way off, then the filter leans on the prediction instead. If the process covariance is large, because our model of the physics is rough, then the filter leans on the measurement. Everything the filter does about trust is encoded in those two matrices, which is also why an untuned filter behaves badly even when the equations are correct.

Notation

Before the equations, the symbols. Every one of them is either something we estimate, something we model, or something we measure:

Symbol	Name	What it is
$x$	state	what we are trying to estimate, such as position and velocity
$P$	state covariance	how uncertain we currently are, and how the variables correlate
$F$	process matrix	how the state evolves on its own over one timestep
$Q$	process covariance	how much we distrust that model of the physics
$B$ , $u$	control matrix and input	what we commanded, and how it enters the state
$H$	measurement matrix	how a state would appear to the sensor
$R$	measurement covariance	how much we distrust the sensor
$K$	Kalman gain	how far to move toward the measurement, computed each step

Predict

The prediction step pushes the state forward through the process model and adds whatever we commanded, which gives us the prior:

$${\hat{x}}_{k\mid k-1}=F{\hat{x}}_{k-1\mid k-1}+B{u}_k$$

At the same time the covariance has to grow, because a step taken on a model we do not fully trust leaves us less certain than we were before:

$$P_{k\mid k-1}=F{P}_{k-1\mid k-1}{F}^T+Q$$

That second equation is the one people skip, and skipping it is how a filter ends up convinced it knows exactly where something is while quietly coasting on physics alone.

Update

The update step compares what the sensor reported against what our prior says the sensor should have reported, and the difference between those two is the innovation, $z_k-H{\hat{x}}_{k\mid k-1}$ .

How much of that difference we act on is the Kalman gain, which is where the two sets of covariances finally meet:

$$K_k=P_{k\mid k-1}{H}^T{\left(H{P}_{k\mid k-1}{H}^T+R\right)}^{-1}$$

Then we move the estimate along the innovation by that amount and shrink the covariance, because we have just learned something and should be more certain than we were a moment ago:

$${\hat{x}}_{k\mid k}={\hat{x}}_{k\mid k-1}+K_k\left(z_k-H{\hat{x}}_{k\mid k-1}\right)$$ $$P_{k\mid k}=\left(I-K_{k}H\right)P_{k\mid k-1}$$

Everything above assumes $F$ and $H$ are matrices that do not depend on the state. When that stops being true, as it does the moment a rotation by heading enters the observation model, we need the extended version instead, which is what the localizer redesign is about.

Discrete filters→Ball filter in practice→