Kalman Filter

The Kalman filter describes the evolution of a state estimate with time. We're concerned with a state described by a probability distribution. The distribution is assumed to be Gaussian, so it's described with a mean $x$ and a variance $\Sigma$.

The system evolves according to a model $F$. So $x_1 = F x_0$ and $\Sigma_1 = F \Sigma_0 F^T$.

Additionally, we have a measurement of the state. This is also assumed to be a Gaussian distribution, with mean $z$ and variance $R$.

The Kalman trick is to realize that we have, at this point, two distributions describing our state knowledge from different avenues, and we should combine these to get our actual current knowledge distribution $x$ and $\Sigma$. For Gaussian distributions this is particularly simple, and the combined distribution is still Gaussian, so can be described with a combined mean and covariance.

$$x = x_1 + \frac{\Sigma_1 (z - x_1)}{\Sigma_1 + R}$$
$$\Sigma  = \Sigma_1 - \frac{\Sigma_1^2}{\Sigma_1 + R}$$

There's a common factor here, call it $K$

$$K \equiv \frac{\Sigma_1}{\Sigma_1 + R}$$

This is sometimes called the Kalman gain.

$$\begin{align} x &= x_1 + K (z - x_1) \\ \Sigma &= \Sigma_1 - K \Sigma_1 \end{align}$$

From here, let us assume a multi-variate situation, so that $x$ is a vector, and use $\Sigma$ for the covariance matrix, instead of $\Sigma$ for variance.

Just one more thing gets us to a description of the Kalman update. The measurement space, with its particular units, etc., might not be the same as our state space. Let's use $H$ to get back and forth between these domains: $\hat{x} = H x$ and $\hat{\Sigma} = H \Sigma H^T$ We can now write the Kalman update:

$$\begin{align} \hat{x}  &= \hat{x_1} + K (z-\hat{x_1}) \\ \hat{\Sigma} &= \hat{\Sigma_1} -K \hat{\Sigma_1} \\ K &\equiv  \hat{\Sigma_1} [ \hat{\Sigma_1} + R ]^{-1} \end{align}$$

$$\begin{align} H x  &= H x_1 + K (z-H x_1) \\ H \Sigma H^T &= H \Sigma_1 H^T -K H \Sigma_1 H^T \\ K &\equiv  H \Sigma_1 H^T [ H \Sigma_1 H^T  + R ]^{-1} \end{align}$$

We can knock an $H$ off the front of the equation for $x$ and off the front and back of the equation for $\Sigma$, giving the final update relations.

$$\begin{align} x  &= x_1 + K' (z-H x_1) \\ \Sigma  &= \Sigma_1  -K' H \Sigma_1 \\ K' &\equiv \Sigma_1 H^T [ H \Sigma_1 H^T  + R ]^{-1} \end{align}$$