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

\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 \#4 (Sept 4, 2008)}{Lecturer: Drew Bagnell}{Scribe: Mark Desnoyer}{Particle Filters}

\tableofcontents

\section{Motivation}
Last time we looked at discrete Bayes filters to probabilistically identify a state. However, this technique requires discretizing the entire state space and keeping track of each cell at all times. For many applications, like localizing a robot in a gridded room, this can be very computationally expensive. So, instead of breaking up the state space, we will describe the current state as a sample of possible states.

\section{Intuition}
Describing the current state as a set of samples can be expressed as:

\begin{equation}
  E_p[x] = \frac{1}{R} \sum_{i=1}^R f(x_{i}) \qquad \text{where} \qquad x_i \qquad \text{is sampled from} \qquad p(x_t | z_0, z_1, \ldots, z_t)
\end{equation}

However, it is often not possible to sample from $p(x_t | z_0, z_1, \ldots, z_t)$ directly, so instead, we will sample from a different distribution and them make a correction. This process is called importance sampling.


\section{Particle Filtering}
\subsection{Importance Sampling}
Importance sampling is the process of sampling from a different distribution $q$, which is related to $p$ to compute a value we're interested in and then correcting the result. This works because

\begin{align}
  E_p[f(x)] &= \sum_{x} p(x) \frac{q(x)}{q(x)} f(x) \nonumber \\
            &= \sum_{x} q(x)(\frac{p(x)}{q(x)} f(x)) \nonumber \\
            &= E_q[ \frac{p(x)}{q(x)} f(x)] \nonumber
\end{align}

Often, the motion model is used for $q$:
\begin{equation}
  q(x) = p(x_{t+1}|x_t)
\end{equation}

and the observation model is used for $\frac{p}{q}$:

\begin{align}
  \frac{p_i}{q_i} &= p(z | x_i) \\
                  &= w_i
\end{align}

The weights also need to be normalized, which can be done by:
\begin{equation}
  \hat{w_i} = \frac{w_i}{\sum w_i}
\end{equation}

This leads to the following particle filtering algorithm:

\begin{algorithm}
  \caption{Basic Particle Filter Algorithm}
  \begin{algorithmic}
    \FORALL{$i$}
    \STATE sample $x_{0}^i$ from $p(x_0)$
    \ENDFOR
    \FORALL{$t$}
    \FORALL{$i$}
    \STATE sample $x_{t}^i$ from $p(x_t | x_{t-1}^i)$
    \STATE $w_i \quad *= p(z_t | x_{t}^i)$
    \ENDFOR
    \STATE $\hat{w_i} = \frac{w_i}{\sum w_i}$
    \ENDFOR
    \STATE $E[f(x)] = \sum_i \hat{w_i}f(x_i)$
  \end{algorithmic}
\end{algorithm}

\subsection{Resampling}
However, because the observation model is only used to reweight the particles, as time increases, the particles will tend to diverge to different areas of the search space. In this case it is very easy to have one particle with a very high weight and many with very small weights, distributed over a very large search space. This is very inefficient.

So, to help solve this problem, we resample the data at every time step, thus pruning uninteresting solutions. This sampling is done so that the resulting particles are chosen from the previous set with a probability proportional to the previous weights (Figure 1.1):

\begin{equation}
p(x^i \leftarrow x^j) \quad \alpha \quad w^j
\end{equation}

After resampling, all the weights are then reset to 1. The resulting modified algorithm is:

\begin{algorithm}
  \label{WithResamplingAlgorithm}
  \caption{Particle Filter Algorithm With Resampling}
  \begin{algorithmic}
    \FORALL{$i$}
    \STATE sample $x_{0}^i$ from $p(x_0)$
    \ENDFOR
    \FORALL{$t$}
    \FORALL{$i$}
    \STATE sample $x_{t}^i$ from $p(x_t | x_{t-1}^i)$
    \STATE $w_i \quad *= p(z_t | x_{t}^i)$
    \ENDFOR
    \STATE $\hat{w_i} = \frac{w_i}{\sum w_i}$
    \FORALL {$i$}
    \STATE draw $k$ with probability $\alpha \quad w_i$
    \STATE $y_{t}^i = x_{t}^k$
    \ENDFOR
    \FORALL {$i$}
    \STATE $x_{t}^i = y_{t}^i$
    \STATE $w_i = 1$
    \ENDFOR
    \ENDFOR
    \STATE $E[f(x)] = \sum_i \hat{w_i}f(x_i)$
  \end{algorithmic}
\end{algorithm}

\newpage

\subsection{Other Names}
This technique has many names depending on the field. For example:

\begin{itemize}
\item Condensation - Computer Vision
\item Particle Filtering - Robotics
\item SIR Filter - Statistics
\item Survival of the Fittest
\end{itemize}

\section{Failure Modes}
\subsection{Bad Motion Model}
If the motion model is not very good, it is possible to detect the same thing many times and not recognize that it is the same object. This leads to a large number of extra particles, which is inefficient.

\subsection{Sensor Model is Too Good}
If the sensor model is too good, particle filtering can fail. For example, say has a very sharp spike where the object is. Now, when the sensor model is sampled by the particles, a few things can happen, which are undesirable.

Only one particle might be in the vicinity of the spike. This will cause its weight to dwarf the other particles and so when resampling occurs,  only one particle will exist. This means that when the situation changes, there might not be enough diversity of particles to be able to handle the disturbance.

Even worse, if all of the particles miss the spike in the observation model, then they will be reweighted based on the low-level differences. Thus, the weights are effectively noise and the particle filtering algorithm might not be able to converge.

\subsection{Resampling Too Aggressively}
If resampling occurs too often, it is possible to throw out a mode of solutions inadvertently.

\section{Fixes to Failure Modes}
\subsection{Overestimate Noise}
Overestimating the noise of the sensor model smooths it out, avoiding the possibility of having a model that is too good. However, this will add extra computation to the solution. Plus, it's a hack.

\subsection{Use More Particles}
Using more particles will often help with any of the above problems, however, the computational complexity is directly related to the number of particles used, so this approach can be expensive. Also, particle filters tend to be robust to bugs if you just use more particles, so it is possible that you have a bug if you need to increase your particles significantly to get a solution.

\subsection{Improve the Motion Model}
Improving the motion model will help if the root cause is a bad model.

\subsection{Rao-Blackwellization}
We'll discuss this in depth in a later lecture, but the basic idea is to make each particle smarter and thus carry more information. For example, each particle could contain a Kalman filter.

\subsection{Change Q}
When doing the importance sampling, we usually use the motion model. However, $Q$ only needs to be a different distribution, which we can later adjust. For example, we could sample from the observation model and weight by the motion model.

\subsection{Low Variance Resampling}
When resampling, instead of pulling from the distribution independent samples, we can choose a random number $r$ and the choose particles $u = r + \frac{m-1}{M}$. See Figure 4.7 from Probabilistic Robotics.

\begin{center}
\includegraphics{lowVarianceResampling.eps}\\
From Probabilistic Robotics pg. 111
\end{center}

This technique tends to result in samples that have lower noise and is guaranteed not to drop particles as long as their probability is greater than $\frac{1}{M}$.

\subsection{Only Resample When Necessary}
If resampling is being done too aggresively, it is a good idea to resample less frequency, hopefully based on the current state of the particle filters. For example, one could resample only when the variance of the weights go over a threshold.

\end{document}
