\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}}
\newcommand{\partialat}[3]{\frac{\partial {#1}}{\partial {#2}}(#3)}

\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, F09)}{Lecture \#20 (Oct. 29)}{Lecturer: Drew Bagnell}{Scribe: Alberto Rodriguez}{Gaussian Processes (part 1)}

\section{Practical Kalman Filtering (continuation)}
One of the problems with the formulation of Kalman Filter is that it only has nice closed form update rules when the dynamics of the system is linear. There are two ways to deal with that limitation:

\begin{enumerate}
  \item Linearize the dynamics at every step (\emph{Iterative Linearization}).
  \item Do sample based estimation of the necessary statistics (\emph{Montecarlo Kalman Filter}).
\end{enumerate}

For the rest of the section we will suppose that we are dealing with the system:

\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}

\subsection{Iterative Linearization}
As seen in last lecture, the most straightforward way to apply Kalman Filtering whenever the dynamics is not linear is to use first order approximation of both the motion and observation models at every step.

\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}

In the last lecture we saw how this approximation yields new update rules. In those update rules,  matrices A and C are iteratively being estimated by the Jacobian (best first order Taylor approximation of the system) of the  of the motion and observation models respectively. The problem with this approach is that the first order Taylor expansion of the dynamic equations of the system is not a robust approximation for most non-linear functions. 

An \emph{Statistically Linearized Kalman Filter} tries to overcome that limitation by approximating the Jacobian matrix of the system in a broader region centered in the state of the system.

\subsection{Montecarlo Kalman Filter}

The second option for dealing with non-linear systems is to replace the update rules by sampled based estimations. 

\subsubsection*{Motion Model}
Given the equation of the motion model $x_{t+1} = f(x_t) + \epsilon$, $\mu_t$ and $\Sigma_t$ we need to estimate $\mu^{-}_{t+1}$ and $\Sigma^-_{t+1}$. For that we draw samples from the prior distribution ${x_t^i}$ and pass them through $f(x)$. That way, and with the law of large numbers in hand, we can estimate:

\begin{eqnarray}
\mu^-_{x_{t+1}} & = & \frac{1}{N}\sum_{i=1}^N f(x_t^i)\\
\Sigma^-_{x_{t+1}} & = & \frac{1}{N}\left[\sum\left(f(x_t^i) - \mu^-_{x_{t+1}}\right)\cdot\left(f(x_t^i) - \mu^-_{x_{t+1}}\right)^T\right] + Q
\end{eqnarray}
NOTE: $Q$ is additive noise, uncorrelated with $x_t$.

\subsubsection*{Observation Model}
Given the equation of the observation model $y_{t+1} = g(x_{t+1}) + \delta$ the update rules are be the same as before:

\begin{eqnarray}
\mu^+_{x_{t+1}} & =&  \mu^-_{x_{t+1}} + \Sigma_{XY}\Sigma_{YY}^{-1}\left(y_t - \mu_y\right)\\
\Sigma^+_{x_{t+1}} & =&  \Sigma^-_{t+1} - \Sigma_{XY}\Sigma_{YY}^{-1}\Sigma_{YX}
\end{eqnarray}

But now we approximate $\mu_y$, $\Sigma_{YY}$ and $\Sigma_{XY}$ as:

\begin{eqnarray}
\mu_x & = & \frac{1}{N} \sum_{i=1}^N g(x^i_{t+1})\\
\Sigma_{YY} & = & \frac{1}{N}\left[\sum_{i=1}^N\left(g(x_{t+1}^i) - \mu_y\right)\cdot\left(g(x_{t+1}^i - \mu_y\right)^T\right] + R\\
\Sigma_{XY} & = & \frac{1}{N}\left[\sum_{i=1}^N\left(x_{t+1}^i - \mu_{x_{t+1}}\right)\cdot\left(g(x_{t+1}^i - \mu_y\right)^T\right]
\end{eqnarray}

In comparison with a standard Particle Filter, a Montecarlo Kalman Filter forces some smoothing on the uncertainty what simplifies the process to get a solution. On the other hand it will always be unimodal, while a Kalman Filter can perfectly maintain several modes in the estimation of the distribution.

\subsection{Sigma-Point Filter}
A Sigma-Point Filter has exactly the same formulation than a Montecarlo Kalman Filter but it draws samples in a deterministic way from interesting locations. Suppose  ${\Delta_i}$ are the eigenvectors of the covariance matrix $\Sigma_{x_t}$. Then we sample the points as:

\begin{displaymath}
\mu_{x_{t}} \pm\lambda_i\Delta_i \ \ \ \  i=1\ldots D
\end{displaymath}

where $D$ is the dimension of $x_t$ and $\lambda_i$ is proportional to the eigenvalue correspondent to eigenvector $\Delta_i$. Then, the update rule for the motion model becomes:

\begin{eqnarray}
\mu^-_{x_{t+1}} & = & \frac{1}{N}w_i\sum_{i=1}^N f(x_t^i)\\
\Sigma^-_{x_{t+1}} & = & \frac{1}{N}w_i\left[\sum\left(f(x_t^i) - \mu^-_{x_{t+1}}\right)\cdot\left(f(x_t^i) - \mu^-_{x_{t+1}}\right)^T\right] + Q
\end{eqnarray}

There are different versions of Sigma-Point filters and they all differ on how weights $w_i$ are selected. All versions chose weights so that the method behaves perfectly for a gaussian model (linear dynamics) and then optimize the weights for with a different criteria. Different versions include Unscented Kalman Filter, Central Difference Kalman Filter, ... 

The computational cost of Sigma-Point type filters is $O(d^3)$ for the eigenvector finding (usually implemented by the SVD decomposition) plus $2d+1$ evaluations of the motion model $f(x)$.

In comparison with a Montecarlo Kalman Filter, a Sigma-Point Filter needs less particles to run and, hence, reduces the number of motion model evaluations, than can be costly. It only needs to be implemented once because all the process is independent of the model ($f,g$). However, it can perform really bad if facing an adversarial problem, because it samples the space in a deterministic way.

\section{Gaussian Processes}
[\emph{NOTE: State of the art for non-linear regression.}]

A gaussian process is a random stochastic process where correlation is introduced between neighboring samples (think of a stochastic process as a sequence of random variables). The same way that an instance of a random variable is a single sample, an instance of a stochastic process can be thought as vector of samples:

\begin{equation}
X = \left[ x_1, x_2, x_3 \ldots\right]
\end{equation}

Gaussian Processes artificially introduce correlation between close samples in that vector in order to enforce some sort of smoothness on the succession of samples. The way that correlation is introduced is by constructing the joint probability distribution of the long vector of samples. Gaussian processes assume that probability distribution to be a multidimensional gaussian:

\begin{equation}
p(X^i) = \frac{1}{z} e^{(X - \mu)\Sigma^{-1}(X - \mu)}
\end{equation}
The correlation between samples in the succession $X^i$ depends on matrix $\Sigma$. In Gaussian Processes the covariance matrix is constructed as the Gram matrix of the samples with some desired kernel $\kappa(\cdot,\cdot)$ as the inner product:

\begin{equation}\label{eq:kappa}
\begin{gathered}
\Sigma_{ff}=\left( \begin{array}{cccc}
\kappa(x_1,x_1)& \kappa(x_1,x_2) & \dotsb & \kappa(x_1,x_n)\\
\kappa(x_2,x_1)& \kappa(x_2,x_2) & \dotsb & \kappa(x_2,x_n)\\
\vdots & & & \vdots\\
\kappa(x_n,x_1)& \kappa(x_n,x_2) & \ldots & \kappa(x_n,x_n)\end{array} \right)
\end{gathered}
\end{equation}

In order to introduce correlation between neighboring samples, the kernel $\kappa$ is usually designed to have small support and centered around zero (i.e. a triangular or a gaussian). The kernel usually can be described as a function of the distance $x_i-x_j$.

\subsection{Gaussian Processes as a prior in function space}
A gaussian process can be used as a prior probability distribution over the space of functions when using a Bayesian approach. By doing so, we are imposing that functions where neighboring samples are correlated are more probable. We are enforcing functions to have some sort of smoothness. 

Suppose we have some estimates (mean and variance) of the value of a function f(x) in certain locations $F = \left[f(x_1),f(x_2),f(x_3) \ldots f(x_n)\right]$. Then we can interpret vector $F$ as a gaussian process and give it a specific probability:

\begin{equation}
p(F) = \frac{1}{z} e^{(F - \mu)\Sigma^{-1}(F - \mu)}
\end{equation}

With $\Sigma$ constructed the same way as in equation \ref{eq:kappa} so that we introduce correlation between the values of $f$ at neighboring locations $x_i, x_j$. This construction will allow us to infer the most probable value of the function in other locations.   

\end{document}
