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

\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 \#3 (September 2, 2008)}{Lecturer: Drew Bagnell}{Scribe: Alex Styler}{Filtering Theory}

\section{Occupancy Mapping}
\subsection{Log-odds of the Belief}

From last time:

\begin{equation}
  \frac{p(x|z_{1:t})}{p(\bar{x}|z_{1:t})} = \left[ \frac{p(x|z_t)}{p(\bar{x}|z_t)} \right] \left[ \frac{p(\bar{x})}{p(x)} \right]
\left[ \frac{p(x|z_{1:t-1})}{p(\bar{x}|z_{1:t-1})} \right]
\label{felix_preveqn}
\end{equation}

The next step is to take the log of~\eqref{felix_preveqn} to get the log odds of the belief $bel_t(x)$, denoted as $l_t(x)$:

\begin{equation}
  l_t(x) = l_{t-1}(x) + log\frac{p(x|z_t)}{p(\bar{x}|z_t)} +log\frac{p(\bar{x})}{p(x)}
\label{logodds_eqn}
\end{equation}

The $\bar{x}$ in the numerator of~\eqref{logodds_eqn} seems contrary to intuition, but if you rearrange the terms:

\begin{equation}
  l_t(x) = l_{t-1}(x) + log\frac{p(x|z_t)}{p(x)} + log\frac{p(\bar{x})}{p(\bar{x}|z_t)}
\label{logodds2_eqn}
\end{equation}

it makes more sense.  If the probability of $\bar{x}$ decreases with the observation $z_t$: $p(\bar{x}) > p(\bar{x}|z_t)$, it causes the log odds of the belief to increase.

\subsection{Inverse Beam Sensor Model}
\begin{figure}[h!t]
  \centering
  \includegraphics[width=5in]{beam_figure.png}
  \caption{Examples of inverse sensor model}
  \label{fig_1}
\end{figure}

Figure~\ref{fig_1} (c) shows the typical result from a ray trace in the inverse beam sensor model.  All cells start gray: let's say $p(m_i) = 0.5$.  After the observation, cells penetrated by the beam decrease their probability to contain an obstacle, cells that reflect the beam increase their probability, and cells beyond the reflection do not update as no information was gained about their state.

\subsection{Problems With Occupancy Mapping}

Occupancy mapping has a few problems:

\begin{itemize}
\item Non-independence of individual readings
\item Cells might be 'partially' filled, implying the districtized state model is bogus
\item Cells aren't naturally independent, especially when dealing with walls and large obstacles
\item Likelihood doesn't factorize over cells
\end{itemize}

The problem of partially filled cells can be handled using Density Mapping ...

\section{Density Mapping}
\subsection{Why Density Mapping?}

Consider a districtized world with a robot and a tree.  Let's consider one cell to be 10\% filled by a twig from the tree.  In binary occupancy mapping, the cell might start with a $p(x_i) = 0.5$.  After 10 measurements, in which 9 penetrate the cell and only 1 reflects, the binary occupancy mapping will quickly reach the conclusion that the cell is empty.  Assuming the cell is empty could be catastrophic for some robots.  Density mapping is different, and tries to predict "`how full"' a cell is using a real-number density instead of a binary full/empty state.

\subsection{The Beta Distribution}
\begin{figure}[h!t]
  \centering
  \includegraphics[width=5in]{betadists.png}
  \caption{Examples of Beta Distributions}
  \label{fig_2}
\end{figure}

Density mapping typically uses a beta distribution to keep track of a cell's density.  Some example beta distributions are shown in Figure~\ref{fig_2}.  Initial parameters are tuned to the expected environment: maybe having every cell weighted to be more likely empty or low density if obstacles are sparse.  The beta-distribution is represented as:
\begin{equation}
  p(x|\alpha, \beta) = \frac{x^{\alpha-1}(1-x)^{\beta-1}}{z}
\label{beta_eqn}
\end{equation}

Where $z$ is an arbitrary normalizing constant to make the integral evaluate to $1$.

\subsection{The Bayes Update}

We represent the probability of a beam hitting/reflecting off an obstacle in a cell of a world with a simple Bernoulli distribution:
\begin{equation}
 p(O^{i}_t = hit) = bernoulli(x^{i}) = x^{i}
\label{hit_eqn}
\end{equation}
\begin{equation}
 p(O^{i}_t = passthru) = 1 - x^{i}
\label{pass_eqn}
\end{equation}

Given a hit, what happens when we use the Bayes filter to update the prior with the likelihood?
\begin{eqnarray*}
Belief &=& \frac{Prior * Likelihood}{Normalizer}	\\
Belief &=& \left[\frac{x^{\alpha-1}(1-x)^{\beta-1}}{z}\right][x] \\
Belief &=& \left[\frac{x^{(\alpha+1)-1}(1-x)^{\beta-1}}{z}\right] \\
\end{eqnarray*}

So, given a hit, all we need to do to update the prior is increment $\alpha$.  The same can be shown that for a passthrough, $\beta$ is incremented.  As one is repeatedly incremented, the distribution skews in the appropriate direction and sharpens.

\subsection{Problems}

A problem with density mapping is that sensor noise is not accounted for.  This can be mitigated by throwing away extremely unlikely measurements, but is a little bit of a hack.

The problem with all districtized mapping is that the computation required increases exponentially with respect to the number of dimensions.  A few might be reasonable but as dimensionality increases the computation becomes intractable.

\section{Sampling}

Instead of using districtized mapping, another method is to sample points from the continuous space.  So far, we've been intersted in estimating $p(x_t| ...)$.  Usually we're interested in the distribution to calculate some meaningful values like the mean state~\eqref{mean_eqn}, the state variance~\eqref{var_eqn}, or some marginal probability of a state variable~\eqref{marg_eqn}.
\begin{equation}
E_p[x] = \sum_{x}{p(x)x}
\label{mean_eqn}
\end{equation}
\begin{equation}
Var(x) = E_p[(x - E_p[x])^2]
\label{var_eqn}
\end{equation}
\begin{equation}
E_p[1_{X_i = x_i}] = p(X_i = x_i)
\label{marg_eqn}
\end{equation}

One method to approximate these values is using a sampling approach:
\begin{equation}
x_{i=1...R} \sim p(x)
\end{equation}

To approximate the expectation of some function $f(x)$, $E_{p(x)}[f(x)]$, we calculate:
\begin{equation}
\hat{\mu}_f = \frac{1}{R}\sum_{i=1}^{R}f(x^i)
\end{equation}

By the Strong Law of Large Numbers, as $R$ increases, $\hat{\mu}_f$ will approach $E_{p(x)}[f(x)]$
\begin{equation}
Var(\hat{\mu}_f) = \frac{Var_{p}[f]}{R}
\end{equation}

More samples yeild a better, more accurate estimate, but are limited by computational feasibility and time.

\subsection{The Derivation of the Normalized Importance Filter}
Given some starting state, $x_0$, we're interested in sampling the following distribution.
\begin{equation}
p(x_1 | z_1) = \frac{p(z_1|x_1)p(x_1|x_0)}{p(z_1)}
\label{wanted_eqn}
\end{equation}

But there is no easy way to sample from the product of our motion model distribution, $p(x_1|x_0)$, and our measurement model distribution, $p(z_1|x_1)$ without massive computation.  To achieve this, we will instead use Importance Sampling, which involves sampling from the wrong distribution and then correcting it:

\begin{eqnarray*}
E_{p}[f(x)] &=& \sum_{x}p(x)f(x)\\
&=& \sum_{x}p(x)f(x)\frac{q(x)}{q(x)}\phantom{ww}\forall q(x)\neq 0\\
&=& \sum_{x}q(x)\frac{p(x)}{q(x)}f(x)\\
&=& E_{q}\left[\frac{p(x)}{q(x)}f(x)\right]\\
\end{eqnarray*}
	
So instead, let's draw our samples from the distribution $Q$ and correct later to $p$:
\begin{eqnarray*}
x_{i=1...R} &\sim& Q(x)\\
\hat{\mu}^{p}_{f} &=& \frac{1}{R}\sum_{i=1}^{R}\left[\frac{p(x^i)}{q(x^i)}\right]f(x^i)
\end{eqnarray*}

The quantity in brackets, $\left[\frac{p(x^i)}{q(x^i)}\right]$, is known as the importance weight, $w^i$ and corrects the samples from the $Q$ distribution to the $p$ distribution.  This lets us rewrite the equation as:
\begin{eqnarray*}
\hat{\mu}^{p}_{f}&=&\frac{1}{R}*\sum_{i=1}^{R}w^{i}*f(x^i)\\
w^{i}&=&\left[\frac{p(x^i)}{q(x^i)}\right]
\end{eqnarray*}

Again, we want to sample from the distribution in~\eqref{wanted_eqn}.  Instead, lets use our motion model,  $p(x_{t+1}|x_{t})$ as Q and do important sampling.  Forgetting about the normalizer $p(z_{t+1})$ for the moment,
\begin{eqnarray*}
\hat{\mu}^{p}_{x} &=& \frac{1}{R}\sum_{i=1}^{R}\left[\frac{p(x^i)}{q(x^i)}\right]x^i \\
\hat{\mu}^{p}_{x} &=& \frac{1}{R}\sum_{i=1}^{R}\left[\frac{p(z_{t+1}|x_{t+1})p(x_{t+1}|x_t)}{p(x_{t+1}|x_t)}\right]x^i \\
\hat{\mu}^{p}_{x} &=& \frac{1}{R}\sum_{i=1}^{R}p(z_{t+1}|x_{t+1})x^i
\end{eqnarray*}

By sampling from the motion model as $Q$, which is easily computed, we can correct using the measurement model.  The last problem to solve is normalizing the result.  Using the same samples from $Q$, the motion model:
\begin{equation}
p(z_{t+1}) = \frac{1}{R}\sum_{i=1}^{R}p(z_{t+1}|x_{t+1})
\end{equation}

Combining the two,
\begin{eqnarray*}
\hat{\mu}^{p}_{x} &=& \frac{\frac{1}{R}\sum p(z_{t+1}|x_{t+1})x^i}{\frac{1}{R}\sum_{i=1}^{R}p(z_{t+1}|x_{t+1})}\\
\hat{\mu}^{p}_{x} &=& \frac{\sum p(z_{t+1}|x_{t+1})x^i}{\sum_{i=1}^{R}p(z_{t+1}|x_{t+1})}
\end{eqnarray*}

\subsection{The Normalized Importance Sampling Algorithm}
\begin{itemize}
\item Sample from the motion model $x_{i=1...R} \sim p(x_{t+1}|x_t)$
\item Compute the weights $w^i = p(z_{t+1}|x^i)$ from the measurement model
\item Compute $\hat{w}^i = \frac{w^i}{\sum{w^i}}$
\item Compute $\hat{\mu}_{f(x_{t+1})} = \sum{\hat{w}^{i}f(x_{i})}$
\end{itemize}

Next time: adding multiple measurement iterations and the particle filter...
\end{document}
