if_practical.tex~

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

\subsubsection{Adding New Features and Updating the Filter}

The robot measures the relative position to the point features on the
x-axis. We build the map from new observations and update the filter
with those of known landmarks\footnote{Of course, we are assuming that
  we've solved the data association problem. While this is
  challenging for any SLAM algorithm, additional complications arise
  with the information form \cite{eustice05b}.}. New features are added
according to the state augmentation process described in (2) of the
notes, just like we do with new poses in the time projection
step. Notice in `Figure 1' that the new features share information
only with the vehicle pose and not any other map elements.

In the case where the robot has observed a feature that is already in
the map, the IF updates the information matrix and vector per (6) in
the notes. The matrix is modified by adding information only to the
elements associated with these features and the robot pose. This ``new
information'' has the effect of strengthening the constraints between
the robot and the map that have decayed as a consequence of the time
projection step.


\subsection{Things to Try}

Several parameters that control the simulation are specified at the
beginning of \firstfile\ as fields of the \texttt{Params}
structure. These include those listed below. Try changing these to
modify the IF behavior.

\begin{table}[h]
  \begin{tabular}{|l|p{107mm}|}
    \hline
    \texttt{Params.Q} & The variance of the
    Gaussian odometry noise.\\
    \hline
    \texttt{Params.R} & The variance of the noise that corrupts
    feature observations.\\
    \hline
    \texttt{Params.density} & Density of features in the
    environment. \\
    \hline
    \texttt{Params.nObsPerIteration} & Maximum number of features
    that the robot can observe at any one time.\\
    \hline
  \end{tabular}
\end{table}


\subsection{Take-Home Message}

One obvious benefit of the information filter is that, when the
measurement model is linear, the update step is simple. The same
update for the Kalman Filter is quadratic in the size of the state. On
the other hand, the time projection step is computationally expensive
for the IF but efficient for the KF. Intuitively, this is due to the
fact that, in marginalizing over the old robot state, we have to transfer
information to the new pose as well as create links among all
landmarks that were linked to the old robot state. When there are a
bounded number of constraints between the robot and map, this is
\emph{easy} but, as the map grows and the matrix becomes dense, this
process is no longer tractable \cite{thrun04a, walter05a}.


\section{Two-dimensional SLAM Simulation}

The two-dimensional simulation is very much like the 1D simulation,
though there are a few differences. For one, the default settings
of the code create an environment with many more features and allow
the robot to observe multiple landmarks at once. Consequently, the
information matrix will be larger and have a more complex
structure. Secondly, the robot will follow a counter-clockwise path
that results in a loop closure as the robot comes back to the
(approximate) starting location. This point in the simulation is
particularly interesting since you can see how the information matrix
changes as the vehicle re-observes features that it hadn't seen since
the beginning of the run.


\subsection{Starting the Simulation}

In order to start the simulation, type \texttt{slam\_if\_2d} at the Matlab
prompt. A window, `Figure 1', is created with a pair of axes
side-by-side. The one on the left is a plot of the simulated
environment while the schematic on the right depicts the
information matrix. The legend is the same as in the 1D simulation
with the addition of red three-sigma uncertainty ellipses around the
position estimates. With regards to the plot of the information
matrix, we visualize the determinant of each $2 \times 2$ sub-block (x and y) of
the matrix.
%
%
\begin{figure}[ht]
  \center
  \includegraphics[width=0.6\columnwidth]{./Graphics/slam_if_2d.eps}
  \caption{The main figure window for
    \secondfile.} \label{fig:slam_if_2d}
\end{figure}
%
%

\subsection{Displays}

Alongside the main Matlab window, additional displays will pop up,
with the following roles:

\begin{table}[h]
  \begin{tabular}{|l|l|p{88mm}|}
    \hline
    Figure 1 (left) & Simulation Plot & Display of the ground-truth feature
    locations (black dots) together with the current SLAM estimates for
    the vehicle (red triangle) and feature (red ``x'') positions. The red ellipses signify the $3\sigma$ uncertainty bounds.\\
    \hline
    Figure 1 (right) & 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 & The window shows the form of the
    information matrix immediately prior to the current step in the filter.\\
    \hline
    Figure 3 & Shared Information History & Plot of the determinant of the
    off-diagonal sub-matrix between the first few features and the robot pose as a
    function of time.\\
    \hline
  \end{tabular}
\end{table}

\subsection{Action Sequence}
Prior to looping through the simulation, the main Matlab window asks
whether or not you want verbose output. In verbose mode, the function
will pause for the time prediction and measurement update steps as
well as when new features are added. By turning this option off, the
program will loop through the simulation without stopping.

The simulation iterates through the action sequences as described in
the table below. The plots of the simulation and current information
matrix in `Figure 1' are updated and, in verbose mode, the previous
information matrix is shown in `Figure 2' for comparison. Hit
\texttt{Enter} to continue or \texttt{Control-C} to abort the program.

\begin{table}[h]
  \begin{tabular}{|l|l|p{125mm}|}
    \hline
    1 & Robot moves & The new position of the vehicle, $x_v(t+1)$, is
    estimated from odometry. The filter performs time prediction as one
    single step.\\
    \hline
    3 & Observations & The robot measures the relative $(x,y)$ position of
    neighboring features as indicated by lines drawn between the vehicle
    and map.\\
    \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 known landmarks.\\
    \hline
  \end{tabular}
\end{table}


Once the simulation has finished, `Figure 3' shows the history of the
shared information between the robot and the first few features that
were added to the map. Unlike the 1D simulation where the vehicle
didn't really close any loops, you may notice that there are points
where the links strengthen over time. This is a result of loop closure.

\subsection{Things to Try}
Once the simulation has finished, take a closer look at the resulting
information matrix for the map. If you type ``\texttt{global
  TheJournal}'' at the Matlab prompt, you will find that the
information matrix is stored as \texttt{TheJournal.Lambda} while
\texttt{TheJournal.eta} is the information vector. Note that you will
probably have to type ``\texttt{global TheJournal}'' every time you run
the simulation if you want to access global variables.

\subsection{Matrix Density}
The time history plots of the shared information for the 1D and 2D
simulations show, to some extent, how the magnitude of the
off-diagonal terms decays with time. In order to get a better idea of
the distribution of values within the matrix, plot a histogram over
the magnitudes. Matlab provides the \texttt{hist} function to
compute histograms over a (optional) desired number of bins (type
``\texttt{help hist}''). Rather than the original information matrix,
though, look at the normalized matrix returned by a call to
\texttt{rhomatrix}.

You should see a large disparity in the distribution with a
significant majority of the terms nearly zero. What fraction of the
total number of elements within the matrix falls within the left-most
bins (see ``\texttt{help numel}'' and ``\texttt{help nnz}'')?  If
these terms were actually zero and did not need to be explicitly
stored, how much memory would we save? Assume double precision (8
bytes) and that you can ignore the overhead necessary to store sparse
arrays. In our simulation, this improvement is minor since we are
tracking only a few states. On the other hand, calculate how much
memory would we save if we had 10,000 states\cite{eustice05b} and an
equivalent percentage of zeros in the information matrix?

\subsection{Effects of Noise Strength}
The extent of the noise that corrupts the motion and observation data
is one factor that affects the strength and rate of decay of the
shared information between the robot and map. How you think that less
observation noise would affect the magnitude of the off-diagonal terms
in the information matrix? How about a decrease in the motion model
uncertainty? Edit \secondfile\ and modify the measurement and motion
noise covariances, \texttt{Params.R} and \texttt{Params.Q}, and check
to see if your intuition is correct. To speed up the simulation, you
can turn off the plotting by setting \mbox{\texttt{Params.PlotSwitch}
  = 0} and disabling verbose mode.


\subsection{Computation Time}
One advantage of the canonical form is that the number of computations
in the measurement update step is proportional to the number of
observations and not the size of the map. Contrast this with the KF,
which is quadratic in the number of features. For the 2D code, we have
tried to be efficient in the way that we store the information matrix
as well as in our implementation of the measurement update step. Over the
course of the simulation, the function has kept a history of the
elapsed time for each time projection and measurement update. These
are stored as \texttt{TheJournal.ProjectionTime} and
\texttt{TheJournal.UpdateTime}, respectively, with one column for each
record. The columns are of the form $\left[k \; n \;
  \Delta_t\right]^\top$ where $k$ is the simulation time stamp, $n$
the current size of the state vector, and $\Delta_t$ is the
computational time required for the projection/update. Plot the
computation times (third row) as a function of the number of states
(second row).

\subsection{Additional Simulation Parameters}
There are a few other parameters defined as fields of the
\texttt{Params} structure within \secondfile. These include
\texttt{Params.density} and \texttt{Params.nObsPerIteration} that we
defined earlier, as well as \texttt{Params.MaxObsRange}. If you have
time, you can try changing these to see how they influence the
information filter.
    

\bibliography{bibliography}




\end{document}