The Kalman Filter Algorithm
Algorithm overview
The Kalman filter targets a discrete-time system described by a linear state-space model:
\begin{align} \boldsymbol{x}_k &= \boldsymbol{F}_k \boldsymbol{x}_{k-1} + \boldsymbol{w}_k, \quad \boldsymbol{w}_k \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{Q}_k) \\ \boldsymbol{y}_k &= \boldsymbol{H}_k \boldsymbol{x}_k + \boldsymbol{v}_k, \quad \boldsymbol{v}_k \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{R}_k) \end{align}Here $\boldsymbol{x}_k$ is the unobservable true state, $\boldsymbol{y}_k$ is the observation, and $\boldsymbol{w}_k$ and $\boldsymbol{v}_k$ are the process noise and observation noise, assumed to be mutually independent Gaussians. $\boldsymbol{F}_k, \boldsymbol{H}_k$ are known matrices. The goal of the Kalman filter is to obtain the best estimate $\hat{\boldsymbol{x}}_{k|k}$ of $\boldsymbol{x}_k$ from the observation sequence $\boldsymbol{y}_{1:k}$ so far.
This estimation can be carried out recursively by alternating two steps: predict and update.
Predict step
From the previous estimate, predict the current state and covariance.
- State prediction: $\hat{\boldsymbol{x}}_{k|k-1} = \boldsymbol{F}_k \hat{\boldsymbol{x}}_{k-1|k-1}$
- Covariance prediction: $\boldsymbol{P}_{k|k-1} = \boldsymbol{F}_k \boldsymbol{P}_{k-1|k-1} \boldsymbol{F}_k^T + \boldsymbol{Q}_k$
Update step
Use the observation to correct the prediction.
- Innovation covariance: $\boldsymbol{S}_k = \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k$
- Kalman gain: $\boldsymbol{K}_k = \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{S}_k^{-1}$
- Innovation: $\boldsymbol{e}_k = \boldsymbol{y}_k - \boldsymbol{H}_k \hat{\boldsymbol{x}}_{k|k-1}$
- State update: $\hat{\boldsymbol{x}}_{k|k} = \hat{\boldsymbol{x}}_{k|k-1} + \boldsymbol{K}_k \boldsymbol{e}_k$
- Covariance update: $\boldsymbol{P}_{k|k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1}$
For the derivation of each equation, see the derivation page.
Pseudocode
The Kalman filter computation can be written in pseudocode as follows (bold lowercase denotes a column vector, bold uppercase a matrix).
// Initial values (prior estimate at time 0)
$\hat{\boldsymbol{x}}_{0|0} = $ (initial state estimate);
$\boldsymbol{P}_{0|0} = $ (initial error covariance matrix);
// Iterate prediction and estimation
for (k = 1, 2, 3, ...) {
| // Predict step | ||
| $\hat{\boldsymbol{x}}_{k|k-1} = \boldsymbol{F}_k \hat{\boldsymbol{x}}_{k-1|k-1}$; | // State prediction | |
| $\boldsymbol{P}_{k|k-1} = \boldsymbol{F}_k \boldsymbol{P}_{k-1|k-1} \boldsymbol{F}_k^T + \boldsymbol{Q}_k$; | // Predicted error covariance | |
| // Update step | ||
| $\boldsymbol{S}_k = \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k$; | // Innovation covariance | |
| $\boldsymbol{K}_k = \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{S}_k^{-1}$; | // Kalman gain | |
| $\boldsymbol{e}_k = \boldsymbol{y}_k - \boldsymbol{H}_k \hat{\boldsymbol{x}}_{k|k-1}$; | // Innovation (residual) | |
| $\hat{\boldsymbol{x}}_{k|k} = \hat{\boldsymbol{x}}_{k|k-1} + \boldsymbol{K}_k \boldsymbol{e}_k$; | // State estimate | |
| $\boldsymbol{P}_{k|k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1}$; | // Estimated error covariance | |
}
Meaning of each variable
| Variable | Meaning | Size |
|---|---|---|
| $\boldsymbol{x}_k$ | True state at time $k$ (unknown) | $n \times 1$ |
| $\boldsymbol{y}_k$ | Observation at time $k$ (observable) | $m \times 1$ |
| $\hat{\boldsymbol{x}}_{k|k-1}$ | State prediction for time $k$ using information up to time $k-1$ | $n \times 1$ |
| $\hat{\boldsymbol{x}}_{k|k}$ | State estimate for time $k$ using information up to time $k$ | $n \times 1$ |
| $\boldsymbol{P}_{k|k-1}$ | Covariance matrix of the prediction error | $n \times n$ |
| $\boldsymbol{P}_{k|k}$ | Covariance matrix of the estimation error | $n \times n$ |
| $\boldsymbol{K}_k$ | Kalman gain (the optimal gain that minimizes the estimation error) | $n \times m$ |
| $\boldsymbol{e}_k$ | Innovation (residual): the difference between the observation and the predicted observation | $m \times 1$ |
| $\boldsymbol{S}_k$ | Covariance matrix of the innovation | $m \times m$ |
| $\boldsymbol{F}_k$ | State-transition matrix | $n \times n$ |
| $\boldsymbol{H}_k$ | Observation matrix | $m \times n$ |
| $\boldsymbol{Q}_k$ | Covariance matrix of the process noise $\boldsymbol{w}_k$ | $n \times n$ |
| $\boldsymbol{R}_k$ | Covariance matrix of the observation noise $\boldsymbol{v}_k$ | $m \times m$ |
Here $n$ is the dimension of the state vector and $m$ is the dimension of the observation vector.
The predict step in detail
In the predict step, the current state is predicted from the previous estimate.
State prediction
\begin{equation} \hat{\boldsymbol{x}}_{k|k-1} = \boldsymbol{F}_k \hat{\boldsymbol{x}}_{k-1|k-1} \end{equation}The state-transition matrix $\boldsymbol{F}_k$ projects the previous estimate onto the current time. When a control input $\boldsymbol{u}_k$ is present:
\begin{equation} \hat{\boldsymbol{x}}_{k|k-1} = \boldsymbol{F}_k \hat{\boldsymbol{x}}_{k-1|k-1} + \boldsymbol{B}_k \boldsymbol{u}_k \end{equation}Covariance prediction
\begin{equation} \boldsymbol{P}_{k|k-1} = \boldsymbol{F}_k \boldsymbol{P}_{k-1|k-1} \boldsymbol{F}_k^T + \boldsymbol{Q}_k \end{equation}The error covariance also propagates through the state transition. The first term is the part where "the existing uncertainty $\boldsymbol{P}_{k-1|k-1}$ is transformed by $\boldsymbol{F}_k$", and the second term is the uncertainty newly added by the process noise.
Where $\boldsymbol{F}_k \boldsymbol{P} \boldsymbol{F}_k^T$ comes from
If a random vector $\boldsymbol{z}$ has covariance $\boldsymbol{P}$, then the covariance of the linear transformation $\boldsymbol{F} \boldsymbol{z}$ is $\mathbb{E}[(\boldsymbol{F}\boldsymbol{z})(\boldsymbol{F}\boldsymbol{z})^T] = \boldsymbol{F} \mathbb{E}[\boldsymbol{z}\boldsymbol{z}^T] \boldsymbol{F}^T = \boldsymbol{F} \boldsymbol{P} \boldsymbol{F}^T$. Since $\boldsymbol{w}_k$ is independent of $\boldsymbol{x}_{k-1}$, the covariance of the sum is the sum of the two covariances.
The update step in detail
In the update step, the observation is used to correct the prediction.
Innovation (residual)
\begin{equation} \boldsymbol{e}_k = \boldsymbol{y}_k - \boldsymbol{H}_k \hat{\boldsymbol{x}}_{k|k-1} \end{equation}The difference between the actual observation and the predicted observation. This difference is used to correct the state estimate.
Innovation covariance
\begin{equation} \boldsymbol{S}_k = \boldsymbol{H}_k \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T + \boldsymbol{R}_k \end{equation}The covariance matrix of the innovation. It includes both the prediction error and the observation noise.
Kalman gain
\begin{equation} \boldsymbol{K}_k = \boldsymbol{P}_{k|k-1} \boldsymbol{H}_k^T \boldsymbol{S}_k^{-1} \end{equation}The weight that decides how much to trust the prediction versus the observation.
- Large observation noise $\boldsymbol{R}_k$ → $\boldsymbol{K}_k$ becomes small, and the prediction is trusted more.
- Large prediction error $\boldsymbol{P}_{k|k-1}$ → $\boldsymbol{K}_k$ becomes large, and the observation is trusted more.
State update
\begin{equation} \hat{\boldsymbol{x}}_{k|k} = \hat{\boldsymbol{x}}_{k|k-1} + \boldsymbol{K}_k \boldsymbol{e}_k \end{equation}Add the innovation, weighted by the Kalman gain, to the predicted value.
Covariance update
\begin{equation} \boldsymbol{P}_{k|k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1} \end{equation}Incorporating the observation reduces the uncertainty of the estimate.
Joseph form (numerically stable version)
To improve numerical stability, the following equivalent form is also widely used:
\begin{equation} \boldsymbol{P}_{k|k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1} (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k)^T + \boldsymbol{K}_k \boldsymbol{R}_k \boldsymbol{K}_k^T \end{equation}Its equivalence to the standard form $(\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}_k) \boldsymbol{P}_{k|k-1}$ can be shown by substituting the optimal $\boldsymbol{K}_k$ (see the "derivation" page). Whereas the standard form easily loses symmetry to numerical error in the matrix product, the Joseph form is written as a sum of quadratic forms $A\boldsymbol{P}A^T + B\boldsymbol{R}B^T$, so it stays symmetric and positive semidefinite to machine precision. A further practical advantage is that it gives the correct covariance for any $\boldsymbol{K}_k$ (even a non-optimal one).
On initialization
To start the Kalman filter, an initial state estimate $\hat{\boldsymbol{x}}_{0|-1}$ and an initial error covariance matrix $\boldsymbol{P}_{0|-1}$ must be set.
Setting the initial state
- If prior information is available: set it based on that information.
- If no prior information is available: use the zero vector or an estimate based on the first observation.
Setting the initial covariance
- When the uncertainty is large: a large value (e.g. $\boldsymbol{P}_0 = 1000 \boldsymbol{I}$).
- When the initial state is trusted: a small value.
Effect of initialization
Even with a poor choice of initial values, the Kalman filter converges to the correct estimate given enough data. However, underestimating the initial covariance can slow convergence.
Frequently Asked Questions
- What are the two computation steps of the Kalman filter?
- They are the predict step and the update step. In the predict step, the state-transition model predicts the next state and error covariance. In the update step, the Kalman gain is computed from the actual observation, and the state and covariance are corrected. Repeating these two steps yields the optimal estimate.
- What is the Kalman gain?
- The Kalman gain $K$ is a weight determined by the ratio of the predicted error covariance $P$ to the observation-noise covariance $R$. It is computed as $K = PH^T(HPH^T + R)^{-1}$. The larger the prediction uncertainty (large $P$), the more the observation is trusted; the larger the observation noise (large $R$), the more the prediction is trusted.
- What is the Joseph form of the covariance update?
- The Joseph form updates the covariance with the quadratic expression $(I - KH)P(I - KH)^T + KRK^T$. It is equivalent to the standard form $(I-KH)P$, but because it avoids the loss of symmetry and positive semidefiniteness caused by rounding errors in matrix products, it is recommended for numerically stable implementations.