\documentclass[11pt]{article}
\usepackage{latexsym}
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{amsthm}
\usepackage{epsfig}
\usepackage[tight]{subfigure}

\newcommand{\handout}[5]{
  \noindent
  \begin{center}
  \framebox{
    \vbox{
      \hbox to 5.78in { {#1} \hfill #2 }
      \vspace{4mm}
      \hbox to 5.78in { {\Large \hfill #5  \hfill} }
      \vspace{2mm}
      \hbox to 5.78in { {\em #3 \hfill #4} }
    }
  }
  \end{center}
  \vspace*{4mm}
}

\newcommand{\lecture}[5]{\handout{#1}{#2}{#3}{#4}{#5}}

\newtheorem{theorem}{Theorem}
\newtheorem{corollary}[theorem]{Corollary}
\newtheorem{lemma}[theorem]{Lemma}
\newtheorem{observation}[theorem]{Observation}
\newtheorem{proposition}[theorem]{Proposition}
\newtheorem{definition}[theorem]{Definition}
\newtheorem{claim}[theorem]{Claim}
\newtheorem{fact}[theorem]{Fact}
\newtheorem{assumption}[theorem]{Assumption}

% 1-inch margins, from fullpage.sty by H.Partl, Version 2, Dec. 15, 1988.
\topmargin 0pt
\advance \topmargin by -\headheight
\advance \topmargin by -\headsep
\textheight 8.9in
\oddsidemargin 0pt
\evensidemargin \oddsidemargin
\marginparwidth 0.5in
\textwidth 6.5in

\parindent 0in
\parskip 1.5ex
%\renewcommand{\baselinestretch}{1.25}

\begin{document}

\lecture{Statistical Techniques in Robotics (16-831, F08)}{Lecture \#20 (Oct. 30)}{Lecturer: Drew Bagnell}{Scribe: Alex Grubb}{Extensions to Kalman Filtering}

\section{The Extended Kalman Filter}
Previously, we used a Kalman Filter which could only model linear
transformations:
\begin{eqnarray}
x_{t+1} &=& A x_t + \varepsilon, \qquad
    \textrm{where}~\varepsilon \sim \mathcal{N}(0, Q)\\
y_{t+1} &=& A x_{t+1} + \delta, \qquad
    \textrm{where}~\delta \sim \mathcal{N}(0, R)
\end{eqnarray}

Now we would like to be able to model non-linear transformations with our
filter:
\begin{eqnarray}
x_{t+1} &=& f(x_t) + \varepsilon, \qquad
    \textrm{where}~\varepsilon \sim \mathcal{N}(0, Q)\\
y_{t+1} &=& g(x_{t+1}) + \delta, \qquad
    \textrm{where}~\delta \sim \mathcal{N}(0, R)
\end{eqnarray}

\newcommand{\partialat}[3]{\frac{\partial {#1}}{\partial {#2}}(#3)}

One way to deal with this non-linearity is to use the taylor expansion.  In
this case we approximate the transformations using the first order taylor
expansion evaluated at the mean estimate of $x$ at time $t$, $\mu_t$:
\begin{eqnarray}
x_{t+1} &\approx& f(\mu_t) + \partialat{f}{x}{\mu_t} (x_t - \mu_t)
                  + \varepsilon\\
y_{t+1} &\approx& g(\mu_{t+1}) + \partialat{g}{x}{\mu_{t+1}}
                  (x_{t+1} - \mu_{t+1}) + \delta
\end{eqnarray}

The first order derivative term $\partialat{f}{x}{\mu_t}$, also referred to
as the Jacobian, is used here as the linear transformation, while the
$f(\mu_t)$ term just serves to shift the mean of the transformation.

\subsection{New Update Rules}
Using the non-linear transformation above, we get the following new update
equations for our mean and variance estimates for the motion model:
\begin{eqnarray}
\mu^-_{t+1} &=& f(\mu^+_t) \\
\Sigma^-_{t+1} &=& J_f \Sigma^+_t J_f^T + Q
\end{eqnarray}

And for the sensor model:
\begin{eqnarray}
\mu^+_{t+1} &=& \mu^-_{t+1} + \Sigma^-_{t+1} J_g^T
                (J_g \Sigma^-_{t+1} J_g^T + R)^{-1} [y_{t+1} - g(\mu^-_{t+1})]\\
\Sigma^+_{t+1} &=& \Sigma^-_{t+1} - \Sigma^-_{t+1} J_g^T
                (J_g \Sigma^-_{t+1} J_g^T + R)^{-1} J_g \Sigma^-_{t+1}
\end{eqnarray}

where $J_f = \partialat{f}{x}{\mu^+_t}$ and $J_g = \partialat{g}{x}{\mu^-_{t+1}}$.

\subsection{Problems with Extended Kalman Filtering}
\begin{itemize}
\item Taylor expansion is a poor approximation of most non-linear functions.

\item Very difficult to implement correctly: you will most likely get the Jacobian wrong
on the first try.  Even when the Jacobian is wrong, it can be hard to tell whether your
implementation is incorrect or the poor filtering results are due to the Taylor expansion.
\end{itemize}

\subsection{Example: Non-Linear Regression}
In this example we want to use an EKF for a non-linear regression problem.  In this case
our state vector is a set of weights $w$ and we wish to estimate the mean and variance
$\mu_w, \Sigma_w$ with our EKF, which we will just refer to as $\mu$ and $\Sigma$ from
here on.

The motion model is for stationary weights:
\begin{eqnarray}
\mu^-_{t+1} &=& \mu^+_t\\
\Sigma^-_{t+1} &=& \Sigma^+_t
\end{eqnarray}

For our observation model we want to use the non-linear sigmoid function
$\sigma(x) = \frac{1}{1 + \exp(x)}$:
\begin{eqnarray}
y_t &=& \sigma(w^Tx) + \delta, \qquad \textrm{where}~~\delta \sim \mathcal{N}(0, \gamma^2)\\
\end{eqnarray}

Note that our output in this case is a single value, so our Jacobian will be a vector.
We can find the Jacobian $J_{\sigma}$ using the chain rule and the fact that
$\frac{\partial \sigma}{\partial x} = \sigma(x) (1 - \sigma(x))$.  Here we actually
want to find the derivative of $g(w) = \sigma(w^Tx)$, so we use the chain rule
with $z = w^Tx$.

\begin{eqnarray} 
\frac{\partial{g}}{\partial{w}} &=& \frac{\partial{g}}{\partial{z}}\frac{\partial{z}}{\partial{w}}\\
    &=& \sigma(w^Tx) (1 - \sigma(w^Tx)) x^T\\
J_{\sigma} &=& \partialat{g}{w}{\mu}\\
          &=& \sigma(\mu^Tx) (1 - \sigma(\mu^Tx)) x^T
\end{eqnarray}

This gives update equations:
\begin{eqnarray}
\mu^+_{t+1} &=& \mu^-_{t+1} + \Sigma^-_{t+1} J_{\sigma}^T
                (J_{\sigma} \Sigma^-_{t+1} J_{\sigma}^T + \gamma^2)^{-1} [y_{t+1} - \sigma({\mu^-_{t+1}}^T x)]\\
\Sigma^+_{t+1} &=& \Sigma^-_{t+1} - \Sigma^-_{t+1} J_{\sigma}^T
                (J_{\sigma} \Sigma^-_{t+1} J_{\sigma}^T + \gamma^2)^{-1} J_{\sigma} \Sigma^-_{t+1}
\end{eqnarray}

\section{Other Non-linear Techniques}

\subsection{Statistical Linearization}
One initial improvement on the linearization above is to use a method other than
the Taylor expansion to find a linear approximation for $f$ and $g$.
In statistical linearization we sample points around the mean estimate and
then fit a line, plane, etc. to the sampled points and use that linear
approximation to run the Kalman filter.

\subsection{Unscented Kalman Filter}
Another sampling-based method for dealing with non-linearities is to use the
unscented transform.  Here we simply sample points from the current mean and
covariance and transform them according to the non-linear function $f$ in our model,
and then fit a new Gaussian to these transformed points, giving an updated
mean and covariance matrix.  This transformation is called the unscented
transformation, from which the UKF derives it's name.  For a pictorial
comparison, see Figure \ref{fig-ukf}.


\begin{figure}[h]
\centerline{
  \mbox{\includegraphics[width = 5in]{ut.eps}}}
  \caption{A comparison of the actual transformation, and the approximations given
    by the linear approximation and the unscented transformation. 
    (Source: http://cslu.cse.ogi.edu/nsel/ukf/node6.html)}
\label{fig-ukf}
\end{figure}

Typically the points used are the mean, and then points sampled at set distances
along the principal axes of the Gaussian (the eigenvectors of the covariance matrix).
The transformed points are also usually weighted based on the original mean and
covariance and selection of these points, and a new distribution is fit to
the weighted points.

The above method works for the motion model, but for the sensor model we need to
operate a little differently.  Here we transform the sampled points according to
our sensor transform $g$, and then use the sampled points and the original points
to estimate the unknown portions of the mean and covariance in block form:
\begin{eqnarray}
\mathcal{N}(
\left[ \begin{array}{c} \mu_x \\ \mu_y \end{array} \right]
,
\left[ \begin{array}{cc} \Sigma_{xx} & \Sigma_{xy} \\
           \Sigma{yx} & \Sigma {yy} \end{array} \right])
\end{eqnarray}

For example, the matrix $\Sigma_{yy}$ can be found by simply taking the covariance
of the transformed points.  Once we have estimated all quantities above, the
resulting conditional distribution is easily found using the standard
equations, E.G. the conditional mean is given as:
\begin{eqnarray}
\mu_{x|y} = \mu_x + \Sigma_{xy} (\Sigma_{yy})^{-1} [y - \mu_y]
\end{eqnarray}

There are a number of different filters that use this technique, but differ in
how the select the points used and how the points are weighted.  This includes:
Sigma Point, Unscented, and Central Difference Filters.

\subsubsection{Advantages / Disadvantages}
\begin{itemize}
\item Advantage: You only need to implement the UKF once, as the implementation
is independent of the models (I.E. $f,g$) used.  It's also easier to implement
correctly.

\item Disadvantage: It will be slower than the regular Kalman filtering,
and you don't gain anything if your transformation is linear.
\end{itemize}

\end{document}
