\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, F09)}{Lecture \#2 (Thursday August 27)}{Lecturer: Drew Bagnell}{Scribe: Supreeth Achar}{Filtering Theory}

\section{Filtering Problems}
\subsection{Overview}

The goal of filtering is to maintain an online estimate of the state ($x_t$) of a system. In a localization problem this state would be the pose of the robot, in mapping the state would be a map of the environment and in SLAM the state would be both the pose and map. A general filter would have the following inputs

\begin{itemize}
\item A timestep indexed stream of data ($d$) containing observations ($z_{1:t}$) and actions ($u_{1:t}$) : $d=\{u_{1:t}, z_{1:t}\}$
\item A probability distribution modeling the initial state : $p(x_0)$
\item Action Model or Motion model
  \begin{equation}
    p(x_t|x_{t-1}, u_t)
  \end{equation}
\item Measurement model or Observation model
  \begin{equation}
    p(z_t|x_t)
  \end{equation}
\end{itemize}

At each timestep we want the filter to output a probability distribution of the current state $x_t$ given all available information $d=\{u_{1:t}, z_{1:t}\}, p \left(x_0\right) $. This is the distribution $p(x_t | u_{1:t}, z_{1:t}, x_0)$ and is denoted by the shorthand notation $Bel(x_t)$.

Filters are defined recursively, with the output at time instant $t$ being an input to the filter at $t+1$.

\subsection{Other Notations}

Filtering of the form described here has applications in many different domains including robotics, electrical engineering and computer science. As a result, notations and terminologies used can vary.
State ($x$) may be denoted by $s$ or if the state being estimated is a map, $m$ might be used. Action $u$ is also called the control input ($u$) or the decision ($a$). Measurements ($z$) are also called observations and may be represented by $y$, $o$ or $x$. The motion model is also called the state transition function or the plant dynamics.
  
\section{Markov Assumptions}

The amount of measurement and motion data ($z_{1:t}$ and $u_{1:t}$ grows as time progresses. Using all of this data to calculate $x_t$ at each filter timestep is not feasible. To overcome this difficultly, it is assumed that the observation model and action model are Markovian which means that measurements made and state updates are independent of past states ($x_{1:t-1}$) if the current state ($x_t$) is known

\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 Action model Markov assumption
  \begin{equation}
    p\left(x_{t+1}|x_{0:t},u_{1:t},z_{1:t-1}\right)=p\left(x_{t+1}|x_{t},u_t\right)
  \end{equation}
\end{itemize}

For a robot, the observations are dependent on the map $(m)$, for simplicity this dependence is not shown explicitly. These assumptions seem justified but may not always be true in practice. For example the Markovian motion model may be innaccurate if the dynamics of the state are not included. If the actual environment differs from the map used to generate expected observations then the Markov assumption for the observation model does not hold. Even so, these Markovian approximations are important as 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}

This derivation gives an expression for the state distribution at time $t$, $Bel(t)$ in terms of the distribution at time $t-1$ $Bel(t-1)$

\begin{tabular}{rclr}
$Bel(x_t)$ & = & $p(x_t|z_{1:t}, u_{1:t})$ & \\
           & = & $\eta \: p(z_t|x_t, z_{1:t-1}, u_{1:t}) \:p(x_t| u_{1:t}, z_{1:t-1})$                                                       & Bayes' Rule \\
           & = & $\eta \: p(z_t|x_t) p(x_t| u_{1:t}, z_{1:t-1})$                                                                             & Markov Assumption \\
           & = & $\eta \: p(z_t|x_t) \int \left[ p(x_t| x_{t-1}, u_{1:t}, z_{1:t-1})\:   p(x_{t-1}| u_{1:t}, z_{1:t-1}) \right] \: dx_{t-1}$ & $P(A) = \int P(A|B)P(B)dB$\\
           & = & $\eta \: p(z_t|x_t) \int \left[ p(x_t| x_{t-1}, u_t)\: p(x_{t-1}| u_{1:t-1}, z_{1:t-1}) \right] \: dx_{t-1}$                & Markov Assumption\\
           & = & $\eta \: p(z_t|x_t) \int \left[ p(x_t| x_{t-1}, u_t)\: Bel(x_{t-1}) \right] \: dx_{t-1}$                                    & Definition of $Bel(x_t)$\\
\end{tabular}

\section{Example: Piecewise Constant Filter}

Figure~\ref{fig_1Dex} shows how a Bayes Filter can be used for robot localization. Consider an imaginary robot that moves in one dimension along the length of corridor and has a sensor that tells it whether it is in front of a door or not. The robot also has a `map', a list of positions at which doors are located along the corridor. The robot starts of with no clue to where it is on the corridor, so the initial pose belief is a uniform distribution as shown in ~\ref{fig_1Dex}a. As door observations are made, the belief distribution forms peaks which tend to flatten out as the robot moves.

\section{Implementing a Bayes Filter}

Implementing a Bayesian recursive filter for continuous state variables as shown above is usually not possible computationally. The probability distributions desribed can in general take on any form and so closed form solutions do not exist to the integrals. A number of different assumptions and approximations can be made to design a filter based on Bayesian recursive estimation. Some examples of practical techniques based on the Bayes Filter are

\begin{enumerate}
\item Kalman Filters (KF, EKF,UKF etc)
\item Histogram Filters
\item Particle Filters
\item Hidden Markov Models (HMMs) - These are just discrete Bayes Filters
\end{enumerate} 

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

Markov localization uses a histogram filter. The basic idea is to discretize the (continuous) state space into a number of bins. Each of these bins has a probability mass associated with it. If the number of bins is sufficient, the probability mass distribution over these bins is an accurate approximation of the underlying distribution in the state space. The integrals in the Bayes Filter are replaced with summations which are easier to compute and which need to be calculated over a finite number of cells.

For Markov localization, the state space is the pose of the robot. This space is quantized into a rectangular grid and each cell ($k$) in the grid has a probability mass ($p_{k,t}$) associated with it that equals the probability of the robot being somewhere within that cell in the pose space. Motion causes the pose distribution to shift and flatten out due to the increased uncertainity in position while measurements tend to create peaks in the distribution. Figure~\ref{fig_markov} shows an example of Markov Localization, a likelihood estimate is maintained for all the cells. The number of cells grows exponentially in the number of dimensions, so for high dimensional states this method is not practical$ $.

\begin{algorithm}
  \caption{grid\_based\_localizer($\{p_{k,t-1}\}, u_t, z_t, m$)}
  \begin{algorithmic}[1]
    \FORALL{$k$}
    \STATE $p'_{k,t} = \sum_i p_{i,t-1}$\textbf{motion\_model}$(x_k,u_t,x_i)$
    \STATE $p_{k,t} = \eta p'_{k,t}$\textbf{measurement\_model}$(z_t,x_k,m)$
    \ENDFOR
    \STATE \textbf{return} $\{p_{k,t}\}$
  \end{algorithmic}
\end{algorithm}  

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

\section{Forward Sensor Models}

Bayesian Filtering techniques for robot state estimation require some probability density function that gives the likelihood of a measurement from a certain pose. This is $p(z_t|x_t,m)$ and is called the forward sensor model. A typical range finding sensor like a laser will return a number of measurements in a single scan $z_t = \{z_t^1, z_t^2, \ldots, z_t^K\}$. In reality measurements in a scan will depend on each other, but modeling this dependency is complicated so it will be assumed that scan readings are independent although this can lead to overconfident likelihoods.

\begin{equation}
p(z|x,m) = \prod_{k=1}^Kp\left(z^k|x,m\right)
\end{equation}

There are a number of sources of error in measurements (See Figure~\ref{fig_sensing_errors}). These are
\begin{itemize}
	\item Readings from objects not present in the map (people, furniture etc)
	\item Noise around a 'true' measurement
	\item 'Random' measurements
	\item Sensor returning maximum range
\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 use a linear combination of the distributions to get a ``Pseudo-density'' distribution (Figure~\ref{fig_measurement_model}). The mixing coefficients should sum to one and can be determined experimentally by collecting ground truthed range data and finding the coefficients that provide the best fit.

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