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

\usepackage{algorithm}
\usepackage{algorithmic} 

\usepackage{verbatim}

\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}
\newtheorem{note}[theorem]{Note}

% 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, F10)}{Lecture \#02
  (Thursday August 26)}{Lecturer: Drew Bagnell}{Scribe: Hatem
    Alismail}{Introduction to Filtering}


\section{Probability} 

A Random Variable\footnote{It is not random and it is not a variable, it is
  easier to think of a random variable as an \em{unknown quantity}.} is a function from the probability
  space to some other space, usually $\mathbb{R}^N$. Usually, random variables
  are denoted with a capital letter (e.g. $X$, $Y$, $A$ ...). For example, a
  random variable might be a coin flip, which takes one of two possible values: head
  or tail. For a fair coin: $P(X = head) = P(X = tail) = 0.5$. 

  In the discreet random variable case, such as a coin flip, the probability
  mass function (PMF) is a function that assigns a probability value for random
  variable taking a specific value.  Values of the PMF must sum up to $1$. 

  In the case of a continuous random variable, such as a sonar reading, the
  probability distribution function (PDF) is the function that assigns a
  probability value for a random variable being within an interval. 

  The PDF must always integrate to $1$, $\int p(x)dx = 1$

  \begin{note} 
  For a continuous random variable $p(X = x) = 0$. That is, the probability
  the random variable taking the specific value $x$ is always zero.
  \end{note} 

  \begin{note} 
  Only the integral $\int p(x)dx$, not $p(x)$, over all possible values integrates to $1$. 
  \end{note}

\subsection{Probability Axioms} 
The axioms of probability are:
  \begin{itemize}  
    \item Probabilities must be bounded between 0 and 1, $0 <= P(x) <= 1$
    \item Probability of truth is always 1, $P(\texttt{true}) = 1$ 
    \item Probability of false is always 0, $P(\texttt{false}) = 0$ 
    \item Probability of $X$ or $Y$ is the probability of $X$ plus the
    probability of $Y$ minus the probability of the intersection of $X$ and
    $Y$ (see Figure~\ref{fig:XorY}),  $P(X \vee Y) = P(X) + P(Y) - P(X \wedge Y)$
  \end{itemize}  

  \begin{figure}[!htp]
    \centering 
    \includegraphics[width=0.5\textwidth]{figs/p_x_or_y.png} 
    \caption{The probability of the event $X$ {\it or} $Y$ is the is the area of the
    red circle + the area of the blue circle - the intersection of the two
      circles so that we do not over count.}
    \label{fig:XorY}
  \end{figure}

From these axioms only we can {\em prove} several other statements, such as
$P(\neg X) = 1-P(X)$. 


The Gaussian PDF is probably the most famous/useful PDF, which is
parameterized by two values, the mean ($\mu$) and the variance ($\sigma^2$).
It can be written as: 
\begin{equation}  
  p(x) =
  \frac{1}{2\pi\sigma^2}\exp{\left(-\frac{1}{2}\frac{(x-\mu)^2}{\sigma^2}\right)}
\end{equation} 

This definition can be generalized for $|x|>1$, by replacing the mean with a
mean vector and replacing the variance with a symmetric positive semidefinite
matrix, called the covariance matrix $(\Sigma)$. This is called a multivariate Gaussian
distribution can be written as: 
\begin{equation} 
  p(x) = \det\left(2\pi\Sigma\right)^{-\frac{1}{2}}
\exp\left(-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)\right)
\end{equation}  

\subsubsection{Bayes Rule}
Relates $p(x|y)$ to its ``inverse'', $p(y|x)$. For $p(y)>0$:
\begin{equation} 
  p(x|y) = \frac{p(y|x)p(x)}{p(y)} = \frac{p(y|x) p(x)}{\sum_{x'}p(y|x')p(x')}
\end{equation}
In many situations, it is convenient to avoid writing out the normalization
factor explicitly. For this purpose, $\eta$, or $1/Z$ is used to denote the
normalizer: $p(x|y) = \eta p(y|x)p(x)$.


\section{State Estimation}
State estimation is ``the problem of estimating quantities from sensor data
that are not directly observable, but can be inferred.`` 

\begin{definition}
A state is aspects of the environment and the robot that may impact the future.
\end{definition}
There are two types of states, a {\it static} state, such as walls, and a {\it
  dynamic} state, such as the robot's pose. The state at time $t$ will be
  denoted with $x_t$.


\section{Filtering} 
In filtering, we want to obtain an estimate of $x_t$ over time. For example,
   in a localization problem $x_t$ is the pose of the robot. In mapping, the
   state is the map of the world, and in SLAM, the state is both, the robot's pose
   and the map of the environment.

  Inputs to a general filtering problem are: 
  \begin{itemize} 
    \item Data: time indexed set of observations ($z_{1:t}$) and control
    actions ($u_{1:t}$). Generally, the robot observers the environment and
    performs a control action at every time step. Data can be denoted as:
    $d=\{z_{1:t}, u_{1:t}\}$. 

    \item Probability distribution of the initial state $p(x_0)$. 

    \item Motion/action model that relates the probability of the current
    state to the previous state and observations: $p(x_t | x_{t-1}, u_t)$ 

    \item Observation/measurement model, which is a model of how measurements
    are generated: $p(z_t | x_t)$.
  \end{itemize} 

  See Figure~\ref{fig:xuz} to see the evolution of control, states and
  observations. 

  \begin{figure}[htp] 
    \centering
    \includegraphics[width=0.7\textwidth]{figs/xuz.png}
    \caption{Evolution of controls, states, and observations}
    \label{fig:xuz}
  \end{figure} 


\subsection{Belief distribution}  
A belief is a reflection of the robot's internal knowledge about the state and
it is represented through conditional probability distributions. 
\begin{equation}
  bel(x_t) = p(x_t | z_{1:t},u_{1:t})
\end{equation}
, which is the posterior probability over state variables conditioned on data
(observations and actions).   



\subsection{The Bayes Filter}
Bayes filter is a general algorithm to compute belief from observations and
control data. A discrete Bayes filter algorithm is shown in
Algorithm~\ref{alg:bayes_alg}. 

\begin{algorithm} 
\caption{Discrete\_Bayes\_Filter ($Bel(x),d$)} 
\label{alg:bayes_alg} 
  \begin{algorithmic}[1]
    \STATE $\eta = 0$  
    \IF {$d$ is a {\it perceptual} data item $z$}  
      \FORALL {$x$}  
        \STATE $Bel'(x) = P(z|x) Bel(x)$
        \STATE $\eta = \eta + Bel`(x)$
      \ENDFOR
      \FORALL {$x$} 
        \STATE $Bel'(x) = \eta^{-1}Bel'(x)$
      \ENDFOR
    \ELSIF {$d$ is an {\it action} data item $u$} 
              \FORALL {$x$} 
                \STATE $Bel'(x) = \sum_{x'} P(x|u,x') Bel(x')$
              \ENDFOR
    \ENDIF
    \RETURN $Bel'(x)$
    \end{algorithmic}
\end{algorithm}

\subsection{Derivation of the Bayes filter}  
Below is the mathematical derivation of the Bayes filter:
\begin{align*} 
Bel(x_t) &= P(x_t | u_1, z_1, \ldots, u_t, z_t) & {}  \\
         &= \eta P(z_t | x_t, u_1, z_1, \ldots, z_{t-1}, u_t) P(x_t | u_1, z_1, \ldots,
                                                       z_{t-1}, u_t) & \text{Bayes rule} \\
         &= \eta P(z_t | x_t) P(x_t | u_1, z_1, \ldots, z_{t-1}, u_t, x_{t-1}) &
         \text{Markov} \\ 
         &= \eta P(z_t | x_t) \int{ P(x_t|, u_1, z_1, \ldots, z_{t-1}, u_t)
           P(x_{t-1} | u_1, z_1 \ldots, z_{t-1}, u_t) dx_{t-1} } & \text{Total probability} \\  
         &= \eta P(z_t | x_t) \int{ P(x_{t-1} | u_t, x_{t-1})
              P(x_{t-1} | z_1, u_1, \ldots, z_{t-1}, u_{t-1}) dx_{t-1}} & \text{Markov (removed $u_t$)} \\ 
        &= \eta P(z_t | x_t) \int{ P(x_t | u_t, x_{t-1}) Bel(x_{t-1}) dx_{t-1}
        } & \text{Def. of $Bel(x_t)$} \\
\end{align*}


\subsection{The Markov Assumption}
The Markov assumption states that current state is conditionally independent
from anything else given the previous state. While the Markovian world
assumption may not be realistic in many situations\footnote{For example,
  correlations between adjacent laser beams. If there was a specular
    reflection at beam $a$, then it is likely that neighboring beams will
    experience similar behavior.}, it simplifies the computations needed for
    state estimation. 

For an observation model, the Markov assumption can be expressed as:
\begin{equation} 
  P(z_t | x_0, x_1, u_1, z_1, \ldots, x_t, u_t, z_{t-1}) = P(z_t | x_t)
\end{equation} 
And for an action model\footnote{In the slides, there was a missing $z_{t-1}$}
\begin{equation} 
  P(x_{t+1} | x_0, x_1, u_1, z_1, \ldots, x_t, u_t, z_{t-1}) = P(x_{t+1} |
                                                                 x_t, u_t,
                                                                 z_{t-1})
\end{equation} 


\section{Example: Markov (Grid-based) Localization}
\label{sec:markov_loc}
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
uncertainty in position while measurements tend to create peaks in the
distribution.\footnote{In general, actions increase uncertainty while
  measurements reduce uncertainty.}
  
  Figure~\ref{fig:markov_loc} shows an example of Markov Localization,
  a likelihood estimate is maintained for all the cells. Initially, (a) the robot
  does not know its position, hence the belief is uniform. (b) shows the
  robot's belief about its location after observing a door, the belief is
  larger in locations of doors in the map. However, a single measurement is
  not enough to resolve the $3$ doors ambiguity; the robot can be in front of
  any of the doors. In (c) the robot moves, and the belief of its location
  shift in the direction of movement. In (d), the robot observes another door.
  Based on this observation, uncertainty is reduced, and robot is able to
  localize itself fairly accurately. Finally, (e) as the robot moves
  uncertainty increases.
  
  It is worth mentioning that the number of cells grows exponentially in the number of dimensions, so for
  high dimensional states this method may not be 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{comment}
Given a map of the environment, the robot needs to localize itself based on
its observation model and action model. First, the space is discretized into
small bins. Each of the bins has a certain probability mass value. Figure~\ref{fig:markov_loc} shows an example of grid-based localization.
\end{comment}

\begin{figure}   
  \centering
  \includegraphics[width=0.85\textwidth]{figs/markov_grid_localization.png}
  \caption{Basic idea of Markov localization: a robot during global
    localization}
  \label{fig:markov_loc}
\end{figure}



\section{Forward Sensor Model}
\label{sec:sensor_model}

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:sensor_err}). These are
\begin{itemize}
	\item Readings from objects not present in the map (people, furniture, etc)
	\item Noise around a 'true' measurement 
	\item Maximum range reading (e.g.\ no return indicating $80m$ max range for SICK)
	\item 'Random' measurements
\end{itemize} 

\begin{figure}[htp]  
\centering 
\includegraphics[width=0.8\textwidth]{figs/sonar_err.png} 
\caption{(a) typical ultrasound scan, (b) misreading in ultrasonic
  SENsing.(Figure 6.1 in the book)}
\label{fig:sensor_err}
\end{figure} 


\subsection{Beam Models of Range Finders}
Range finders measure range to nearby objects along a beam, such as laser
ranger finders, or along a cone, such as ultrasonic sensors. The desired model
$p(z_t | x_, m)$ is a mixture of four densities that correspond to types of
error (shown in Figure~\ref{fig:noise}, which are: 

\begin{enumerate} 
  \item {\bf Local measurement noise.} In an ideal world, we'd except the range
  finder to give a perfect answer. However, due to some factors such as the finite resolution of
  the scanner, the atmosphere of the environment, etc. we'd expect to see some
  noise

  \item {\bf Unexpected objects.} Accounts for dynamic objects (such as
                                                                people) that
  are absent from the robot's static map.

  \item {\bf Failures (max range).} In some cases of a reflection, or transparent
  objects, range measurements are reported as maximum range and fails to
  report a correct range. 

  \item {\bf Random measurement.} When the sensor is \emph{really} having a bad day... 
\end{enumerate} 

\begin{figure}[htp]
  \centering
  \includegraphics[width=.9\textwidth]{figs/noise.png}
  \caption{Components of error model}
  \label{fig:noise}
\end{figure}

We can use a linear combination of the distributions to get a
``Pseudo-density'' distribution (Figure~\ref{fig:combined}). The mixing coefficients should sum to one and can be determined experimentally by collecting ground truth range data and finding the coefficients that provide the best fit.

\begin{figure}[htp]
\centering
\includegraphics[width=.85\textwidth]{figs/combined.png}
\caption{Combined error model}
\label{fig:combined}
\end{figure}

\newpage
\vfill{}
\section*{Acknowledgments}
\begin{itemize}  
  \item ``Probabilistic Robotics'' by S. Thurn, W. Burgard, and D.
  Fox. 
    \begin{itemize} 
      \item  Figures and equations
    \end{itemize}

  \item Lecture notes from Fall 2008 by Supreeth Achar
    \begin{itemize} 
      \item Section~\ref{sec:markov_loc} with some modifications.
      \item Section~\ref{sec:sensor_model} with some modifications.
    \end{itemize}
\end{itemize}

\end{document} % Done!


