Kalman Filter

Kalman filtering (and filtering in general) considers the following setting: we have a sequence of states $x_t$ , which evolves under random perturbations over time. Unfortunately we cannot observe $x_t$ , we can only observe some noisy function of $x_t$ , namely, $y_t$ . Our task is to find the best estimate of $x_t$ given our observations of $y_t$ .

Consider the equations

$\begin{aligned} \bm x_{t+1} &= A_t \bm x_t + B_t \bm a_t + \bm \epsilon_{t}\, \\ \bm y_{t+1} &= C_t\bm x_{t+1} + \bm \eta_t\, .\end{aligned}$

where $\epsilon_t \sim \mathcal N ( 0, \Sigma^\epsilon_t)$ , $\eta_t ) \sim \mathcal N ( 0, \Sigma^\eta_t)$ and $\epsilon_t$ and $\nu_t$ are independent. (We let $\Sigma^{\epsilon}$ be the sub-matrix of the covariance matrix corresponding to $\epsilon$ and so forth…)

The Kalman filter has two update stages: a prediction update and a measurement update. These are

Screenshot 2019-05-14 at 07.12.37.png and

where

$\begin{aligned} K_t = P_{t+1\, |\, t} C^\top_t ( C_t P_{t+1|t} C_t^\top + \Sigma^\eta_t )\, .\end{aligned}$

The matrix $K_t$ is often referred to as the Kalman Gain. Assuming the initial state $x_0$ is known and deterministic $P_{0|0} =0$ in the above.

We will use the following proposition, which is a standard result on normally distributed random vectors, variances and covariances,

Prop 1. Let $u$ be normally distributed vector with mean $\bar{u}$ and covariance $\Sigma_{ u}$ , i.e.

$\bm u \sim \mathcal N (\bar{\bm u}, \Sigma_{\bm u })\, .$

i) For any matrix $A$ and (constant) vector $c$ , we have that

$A\bm u +\bm c \sim \mathcal N (A \bar{\bm u} + \bm c , A \Sigma_{\bm u \bm u} A^\top )\, .$

ii) If we take $u = ( v, w)$ then $w$ conditional on $v$ gives

$(\bm w | \bm v) \sim \mathcal N (\bar{\bm w} + \Sigma_{w v} \Sigma^{-1}_{v v} (\bm v - \bar{\bm v}) ,\Sigma_{ww} - \Sigma_{w v} \Sigma^{-1}_{v v} \Sigma_{v w} )$

iii) $Var ( A u ) = A\Sigma_u A^\top$ , $Cov ( Au , Bu) = A \Sigma_u B^\top$ .

We can justify the Kalman filtering steps by proving that the conditional distribution of $x_{t+1}$ is given by the Prediction and measurement steps. Specifically we have the following.

Theorem 1.

$\begin{aligned} (\bm x_{t+1} | \bm y_{[0:t]}, \bm a_{[0:t]} ) & \sim \mathcal N ( \hat{\bm x}_{t+1|t } , P_{t+1|t} ) \\ (\bm x_{t+1} | \bm y_{[0:t+1]}, \bm a_{[0:t]} ) & \sim \mathcal N ( \hat{\bm x}_{t+1|t+1 } , P_{t+1|t+1} )\end{aligned}$

where $y_{[0:t]} := (y_0,...,y_{t})$ and $a_{[0:t]} := (a_0,...,a_{t})$ .

Proof. We show the result by induction supposing that

$(\bm x_{t} | \bm y_{[0:t]}, \bm a_{[0:t]} ) \sim \mathcal N ( \hat{\bm x}_{t|t } , P_{t|t} )\, .$

Since $x_{t+1}$ is a linear function of $x_t$ , we have that

$(\bm x_{t+1} | \bm y_{[0:t]}, \bm a_{[0:t]} ) \sim \mathcal N ( \hat{\bm x}_{t+1|t } , P_{t+1|t} )\, .$

where, by Prop 1ii), we have that

$\hat{\bm x}_{t+1} = A_t \hat{\bm x}_{t|t} + B_t a_t\, , \qquad P_{t+1|t} = A_t P_{t|t} A^\top_t + \Sigma^\epsilon_t\, .$

Given $y_{t+1} = C_t x_{t+1} + \eta_t$ , we have by Prop 1iii) that $Var(y_{t+1} | y_{[0,t]}, a_{[0,t]}) = C_t P_{t+1|t} C^\top_t$ and $Cov(x_{t+1}, y_{t+1} | y_{[0,t]}, a_{[0,t]}) = P_{t+1|t} C^\top_t$ . Thus

$\begin{aligned} ( [\bm x_{t+1} , \bm y_{t+1}] | \bm y_{[0:t]} , \bm a_{[0:t]} ) \sim \mathcal N \left( [ \hat{\bm x}_{t+1|t } , C_t \hat{\bm x}_{t+1|t } ] , \left[ \begin{array}{ll} P_{t+1|t} & P_{t+1} C^\top_t \\ C_t P_{t+1|t} & C_t P_{t+1} C_t^\top + \Sigma^\eta_t \end{array} \right) \right]\, .\end{aligned}$

Thus applying Prop 1ii), we get that

$\begin{aligned} (x_{t+1} | y_{[0:t+1]} , a_{[0:t]} ) & = ( (x_{t+1} | y_{[0:t]} , a_{[0:t]} ) | y_{t+1}) \\ & \sim \mathcal N \Big( \hat{\bm x}_{t+1|t} + P_{t+1 | t} C^\top_t [ C_t P_{t+1} C^\top_t + \Sigma^\eta_t ]^{-1} ( y_{t+1} - C_t \hat{\bm x}_{t+1|t} ) \, ,\\ & \qquad\qquad \qquad\quad P_{t+1|t} - P_{t+1|t} C^\top_t [ C_t P_{t+1|t} C_t^\top + \Sigma^\eta_t ]^{-1} C_t P_{t+1|t} \Big)\, . \end{aligned}$

$ \square$

Share this:

Like this:

Leave a Reply Cancel reply