tutorials/kf.tex

   1 \documentclass[12pt]{article}%
   2 \usepackage{amsmath}
   3 \usepackage{amsfonts}
   4 \usepackage{amssymb}
   5 \usepackage{graphicx}
   6 \usepackage{fullpage}
   7 \usepackage{color}
   8 \usepackage{hyperref}%
   9
  10 \begin{document}
  11
  12 We present the Kalman filter in perhaps the most used form, as extended to nonlinear models.
  13  Consider a discrete time model of some natural
  14 process. At time step $k$, the model has state $u_{k}\in\mathbb{R}^{n}$, which
  15 can be approximated from the previous step $u_{k-1}$ by applying the model
  16 $\mathcal{M}$ to get a forecast $u_{k}^{f}=\mathcal{M}\left(  u_{k-1}\right)
  17 $. We model uncertainty in the model itself by adding normally distributed
  18 noise with mean zero and covariance $Q$ to the uncertainty of $u_{k}^{f}$. We
  19 also need to estimate now the uncertainty in the previous state $u_{k-1}$
  20 propagates to the uncertainty of the forecast $u_{k}^{f}$. So, assume that the
  21 model is differentiable and quantify the uncertainty of the state by a
  22 covariance matrix. That is,  assume that at step $k-1$, the state has
  23 (approximately) normal distribution with mean $u_{k-1}$ and covariance
  24 $P_{k-1}$. Using the Taylor expansion of order $1$ of the model operator at
  25 $u_{k-1}$, $\mathcal{M}\left(  u\right)  \approx\mathcal{M}\left(
  26 u_{k-1}\right)  +\mathcal{M}^{\prime}\left(  u_{k-1}\right)  \left(
  27 u-u_{k-1}\right)  $, where $\mathcal{M}^{\prime}\left(  u_{k-1}\right)  $ is
  28 the Jacobian matrix of $\mathcal{M}$ at $u_{k-1}$. It can be shown that the
  29 forecast has then (approximately)\ normal distribution with mean and
  30 covariance
  31 $$
  32 u_{k}^{f}=\mathcal{M}\left(  u_{k-1}\right)  ,\ P_{k}^{f}=\mathcal{M}\left(
  33 u_{k-1}\right)  P_{k-1}\mathcal{M}^{\prime}\left(  u_{k-1}\right)  +Q
  34 $$
  35 At time $k$, we also have an observation $d_{k}\approx Hu_{k}$, where $H$ is a
  36 given observation operator, and we want to find $u_{k}$ so that both
  37 $$
  38 u_{k}\approx u_{k}^{f}\text{ and }d_{k}\approx Hu_{k}.
  39 $$
  40 We quantify the uncertainly of the error of observation $d_{k}$ by a covariance
  41 matrix $R$: assume that the observation error has normal probability
  42 distribution with a known covariance $R$. Then, the likelihood of state $u$ is
  43 proportional to $e^{-\left\Vert d_{k}-Hu\right\Vert _{R^{-1}}^{2}/2}$, where
  44 we used the notation for the norm $\left\Vert v\right\Vert _{A}%
  45 =\left(v^{\top}Av\right)^{1/2}$ induced by a positive definite matrix $A$. Similarly, we quantify the
  46 uncertainty of the state by a covariance matrix $P_{k}$. That is, the forecast
  47 state has (approximately) normal distribution with mean $u_{k}^{f}$  and covariance
  48 $P_{k}^{f}$. From the Bayes theorem of statistics, the probability distribution
  49 of the state after taking the data into account has density%
  50 $$
  51 p_{k}\left(  u\right) \propto e^\frac{-\left\Vert d_{k}
  52 -Hu\right\Vert_{R^{-1}}^{2}}{2}e^\frac{-\left\Vert u-u_{k}^{f}\right\Vert _{
  53 {P_{k}^f}^{-1}  }^{2}}{2}%
  54 $$
  55 where $\propto$ means proportional.
  56 Note that the probability density at $u$ is maximal when $\left\Vert
  57 d_{k}-Hu\right\Vert _{R^{-1}}^{2}+\left\Vert u-u_{k}\right\Vert _{{P_{k}^{f}}^{-1}}^{2}$
  58  is minimal, which quantifies the statement that $d_{k}\approx
  59 Hu_{k}$ and $u\approx u_{k}^{f}$.  By a direct computation completing the
  60 square and using the Sherman-Morrison-Woodbury formula,
  61 $$p_{k}\left(
  62                 u
  63            \right) \propto
  64 e^{-\frac{
  65         \left\Vert u-u_{k
  66                  }
  67         \right\Vert_
  68                 {P_{k
  69                       }^{-1}
  70                 }^{2}
  71         }
  72         {2}},
  73 $$
  74 which is the density of the normal distribution with the mean
  75 $$
  76 u_{k}^{f}=u_{k}^{f}+K_{k}(d-Hu_{k}^{f}),\ \text{where }K_{k}=P_{k}%
  77 ^{f}H^{\mathrm{T}}(HP_{k}^{f}H^{\mathrm{T}}+R)^{-1}%
  78 $$
  79 and covariance
  80 $$
  81 P_{k}=\left(  \left(  P_{k}^{f}\right)  ^{-1}+H^{\mathrm{T}}R^{-1}H\right)
  82 ^{-1}=(I-KH)P_{k}^{f}.
  83 $$
  84
  85 These are the equations of the extended Kalman filter. The original Kalman (1960) filter was
  86 formulated for a linear process. The extension to the
  87 nonlinear case made broad array of applications possible, including the Apollo spacecraft naviation (McGee and Schmidt, 1966),  and is
  88 still a de-facto standard in navigation and GPS.
  89
  90 \end{document}