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

\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 \#24 (November 12, 2009)}{Lecturer: Drew Bagnell}{Scribe: Bryan Wagenknecht}{SLAM, Fast SLAM, and Rao-Blackwellization}

\section{About SLAM}
Simultaneous localization and mapping (SLAM) is one of the canonical problems in robotics. A robot set down in an unknown environment with noisy odometry and sensing needs to figure out where it is in the world and learn details about the world at the same time. It is a sort of chicken-egg problem where the mapping problem is relatively easy given exact knowledge of the robot's location, and localizing the robot is relatively easy given a good map of the environment. The challenge is found in performing these tasks simultaneously.

\subsection{Variations on the SLAM problem}
SLAM can be approached in two ways:
\begin{itemize}
\item{Batch: given all data $y_{1:T}$ up front, solve for robot path $x_{1:T}$ and map $M$ all at once (we won't cover this)}
\item{Online/filtering: compute $\left\{x_t, m_t\right\}$ at each timestep as data $y_t$ is received (we focus on this)}
\end{itemize}
The mapping part of SLAM can also be approached in two ways:
\begin{itemize}
\item{Filling an occupancy grid $M = \left\{m_1, m_2, ...\right\}$ (we'll come back to this)}
\item{Tracking landmarks $l_i = (m_{xi}, m_{yi})$ (focus on this for now)}
\end{itemize}

\section{Basic Approaches to SLAM}
\subsection{PF over joint}
The first naive solution is to build a Particle Filter to sample from the joint distribution \[p(r_x, r_y, m_{x1}, m_{y1}, m_{x2}, m_{y2}, ...)\] This becomes bad very quickly due to the high dimensionality of the state space when landmark position is included:
\begin{itemize}
\item{PF doesn't scale well with dimensionality - particle sparsity means a particle that is good at describing one landmark is probably bad at describing others}
\item{Observation distributions/densities get very sharp due to multiplying distributions}
\end{itemize}

\subsection{XKF over joint}
The next best solution (often called the ``classical solution'') might be to use your favorite Kalman Filter (XKF here could mean EKF, UKF, etc.) to track all variables in one joint distribution \[p(r_x, r_y, m_{x1}, m_{y1}, m_{x2}, m_{y2}, ...)\] This results in a big covariance matrix with the marginal gaussian distribution of each landmark contained in blocks along the covariance diagonal. Off-diagonal terms are correlations from multiple landmarks appearing in the robot's view at once.

The motion update would look like:
\[
\left[
\begin{array}{c}
r_x \\
r_y \\
m_{x1} \\
m_{y1} \\
\vdots \end{array}
\right]_{t+1}
=
\left[
\begin{array}{cc}
	\left[
	\begin{array}{cc}
	\multicolumn{2}{c}{\multirow{2}{*}{$A_r$}} \\
	& \end{array}
	\right] & 0\\
0 & \left[
	\begin{array}{ccc}
	&&\\
	\multicolumn{3}{c}{I}\\
	&&\\ \end{array}
	\right]
\end{array}\right]
\left[
\begin{array}{c}
r_x \\
r_y \\
m_{x1} \\
m_{y1} \\
\vdots \end{array}
\right]_{t}
+
\left[
\begin{array}{c}
\epsilon_x \\
\epsilon_y \\
\\
0\\
\\\end{array}
\right]
\]

and the observation model would look like:
\[
\left[
\begin{array}{c}
range_i \\
bearing_i \end{array}
\right]
= 
\left[
\begin{array}{c}
\sqrt{(r_x - m_x)^2 + (r_y - m_y)^2} \\
atan2((r_y - m_y),(r_x - m_x)) \end{array}
\right]
+
\left[
\begin{array}{c}
\epsilon_1 \\
\epsilon_2 \end{array}
\right]
\]

\subsubsection{Potential problems with this approach}
\begin{itemize}
\item{The solution could be multimodal, especially with bad odometry}
\item{Data association: the filter must be able to distinguish between landmarks, which can cause problems with ``loop closure'' where the robot returns to a location it has already mapped and landmarks need to matched with those previously observed.}
\subitem{Maximum Likelihood data association with hard thresholds is often used for this}
\item{Scales as $O(n^2)$ where $n$ is number of landmarks}
\end{itemize}

\subsubsection{Some hacks that could be helpful}
\begin{itemize}
\item{Use submaps (segment the large world map into smaller maps) and work with smaller chunks of the covariance matrix.}
\item{Throw out confusing landmarks}
\end{itemize}

\section{SLAM as a Bayes Net}
If we draw the Bayes Network representation of SLAM we get something like Figure \ref{BayesNet}. Here the data associations are presumed known and fixed.

	\begin{figure}[h]
      \centering
      \includegraphics[scale=0.65]{BayesNet.png}
      \caption{Bayes Network representation of SLAM. Robot states $r_i$ form a Markov chain and data associations (which landmarks $l_i$ are in view at each observation $y_i$) are presumed known and fixed. Conditioning on robot states causes independence between landmark locations.}
      \label{BayesNet}
   \end{figure}

\subsection{Fast SLAM factorization}
Investigation of the Bayes Net shows us that we can make the landmark locations independent by conditioning on the robot locations. This allows us to factorize the distribution as
\[p(r,m) = p(r) \cdot p(m|r) = p(r) \cdot \prod_i p(l_i|r)\]
Thus we can track the landmark locations independently by conditioning on the assumed robot position.

Fast SLAM is an algorithm that runs a particle filter in robot location space using augmented particles. Each particle carries a history of its past locations $r_1...r_t$ and an individual Kalman filter (consisting of $\mu_{l_i}$ and $\Sigma_{l_i}$) for each landmark $l_i$ it sees. This is visualized in Figure \ref{AugmentedParticle}.

	\begin{figure}[h]
      \centering
      \includegraphics[scale=0.65]{AugmentedParticle.png}
      \caption{Fast SLAM particles include a history of their positions and individual Kalman filters for each landmark.}
      \label{AugmentedParticle}
   \end{figure}

\section{Smarter Approach: Fast SLAM}
\subsection{Fast SLAM 1.0}
The first version of Fast SLAM runs a particle filter with augmented particles, then applies importance weighting to the particles using the latest estimates of any landmarks within view. The landmarks are tracked independently in Kalman filters within each particle.

\subsubsection{Fast SLAM 1.0 Algorithm}
\begin{enumerate}
	\item{Motion update: for all $particle_i$, move $r_i$ according to motion model (leave KF's for landmarks alone)}
	\item{Observation update: weight $particle_i$ by $p(y|associations\:l, r_i)$
	\[ p(y|associations\:l, r_i) = \frac{1}{\sqrt{(2\pi)^2\left|\Sigma_l\right|}}\exp\left[-\frac{1}{2}(y-\mu_y)^T\Sigma_l(y-\mu_y)\right] \]}
	\item{Run KF update on each particle's estimate of landmark $l$}
	\item{Resample particles}
\end{enumerate}

\subsubsection{Timestep Complexity: $O(l)$}
\begin{itemize}
\item{Data association is $O(l)$ (use KD tree)}
\item{Memory copy is $O(l)$}
\end{itemize}

\subsubsection{Occupancy Grids}
\begin{itemize}
\item{In theory each particle should carry its own map grid (instead of KF's)}
\item{Practically this is too memory intensive, use data structure tricks: DP-SLAM}
\subitem{Use a single map grid with data for all relevant particles in each cell}
\end{itemize}

\subsection{Fast SLAM 2.0}
The problem with Fast SLAM is paricle deprivation, especially for loop closure. The key is to have a smarter motion model (sampling distribution).

\subsubsection{Fast SLAM 2.0 Algorithm}
\begin{enumerate}
\item{Motion update: Sample robot position from Kalman filter $\tilde{p}(x_{t+1}|y_{t+1},x_t$}
\item{Observation update: Weight $particle_i$ by
\[ w_i \propto \frac{p(y_{t+1}|associations\:l, r_i)}{\tilde{p}(x_{t+1}|y_{t+1},x_t)} \]}
\end{enumerate}

\section{Wrap-up Topics}
\subsection{Data association}
If we want to be lazy with data association (or we can't label the landmarks a priori) then we can track associations within the particle. Wrong assignments are corrected in retrospect for free through particle resampling.
\begin{itemize}
\item{For each landmark $l_i$, sample associations from $p(y|l_i,r)$}
\end{itemize}

\subsection{Rao-Blackwell theorem}
The Rao-Blackwell theorem says running an algorithm that samples from a distribution on one variable $p(x_1)$ then determines another variable $x_2$ analytically conditioned on $x_1$ will do no worse than an algorithm that samples from the joint distribution $p(x_1,x_2)$.
\[ var\left[sampler(x_1,x_2) \right] \geq var\left[sampler(x_1) + does\:x_2|x_1\:analytically\right] \]
Fast SLAM is an example of Rao-Blackwellization since by using KF's inside particles, we don't have to sample landmark info from the whole joint distribution. In general Rao-Blackwellized filters make for the most efficient SLAM algorithms.
\end{document}
