Table of Contents

Kalman filter

The Kalman filter is used to estimate the state of a process $X$, given noisy measurements $Z$.

The construction of the Kalman filter makes the following assumptions:

  1. $X$ is a linear process. So, basically, the current state of $X$ can be obtained by multiplying the previous state by a matrix $F$…
  2. …except for the fact that we can also control the process by modifying control variables $U$, which has a linear contribution to the state of process $X$ via a matrix $B$.
  3. There is a linear relationship between $Z$ (the measurement) and $X$ (the state), given by a matrix $H$.
  4. $X$ and $Z$ are subject to Gaussian noise.

These conditions can be expressed as the following equations:

$$ \begin{aligned} x_k & = Fx_{k-1} + Bu_k + w_k \\ z_k & = H_k x_k + v_k \end{aligned} $$

where $x_k$ and $z_k$ are the state and the measurements in discrete time step $k$, $u_k$ are control inputs and $v_k$ and $w_k$ represent noise, more precisely, $v_k$ has a Gaussian distribution $N(0, Q)$ and $w_k$ has a Gaussian distribution $N(0, R)$ where $Q$ and $R$ are covariance matrices.

The filter

The Kalman filter keeps an estimation of the current state $\hat{x}_{k}$ and an error covariance estimate $P_k$.

Kalman filtering operates in two stages:

1. Predict: calculate the “a priori” estimates, which are denoted with a minus sign as a superscript:

$$ \begin{aligned} \hat{x}^{-}_{k} & = F\hat{x}_{k-1} + Bu_k \\ P_k^{-} & = FP_{k-1}F^{T} + Q \end{aligned} $$

2. Correct:

$$ \begin{aligned} K_k &= P_k^{-}H^{T}(HP_k^{-}H^{T}+R)^{-1} \\ \hat{x}_{k} &= \hat{x}^{-}_{k} + K_k(z_k-H\hat{x}^{-}_{k})\\ P_k &= (I-K_k H)P_k^{-} \end{aligned} $$

$K_k$ is called gain or blending factor. $z_k-H\hat{x}^{-}_{k}$ is called innovation and it measures the agreement between the predicted measurement $H\hat{x}^{-}_{k}$ and the actual measurement $z_k$.

References

An Introduction to the Kalman Filter by Greg Welch and Gary Bishop.