Kalman Filter

Kalman filter is an optimal estimation algorithm. It can help us to estimate a system state when it cannot be measured directly. This blog discusses the Kalman Filter which is widely used to filtering the noise and disturbance in a random system.

In a practical dynamic system, there are three types of uncertainties:

random disturbance
measurement noise
uncertainty of system parameters

So we encounter such a problem: how we estimate the state of a system from the measurement affected by noises? Here the Kalman filter gives an answer.

Prior knowledge

Bayesian estimation, see , you should look for the prior and posterior.

What is Kalman filter

First, always remember that the Kalman filter is an optimal algorithm, when we mention optimal, that means we are going to solve an optimal problem

$\min E\left \| x-\hat{x} \right \|^{2}$

So now, let’s give out the basic dynamic system model we are going to deal with and the mathematical description about Kalman filter.

Basic dynamic system

In a Kalman filter, we assume that the real state at time $k$ is determined by last time $k-1$:

$\mathbf{x}_{k}=\mathbf{F}_{k} \mathbf{x}_{k-1}+\mathbf{B}_{k} \mathbf{u}_{k}+\mathbf{w}_{k}$

where

$\mathbf{F}_{k}$ is the transfer matrix
$\mathbf{B}_{k}$ is the input matrix
$\mathbf{w}{k}$ is the process noise satisfies $\mathbf{w}{k} \sim N\left(0, \mathbf{Q}_{k}\right)$.

At time $k$, a measurement $\mathbf{z}{k}$ of $\mathbf{x}{k}$ satisfies:

$\mathbf{z}_{k}=\mathbf{H}_{k} \mathbf{x}_{k}+\mathbf{v}_{k}$

where

$\mathbf{H}_{k}$ is the observation matrix
$ \mathbf{v}{k} $ is the measurement noise satisfies $ \mathbf{v}{k} \sim N\left(0, \mathbf{R}_{k}\right) $.

Mathematical description

The state of a Kalman filter can be represented by the following two variables:

$\hat{\mathbf{x}}_{k|k-1}$: estimation at time $k-1$ without current measurement（先验状态估计，不太好的估计）.
$\hat{\mathbf{x}}_{k|k}$: estimation at time $k$ based on the current measurement（后验状态估计，比较好的估计）.
$\hat{\mathbf{P}}_{k|k-1}$: prior covariance matrix of estimation error
$\hat{\mathbf{P}}_{k|k}$: posterior covariance matrix of estimation error, used for judge the estimation precision.

$\hat{\mathbf{P}}_{k|k}=E[(\mathbf{x}_k-\hat{\mathbf{x}}_{k|k})(\mathbf{x}_k-\hat{\mathbf{x}}_{k|k})^T]$

There are two main steps in Kalman filter, estimation and update:

Estimation (“Predict with prior knowledge”)

$\begin{equation} \begin{aligned} \hat{\mathbf{x}}_{k | k-1} &=\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1 | k-1}+\mathbf{B}_{k} \mathbf{u}_{k}\\ \mathbf{P}_{k | k-1} &=\mathbf{F}_{k} \mathbf{P}_{k-1 | k-1} \mathbf{F}_{k}^{T}+\mathbf{Q}_{k} \end{aligned} \end{equation}$

Update based on measurement (“Correct using posterior knowledge”)

First we need to compute three variables:

$\begin{equation} \begin{array}{l}{\tilde{\mathbf{y}}_{k}=\mathbf{z}_{k}-\mathbf{H}_{k} \hat{\mathbf{x}}_{k | k-1} } \\ {\mathbf{S}_{k}=\mathbf{H}_{k} \mathbf{P}_{k | k-1} \mathbf{H}_{k}^{T}+\mathbf{R}_{k}} \\ {\mathbf{K}_{k}=\mathbf{P}_{k | k-1} \mathbf{H}_{k}^{T} \mathbf{S}_{k}^{-1}}\end{array} \end{equation}$

$\tilde{\mathbf{y}}_{k}$ is the measurement residual (error between measurement and estimation)
$\mathbf{S}_{k}$ is the covariance matrix of measurement residual
$\mathbf{K}_{k}$ is the optimal Kalman gain

Then we use them to update variable $\mathbf{x}$ and $\mathbf{P}$

$\begin{equation} \begin{aligned} \hat{\mathbf{x}}_{k | k} &=\hat{\mathbf{x}}_{k | k-1}+\mathbf{K}_{k} ({\mathbf{y}}_{k}-\mathbf{H\hat{\mathbf{x}}_{k | k-1}})\\ \mathbf{P}_{k | k} &=\left(I-\mathbf{K}_{k} \mathbf{H}_{k}\right) \mathbf{P}_{k | k-1} \end{aligned} \end{equation}$

The first function is the updated state estimation
The second is the updated covariance matrix of estimation error

从上面的公式可以看出，卡尔曼增益$\mathbf{K}$实际代表了预测误差与测量误差之间的比重，当$\mathbf{K}=0$，则完全依靠预测，而当$\mathbf{K}=1$，则完全依靠测量。对于上式，两边各减去真实状态$\mathbf{x}_{k}$，可得：

$\hat{\mathbf{x}}_{k | k} - \mathbf{x}_{k | k} =\hat{\mathbf{x}}_{k | k-1} - \mathbf{x}_{k | k}+\mathbf{K}_{k} ({\mathbf{y}}_{k}-\mathbf{H\hat{\mathbf{x}}_{k | k-1}})$

即后验估计误差与先验估计误差之间的关系：

$\mathbf{e}_{k | k} ={\mathbf{e}}_{k | k-1}+\mathbf{K}_{k} ({\mathbf{y}}_{k}-\mathbf{H\hat{\mathbf{x}}_{k | k-1}})$

那么问题来了，如何选择$\mathbf{K}$，从而达到最优呢？我们可以

Application

In this section we are going to design a Kalman filter for the inverted pendulum mentioned in State feedback control.