Derivation of the Kalman Filter

Derivation Overview

In this article we derive the Kalman filter equations mathematically. The guiding principle is to minimize the expected estimation error power $E\left[\|\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}\|^2\right]$.

For the basic setup of the Kalman filter, see the overview page.

Assumed Model

We assume the following state-space model.

The noises $\boldsymbol{w}_k, \boldsymbol{v}_k$ have mean $\boldsymbol{0}$ and are uncorrelated with any other signal. Their covariance matrices $\boldsymbol{Q}_k = E[\boldsymbol{w}_k\boldsymbol{w}_k^T]$ and $\boldsymbol{R}_k = E[\boldsymbol{v}_k\boldsymbol{v}_k^T]$ are assumed known.

Deriving the Expected Estimation Error Power

What Is Being Minimized

The quantity to minimize is the expected estimation error power $E[\|\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k}\|^2]$. This equals the trace of the estimation error covariance matrix $\boldsymbol{P}_{k|k}$:

The diagonal entries $(P_{k|k})_{ii}$ of $\boldsymbol{P}_{k|k}$ are the estimation error variances $E[(x_{k,i} - \hat{x}_{k|k,i})^2]$ of the $i$-th state variable, so $\text{tr}(\boldsymbol{P}_{k|k}) = \sum_i (P_{k|k})_{ii}$ is the sum of the estimation error variances of all state variables. Minimizing it yields the overall most accurate estimate.

The Error Signal Equation

The structure of the Kalman filter (see the overview) corrects the state estimate using the difference between the predicted and actual observations. Since the predicted observation is $\hat{\boldsymbol{y}}_k = \boldsymbol{H}_k \hat{\boldsymbol{x}}_{k|k-1}$, the error signal (the innovation) $\boldsymbol{e}_k$ is

We assume that the state estimate $\hat{\boldsymbol{x}}_{k|k}$ is obtained by adding this error linearly to the prediction $\hat{\boldsymbol{x}}_{k|k-1}$ (the linear-estimator assumption):

Here the matrix $\boldsymbol{K}_k$ is an unknown weight, and the goal of this derivation is to find the $\boldsymbol{K}_k$ that minimizes the estimation error power. From this equation, the state estimation error $\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}$ is

and the expected value of its power is

The noise $\boldsymbol{v}_k$ has mean $\boldsymbol{0}$ and is uncorrelated with the other signals $\boldsymbol{x}_k$ and $\hat{\boldsymbol{x}}_{k|k-1}$, so equation \eqref{eq:error-power-1} can be written as

Using this relation, equation \eqref{eq:error-power-2} becomes

Since $\boldsymbol{K}_k$ and $\boldsymbol{H}_k$ are deterministic, they can be taken outside the expectation $E[\cdot]$:

Denoting the predicted error covariance matrix by

and using $\boldsymbol{R}_k = E[\boldsymbol{v}_k\boldsymbol{v}_k^T]$, we get

Expanding the first term,

Letting

we obtain

which is the quantity to be minimized.

Derivation of the Optimal Kalman Gain $\boldsymbol{K}_k$

Inside the Kalman filter, only $\boldsymbol{K}_k$ is free to choose. How should we pick $\boldsymbol{K}_k$ so as to minimize the expected estimation error power?

At the $\boldsymbol{K}_k$ that minimizes the expected power of the estimation error $\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k}$, we must have

We apply these formulas to each term of equation \eqref{eq:error-power-final}. The $\boldsymbol{P}_{k|k-1}$ term does not depend on $\boldsymbol{K}_k$, so its derivative is 0. For the rest:

• $\text{tr}[\boldsymbol{K}_k (\boldsymbol{H}_k \boldsymbol{P}_{k|k-1})]$ has the form $\text{tr}[\boldsymbol{X}\boldsymbol{A}]$ with $\boldsymbol{A}=\boldsymbol{H}_k \boldsymbol{P}_{k|k-1}$, so its derivative is $\boldsymbol{A}^T = \boldsymbol{P}_{k|k-1}^T \boldsymbol{H}_k^T$.
• $\text{tr}[(\boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T) \boldsymbol{K}_k^T]$ has the form $\text{tr}[\boldsymbol{A}\boldsymbol{X}^T]$, so its derivative is $\boldsymbol{A} = \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T$.
• $\text{tr}[\boldsymbol{K}_k \boldsymbol{S}_k \boldsymbol{K}_k^T]$ has the form $\text{tr}[\boldsymbol{X}\boldsymbol{A}\boldsymbol{X}^T]$ with $\boldsymbol{A}=\boldsymbol{S}_k$, so its derivative is $\boldsymbol{K}_k (\boldsymbol{S}_k + \boldsymbol{S}_k^T)$.

Adding these together, minding the signs:

Since $\boldsymbol{P}_{k|k-1}$ and $\boldsymbol{S}_k$ are symmetric matrices (covariance matrices are always symmetric), we have $\boldsymbol{P}_{k|k-1}^T = \boldsymbol{P}_{k|k-1}$ and $\boldsymbol{S}_k^T = \boldsymbol{S}_k$, which gives

Hence

and multiplying both sides by $\boldsymbol{S}_k^{-1}$ from the right:

Predicted Error Covariance Matrix $\boldsymbol{P}_{k|k-1}$

The $\boldsymbol{P}_{k|k-1} = E[(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1})(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1})^T]$ needed to compute equation \eqref{eq:Kk} contains the unknown state $\boldsymbol{x}_k$, so as it stands the Kalman gain $\boldsymbol{K}_k$ cannot be evaluated. We therefore derive a recursion that obtains $\boldsymbol{P}_{k|k-1}$ from the estimation error covariance $\boldsymbol{P}_{k-1|k-1}$ at the previous sample.

From equation \eqref{eq:state}, the state evolves as $\boldsymbol{x}_k = \boldsymbol{F}_k \boldsymbol{x}_{k-1} + \boldsymbol{w}_k$, and the prediction $\hat{\boldsymbol{x}}_{k|k-1}$ is

so the error of the prediction $\hat{\boldsymbol{x}}_{k|k-1}$ with respect to the state $\boldsymbol{x}_k$ is

and equation \eqref{eq:Pkk-1} becomes

Since $\boldsymbol{w}_k$ is uncorrelated with signals at time $k-1$ and earlier (namely $\boldsymbol{x}_{k-1}, \hat{\boldsymbol{x}}_{k-1|k-1}$) and has mean $\boldsymbol{0}$, the two cross terms $E[\boldsymbol{F}_k(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})\boldsymbol{w}_k^T]$ and $E[\boldsymbol{w}_k (\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1|k-1})^T \boldsymbol{F}_k^T]$ that appear after expansion both vanish. Hence

Denoting the estimation error covariance matrix by

its value one sample earlier is $\boldsymbol{P}_{k-1|k-1} = E[(\boldsymbol{x}_{k-1} - \hat{\boldsymbol{x}}_{k-1|k-1})(\boldsymbol{x}_{k-1} - \hat{\boldsymbol{x}}_{k-1|k-1})^T]$, so

Estimated Error Covariance Matrix $\boldsymbol{P}_{k|k}$

We now describe how to compute the $\boldsymbol{P}_{k-1|k-1}$ used in equation \eqref{eq:Pkk-1-result}.

Starting from equation \eqref{eq:estimation-error} for the error of the estimate $\hat{\boldsymbol{x}}_{k|k}$, $$\boldsymbol{x}_k-\hat{\boldsymbol{x}}_{k|k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)( \boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1}) - \boldsymbol{K}_k \boldsymbol{v}_k$$ we compute the estimation error covariance matrix $\boldsymbol{P}_{k|k}$.

Since $\boldsymbol{v}_k$ has mean $\boldsymbol{0}$ and is uncorrelated with $\boldsymbol{x}_k$ and $\hat{\boldsymbol{x}}_{k|k-1}$, the cross term $E[(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)(\boldsymbol{x}_k - \hat{\boldsymbol{x}}_{k|k-1}) \boldsymbol{v}_k^T \boldsymbol{K}_k^T]$ that arises on expansion, together with its transpose, both vanish. Collecting the remaining two terms gives

Expanding,

By equation \eqref{eq:Sk}, $\boldsymbol{S}_k = \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k$, so

By equation \eqref{eq:KS-PH}, $\boldsymbol{K}_k \boldsymbol{S}_k = \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T$, so the second term vanishes and