if_practical.tex~

这是移动机器人同时定位与地图创建的第三部分

\documentclass[a4paper,12pt]{article}

\usepackage{times,graphicx,amsmath,psfrag,mathshortcuts,multirow}


\setlength{\oddsidemargin}{-7mm}	
\setlength{\evensidemargin}{-7mm}
\setlength{\topmargin}{-14mm}	
\setlength{\parindent}{0mm}
\setlength{\parskip}{1mm}
\setlength{\textwidth}{173mm}
\setlength{\textheight}{244mm}
\setlength{\unitlength}{1mm}

%\input newcom.tex
%\input symbols.tex


\newcommand{\coursetitle}{SLAM Summer School 2006}
\newcommand{\tutorialtitle}{Practical 3: Introduction to the Information Filter}
\newcommand{\docauthor}{M. Walter and J. Leonard, MIT; R. Eustice, University of Michigan}
\newcommand{\docemails}{[e-mail], [e-mail], [e-mail]}
\newcommand{\firstfile}{\texttt{slam\_if\_1d.m}}
\newcommand{\secondfile}{\texttt{slam\_if\_2d.m}}


\begin{document}


\bibliographystyle{plain}


\begin{center}

\hrule
\vspace{2mm}
{\LARGE\bf \coursetitle}

\vspace{2mm}
{\Large\bf \tutorialtitle}

\vspace{2mm}
{\large \docauthor}
\\
{\large \texttt{\docemails}}


\end{center}


\hrule

%\hrule
\vspace{5mm}



%\vspace{10mm}
%\hrule
%\vspace{2mm}

\setcounter{page}{1}
\setcounter{section}{0}



\section{Introduction}

The information (canonical) form of the Gaussian has recently received
a fair bit of attention as a possible alternative for the scalability
problems of the EKF. This practical explores the use of the
information filter (IF) for feature-based SLAM. The session will
hopefully provide a basic understanding of localization and mapping
with the information filter.

Today's lab considers two simulations of a robot operating in
one-dimensional and two-dimensional worlds. In both cases, the vehicle
moves according to a linear, constant-velocity model and makes
relative observations of point features in the environment, both
subject to Gaussian noise.

In addition to this explanation of the lab, we have included a brief
introduction to the information form and its application to SLAM. The
supplement addresses the basic steps of the IF including time
projection, measurement updates, and the addition of new features to
the map, i.e. the components that are fundamental to any SLAM
algorithm. It might be useful to skim this over before going ahead
with the lab since we will refer to it several times later on.

\section{Setup}

Obtain the files from the SSS06 CDROM or from the website at 
\texttt{http://www.robots.ox.ac.uk/\~{}SSS06/} and run matlab in the \texttt{code/} directory.


There are two main files in the directory: \firstfile\ performs the
one-dimensional simulation while \secondfile\ implements the
two-dimensional SLAM information filter.


\section{Fundamentals of the Information Filter}

As with the standard Kalman Filter, the marginalization and
conditioning processes form the basis behind the time projection and
measurement update steps. The one-dimensional simulation is meant to
explore the basics of the SLAM information filter in terms of these
two processes.

\subsection{Starting the Simulation}

Type \texttt{slam\_if\_1d} at the Matlab prompt to start the simulation.  The
program will create a window titled `Figure 1' that contains two axes,
the top a rendering of the environment and, below that, a schematic of
the information matrix. Figure \ref{fig:slam_if_1d} below shows an
example. Within the top plot, the red triangle denotes the estimated
position of the robot, the black dots correspond to the ground-truth
feature locations, and each red ``x'' indicates their estimated
position. The rows and columns of the information matrix are labeled
with the corresponding state elements, $x_v(t)$ for the vehicle pose
and id numbers for the features.
%
%
\begin{figure}[ht]
  \center
  \includegraphics[width=0.6\columnwidth]{./Graphics/slam_if_1d.eps}
  \caption{The main figure window for \firstfile.} \label{fig:slam_if_1d}
\end{figure}
%
%

The function pauses as it steps through the various key points in the
IF algorithm. Hitting \texttt{Enter} causes the simulation to
continue. It can be aborted at any time by hitting \texttt{Control-C}.

Note that the \firstfile\ file contains various parameters that
control the simulation, most of which are fields of the global
\texttt{Params} structure. Meanwhile, the majority of the filter
variables are stored within \texttt{TheJournal}\footnote{I think that
  I adopted this notation from Paul Newman.}. Feel free to change
these settings to see how they affect the filter and have a look at
the contents of \texttt{TheJournal}.

\subsection{Displays}

Alongside the main Matlab window, various displays will pop up which
have the following roles:

\begin{tabular}{|l|l|p{83.5mm}|}
  \hline
  Figure 1 (top) & Simulation Plot & Shows the ground-truth feature
  locations (black dots) together with the current SLAM estimates for
  the vehicle (red triangle) and feature (red ``x'') positions.\\
  \hline
  Figure 1 (bottom) & Information Matrix & Depicts the structure of the
  information matrix. Shade intensity corresponds to the magnitude of
  the normalized matrix elements, white being zero and black being one.\\
  \hline
  Figure 2 & Information Matrix & Schematic of the information matrix
  immediately prior to the current iteration.\\
  \hline
  Figure 3 & Shared Information History & Plot of the magnitude of the
  shared information between each feature and the robot pose as a
  function of time.\\
  \hline
\end{tabular}


\subsection{Action Sequence}

In the simulation, the robot starts at the origin and moves along the
x-axis, observing features along the way. The program pauses each time
the robot moves as the filter executes the time projection step as
well as for each of the measurement updates. Each time you hit \texttt{Enter},
the simulation will carry out the next action, following
the sequence:

\begin{table}[!h]
  \begin{tabular}{|l|l|p{125mm}|}
    \hline
    1 & Robot moves (a) & The new position of the vehicle, $x_v(t+1)$, is
    estimated from odometry, and added to the state vector. The
    state, $\bvec{x} = \left[x_v(t+1) \; x_v(t) \; \bvec{M}\right]$,
    includes the current and previous robot poses as well as the map.\\
    \hline
    2 & Robot moves (b) & The time projection step concludes as the old
    robot pose, $x_v(t)$, is removed from the state vector. \\
    \hline
    3 & Observations & The robot observes neighboring features as
    indicated by empty circles plotted around the true feature locations
    (black dots).\\
    \hline
    4 & New features added & Any new features that are observed are added
    to the map.\\
    \hline
    5 & Update & The filter performs an update step based upon
    measurements of the relative location of known landmarks.\\
    \hline
  \end{tabular}
\end{table}

After all of these actions, one full step is complete and the
program cycles back to action 1.

Note that you can set the code to run through the simulation without
these pauses by setting \newline \mbox{\texttt{Params.verbose} = 0} within \firstfile.

\subsection{Things of Note}

Let us take a closer look at the different action sequences of the
simulation.

\subsubsection{Time Projection Step}

In order to get a better understanding of the time projection step as
performed in the information form, we break it up into two
processes. In the first, the new estimate for the robot pose,
$x_v(t+1)$, is added to the state, together with the previous pose and
map, based upon odometry information. Notice in `Figure 1 (bottom)' that
a row and column corresponding to the new pose have been added to the
information matrix and that it shares information only with the old
pose. Otherwise, the matrix remains unchanged as the
sub-block associated with $x_v(t)$ and $\bvec{M}$ is the
same as prior to augmenting the state.

The information matrix and vector now describe the posterior
distribution over the two poses and the map. The final component of
the time projection step is to marginalize this distribution over the
old pose, $x_v(t)$. Unlike state augmentation, this marginalization
significantly alters the structure of the information matrix. To see
this, compare the the new matrix to the form prior to marginalization
(shown in `Figure 2'). All shared information between the map and old
pose has ``moved'' over to the new vehicle state in the form of
off-diagonal elements in the first row and column. Also, this set of
features is now fully-connected while many of them may not have been
linked immediately before. At the same time though, a close
inspection reveals that old constraints between the robot and map,
which have been transferred to the new pose, are now a little
weaker. What we see is consistent with Figure 2 of the supplementary
notes in which the marginalization component of the time projection
step populates the information matrix while also weakening many of its
off-diagonal elements.

While links weaken as a result of the marginalization, they never
decay to zero. Together with the constraints that the process creates
among some of the features, the time projection step has the important
consequence of populating the information matrix. As the simulation
progresses, the matrix appears to be sparse but, as we indicate with
the dots, the it is actually filled-in.