if_background.tex

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

\documentclass[a4paper,12pt]{article}

\usepackage{times,epsfig,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}{Supplement to the Information Filter Practical}
\newcommand{\docauthor}{M. Walter, Massachusetts Institute of Technology}
\newcommand{\docemails}{[e-mail]}


\begin{document}

\bibliographystyle{unsrt}


% Title info
\begin{center}
\hrule
\vspace{2mm}
{\LARGE\bf \coursetitle}

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

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

\end{center}
\hrule
\vspace{5mm}


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



\section{Background}
Kalman Filter SLAM algorithms represent Gaussian distributions in
terms of a covariance matrix, $\mat{\Sigma}$, and mean vector,
$\mat{\mu}$. The information (canonical) form is an alternative
parametrization that describes the Gaussian by the information matrix,
$\mat{\Lambda}$, and information vector, $\mat{\eta}$. The two
representations are related according to:
%
\begin{equation} \label{eqn:sftoif}
\mat{\Lambda} = \mat{\Sigma}^{-1} \quad
\mat{\Lambda} \mat{\mu} = \mat{\eta}
\end{equation}
%
You can easily derive this relationship by playing with the quadratic
term within the Gaussian exponential, i.e. $(\bvec{x}-\bvec{\mu})^\top
\mat{\Sigma}^{-1}(\bvec{x}-\bvec{\mu})$.

One of the major limitations of the Kalman Filter is that we must keep
track of the covariance matrix\footnote{Of course, the covariance
  matrix is also a big help since it allows us to take advantage of
  the correlation between the states to improve our estimates when we
  run the update step.}. This
matrix is dense as we show in the left-hand side of Figure
\ref{fig:sigma_lambda} and storing it requires an amount of memory
that is quadratic in the size of the map. Secondly, the computational
expense of the KF update step is $\mathcal{O}(n^2)$, where $n$ is the number of
states. Together, these costs restrict KF SLAM algorithms to
relatively small maps, on the order of hundreds of features.

On the other hand, the corresponding information matrix, shown on the
right in Figure \ref{fig:sigma_lambda} tends to be ``relatively''
sparse. Some of the terms are large while the majority are, in
proportion, very small. If these terms were actually zero, a careful
implementation of the IF can shed most of the computational and memory
burden \cite{thrun04a}.
%
%
\begin{figure}[!ht]
  \center
  \includegraphics[width=0.3\columnwidth]{./Graphics/densecov}
  \hspace{3mm}
  \includegraphics[width=0.3\columnwidth]{./Graphics/spinf}
  \caption{A comparison of a typical normalized covariance matrix
    (left) and the corresponding normalized information matrix
    (right). Darker shades correspond to larger
    magnitudes.} \label{fig:sigma_lambda}
\end{figure}
%
%

\subsection{Interpretation as a Graphical Model}
A particularly useful property of the canonical form is that it
explicitly describes the structure of the undirected graphical model
associated with the distribution. More specifically, nonzero
off-diagonal terms in the information matrix indicate the existence
(and strength) of links between the corresponding variables in the
Gaussian Markov Random Field (GMRF) \cite{speed86}. For states that do
not ``share information'' within the matrix, there is no edge between
the corresponding nodes in the graph. The schematic on the left in
Figure \ref{fig:projection} shows an example in which each feature
shares information only with the robot pose and, in turn, the only
edges are those to the robot.

Since an undirected graph encodes the conditional independence between
states \cite{jordan04}, those that are not directly connected
according to the information matrix are conditionally independent of
one another given their immediate neighbors. This characteristic of
the information form is particularly useful in understanding many of
the issues related to IF SLAM.

%
%
\section{Duality Between the Standard and Information Forms}
Gaussian-based algorithms, like any other Bayesian approach to the
SLAM problem, rely upon marginalization and conditioning to keep track
of the state distribution. The time projection step can be thought of,
in part, as a marginalization while measurement updates condition the
state on new observations.

It is fairly easy to show that the standard (covariance) and
information forms are duals of one another when it comes to the math
involved in marginalization and conditioning. As we show in Table
\ref{tab:margcond}, the structure of the marginalization step for the
canonical form is very similar to the conditioning step for the
covariance form and vice-versa.

%   Table: Standard vs. Canonical duality 
\input{table.margcond.onecolumn}

%
%
\section{Basic SLAM Steps}
The information filter SLAM algorithm, just like the KF approach,
iterates over the time projection and measurement update steps and adds
new features to the map when they are observed. In this section, we
take a quick look at how these steps are implemented in the IF.
%
% FIGURE: sparsification strategy
%------------------------------------------------------
\begin{figure}[t]
  \center
  \psfrag{vp}[][lc][0.6]{\tiny{$\mathrm{x}_{t+1}$}}
  \psfrag{v}[][cc][0.6]{\tiny{$\mathrm{x}_{t}$}}
  \psfrag{f1}[][lc][0.6]{\tiny{$\mathrm{m}_1$}}
  \psfrag{f2}[][lc][0.6]{\tiny{$\mathrm{m}_2$}}
  \psfrag{f3}[][lc][0.6]{\tiny{$\mathrm{m}_3$}}
  \psfrag{f4}[][lc][0.6]{\tiny{$\mathrm{m}_4$}}
  \psfrag{f5}[][lc][0.6]{\tiny{$\mathrm{m}_5$}}
  \includegraphics[width=0.5\columnwidth]{./Graphics/projection}
  \caption{The time projection step of the information filter viewed
    as state augmentation followed by marginalization. The elements of
    the information matrix are shaded according to their magnitude
    with the strongest constraints shown as being
    darkest.} \label{fig:projection}
\end{figure}
%
%
%

%
\subsection{Adding New Features to the Map}
Suppose that our state vector, $\bvec{\xi}_t = \bigl[\bvec{x}_t^\top
\; \bvec{M}^\top \bigr]^\top$, currently consists of the robot pose,
$\bvec{x}_t$ and a number of features, $\bvec{M} =
\left\{\bvec{m}_1,\bvec{m}_2,\ldots,\bvec{m}_n\right\}$. The
information parametrization of the Gaussian distribution over the
state is
%
%
\begin{equation*}
  \mat{\Lambda}_t =
  \begin{bmatrix}
    \mat{\Lambda}_{x_t x_t} & \mat{\Lambda}_{x_t M} \\
    \mat{\Lambda}_{M x_t} & \mat{\Lambda}_{M M}
  \end{bmatrix} \quad
  \bvec{\eta}_t = 
  \begin{bmatrix}
    \bvec{\eta}_{x_t} \\
    \bvec{\eta}_M
  \end{bmatrix}.
\end{equation*}
%
%
The robot observes a new feature according to the model
%
%
\begin{equation*}
  \bvec{m}_{n+1} = \mat{G}\bvec{x}_t + \bvec{v}_t
\end{equation*}
%
%
where $\bvec{v}_t \sim \normalcov{\bvec{0}}{\mat{R}}$ denotes zero
mean, white Gaussian noise. We add the new landmark to the end of the
state vector, $\bvec{\xi}_t = \bigl[\bvec{x}_t^\top
\; \bvec{M}^\top \; \bvec{m}_{n+1}^\top \bigr]^\top$, and modify the
information matrix and vector per \eqref{eqn:addfeature}.
%
%
\begin{equation} \label{eqn:addfeature}
  \bar{\mat{\Lambda}} = 
  \begin{bmatrix}
    \left(\mat{\Lambda}_{x_t x_t} + \mat{G}^\top \mat{R}^{-1} \mat{G}
    \right) & \mat{\Lambda}_{x_t M}
    & -\mat{G}^\top \mat{R}^{-1} \\
    \mat{\Lambda}_{M x_t} & \mat{\Lambda}_{M M} & \mat{0} \\