Linear Kalman Filter

Topics: State Estimation

Insights

The Kalman filter is an optimal and recursive state estimator. The general idea is as follows:
There are 2 sources of information, a model and observations (usually sensor measurements). Each of these sources has their own degree of confidence, modeled as added white gaussian noise. For example, there could me unmodeled dynamics or stochasticity in the model, or additive noise in the sensors. The job of the Kalman Filter is to combine the guesses of the model and observations to obtain a more accurate estimate of the states. It is particularly useful for states that are are not directly measured, like the State-of-Charge (SoC) in a battery or to combine multiple sensors into one estimate, also known as sensor fusion.

Algorithm

Consider a linear, time-invariant, state-space model

$$\begin{gather} x_k = A \; x_{k-1} + B \; u_{k-1} + w_{k-1} \\ y_k = C \; x_{k} + D \; u_k + n_k \end{gather}$$

where $w_k$ is the process noise and $n_k$ is the measurement noise. They are white and gaussian, and have 0 cross-correlation.

The Kalman Filter sets up an estimated model of the states $\hat{x}_k$ and its goal is to minimize the mean-squared error between the real and estimated. In reality, we do not know the real states (that's why we are estimating them), so we use the covariance to get the optimal $\hat{x}_k$. The covariance of the error term $e_k=x_k - \hat{x}_k$ is:
$P_k = \mathbb{E} \Big[(x_k - \hat{x}_k)(x_k - \hat{x}_k)^{\top} \Big]$
The covariance is a quadratic term, hence the analogy to the mean squared error.
We define 2 other covariances:
$Q = \mathbb{E}[w_k \; w_k^T]$: covariance of the process noise
$R = \mathbb{E}[n_k \; n_k^T]$: covariance of the measurement noise

The Linear Kalman Filter is divided into 2 major parts: prediction and update, each with 3 steps: prediction + update = estimation. In the prediction phase, the KF produces an estimate of the state variables along with their uncertainties. When the next measurement is obtained, the estimates are updated using a weighted average, with more weight given to estimates with greater certainty.

In what follows, the - superscript is used for predicted values and the + superscript is used for estimated values.

I. Prediction

1. Predict the state $\hat{x}_k^{-}$ using the state equation $\hat{x}_k^{-} = A \; \hat{x}_{k-1}^{+} + B \; u_{k-1}$
2. Predict the error covariance $P_{k}^{-} = A \; P_{k-1}^{+} \; A^{\top} + Q$
3. Predict the system output $\hat{y}_k$ using the output equation $\hat{y}_k = C \; \hat{x}_{k}^{-} + D \; u_k$

II. Update

1. Calculate the Kalman gain $L_{k} = P_{k}^{-} \; C^{\top}[C \; P_{k}^{-} \; C^{\top} + R] ^ {-1}$
2. Correct the state prediction based on the output error to get the new estimate $\hat{x}_{k}^{+} = \hat{x}_{k}^{-} + L_{k}(y_k - \hat{y}_k)$
3. Update the error covariance estimate $P_{k}^{+} = (I - L_{k} \; C) P_{k}^{-}$

References

1. https://en.wikipedia.org/wiki/Kalman_filter