I highly recommend using a large screen to read this page. The LaTeX plug-in I’m using cuts off the converted math equation images when a small screen (e.g. phone) is used.
I’ve had a post on Kalman Filter intro last time. While that intro might be sufficient for some people, the derivation of it is often sought. So I’d like to cover that in this post.
I’d say there are two ways of deriving the Kalman filter: 1) using definition of mean and covariance, and 2) work out the math of hidden Markov model with a number of Gaussian distributions. In this post, I’ll take the first route.
Let us describe the linear system as follows:
where the represent the state, control input, and the measurement at time , respectively. The matrices represent the state transition model, control model, and measurement model, respectively.
We model the processing and measurement noise, , respectively, as a zero-mean white Gaussian :
Let’s initialize the current state at time :
Now, note that the expectation operator is a linear operator. So, if we take the operator of the predicted state:
Note that at the end, the term disappears because the expectation of the processing noise is zero. To compute the predicted covariance:
Note that from the third equation from the bottom to the second, the cross terms between the state and the processing noises are dropped because the noise is zero mean white Gaussian.
Conditional distribution of multivariate Gaussian distribution
Before we dive into the measurement update equations’ derivation, we need to know how to obtain the condition distribution (i.e. ) of a jointly Gaussian random vector. Suppose we have two random vectors and which are jointly Gaussian (or multivariate Gaussian):
The conditional distribution: is computed by . Once this calculation is carried out, you’ll end up with following equations:
where the mean and covariance are:
Measurement Update Equations
After measurements are used to update current estimate, it provides the posterior distribution of . Using the equations above, the posterior mean and covariance are:
Let’s find the equations for those covariance terms:
Putting these together, we get Kalman filter measurement update equations: