\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 \#2 (Thursday August 28)}{Lecturer: Drew Bagnell}{Scribe: Felix Duvallet}{Filtering Theory}

\section{Filtering Problems}
\subsection{Givens}

Here is what we have:

\begin{itemize}
\item Stream of data: $d=\{u_1, z_2, u_2, z_2, \ldots, u_t, z_t\}$
\item Inititial distribution over states: $p(x_0)$
\item Motion model
  \begin{equation}
    p(x_t|x_{t-1}, u_t)
  \end{equation}
\item Observation model
  \begin{equation}
    p(z_t|x_t)
  \end{equation}
\end{itemize}

\subsection{Compute}

And this is what we want:
\begin{itemize}
\item Posterior state distribution
  \begin{equation}
    Bel(x_t) = p(x_t|d)
  \end{equation}
\end{itemize}

NOTE: We never actually get states ($x_i$) given to us, but since this is a recursive process having $p(x_0)$ is enough.

  
\subsection{Markov Assumptions}
Both of these assume that past and future data are independent if one knows the current state.
\begin{itemize}
\item Observation model Markov assumption
  \begin{equation}
    p\left(z_t|x_{0:t},u_{1:t},z_{1:t-1}\right)=p\left(z_t|x_t\right)
  \end{equation}
\item Transition/Action model Markov assumption
  \begin{equation}
    p\left(x_t|x_{0:t-1},u_{1:t},     z_{1:t-1}\right)=p\left(x_t|x_{t-1},u_t\right)
  \end{equation}
\end{itemize}

Note that these assumption are not always true (for example if we have unmodeled dynamics not included in the state), but they are practical and make computation tractable.


\begin{figure}[h!t]
  \centering
  \includegraphics[width=10cm]{img/fig8.1.eps}
  \caption{Grid localization}
  \label{fig_1Dex}
\end{figure}

  
\section{Derivation of the Bayes Filter}
In this derivation, we assume that we correctly initialized the prior belief ($Bel(x_0)$).  It also requires that controls be chosen at random.

\begin{tabular}{rclr}
  $Bel(x_t)$ & = & $p(x_t|z_{1:t})$ & \\
  & = & $\eta \: p(z_t|x_t, z_{1:t-1}) \:p(x_t|z_{1:t-1})$ &   Bayes'        Rule \\
& = & $\eta \: p(z_t|x_t) \: p(x_t|z_{1:t-1})$ & Markov Assumption \\
& = & $\eta \: p(z_t|x_t) \int \left[ p(x_t|z_{1:t-1},x_{t-1})\:   p(x_{t-1}|z_{1:t-1}) \right] \: dx_{t-1}$ & Law of Total Probability \\
& = & $\eta \: p(z_t|x_t) \int \left[ p(x_t|x_{t-1})\: Bel(x_{t-1})       \right] \: dx_{t-1}$ & Markov Assumption
\end{tabular}



\section{Bayes Filter as Algorithm}

Here is the Bayes Filter implemented as an algorithm:

\begin{algorithm}
  \caption{filter($Bel(x)$, data):}
  \begin{algorithmic}[1]
    \IF{data is a perception ($z$)}
    \FORALL{$x$}
    \STATE $Bel'(x) = p(z|x)Bel(x)$
    \STATE $\eta = \eta + Bel'(x)$
    \ENDFOR
    \FORALL{$x$}
    \STATE $Bel'(x) = \frac{1}{\eta} Bel'(x)$
    \ENDFOR
    \ELSIF{data is an action ($u$)}
    \STATE $Bel'(x) = \sum_{\hat{x}}Bel(\hat{x}) \: p\left(x|action, \hat{x}\right) $
    \ENDIF
    \STATE \textbf{return} $Bel'(x)$
    
  \end{algorithmic}
\end{algorithm}

\section{Example: Piecewise Constant Filter}
Figure~\ref{fig_1Dex} shows a simple 1D example.  A sensor tells the robot the state of the door.  Note that the belief begins as a uniform distribution, and then changes as observations and actions are processed.

\section{Markov Localization (Grid-based Localization)}

In this simplistic implementation shown in Figure~\ref{fig_markov}, one keeps track of the likelihood for all possible states.  As the robot moves, the uncertainty in position is reduced.

\begin{figure}[h!t]
  \centering
  \includegraphics[width=9cm]{img/fig8.6.eps}
  \caption{Localization in a Museum}
  \label{fig_markov}
\end{figure}

NOTE: This quickly becomes intractable as the number of dimensions increases.


\section{Forward Sensor Models}


\begin{itemize}
  \item Beam-based sensors (lasers, sonars, stereo vision)
  \item Observations:  A scan $z$ consists of $K$ measurements:
    \begin{equation}
      z = \{z_1, z_2, \ldots, z_K\}
    \end{equation}
  \item Likelihood of observation (given map $m$):
    \begin{equation}
      p(z|x,m) = \prod_{k=1}^Kp\left(z_k|x,m\right)
    \end{equation}
    \item NOTE: The individual measurements are independent given the position.  This assumption is not generally true, as this can lead to overconfident likelihoods, but it is (once again) practical for now.
    \item Sources of measurement error (See Figure~\ref{fig_sensing_errors}):
      \begin{itemize}
      \item Beams reflected by obstacles
      \item People
      \item Random measurements
      \item Max range measurements
      \end{itemize}
\end{itemize}


\begin{figure}[!ht]
  \centering
  \includegraphics[width=10cm]{img/fig6.1.eps}
  \caption{Typical ultrasound errors}
  \label{fig_sensing_errors}
\end{figure}


We would like to develop a model for the proximity sensors used that accounts for the various sources of noise.  It will incorporate four types of measurement errors shown in Figure~\ref{fig_measurement_model_components}:
\begin{enumerate}
\item Local measurement noise: Accounts for non-idealities in the measurement process (a)
\item Unexpected obstacles: Accounts for dynamic objects which are not represented in the static map (b)
\item Max-range measurement: In the event that the sensor `misses' the target and returns a max-range measurement (c)
\item Random measurements: When the sensor is \emph{really} having a bad day... (d)
\end{enumerate}

\begin{figure}[!ht]
\centering
\includegraphics[width=10cm]{img/fig6.3.eps}
\caption{Components of error model}
\label{fig_measurement_model_components}
\end{figure}

We can do a linear combination of the distributions to get a ``Pseudo-density'' distribution (Figure~\ref{fig_measurement_model}).


\begin{figure}[!ht]
\centering
\includegraphics[width=7cm]{img/fig6.4.eps}
\caption{Combined error model}
\label{fig_measurement_model}
\end{figure}

\section{From Localization to Mapping}
Instead of doing localization, we will try to solve the opposite problem: Given the position of the robot, find the map of the world.

\subsection{Occupancy mapping}
We will partition the world into cells, with each being one of two states: filled or not.  The individual grid cells are $m_i$, and $\vec{m}$ is the vector of all grid cells.  Unfortunately, the curse of dimensionality prevents us from filtering $\vec{m}$.  Instead, we will filter each cell independently, assuming that they are in fact independent.  In truth, this is a very bad assumption to make, but it is practical.

\subsection{Derivation}
Let $x$ represent the state of grid cell $m_i$ (essentially $x \gets m_i$).

\begin{eqnarray*}
  p(x|z_{1:t}) & = & \frac{p(z_t|x)p(x|z_{1:t-1})}{p(z_t|z_{1:t-1})} \\
& = & \frac{p(x|z_t) p(z_t)}{p(x)} \frac{p(x|z_{1:t-1})}{p(z_t|z_{1:t-1})} \\
\end{eqnarray*}

Here we need to use a trick because we have many nasty terms that are hard to calculate.  We will start by computing (by analogy) the likelihood for the opposite event $\bar{x}$
\begin{equation}
  p(\bar{x}|z_{1:t}) = \frac{p(\bar{x}|z_t) p(z_t)}{p(\bar{x})} \frac{p(\bar{x}|z_{1:t-1})}{p(z_t|z_{1:t-1})},
\end{equation}
and then divide the two:
\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(x)}{p(\bar{x})} \right]
\left[ \frac{p(x|z_{1:t-1})}{p(\bar{x}|z_{1:t-1})} \right]
\label{eqn_oddratio}
\end{equation}

Taking the log of~\eqref{eqn_oddratio} gives us the log odds ratio of the belief.  We can use this to turn a series of multiplication and divisions into additions and subtractions (see page 96).

\end{document}
