The CAS Robot Navigation Toolbox - uni-freiburg.desrl.informatik.uni-freiburg.de/publicationsdir/CAS... · 2010-04-27 · The current toolbox version includes support for diﬀerential

The CAS Robot Navigation Toolbox

Users Guide and Reference

Kai O. Arras, CAS–KTHVersion 0.9, September 2004

About This Manual

This manual is an introduction to and reference of the CAS Robot NavigationToolbox. It explains how to work with the delivered examples and how to importown data. The overall structure of the toolbox is explained and the setup file,the file which contains setting and parameters of an experiment, is introduced.

The manual’s reference section explains each m-file, gives an overview ofbuilt-in classes and functions, and illustrates their usage.

The current toolbox version includes support for differential drive robotsand the following sensory data: encoder data, range data and data from retro-reflective beacons. It provides an odometry model for non-systematic errors indifferential drive robots, a line extraction method and an extraction methodfor beacons. By its object oriented design, extensions of the toolbox are easyto make. It is possible to add a robot class (e.g. with a synchro-drive orAckerman steering kinematics), a feature class (e.g. circles modeling trees, finiteline segments, corners) or a new extraction method.

Finally, this manual release is preliminary which has been published on re-quest (version 0.9). Apologies if you encounter incomplete or suboptimal expla-nations. I hope it’s useful anyway!

1

Contents

1 Users Manual 6

1.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.1.1 What Does This Toolbox Do? . . . . . . . . . . . . . . . . 6

1.1.2 What Doesn’t It Do? . . . . . . . . . . . . . . . . . . . . . 7

1.1.3 Why Matlab? . . . . . . . . . . . . . . . . . . . . . . . . . 7

1.1.4 Resources, License and Feedback . . . . . . . . . . . . . . 8

1.2 Getting Started . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.2.1 Toolbox Structure . . . . . . . . . . . . . . . . . . . . . . 8

1.2.2 Initializing the Matlab Session . . . . . . . . . . . . . . . 9

1.2.3 Obtaining Help and Information on Objects . . . . . . . . 10

1.3 Using the Built-In Examples . . . . . . . . . . . . . . . . . . . . 11

1.4 Using the Toolbox With Own Data . . . . . . . . . . . . . . . . . 16

1.4.1 The Setup File of an Experiment . . . . . . . . . . . . . . 16

1.4.2 Defining a Robot . . . . . . . . . . . . . . . . . . . . . . . 17

1.4.3 Defining a Sensor . . . . . . . . . . . . . . . . . . . . . . . 18

1.4.4 Defining the Master Sensor . . . . . . . . . . . . . . . . . 22

1.4.5 Defining Algorithm and User Parameters . . . . . . . . . 23

1.4.6 Importing Sensor Data . . . . . . . . . . . . . . . . . . . . 26

2 Reference 33

2.1 General m-files (lib folder) . . . . . . . . . . . . . . . . . . . . . . 33

2.1.1 chi2invtable . . . . . . . . . . . . . . . . . . . . . . . . 33

2.1.2 compound . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

2.1.3 diffangleunwrap . . . . . . . . . . . . . . . . . . . . . . 34

2.1.4 drawarrow . . . . . . . . . . . . . . . . . . . . . . . . . . 35

2.1.5 drawellipse . . . . . . . . . . . . . . . . . . . . . . . . . 35

2

2.1.6 drawlabel . . . . . . . . . . . . . . . . . . . . . . . . . . 37

2.1.7 drawprobellipse . . . . . . . . . . . . . . . . . . . . . . 38

2.1.8 drawrawdata . . . . . . . . . . . . . . . . . . . . . . . . . 39

2.1.9 drawreference . . . . . . . . . . . . . . . . . . . . . . . . 39

2.1.10 drawrobot . . . . . . . . . . . . . . . . . . . . . . . . . . 39

2.1.11 drawroundedrect . . . . . . . . . . . . . . . . . . . . . . 41

2.1.12 drawtransform . . . . . . . . . . . . . . . . . . . . . . . . 42

2.1.13 icompound . . . . . . . . . . . . . . . . . . . . . . . . . . 43

2.1.14 j1comp . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

2.1.15 j2comp . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

2.1.16 jinv . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

2.1.17 mahalanobis . . . . . . . . . . . . . . . . . . . . . . . . . 45

2.1.18 meanwm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

2.1.19 setangletorange . . . . . . . . . . . . . . . . . . . . . . 47

2.2 Import m-files (import folder) . . . . . . . . . . . . . . . . . . . . 48

2.2.1 closefiles . . . . . . . . . . . . . . . . . . . . . . . . . . 48

2.2.2 openfiles . . . . . . . . . . . . . . . . . . . . . . . . . . 48

2.2.3 readoneline . . . . . . . . . . . . . . . . . . . . . . . . . 48

2.2.4 readonestep . . . . . . . . . . . . . . . . . . . . . . . . . 50

2.2.5 segmentstr . . . . . . . . . . . . . . . . . . . . . . . . . . 51

2.3 Beacon Extraction m-files(featureextr/beaconextr folder) . . . . . . . . . . . . . . . . . . . 52

2.3.1 extractbeacons . . . . . . . . . . . . . . . . . . . . . . . 52

2.4 Line Extraction m-files(featureextr/lineextr folder) . . . . . . . . . . . . . . . . . . . . . 54

2.4.1 calccompactness . . . . . . . . . . . . . . . . . . . . . . 54

2.4.2 calcendpoint . . . . . . . . . . . . . . . . . . . . . . . . 54

2.4.3 extractlines . . . . . . . . . . . . . . . . . . . . . . . . 54

2.4.4 findregions . . . . . . . . . . . . . . . . . . . . . . . . . 56

2.4.5 fitlinepolar . . . . . . . . . . . . . . . . . . . . . . . . 56

2.4.6 isneighbour . . . . . . . . . . . . . . . . . . . . . . . . . 57

2.4.7 mahalanobisar . . . . . . . . . . . . . . . . . . . . . . . . 58

2.4.8 mod2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

2.5 SLAM m-files (slam folder) . . . . . . . . . . . . . . . . . . . . . 59

3

2.5.1 estimatemap . . . . . . . . . . . . . . . . . . . . . . . . . 59

2.5.2 initmap . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

2.5.3 integratenewobs . . . . . . . . . . . . . . . . . . . . . . 59

2.5.4 matchnnsf . . . . . . . . . . . . . . . . . . . . . . . . . . 60

2.5.5 predictmeasurements . . . . . . . . . . . . . . . . . . . . 60

2.5.6 robotdisplacement . . . . . . . . . . . . . . . . . . . . . 61

2.5.7 slam . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

2.6 Map entity methods (@entity folder) . . . . . . . . . . . . . . . . 62

2.6.1 display . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

2.6.2 entity . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

2.6.3 get . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

2.6.4 set . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

2.7 Point feature methods (@pointfeature folder) . . . . . . . . . . . 64

2.7.1 Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

2.7.2 calcinnovation . . . . . . . . . . . . . . . . . . . . . . . 64

2.7.3 display . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

2.7.4 draw . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

2.7.5 get . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

2.7.6 integrate . . . . . . . . . . . . . . . . . . . . . . . . . . 66

2.7.7 pointfeature . . . . . . . . . . . . . . . . . . . . . . . . 66

2.7.8 predict . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

2.7.9 set . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

2.8 (α,r)-line methods (@arlinefeature folder) . . . . . . . . . . . . . 69

2.8.1 Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

2.8.2 arlinefeature . . . . . . . . . . . . . . . . . . . . . . . . 70

2.8.3 calcinnovation . . . . . . . . . . . . . . . . . . . . . . . 70

2.8.4 display . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70

2.8.5 draw . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

2.8.6 get . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

2.8.7 integrate . . . . . . . . . . . . . . . . . . . . . . . . . . 72

2.8.8 predict . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

2.8.9 set . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73

2.9 Map methods (@map folder) . . . . . . . . . . . . . . . . . . . . 74

2.9.1 addentity . . . . . . . . . . . . . . . . . . . . . . . . . . 74

4

2.9.2 display . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

2.9.3 draw . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

2.9.4 drawcorr . . . . . . . . . . . . . . . . . . . . . . . . . . . 75

2.9.5 get . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75

2.9.6 getrobot . . . . . . . . . . . . . . . . . . . . . . . . . . . 76

2.9.7 getstate . . . . . . . . . . . . . . . . . . . . . . . . . . . 76

2.9.8 map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76

2.9.9 set . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

2.9.10 setrobot . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

2.9.11 setstate . . . . . . . . . . . . . . . . . . . . . . . . . . . 78

2.10 Robot methods (@robot folder) . . . . . . . . . . . . . . . . . . . 79

2.10.1 display . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79

2.10.2 get . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79

2.10.3 robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79

2.10.4 set . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80

2.11 Differential drive robot methods(@robotdd folder) . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

2.11.1 Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

2.11.2 display . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

2.11.3 draw . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82

2.11.4 get . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82

2.11.5 predict . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83

2.11.6 robotdd . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84

2.11.7 set . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85

5

Chapter 1

Users Manual

1.1 Introduction

1.1.1 What Does This Toolbox Do?

It is a tool to do localization and SLAM off-board and off-line with sensorydata which have been previously recorded. The toolbox has been designed tobe general with respect to the choice of the feature representation, the robotand the sensor models. This simplifies exchange of representations, methods andapproaches and might help to encourage comparative experiments and studies.In detail, the toolbox...

• decouples features from algorithms. Add your own feature class and a pre-defined number of methods and do localization or SLAM without changingthe localization or SLAM code

• decouples the robot model from algorithms. Add a robot, a predefinednumber of methods and a state prediction function which implementsdead-reckoning (e.g. odometry) for your robot given some (proprioceptive)sensory data. Again, this does not affect the rest of the toolbox

• decouples sensor models from algorithms. Have an arbitrary number ofsensors whose data are in an (almost) arbitrary file format

• allows to exchange a feature extraction or dead-reckoning method withoutchanges in the code

• allows you to add you non-parametric approach to localization and SLAM

• keeps all experimental parameters at a single place. Add your own param-eters there and reuse them where you want. This simplifies reproductionand management of experiments

6

1.1.2 What Doesn’t It Do?

The limitations of the current toolbox release are:

• It is neither an introduction to robot navigation nor to Matlab program-ming. The user is supposed to be more or less familiar with the basicsof vehicle navigation and Matlab. Please refer to the respective literatureotherwise

• Version 1.0 is feature-based. No non-parametric approaches to localizationand SLAM (e.g. particle filters) have been implemented. The structureof the toolbox would allow their integration

• Version 1.0 ships only with a differential drive robot class, a x,y-pointfeature class and a α,r-line feature class1. If this does not match yourrobot setting, you have to add the respective robot and feature classesyourself (refer to the Programmer Manual).

• Single-robot and no 3D support. World, sensors and robots are assumedto be in a 2D-universe

1.1.3 Why Matlab?

Working with a navigation toolbox in Matlab has advantages and drawbacks.With the purpose of a tool for research and education, the advantages are breifly:

• Matlab is a wide-spread, standardized and mature programming environ-ment with a big number of built-in functionalities and additions

• Programming in Matlab is easy

• Matlab runs on many platforms and operating systems

• The toolbox requires no installation or compilation (when Matlab is in-stalled)

Whereas the drawback are mainly two points:

• Matlab is commercial and costly

• Although there are high-speed built-in functions, Matlab is slow1Why this choice of features? It is the choice of maximal consensus in the robotics com-

munity. The x,y-point feature is doubtless the most widely used feature for SLAM due toits simplicity and appropriateness. For the α,r-line feature there is a wide consensus aboutit’s inappropriateness for SLAM. It allows us however to do SLAM in line-like environmentswhere point features are unavailable.

7

1.1.4 Resources, License and Feedback

The toolbox is available under a GNU GPL license and therefore usable byanybody on an ”as is” basis without warranty or charge. A GNU GPL licensemeans however that you are not allowed to use this toolbox or parts of it witha commercial purpose. See details in the attached license file License.txt andthe GNU GPL license text in the file COPYING.

The toolbox’ homepage is at http://www.cas.kth.se/toolbox. It comesas a zipped tar-file and is accompanied by the mentioned licence files. The onlysystem requirement is Matlab version greater or equal than six. No additionalMatlab toolboxes are required.

As any piece of software, this toolbox grows by its use in practice in a varietyof different setting. When you encounter a problem, a bug, you would like to re-port a documentation problem or you would like to make a suggestion for futureversions, do so by writing an email to navtoolbox AT nada.kth.se or directlyto the author kai-oliver.arras AT ieee.org. Please add information onyour computer, OS, Matlab version etc. We do appreciate your feedback!

1.2 Getting Started

Before we dive into details, let us first have a look on the toolbox’ generalstructure. It will be helpful to understand where information flows and in whatform.

1.2.1 Toolbox Structure

Figure 1.1 shows the three-layered structure of the toolbox. On the bottom isthe data generating layer whose output are streams of raw sensory data. Theyhold the contents of the sensor data files.

The second layer is the dead reckoning and feature extraction layer. Giventhe respective streams of raw sensory data (e.g. encoder or IMU values fordead reckoning, range data for feature extraction), the task of this layer is toproduce the robot state prediction x(k + 1|k), P (k + 1|k) and the observationz(k + 1), R(k + 1). The latter is sometimes called the local map.

The top layer is the localization and SLAM method. In case of a localizationexperiment, an a priori map is an additional input to this layer. Given theobservation z(k+1), R(k+1) and the robot state prediction x(k+1|k), P (k+1|k)of each discrete time step k, the algorithm in this layer builds a map or localizesa robot using the implemented method. The toolbox’ default method is anextended Kalman filter with a nearest neighbor matching.

The ”heartbeat” of the toolbox is the discrete time index k. The sensorwhich is defined to be the master sensor triggers a new step. At each new step,the data importer reads the raw data from all sensor files between the last stepk and the new step k + 1. The timestamps associated to the data guaranteetemporal consistency across all sensors. The master sensor, which can be any

8

Proprioceptive Sensory DataExteroceptive Sensory Data

SLAM, Localization

Feature Extraction Method #3


Map


Simulation

Ray Tracing

Box Map

Path Planner

Inverse Kinematics

Trajectory

x(k+1 |k+1) x(k |k) x(k+1 |k+1)

Measurem. Prediction

x(k+1 |k), P(k+1 |k)

z(k+1)

v(k+1), S(k+1)

x(k+1 |k+1), P(k+1 |k+1)

z(k+1), R(k+1)

EKF Update

Matching

non-para navigation

Dead Reckoning Method #2

Dead Reckoning Method #1

File Import

Sensor DataSensor

Data

Data Importer

SensorData

Sensor #1 dataSensor #2 data

Sensor #n data

Figure 1.1: The three layered structure of the toolbox: the data generatinglayer (bottom), the feature extraction and dead reckoning layer (middle) andthe localization and slam layer (top)

sensor, is defined in the set up file of the experiment. Typically it is one of theexteroceptive sensors such as a laser scanner.

Future releases of the toolbox might contain an additional simulation part(gray) in the data generating layer.

1.2.2 Initializing the Matlab Session

After decompression and unpacking, put the directory toolbox at a place ofyour choice. As long as the Matlab path is properly set, it does not matterwhere. Start up Matlab from toolbox or, when started from somewhere else,set toolbox as the current working directory. The current directory window ofMatlab should look similar to figure 1.2. Then execute the initialization scriptinittoolbox.m by typing inittoolbox (without the .m extension). The scriptadds all toolbox-related paths and reads in a lookup table. This has to be doneonly once per Matlab session.

The subdirectories in toolbox reflect the structure of the toolbox. All m-filesrelated to data import are in the folder import and e.g. all m-files related to

9

slam are in the slam directory. Feature extraction has the folder featureextrwhich contains subdirectories for each extraction method. Class directories inMatlab start with a @. They contain all methods of that class.

Figure 1.2: The working directory

1.2.3 Obtaining Help and Information on Objects

If the command window is cleared and the initialization script terminates byinittoolbox: Paths added, lookup table in memory, you are ready to go.Remember that you can always type help filename (without the .m extension)to obtain the help text of a file and help directoryname to get the first linesof all m-files in a directory. As method names can be ambiguous, they must becalled together with their class name using a slash, i.e. help map/set or helprobotdd/draw.

Maps, features and robots are implemented as objects. They have at leasta constructor method, a set, get and a display method. If you have an objectobj, just type obj without semicolon to invoke the display method and see thecontents of the object. The same information in a different format is obtainedby typing get(obj). The call set(obj) displays all property names and theirpossible values for the object obj. In order to access object properties, use theset and get methods.

Example:Let us create and draw a map with a robot, a point feature, and an α, r-linefeature. We first call the constructor method of the map object. The methodtakes a name and a timestamp.

>> G = map('global map',0);

10

Then we create a differential drive robot and add it to the map using theaddentity method. The robot construction method takes a name, an iden-tifier, a x, y, θ-pose, a 3 × 3 pose covariance matrix, the radii of the left andright wheel, the wheel base (all in meters) and a timestamp.

>> r = robotdd('Piggy',[1;2;pi/6],0.001*eye(3),0.2,0.2,0.5,4,0);

>> G = addentity(G,r);

Now type G without semicolon to see the contents of the map (which is merelythe robot). Or type r to see the properties of the robot. Let us create the pointfeature and add it to the map. The constructor method takes an identifier, anx, y-position and a 2× 2 position covariance matrix.

>> p = pointfeature(101,[-2;2],[0.01 0.002; 0.002 0.03]);

>> G = addentity(G,p)

where we left the semicolon away to see the new G immediately. Creating andadding the line feature is similar. Again, we specify an identifier, an α, r positionand the associated covariance matrix

>> l = arlinefeature(201,[-pi/3;2],0.001*eye(2));

>> G = addentity(G,l)

Handling objects in this way is a Matlab-wide standard. You might haveencountered this when editing Matlab figures using set/get.

Finally we want to plot the map using its draw method. As we are notsure about the arguments, we type help map/draw. Let us decide to draw ablue map including identifiers and uncertainty ellipses into the prepared figurewindow number one. As the map contains an infinite line, the result might besomewhat unpredictable and we choose to limit the window’s axes to ±3 m:

>> figure(1); clf; hold on; axis equal;

>> draw(G,1,1,0,'b');

>> axis([-3 3 -3 3]);

The result is shown in figure 1.3.

1.3 Using the Built-In Examples

The current release comes with five pre-cooked experiments: atrium, bakery1,bakery3, sjursflat with beacons, and sjursflat with lines. You can get moreinformation about an experiment via its set up file by typing help setupfilename.In the case of the sjursflat-experiment with beacons help sjursflatbeaconsetupreturns the following output:

Experiment set up file for the Matlab Robot Navigation Toolbox

Date: 07.12.03

Author: Kai Arras, CAS-KTH

11

−3 −2 −1 0 1 2 3−3

−2

−1

0

1

2

3

Figure 1.3: The resulting map

Description: The ’sjursflat’-data set shows the ground floor of Sjur

Vestli’s house, recorded using a SmartRob 2 with a forward looking

SICK LMS200. The data set contains range and beacon/retro-reflector

information. When calling this setup file, beacon-based slam is done.

Try equally atriumsetup, bakery1setup, bakery3setup, sjursflatlinesetup.

The setup file is a text file in Matlab syntax which contains all settings,parameters and constants of an experiment (more on set up files in the nextsection). In this section we describe how to plot the raw data and to build a mapfor each experiment. By ”raw data” we mean in these experiments odometrygiven the raw encoder data and the differential drive robot kinematics and theraw range/bearing data.

The command to plot raw data is drawrawdata and takes a name string, astart step and a number of steps argument. For the sjursflat-experiment withbeacons, type

>> drawrawdata('sjursflatbeaconsetup',1,280);

where we specified the set up file of the experiment and the first 280 steps. Afterthe time it takes to read and draw the data, the outcome should look like figure1.4. Let us close the loop now and build a map. We do this simply with thecommand slam:

>> G = slam('sjursflatbeaconsetup',1,280,3);

The first three arguments are the same as in drawrawdata and the forth ar-gument is a display parameter. Typing help slam reveals the meaning of theparameters:

12

SLAM Simultaneous localization and mapping.

M = SLAM(SETUPFILENAME,STARTSTEP,NSTEPS,DISPLAYMODE) builds a stochas-

tic map from the sensor data specified in the setup file SETUPFILENAME.

The setup file contains the names of all sensor data files, the robot

model, the sensor models and the experimental and algorithmic para-

meters of the experiment.

Execution is started at STARTSTEP and iterates NSTEPS times. DISPLAY-

MODE is between 0 and 4. With 0 the algorithm runs through, with 1 the

global map is drawn at each step, and paused if DISPLAYMODE = 2. with

DISPLAYMODE = 3, the global and the local map is drawn, and paused if

DISPLAYMODE = 4.

See also DRAWRAWDATA.

The command slam implements the Kalman filter-based stochastic map ap-proach [1, 2] using a nearest neighbor-matching strategy. As soon as we startthe SLAM-command, Matlab shows us the local map for each step (the obser-vations) and the growing global map without pausing between the steps. Whenmap building is terminated, the resulting map should look like figure 1.4.

Figure 1.4: Figure 1.5:

The toolbox decouples the feature type from the algorithms. Without chang-ing the code, it can be equally used to build a line-based map. This is what wedo next. First, to get in idea of the experiment, let us draw the raw data:

>> drawrawdata('atriumsetup',1,310);

Then let us build the map:

>> G = slam('atriumsetup',1,310,3);

The outcome is figure 1.7. Although figure 1.7 looks like a dish of uncookedspaghettis, it a correct map with infinite lines in which the loop could havebeen closed.

The set up file specifies the feature type to be used in a pretty simple way. Itspecifies the feature extraction method which in turn produces the observationsz(k + 1), R(k + 1) in form of the local map. Once in a map (local and globalmaps are represented identically), operations on features call methods which

13


are specific for each feature type. Adding a new feature type means adding aproperly named folder and eight methods.

But before we get too technical, let us also plot the data and maps of theother three experiments, all using lines.

>> drawrawdata('bakery1setup',1,160);

>> G = slam('bakery1setup',1,160,3);

The two commands yield figures 1.8 and 1.9. Similarly, the commands


>> drawrawdata('bakery3setup',1,72);

>> G = slam('bakery3setup',1,72,3);

and

>> drawrawdata('sjursflatlinesetup',1,280);

>> G = slam('sjursflatlinesetup',1,280,3);

produce figures 1.10, 1.11, 1.12, and 1.13 respectively. To accelerate executiontype 0 for the last argument of slam.

14



The resulting maps are identical to the corresponding tiff-image files whichaccompany each data set.

In this section we learned how to use the high level commands drawrawdataand slam. As the toolbox decouples feature representations from algorithms, itcan be used with several feature types without changing the code. The com-mand drawrawdata plots the exploration path of the robot based on encoderdata, the range/bearing data and the retro-reflector data provided the respectiveinformation in the data files. slam takes these data and builds a map accordingto a vanilla implementation of the stochastic map approach.

15

1.4 Using the Toolbox With Own Data

This section explains how to import sensor data, define the sensor and robotmodels and adjust the algorithm parameters. All this is done once per experi-ment and at one place, namely, the set up file.

1.4.1 The Setup File of an Experiment

Often one has data of several experimental runs, possibly from different robotsin different environments using different sensors. To reproduce an experiment,to compare results across different runs and to simply keep the overview of alldata can become a time-consuming job and produce code like this:

sigma_rho = 0.01; % Monday experiment: 0.02 % Robby2 sigma_rho=0.003

With the set up file, all parameters are kept together for each experiment.This makes bookkeeping of even large numbers of experiments a feasible task.The set up file is a plain text file in Matlab syntax. There are no namingconventions except the .m extension at the end. Let us have a look at a fewlines of atriumsetup.m, the setup file of the atrium experiment (figure 1.14). Itstarts with a short description of the data and the experiment. According to theMatlab-wide standard that the first commented lines of an m-file appear as thehelp text with the help command, typing help atriumsetup returns this text.The existence of a help text which describes the experiment is not mandatoryfrom a technical point-of-view but highly recommended.

% Experiment setup file for the CAS Robot Navigation Toolbox % Date: 21.12.03% Author: Kai Arras, CAS-KTH% Description: The atrium data set has been recorded by Patric Jensfelt% in the KTH main building, Stockholm. The robot is a Nomad Scout with% a forward looking SICK LMS200. Scan frequency is quite high, greater% than the frequency of the encoder measurements. The data set is% challenging as the environment is highly dynamic.% In this experiment, the scans are downsampled by a factor of five% for accelerating purposes.

% ----- Sensor 1 Model and File Settings ----- %% sensor nameparams.sensor1.name = 'Wheel encoders';% full file name and label to look forparams.sensor1.datafile = 'atrium.cnt';params.sensor1.label = 'E';% is information in file relative 0: no, 1: yesparams.sensor1.isrelative = 1;% index stringparams.sensor1.indexstr = '1,2,3,4';% robot odometry error modelparams.sensor1.kl = 0.00001; % error growth factor for left wheel in [1/m]params.sensor1.kr = 0.00001; % error growth factor for right wheel in [1/m]...

Figure 1.14: Set up file of the atrium experiment

In the set up file a structure called params is declared. Certain fields ofparams are mandatory, they need to be defined. Other fields are optional which

16

take default values when left away. Further, params can contain an arbitrarynumber of user specified fields.2

1.4.2 Defining a Robot

The set up file must contain a definition of the robot which can look like in theexample of figure 1.15 (from bakery1setup.m). For the definition of a robot,

...

% ----- Robot Model ----- %% robot nameparams.robot.name = 'Pluto';% robot class nameparams.robot.class = 'robotdd';% robot form type (see help drawrobot)params.robot.formtype = 5;% robot kinematicsparams.robot.b = 0.60; % robot wheelbase in [m]params.robot.rl = 0.25; % left wheel radius in [m]params.robot.rr = 0.25; % right wheel radius in [m]% initial robot start pose and pose covarianceparams.robot.x = zeros(3,1);params.robot.C = 0.0001*eye(3);

...

Figure 1.15: Robot definition in the bakery1-experiment

the following parameters are needed:

• params.robot.name

The field params.robot.name is a string in single quotes, defining the name ofthe robot.

• params.robot.class

The field params.robot.class is a string in single quotes, defining the nameof the robot class which encodes the vehicle kinematics. In the current toolboxrelease there is only the differential drive robot class robotdd.

• params.robot.formtype

The field params.robot.formtype is an integer between zero and five accordingto the respective argument of drawrobot. The number stands for a shape to beplotted in all figures showing the robot. Refer to the Reference section wheredrawrobot is described.

2An alternative concept would be a modular approach in which each robot and each sensorhave their own definition file. According to the experimental setup, definition files could becombined maximizing their reuse. The problem is however that the same robot, for instance,deployed in different environments (e.g. indoor, outdoor, on hard floors, on carpet) mighthave different model setting for non-systematic dead-reckoning errors. Sensors with variableconfigurations (e.g. for angular resolution, maximal range) pose the same problem. Keepingall parameters together in one file for each experiment is a simple and effective solution

17

• params.robot.b

• params.robot.rl

• params.robot.rr

The fields params.robot.b, params.robot.rl and params.robot.rr are thewheel base, the radius of the left and the radius of the right wheel, all in [m].They define the kinematics of a differential drive robot according to figure 1.16.The robot reference frame R is attached such that the x-axis points in theforward direction of travel.

brR

rL

x

y

Figure 1.16: Differential drive robot with attached frame R

• params.robot.x

• params.robot.C

The fields params.robot.x and params.robot.C are a 3× 1 vector and a 3× 3matrix, respectively. They denote the initial pose and pose covariance fromwhich the robot will start in an experiment. The pose vector x has elementsx, y, θ in units [m], [m], [rad]. Note that C is a covariance matrix and must notbe a negative definite matrix.

1.4.3 Defining a Sensor

The set up file must at least define a proprioceptive sensor such as odometryand an exteroceptive sensor such as a laser range finder or a beacon sensor3. Asthe toolbox has no internal limitation of the number and kind of sensors, thefile can also contain the definition of several sensors of the same type. Sensorsare numbered by an integer i starting from one. This integer is part of the fieldname params.sensor like in params.sensor2.name = 'Leuze P4'.

As an example of a sensor definition, see figure 1.14 where the wheel encodersfor the atrium-experiment are defined. Figure 1.17 shows the example definitionof a laser scanner and a beacon sensor (from sjursbeaconsetup.m).

For the definition of a sensor, the following parameters are needed:3By “beacon sensor” we denote a sensor which is able to measure the position of retro-

reflective beacons in the environment, be this a dedicated sensor or a range finder such as theSICK scanners in certain configurations

18

...

% ----- Sensor 2 Model and File Settings ----- %% sensor nameparams.sensor2.name = 'Sick LMS200 indoor';% full file name and label to look forparams.sensor2.datafile = 'sjursflat.scn';params.sensor2.label = 'S';% temporal and spatial downsampling factorparams.sensor2.tdownsample = 1;params.sensor2.sdownsample = 20;% index stringparams.sensor2.indexstr = '1,2,3,4:2:end,5:2:end';% maximal perception radius of sensor in [m]params.sensor2.rs = 8.0;% angular resolution in [rad]params.sensor2.anglereso = 0.5*pi/180;% constant range uncertainty in [m]params.sensor2.stdrho = 0.02;% robot-to-sensor transform expressed in the% robot frame with units [m] [m] [rad]params.sensor2.xs = [0.2945; 0; 0.0];

% ----- Sensor 3 Model and File Settings ----- %% sensor nameparams.sensor3.name = 'SICK LMS200 Beacons';% full file name and label to look forparams.sensor3.datafile = 'sjursflat.bcn';params.sensor3.label = 'B';% index stringparams.sensor3.indexstr = '1,2,3,4:2:end,5:2:end';% feature extraction m-fileparams.sensor3.extractionfnc = 'extractbeacons';% robot-to-sensor transform expressed in the% robot frame with units [m] [m] [rad]params.sensor3.xs = [0.2945; 0; 0.0];

...

Figure 1.17: Sensor definitions in the sjursflatbeacon-experiment

• params.sensori.name

The field params.sensori.name is a string in single quotes, defining the sensorname and, by the integer i, the sensor number.

• params.sensori.datafile

The field params.sensori.datafile is a string in single quotes with the nameof the data file. There are no naming conventions for data files. Note that thefiles are supposed to be in the Matlab path.

• params.sensori.label

The field params.sensori.label is a string in single quotes, defining the labelby which information of sensor i is marked in the data file. Refer to section1.4.6 for more information.

• params.sensori.tdownsample

• params.sensori.sdownsample

19

The fields params.sensori.tdownsample and params.sensori.sdownsampleare positive integers which denote the temporal and spatial downsampling fac-tors. With tdownsample being n the sensory data are temporally thinned out,that is, only each nth entry in the file is read in. With sdownsample beingm the sensory data are thinned out by the factor m, that is, only each mthmeasurement of the same entry is read in. Spatial downsampling makes onlysense with sensors which deliver more than one measurements at a time such asa laser scanner.

Temporal downsampling can be useful to reduce the amount of data whena sensor delivered its data at a much higher frequency than other sensors inthe experiment. Spatial downsampling can be useful to reduce the number ofmeasurement in a scan of a laser range finder. Both factors have accelerationpurposes.

The fields tdownsample and sdownsample are optional. When undefinedthey will both get the default value 1 (no downsampling).

• params.sensori.indexstr

The field params.sensori.indexstr is a string in single quotes which definesthe way how each line in the data file is interpreted. It is a comma-separated listwith valid Matlab index expressions as elements. Refer to section 1.4.6 whichdiscusses how to import sensory information from the data files in more detail.

• params.sensori.isabsolute

The field params.sensori.isabsolute is a boolean, either 0 or 1. The param-eter determines how the sensory information is read out from the file. Wheneach entry (each line) is a measurement on its own, the flag is to be set to 0.If an actual measurement is only obtained by reading two subsequent entries inthe file (two lines), set isabsolute to 1.

The parameter is introduced in order to be able to read out absolute as wellas relative encoder measurements. With absolute encoder readings (isabsolute= 1), each entry is an wheel angle and by taking the difference of two subsequentmeasurements, one obtains the required angular difference. In case of relativeencoder measurements (isabsolute = 0), the file is supposed to already con-tain these differences required by the odometry model.

• params.sensori.extractionfnc

The field params.sensori.extractionfnc is a string in single quotes, definingthe name of the m-file with an extraction method. An extraction method hasthe task to produce the observations z(k + 1), R(k + 1) given the raw sensorydata of step k + 1. By defining an extraction function (which makes no sensefor proprioceptive sensors) the user determines which sensor is to be taken forslam and localization.

With an undefined extraction function, the data of sensor i are ignored.However, regardless a definition, drawrawdata tries always to plot data of all

20

sensors which are defined in the set up file. Let us have a look at the setup fileof the sjursflatbeacon-experiment (figure 1.17). The command drawrawdataplotted the beacon information as well as the range information (figure 1.4) butfor slam the range information was ignored. This was achieved by the absenceof a extractionfnc field for the range sensor (sensor 2) but the specification ofextractbeacons for the beacon sensor (sensor 3). By the way, the range dataare downsampled by a factor of 20 which explains the fewer points compared tothe raw data plot of the sjursflatline-experiment (figure 1.12).

In the sjursflatline-experiment where line-based slam is done using the samedata set, the definition params.sensor2.extractionfnc = 'extractlines'for the laser scanner replaces the extraction function definition of the beaconsensor.

Note that it with this concept exchanging an extraction function by an otheris very easy and facilitates the comparison of feature extraction methods. Referto the Programmers Manual to learn about the interface definition for extractionfunctions.

• params.sensori.xs

The field params.sensori.xs is a 3×1 vector, defining the deterministic robot-to-sensor transform xS . The data of sensor i are assumed to be represented inS, a reference frame attached to sensor i (figure 1.18). Vector xS with unitsmeter, meter, radian gives the pose of S expressed in the robot frame R. If xs isundefined, it takes the default value xS = (0, 0, 0)T . For proprioceptive sensors(encoders, inertial measurement units), xs has no meaning.

Figure 1.18: Robot-to-sensor transform

Finally, the fields

• params.sensori.rs

• params.sensori.anglereso

• params.sensori.stdrho

are float numbers, specifying the maximal perception radius of the range sen-sor in [m], its angular resolution in [rad] and a constant standard deviation

21

in radial direction in [m]. These parameters are required at several placessuch as drawrawdata or extractlines, the line extraction method includedin this toolbox release. The latter employes a fairly simple uncertainty modelfor range data, that is, no uncertainty in angle and a constant uncertainty σρ

in radial direction. There is no limitation whatsoever to more sophisticatedsensor models. The user has two possiblities: either the data files contain un-certainty information or parameters such as params.sensori.rhoparam1 andparams.sensori.rhoparam2 could be added to implement a user-specified un-certainty model in the extraction function.

Note that the maximal perception radius RS and the angular resolution ∆φmight vary even when using the same sensor. For a SICK LMS 200, for instance,different sensor configurations cause RS and ∆φ to change.

Similarly, the fields

• params.sensori.kl

• params.sensori.kr

are required by the state prediction method of the differential drive robot class.They are float numbers specifying the growth factors of non-systematic odome-try errors with unit 1/[m]. For more information type help robotdd/predict.

1.4.4 Defining the Master Sensor

The master sensor gives the “heartbeat” of the toolbox. It triggers a new stepincreasing the discrete time index k. An example definition is shown in figure1.19 (from sjursflatbeaconsetup.m).

...

% ----- Master Sensor Setting ----- %% define master sensorparams.mastersensid = 2;

...

Figure 1.19: Example definition of the master sensor

The definition in the set up file is done by the field

• params.mastersensid

which must get the identifier value i of one of the sensors defined in the setup file. The master sensor can be any sensor regardless its acquistion speed(it is not required that it is the “slower” sensor). Typically it is one of theexteroceptive sensors such as the laser scanner.

22

1.4.5 Defining Algorithm and User Parameters

Besides the definition of sensors and robots, other parameters for feature ex-traction, slam, localization or user-specified algorithms can also be added tothe params structure. Let us start with the beacon extraction algorithm whichrequires a threshold distance for segmentation in [m], a maximal range valueof a beacon reading to be accepted, expressed as dmax = RS − dignore in [m],and a minimal number of readings for a cluster or segment to be a valid beaconmeasurement.

• params.beaconthreshdist

• params.ignoredist

• params.minnpoints

• params.Cb

The last parameters, Cb is a 2 × 2 matrix, specifying a constant x, y-positionuncertainty of raw beacon readings. Again, Cb can also be replaced by parame-ters for a more complex uncertainty model. An example definition is shown infigure 1.20 (from sjursflatbeaconsetup.m).

...

% ----- Feature Extraction ----- %% jump distance for beacon segmentationparams.beaconthreshdist = 0.15; % in [m]% beacons which are farer away than rs-ignoredist are ignoredparams.ignoredist = 0.1; % in [m]% minimal nof raw points on reflector in order to be validparams.minnpoints = 2;% constant beacon covariance matrixparams.Cb = 0.01*eye(2);

% ----- Data Association ----- %% significance level for NNSF matchingparams.alpha = 0.999;

Figure 1.20: Algorithm parameter definitions in sjursflatbeaconsetup.m

When line extraction is used as in the atrium-experiment, the set up filemust contain definitions of the following parameters (figure 1.21):

• params.windowsize

• params.threshfidel

• params.fusealpha

• params.minlength

• params.compensa

• params.compensr

23

...

% ----- Feature Extraction ----- %% size of sliding windowparams.windowsize = 11; % in number of points% threshold on model fidelityparams.threshfidel = 0.2;% significance level for line fusionparams.fusealpha = 0.9999; % between 0 and 1% minimal length a segment must have to be acceptedparams.minlength = 0.75; % in [m]% heuristic compensation factors for raw data correlationsparams.compensa = 1*pi/180; % in [rad]params.compensr = 0.01; % in [m]% are the scans cyclic?params.cyclic = 0; % 0: non-cyclic or 1: cyclic

% ----- Data Association ----- %% significance level for NNSF matchingparams.alpha = 0.999;

% ----- Slam ----- %% optional axis vector for global map figure. Useful with infinite linesparams.axisvec = [-14 24 -19 13];

Figure 1.21: Algorithm parameter definitions in atriumsetup.m

• params.cyclic

Type help extractlines or refer to [3] for an explanation.

Of course, slam-specific parameters need to be defined:

• params.alpha

The field params.alpha is a float number between 0 and 1, specifying the sig-nificance level α for matching.

• params.axisvec

The field params.axisvec is a 1 × 4 vector, specifying the axis limits of thefigure window in which drawrawdata and slam draw into. Especially withinfinite lines, it makes sense to limit the axis to the area of exploration.

The parameter values of the built-in examples are relatively universal andmight be useful for other experiments. Note that the atrium- and the sjursflat-experiment have been conducted with a SICK LMS 200 laser scanner and thatboth robot have a pretty good odometry. The robot in the bakery-experimentshas a poor odometry which is reflected by much bigger error growth coefficients.

We have seen the way parameters get defined as fields of params. Clearly,the structure params can carry an arbitrary number and kind of fields. If a user-specified algorithm, be this a feature extraction method, a different matchingstrategy or a new slam approach, is added, extend params and pass it as anargument to the algorithm. The general syntax is

params.myparameter = myvalue;

24

The semicolon avoids that the line is printed out each time Matlab evaluatesthe set up file.

25

1.4.6 Importing Sensor Data

This section explains how the toolbox’ data importer works in detail and shedslight onto the three parameters params.sensori.indexstr, params.sensori.isabsolute and params.sensori.label.

Data File Syntax

The general file syntax of data files is one measurement per line. Each measure-ment starts by an optional label string, is followed by a timestamp and by oneor more data entries. The definition in the Extended Bakus Naur Form is

[ label ] timestamp ( data )+

with label being a string and timestamp and data a number. The squarebrackets mean an optional item, the parentheses () mean a group of items andthe + means repeat 1 or more times. A file can contain data of several sensors,distinguished by their label.

Data files are ASCII text files whose lines are terminated by an end-of-line symbol such as CR or CRLF. The toolbox can deal with all common linetermination symbols.

There is no restriction to certain physical units of the data. Also timestampsmay be in milliseconds, seconds or any other temporal unit as long as it wasconsistently used across all sensors. However, the toolbox was thought to workwith the metric system and therefore its use is recommended.

Examples shall illustrate the toolbox’ import functionalities: The data fileshown in figure 1.22 contains measurements from a laser scanner sensor (called'S'). Each line contains a full scan with a varying number of readings. Thesyntax of this file is

'S' t n x1 y1 x2 y2 ... xn yn

where t is the timestamp, n the number of points in the scan, and xi, yi their

S 9014317 46 3.540751 3.190619 2.394898 2.079227 ...S 9020451 356 3.213946 -0.149128 3.012289 -0.186073 ...S 9067931 363 2.861270 -0.113778 2.795251 -0.164106 ......

Figure 1.22: A data file with measurements from a sensor 'S'

Cartesian coordinates.

The data file shown in figure 1.23 contains data from two sources, a scanningsensor and a beacon sensor. The syntax of the respective sensor entries is

'SCANNER' t n θ1 ρ1 θ2 ρ2 ... θn ρn

'BEACON' t n x1 y1 x2 y2 ... xn yn

26

SCANNER 231447 361 -0.0000 -4.4250 0.0386 -4.4408 ...BEACONS 231447 23 0.1540 -4.4233 0.1922 -4.4138 ...SCANNER 231846 361 -0.0000 -3.0510 0.0279 -3.2039 ...BEACONS 231846 20 0.4221 -4.3958 0.4604 -4.3929 ...SCANNER 233709 361 -0.0000 -2.2510 0.0195 -2.2439 ...BEACONS 233709 16 3.0433 -2.8509 3.0672 -2.8236 ......

Figure 1.23: A file with data from a scanner and a beacon sensor

where the points of the scanner θi, ρi are in polar coordinates, and those of thebeacon sensor xi, yi in Cartesian coordinates. The label allows to distinguishseveral sources and to have an arbitrary number of sensors in the same file.Consequently it is also possible to have all sensory data of an experiment in asingle file. There is no restriction of the number or succession of file entries fromdifferent sources as long as each entry (measurement) is on one line.

The data file in figure 1.24 contains data from, say, an ultra-sonic rangesensors with a sophisticated error model. The syntax of each entry is

t ρ θ σ2ρ σ2

θ σρθ

where each range measurement is expressed in polar coordinates and associatedwith a 2 × 2 covariance matrix. As there is a single sensor, no label string isrequired.

239113 9.731406 -0.338036 0.424148 0.016300 -0.00096239432 11.447291 -0.756472 0.326828 0.100545 0.00193239751 3.443056 -0.065482 0.434150 0.054388 0.00034...

Figure 1.24: A data file without label string

In practice, a label string in the beginning of each line helps to keep thefile readable and searchable by hand. A label is also helpful for debugging, forinstance, when a transmission errors truncate a data file at some position andcause an error message to occur.

In any case – label or not – the toolbox uses the string from the parameter

• params.sensori.label

to find those lines in a data file which correspond to sensor i. Therefore,when defining a sensor, we have to tell the toolbox which label it can ex-pect for sensor i. Example definitions are params.sensori.label = 'S' orparams.sensori.label = 'BEACON'. When the lines have no label string, thedefinition must reflect this by an empty string params.sensori.label = ''.

27

Concept

The toolbox can theoretically import data from an unlimited number of sensorsbut there must be one sensor which defines a new step. In an estimation theorycontext it is the sensor which delivers the observations. As this sensor definesthe ”heartbeat” of the toolbox, it is called the master sensor.

The measurements of the master sensor trigger a new step and increase thediscrete time index k. At each new step, the data importer reads the datafrom all sensor files between the last step k and the new step k + 1. Thetimestamps associated to the data guarantee temporal consistency across allsensors regardless whether their data are in several files or in the same file. Themaster sensor – which can be any sensor – is defined in the setup file of theexperiment by the params.mastersensid parameter. Typically it is one of theexteroceptive sensors such as a laser scanner.

The temporal concept for importing data into the toolbox is illustrated infigure 1.26. We assume the master sensor being a laser scanner and sensor n apair of wheel encoders. Measurements of the laser scanner are full scans (e.g.figure 1.22) which are read out at indices tk and tk+1. The measurements ofthe encoder sensors are angular wheel displacements in radian (e.g. figure 1.25).The file importer takes the timestamp of the master sensor and reads out theencoder file until the timestamp of their data is greater or equal. For step tk theposition indicator of the encoder file stops at ti+1 as ti < tk. In the next step,tk+1, the file importer starts to read from the last position ti+1, again until thetimestamp condition is met at tj+1, and so on.

ENC 9014314 -0.12223 20.32011ENC 9014321 -0.13564 20.92810ENC 9014325 -0.15004 21.48605...

Figure 1.25: Example of an encoder data file

tk tk+1

ti ti+1 tj tj+1

master sensor

sensor n T

T

Figure 1.26: Example definition of the master sensor

With figure 1.26 in mind we are also able to understand the paramterisabsolute.

28

The above scheme is correct if the file entries of the encoder file are mea-surements on their own, for instance, angular differences which can directlybe used for odometry. In such a case, the sensory information is relative andisabsolute must be 0. If the file contains raw encoder angles which are (fora forward moving vehicle) monotonically increasing, the sensory information isabsolute (isabsolute must be 1) and the difference is to be taken somewherein the toolbox. With isabsolute being 1 the position indicator is set back fromti+1 to ti before starting to read out data for the next step since otherwise thewheel displacement information ∆t = ti+1 − ti would be lost.

robotdd/predict, the state prediction method of the differential drive robotclass, requires monotonically increasing wheel angles. Encoder data must there-fore be in the latter from and isabsolute is to be set to 1.

Mapping Data

In this subsection we address the question how, given a file with measurementsin a certain syntax, we tell the toolbox how to interpret the data entries fromthe files. This is done with the parameter

• params.sensori.indexstr

which is a string in single quotes. indexstr is a comma-separated list with Mat-lab index expressions as elements. An example is '1,2,3,4:2:end,5:2:end'.Before getting into details, we need to get familiar with the way how Matlabdeals with indexing elements in a matrix. First we create a vector of ten elementsvec:

>> vec = 1:10vec =

1 2 3 4 5 6 7 8 9 10

We can use single indices or a vector of indices to obtain values from vec:

>> vec(3)ans =

3>> vec(5:10)ans =

5 6 7 8 9 10

With index vectors we are able to pick out each 3rd value for instance, or invertthe succession of the returned elements:

>> vec(2:3:10)ans =

2 5 8>> vec(10:-1:6)ans =

10 9 8 7 6

29

The reserved keyword end has the same meaning as length(vec) with theadvantage that no variable name needs to be known:

>> vec(end)ans =

10>> vec(4:2:end)ans =

4 6 8 10

Let’s get back to our parameter: The index string performs a mapping of el-ements on the lines of a data file into the toolbox-internal structure sensors(which you don’t need to know in more detail). Each expression in the comma-separated list of indexstr must correspond to an item on a line. Not everyitem on a line must be assigned. Cross-assignements changing the succession ofdata items and vector-valued mappings are allowed. They shall be illustratedby examples: Suppose the lines in a file have the syntax

label t x y σ2x σ2

y σxy

The index string '1,2,3,4,5,6,7' maps the label label, the timestamp t, thex-coordinate x, the y-coordinate y, and each of the elements of the associatedcovariance matrix, σ2

x, σ2y, σxy, into a separate field of the internal sensors-

structure. With the same file and the index string '2,4,3,5,7,6' the labelis ignored and the succession of x and y and of the covariances is inverted.The string '1,2,3:4,5:7' maps the Cartesian coordinates and the elementsof the covariance matrix into vector-valued field of dimensions 1 × 2 and 1 × 3respectively. With the string '1,2,end:-1:3' the data items of the first twomoments, x, y, σ2

x, σ2y, σxy, are kept together in reverse succession (figure 1.27).

label

t

x

y

sxx

syy

sxy

label t x y sxx syy sxy

indexstr = ‘1,2,3,4,5,6,7’

label

t

x,y

sxx,syy,sxy


indexstr = ‘1,2,3:4,5:7’

label

t

sxy,syy,sxx,y,x


indexstr = ‘1,2,end:-1:3’

t

y

x

sxx

sxy

syy


indexstr = ‘2,4,3,5,7,6’

Figure 1.27: Example mappings with different index string expressions

30

Suppose now that the lines in an other example file carry sensory informationfrom a scanning sensor and have the syntax

t n ρ1 θ1 σ2ρ1

ρ2 θ2 σ2ρ2

.... ρ1 θn σ2ρn

For further processing we want to keep the ρ’s, the θ’s and the σρ’s together.This is done with the index string '1,2,3:3:end,4:3:end,5:3:end' whichbesides the timestamp t and the number of points n picks each 3rd data itemand puts it into one vector.

t

n

rho1,...,rhon

theta1,...,thetan

srr1,...,srrn

t n rho1 theta1 srr1 ... rhon thetan srrn

indexstr = ‘1,2,3:3:end,4:3:end,5:3:end’

Figure 1.28: Example mapping of vector-valued data items

Note that the index 1 in indexstr maps always the first element on the line,regardless whether it is the label string or – in absence of a label string – thetimestamp.

Clearly, the index string determines the way our data is put into the internalstructure sensors. There is no real interpretation of the data, i.e. the meaningof the data is not taken into account at any point during import. The places inthe toolbox where meaning comes into play are those m-files which do somethingwith the data, be this

• Plotting: drawrawdata.m

• Feature extraction: e.g. extractlines.m, extractbeacons.m

• State prediction: e.g. robotdd/predict.m

When you import data in your own format, you are responsible that the corre-sponding m-files correctly interpret your data. The toolbox cannot know thisin advance.

Employing Own Data Without Editing Code

If you want to work with the toolbox and your own data without editing code,you have to stick to the following formats

• Encoder data are (absolute) angles in radian, monotonically increasing forthe left and right wheel when the vehicle is moving forward

• Range finder data (e.g. laser scanner) are in Cartesian coordinates

31

• Beacon sensor data are in Cartesian coordinates.

Figure 1.29 shows how after mapping by the index string expression the internalpresentation shall look like. Example file formats which result in these map-pings are figure 1.25 for encoder data (with isabsolute = 1 and indexstr ='1,2,3,4') and figure 1.23 for laser scanner and beacon data (with indexstr= '1,2,3,4:2:end,5:2:end').

label

t

alphal

alphar

label

t

n

x1,x2,...,xn

y1,y2,...,yn

label

t

n

x1,x2,...,xn

y1,y2,...,yn

a) b) c)

Figure 1.29: Mappings for a) encoder data, b) range finder data, and c) beaconsensor data

32

Chapter 2

Reference

2.1 General m-files (lib folder)

2.1.1 chi2invtable

Lookup table of the inverse of the χ2 cumulative distribution function.

x = chi2invtable(p,v) returns the inverse of the χ2 cumulative distributionfunction (cdf) with v degrees of freedom at the value p. The χ2 cdf with vdegrees of freedom, is the Γ cdf with parameters v/2 and 2.

Opposed to chi2inv of the Matlab statistics toolbox (which might be not partof your Matlab installation), chi2invtable is a lookup table and thereby muchfaster than chi2inv. However, as any lookup table is a collection of samplepoints, accuracy is smaller. Between the sample points of the cdf, a linear in-terpolation is made.

Currently, the function supports the degrees of freedom v between 1 and 10and the probability levels p between 0 and 0.9999 in steps of 0.0001 plus thelevel 0.99999.

Example: A typical usage scenario of chi2invtable (or chi2inv) is dur-ing the matching step of a Kalman filter localization or slam cycle. Given theprobability α and features of dimension n, chi2invtable yields the maximalMahalanobis distance (or gate distance) χ2

n,α which a candidate pairing withinnovation νij and innovation covariance Sij may have in order to be accepted.In other words, the pairing is accepted if the following holds:

νTij Sij νij < χ2

n,α

See also chi2inv.

33

2.1.2 compound

Compound relationship in 2D.

xik = compound(xij,xjk) returns the compound relationship of the two 2Dtransforms xij and xjk which are arranged head-to-tail. x’s are 3 × 1-vectors[x, y, θ]T , orientations within [0..2π[.

Example: Given the transform xWR which expresses entity R in the refer-ence frame of W and transform xRS which represents entity S in the frame ofR, then the composition xWS is the relationship which expresses S in the frameof W :

xWS = xWR ⊕ xRS

Note that the compound operation would be the same as vector addition if therewere no orientations.

xWR

xRS

xWS

Reference: R. Smith, M. Self, P. Cheeseman, ”Estimating Uncertain SpatialRelationships in Robotics,” in Autonomous Robot Vehicles, I.J. Cox and G.T.Wilfong, Eds.: Springer-Verlag, 1990, pp. 167-193.

See also icompound, j1comp, j2comp.

2.1.3 diffangleunwrap

Take difference of two angles and unwrap it.

α = diffangleunwrap(α1,α2) determines the minimal difference α = α1 − α2

between two angles α1 and α2. If either α1 or α2 is Inf, Inf is returned.

Example: The difference of α1 = 107.54 and α2 = −115.97 is α = −136.49.diffangleunwrap is best illustrated with its demo script demodiffangle whichgenerated also this figure.

See also setangletorange.

34

α1

α2

α1 [deg] = 107.5391α2 [deg] = -115.9741

α1- α2 = -136.4867

2.1.4 drawarrow

Draw an arrow.

drawarrow(xs,xe,filled,hsize,color) draws an arrow from xs to xe. Thefirst two elements of xs, xe are interpreted as the x- and y-positions. filledenables and disables head filling, hsize scales the size of the head in [m], andcolor is a [r g b]-vector or a color string such as 'r' or 'g'.

h = drawarrow(...) return a column vector of handles to the graphic ob-jects of the arrow drawing.

Example: The commands

drawarrow([1 3 -pi/1.8],[2 0 pi/30],0,1,'k');

drawarrow([-3 2 -pi/8],[-2 1 3*pi/2],0,0.3,'b');

drawarrow([0.5 -0.5],[0 2.2],1,1,[0.4 0.9 0.1]);

drawarrow([-1 2], [-2 -1],1,0.2,[0.6 0.6 0.2]);

h = drawarrow([0 -1],[-1 -1.6],1,0.5,'r');

set(h,'LineWidth',3);

generate the arrows shown in the figure below. Note that the line width of thelast arrow in the bottom of the figure has been changed using the handle vectorh.

See also drawreference, plot.

2.1.5 drawellipse

Draw ellipse.

drawellipse(x,a,b,color) draws an ellipse at x = [x, y, θ]T with half axesa and b. Orientation θ is the inclination angle of a, regardless if a is smaller or

35

-3 -2 -1 0 1 2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

3

greater than b. color is a [r g b]-vector or a color string such as 'r' or 'g'.

h = drawellipse(...) returns the graphic handle h.


plot(1,-0.4,'k+');

drawellipse([1 -0.4 pi/6],1,0.5,'k')

plot(0.5,0,'k+');

drawellipse([0.5 0 pi/6],0.25,0.5,'b');

plot(1.8,-0.8,'k+');

h = drawellipse([1.8 -0.8 pi/6],0.2,0.2,[0 0.7 0.3]);

set(h,'LineStyle','-.');

generate the ellipses shown below. Note that the line style of the circle can bechanged using the handle vector h.

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-1

-0.5

0

0.5

See also drawprobellipse.

36

2.1.6 drawlabel

Draw scalable text.

drawlabel(x,str,scale,offset,color) draws scalable text str at pose x =[x, y, θ]T imitating the OCR font. With scale = 1, the height of the letters is1 meter. offset shifts the text in [m] from the x, y position in postive x- andy-direction. color is either a [r g b]-vector or a color string such as 'r' or 'g'.

Currently, the following characters are implemented: 0, 1, 2, 3, 4, 5, 6, 7,8, 9, W, R, S, E, F, M, P.

h = drawlabel(...) returns a column vector of handles to all line objectsof the drawing, one handle per line.


plot(-3,2,'k+'); drawlabel([-3 2 0],'123',0.5,0.0,'k');



plot(-1,2,'k+'); drawlabel([-1,2 -2.8],'R1',0.2,0.1,[.5 .5 .5]);

plot( 1,3,'k+'); drawlabel([ 1 3 -pi/1.8],'F30493',0.8,0.3,'g');

plot(-2,-1,'k+'); h = drawlabel([-2 -1 2.6],'S04',0.3,0.1,'r');


plot(-1,-2,'k+'); h = drawlabel([-1 -2 pi/3],'REF',0.3,0.1,'m');


generate the figure shown below. Note that line width can be varied using thehandle vector h.

-3 -2 -1 0 1 2

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

3

See also text.

37

2.1.7 drawprobellipse

Draw elliptic probability region of a Gaussian in 2D.

drawprobellipse(x,C,alpha,color) draws the elliptic iso-probability contourof a Gaussian distributed bivariate random vector x at the significance levelalpha. The ellipse is centered at x = [x, y]T where C is the associated 2 × 2covariance matrix. color is a [r g b]-vector or a color string such as 'r' or 'g'.

x and C can also be of size 3× 1 and 3× 3 respectively.

The function uses chi2invtable instead of chi2inv from the Matlab statis-tics toolbox.

In case of a negative definite matrix C, the ellipse collapses to a line whichis drawn instead.

h = drawprobellipse(...) returns the graphic handle h.


x1 = [ 1,2]; C1 = [0.25 -0.2; -0.2 0.3];

drawprobellipse(x1,C1,0.95,'k');

x2 = [-1,0]; C2 = [0.3 -0.03; -0.03 0.01];


x3 = [-2,3]; C3 = [0.01 0.005; 0.005 0.015];


generate the ellipses shown below. Note that line width can be varied using thehandle vector h.

-3 -2 -1 0 1 2 3 -1

-0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

See also drawellipse, chi2invtable, chi2inv.

38

2.1.8 drawrawdata

Draw raw data of sensors.

drawrawdata(setupfilename,startstep,nsteps) plots the raw data of thesensors specified in the file setupfilename, the setup file of the experiment.Starts visualization at step startstep over nsteps.

As each sensor is unique and requires a specific visualization of its data, thism-file is to be adapted each time a new sensor is employed. Further, also in thecase when the file syntax of a sensor file or the way it is read in changes (usingthe index string in the setup file), the m-file needs an adaptation.

See also readonestep, slam.

2.1.9 drawreference

Draw coordinate reference frame.

drawreference(x,label,size,color) draws a reference frame at pose x =[x, y, θ]T and labels it with the string label. size is the length of the frameaxes in [m], and color is a [r g b]-vector or a color string such as 'r' or 'g'.

h = drawreference(...) returns a column vector of handles to all graphicobjects of the drawing. Remember that not all graphic properties apply to alltypes of graphic objects. Use findobj to find and access the individual objects.


drawreference([0 2 pi/8],'W',1,'k');

drawreference([0 0 pi/3],'R2',0.4,'b');

drawreference([0.8 1.3 -1.8],'S43',0.5,[.8 .5 .1]);

drawreference([2 2.3 -0.3],'',0.6,[.6 .6 .6]);

h = drawreference([2 0.7 pi/9],'98',0.8,[.3 .7 .3]);


generate the robots shown below. Note that line width, line style and color canbe varied using the handle vector h.

See also drawarrow, drawlabel, findobj, plot.

2.1.10 drawrobot

Draw robot.

drawrobot(x,color) draws a robot at pose x = [x, y, θ]T such that the robotreference frame is attached to the center of the wheelbase with the x-axis look-ing forward. color is a [r g b]-vector or a color string such as 'r' or 'g'.

39

-0.5 0 0.5 1 1.5 2 2.5 3

0

0.5

1

1.5

2

2.5

3

drawrobot(x,color,type) draws a robot of type type. Five different mod-els are implemented:

type = 0 draws only a cross with orientation θtype = 1 is a differential drive robot without contourtype = 2 is a differential drive robot with round shapetype = 3 is a round shaped robot with a line at θtype = 4 is a differential drive robot with rectangular shapetype = 5 is a rectangular shaped robot with a line at θ

drawrobot(x,color,type,w,l) draws a robot of type type with width w andlength l in [m].

h = drawrobot(...) returns a column vector of handles to all graphic ob-jects of the robot drawing. Remember that not all graphic properties applyto all types of graphic objects. Use findobj to find and access the individualobjects.


drawrobot([0 1 1.5],'k',0);

drawrobot([1 1 1.4],'k',1);

drawrobot([2 1 1.3],'k',2);

drawrobot([3 1 1.2],'k',3);

drawrobot([4 1 1.1],'k',4);

drawrobot([5 1 1.0],'k',5);

drawrobot([0 -0.8 2.0],'r',2,0.6,0.6);

drawrobot([1 -1.2 1.9],[.5 .5 .4],4,0.5,0.3);

drawrobot([2 -0.8 1.8],[.7 .5 .4],1,0.2,0.8);

drawrobot([3 -1.2 1.7],[.9 .5 .4],5,0.3,0.7);

h = drawrobot([4 -0.8 1.6],'g',3,0.4,0.1);


h = drawrobot([5 -1.2 1.5],'k',0,0.4,0.1);

set(h,'LineWidth',2,'LineStyle',':');

40

generate the robots shown below. Note that line width, line style and color canbe varied using the handle vector h. Refer to section 2.11.3 where the use offindobj is demonstrated.

0 1 2 3 4 5 -2.5

-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

2.5

See also drawroundedrect, drawarrow, findobj, plot.

2.1.11 drawroundedrect

Draw rounded rectangle.

drawroundedrect(x,w,h,r,filled,color) draws a rectangle with round cor-ners of radius r, width w and height h, centered at pose x where x is the 3× 1vector [x, y, θ]T . With filled = 1 the rectangle is filled with color color, withfilled = 0 only the contour is drawn. color is a [r g b]-vector or a Matlabcolor string such as 'r' or 'g'.

Note that 2r must be greater or equal than the smaller of the two values w,h. For 2r = w = h, drawroundedrect draws a circle.

h = drawroundedrect(...) returns the graphic handle h.


drawroundedrect([0.4 1 2.6],1,0.6,0.2,0,'b');

drawroundedrect([1.9 2 2.5],1,1.2,0.1,1,[.4 .9 .0]);

drawroundedrect([1.9 2 2.5],1,1.2,0.1,0,[.2 .7 .0]);

drawroundedrect([3.0 1 2.4],0.2,1.7,0.0,0,'r');

drawroundedrect([4.0 0 2.3],0.4,0.4,0.2,1,[.8 .8 .8]);

h = drawroundedrect([4 0 2.3],0.4,0.4,0.2,0,'k');


h = drawroundedrect([5 1 0.6],0.3,0.7,0.15,0,[.9 .7 .0]);


41

generate the robots shown below. Note that line width, line style and color canbe varied using the handle vector h.

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

-1

-0.5

0

0.5

1

1.5

2

2.5

3

3.5


2.1.12 drawtransform

Illustrates a spatial relationship.

drawtransform(xs,xe,shape,label,color) draws a nice looking curved ar-row from location xs (3× 1) to xe (3× 1) and labels it with the string label.color is a [r g b]-vector or a Matlab color string such as 'r' or 'g'. shapecontrols the shape of the curve: '/' for a S-shape, '\' for a Z-shape, '(' for aleft arc and ')' for a right arc.

h = drawtransform(...) returns a column vector of handles to all graphicobjects of the drawing. Remember that not all graphic properties apply to alltypes of graphic objects.


plot(0,0,'k+'); plot(0.2, 1.5,'k+');

drawtransform([0 0],[0.2 1.5],'/','x1',[.9 .8 .0]);

plot(1,0,'k+'); plot(1.2, 1.5,'k+');

drawtransform([1 0],[1.2 1.5],'\','x2',[.7 .6 .0]);

plot(2,0,'k+'); plot(2.2, 1.5,'k+');

drawtransform([2 0],[2.2 1.5],'(','x3',[.5 .3 .0]);

plot(3,0,'k+'); plot(3.2, 1.5,'k+');

drawtransform([3 0],[3.2 1.5],')','x4',[.3 .0 .0]);

h = drawtransform([0 0],[2 0],')','','k');

set(h,'LineStyle','--');

h = drawtransform([1 0],[3 0],')','','k');

set(h,'LineStyle',':');

42

generate the arrows shown below. Note that line width, line style and color canbe varied using the handle vector h.

0 0.5 1 1.5 2 2.5 3

-0.5

0

0.5

1

1.5

2

x1 x2 x3x4


2.1.13 icompound

Inverse 2D relationship.

xji = icompound(xij) returns the inverted 2D transform xji given the re-lationship xij. All x’s are 3× 1-vectors, all angles within [0..2π[.

Example: Given a feature with attached frame F and the robot with attachedframe R represented both in the world reference frame W by the transformsxWF and xWR, we look for F expressed in the robot reference frame R. Thesolution is the composition of the inverted xWR with xWF :

xRF = xWR ⊕ xWF (2.1)

This frame transform is usually called measurement prediction in the context offeature-based Kalman filter localization or slam (with the simplifying assump-tion that sensor frame and robot frame coincide).

The figure has been generated by the following code:

xwr = [1.0, 2, -1.4];

xwf = [3.2, 1, pi/9];

drawreference([0 0 -pi/19],'W',0.7,'k');

drawreference(xwr,'R',0.7,[0.7 0 0]);

drawrobot(xwr,'k');

drawtransform(zeros(3,1),xwr,'\','x_W_R','k');

drawreference(xwf,'F',0.7,[0 0.7 0]);

drawtransform(zeros(3,1),xwf,'/','x_W_F','k');

drawtransform(xwr,xwf,'/','x_R_F','k');

43

xWR

xWF

xRF

Reference: R. Smith, M. Self, P. Cheeseman, ”Estimating Uncertain SpatialRelationships in Robotics,” in Autonomous Robot Vehicles, I.J. Cox and G.T.Wilfong, Eds.: Springer-Verlag, 1990, pp. 167-193.

See also compound, jinv.

2.1.14 j1comp

First Jacobian of the compound operator.

J = j1comp(xi,xj) returns the Jacobian matrix of the 2D composition of xiand xj derived with respect to the first operand xi. All x’s are 3× 1-vectors, Jis a 3× 3-matrix.

The Jacobian is used to perform first-order error propagation when the inputtransforms x̂ij and x̂jk of the composition x̂ik = x̂ij ⊕ x̂jk are uncertain. Tocalculate the uncertainty of the output x̂ik, the compound operator is derivatedwith respect to the two operands yielding a 3 × 6 Jacobian matrix J⊕ whichconsists of a left 3× 3 half, J1⊕, and a right 3× 3 half, J2⊕.

J1⊕ =δx̂ik

δxij

∣∣∣∣x̂ij

J2⊕ =δx̂ik

δxjk

∣∣∣∣x̂jk

J⊕ = [J1⊕ J2⊕]

With Cijk as the input covariance matrix

Cijk =[

Cij Cijjk

Cjkij Cjk

]the covariance matrix of the output transform Cik is given by the error propa-gation law

Cik = J⊕CijkJT⊕

44

= J1⊕CijJT1⊕ + J1⊕CijjkJT

2⊕ + J2⊕CjkijJT1⊕ + J2⊕CjkJT

2⊕

where the submatrix Cijjk (= CjkijT ) is the cross-correlation between x̂ij and

x̂jk.

See also j2comp, jinv, compound, icompound.

2.1.15 j2comp

Second Jacobian of the compound operator.

J = j2comp(xi,xj) returns the Jacobian matrix of the 2D composition of xiand xj derived with respect to the second operand xj. All x’s are 3× 1-vectors,J is a 3× 3-matrix.

See explanation at j1comp.

See also j1comp, jinv, compound, icompound.

2.1.16 jinv

Jacobian of the inverse compound operator.

J = jinv(xij) returns the Jacobian matrix of the inversion xji of xij. All x’sare 3× 1-vectors.

The Jacobian is used to perform first-order error propagation when the in-put transform x̂ij of the inversion x̂ji = x̂ij is uncertain. To calculate theuncertainty of the output x̂ji, the inverse compound operator is derivated withrespect to the operand yielding the 3× 3 Jacobian matrix J.

J =δx̂ji

δxij

∣∣∣∣x̂ij

With Cij as the input covariance matrix, the covariance matrix of the outputtransform, Cji, is given by the error propagation law

Cji = JCijJT

See also j1comp, j2comp, icompound, compound.

2.1.17 mahalanobis

Calculate the Mahalanobis distance.

d = mahalanobis(v,S) calculates the chi square distributed Mahalanobis dis-tance given the innovation vector v and the innovation covariance matrix S.

45

Formally, with the innovation ν and the innovation covariance matrix S, theMahalanobis distance is

D = νT S ν (2.2)

The Mahalanobis distance is a quadratic form and allows to test on positivedefiniteness of a matrix S. With any ν 6= 0, S is positive definite if D > 0 andpositive semidefinite if D ≥ 0.

See also chi2invtable, chi2inv.

2.1.18 meanwm

Multivariate weighted mean (Information Filter).

[xw,Cw] = meanwm(x,C) calculates the multivariate weighted mean. x is amatrix of dimension m × n where each column is interpreted as a Gaussiandistributed random vector of dimension m× 1. C is a m×m× n matrix whereeach m × m matrix is interpreted as the covariance estimate associated to itsrespective row vector. The function returns the weighted mean vector xw andthe weighted covariance matrix Cw of dimensions m× 1 and m×m respectively.

The multivariate weighted mean is also known as the Information Filter (IF),a batch formulation of the (recursive) Kalman filter. Given the random vectorsx1, x2, ..., xn with associated covariance matrices C1, C2, ..., Cn, the IF calculates

xw = Cw

∑C−1

i xi

C−1w =

∑C−1

i

Example: The Matlab code

x1 = [1.5; 1]; C1 = [0.08 -0.082; -0.082 0.1];

drawprobellipse(x1,C1,0.95,[.9 .5 0]);

plot(x1(1),x1(2),'+','Color',[.9 .5 0]);

x2 = [1; 0]; C2 = [0.18 -0.02; -0.02 0.01];

drawprobellipse(x2,C2,0.95,[.9 .2 0]);

plot(x2(1),x2(2),'+','Color',[.9 .2 0]);

xin = cat(2,x1,x2); Cin = cat(3,C1,C2);

[xw,Cw] = meanwm(xin,Cin);

drawprobellipse(xw,Cw,0.95,'k');

plot(xw(1),xw(2),'k+');

x3 = [3; 3]; C3 = [0.01 0.005; 0.005 0.015];

drawprobellipse(x3,C3,0.95,[0 .9 .7]);

plot(x3(1),x3(2),'+','Color',[0 .9 .7]);

x4 = [3.2; 2.3]; C4 = [0.03 -0.005; -0.005 0.015];


plot(x4(1),x4(2),'+','Color',[0 .9 .4]);

x5 = [2.5; 2.4]; C5 = [0.005 -0.002; -0.002 0.008];


46

plot(x5(1),x5(2),'+','Color',[0 .9 .1]);

xin = cat(2,x3,x4,x5); Cin = cat(3,C3,C4,C5);

[xw,Cw] = meanwm(xin,Cin);

drawprobellipse(xw,Cw,0.95,'k');

plot(xw(1),xw(2),'k+');

generates the figure shown below. Note how the command cat is used to preparethe input arguments.

-0.5 0 0.5 1 1.5 2 2.5 3 3.5 4

0

0.5

1

1.5

2

2.5

3

3.5

See also mean, cat, drawprobellipse.

2.1.19 setangletorange

Put angle into a 2π interval.

ar = setangletorange(a,min) puts angle a into the interval [min..min+2π[.If a is Inf, Inf is returned.

See also diffangleunwrap.

47

2.2 Import m-files (import folder)

2.2.1 closefiles

Closes all slam or localization files.

closefiles(files) closes all open files in the files structure.

See also openfiles.

2.2.2 openfiles

Open files for a slam or localization experiment.

[files,params] = openfiles(filename,startstep) opens the setup file, thedata files, and in case of a localization experiment, the map file as specified inthe setup file of the experiment filename. Returns the structures files andparams. Puts all file position indicators to the appropriate positions such thatthe experiment starts at startstep.

files has the following fields:

files.allopenfiles.mapfidfiles.mastersensidfiles.sensor(i).fidfiles.sensor(i).params

allopen is 1 if all files could have been opened, and 0 otherwise. The ar-ray sensor contains for each sensor its name name, the identifier of the datafile fid, the label label with which a line in the file has to start, its tempo-ral downsampling factor tdsf, its spatial downsampling factor sdsf, the flagisrel which says whether the file contains relative or absolute information, theindex string indexstr, the name of the extraction function funcname and therobot-to-sensor frame transform xs expressed in the robot frame. sensor fur-ther holds the identifier of the master sensor, mastersensid – the sensor whosemeasurements triggers a new step –, and the map file identifier mapfid.

params carries all the parameters of the experiment as fields according to thevalues in the setup file filename The setup file is assumed to be in Matlabsyntax such that an evaluation yields the params structure.

See also readonestep, closefiles.

2.2.3 readoneline

Read one line of a sensor data file.

48

s = readoneline(fid,indexexpr,label) reads one line of a sensor data filereferenced by fid which starts with the label string label using the index ex-pressions in indexexpr and returns the data in the structure s. s contains thefields which are referenced by a corresponding expression in indexexpr.

If an end-of-file is encountered, s is -1.

The files must meet the following requirements: Formatted in plain text withone measurement per line and spaces as separator. Each line starts either by astring and a timestamp or only by a timestamp, followed by an arbitrary num-ber of data.

indexexpr is a cell array containing a list of valid Matlab array indexing ex-pressions. The number of expressions determines the number of fields of s. Theexpressions themselves denote the elements on a line of the sensor file. 1 denotesthe first element which is either the label string or the timestamp, 2 denotesthe timestamp if there is a label string and the first data element otherwise,3 denotes the third element on the line etc. indexexpr is obtained by callingsegmentstr with the index strings defined in the setup file as argument. Anindex string is a comma separated list of indexing expressions.

Example: With a file whose lines have the following format

label timestamp n phi1 rho1 sigmarho1 phi2 rho2 sigmarho2 ...

and the index string in the setup file

'2,3,4:3:end,5:3:end,6:3:end'

the function returns the structure S with fields

s.time = timestamps.data1 = ns.data2 = [phi1 phi2 ... phin]s.data3 = [rho1 rho2 ... rhon]s.data4 = [sigmarho1 sigmarho2 ... sigmarhon]

With the same file and the index string

'1,2,6:3:end'

the function returns the structure S with fields

s.label = labels.time = timestamps.data1 = [sigmarho1 sigmarho2 ... sigmarhon]

See also readonestep, segmentstr.

49

2.2.4 readonestep

Read one step from sensor files.

[eof,sensors] = readonestep(files) reads from the sensor data files ref-erenced by the file identifiers in files and returns the data in the array ofstructures sensors. The length of sensors is given by the number of sensorsdefined in the setup file of the experiment. eof is 1 if an end-of-file in at leastone of the files is encountered.

Refer to openfiles to see the definition of files.

The function reads from the last position of the file position indicators up to thenext unread timestamp of the sensor which has been defined to be the mastersensor by params.mastersensid. The master sensor gives the ”heartbeat” ofthe experiment and triggers a new step.

If for a given step there are no data for one of the sensors, its correspond-ing structure in sensors is empty. Use isempty to test on this.

The files must meet the following requirements: Formatted in plain text withone measurement per line and spaces as separator. Each line starts either by astring and a timestamp or only by a timestamp, followed by an arbitrary numberof data. The way how the data are read in is defined by the index expressionsin the strings params.indexstr1, params.indexstr2, ...

params contains for each sensor temporal and spatial downsample factors params.tdownsmple1(temporal), sdownsmple1 (spatial) which make readdata to thin out the databy reading only each tdownsmple1-th line of the file and each sdownsmple1-thdata in case of vectorized data as specified in the index strings.

Example: Given two data files with lines formatted as

File 1 : ENCODER timestamp left rightFile 2 : LASER timestamp n x1 y1 x2 y2 ... xn yn

and the following experimental parameters:

params.sensor1.tdownsample = 3params.sensor1.sdownsample = 1params.sensor1.indexstr = '1,2,4,3'params.sensor2.tdownsample = 1params.sensor2.sdownsample = 2params.sensor2.indexstr = '1,2,3,4:2:end,5:2:end'params.mastersensid = 2

the function returns the array sensors with fields

sensors(1).paramssensors(1).steps(i).time = timestamp

50

sensors(1).steps(i).data1 = rightsensors(1).steps(i).data2 = left

sensors(2).paramssensors(2).steps.time = timestampsensors(2).steps.data1 = nsensors(2).steps.data2 = [x1 x3 ... xn]sensors(2).steps.data3 = [y1 y3 ... yn]

Only each 3rd line of the first file is read in while of the second file all vectorizeddata are downsampled by a factor of 2. As sensor 2 is the master sensor, thearrays over time, sensors(i).steps, of all other sensors have length ≥ 1, where thelength is determined by the number of measurements between the last times-tamp (encoded in the file position indicator) and the next unread timestamp ofthe master sensor divided by the temporal downsampling factor.

See also readoneline, openfiles, closefiles.

2.2.5 segmentstr

Extract substrings between separators.

expr = segmentstr(str,separator) chops string str into substrings whichstand between separator where separator is assumed to be a character. expris a cell array constituted by the substrings.

Example: The command

segmentstr('1:2:3:end:4:',':')

with colon as separator returns a cell array with fields '1', '2', '3', 'end',and '4'. Typing

segmentstr(' p.j. harvey and john parish ',' ')

with space as separator returns a cell array with fields 'p.j.', 'harvey', 'and','john', and 'parish'.

See also readoneline.

51

2.3 Beacon Extraction m-files(featureextr/beaconextr folder)

2.3.1 extractbeacons

Extract beacons from range data.

L = extractbeacons(beacondata,params,display) extracts beacons from theraw measurements in beacondata and returns them in the local map L.

A beacon with a physical extension (e.g. a retro-reflecting tape) is typicallyhit more than once by the laser beam resulting in several raw measurements ofthat beacon. This extraction method segments the data into clusters of pointswhich belong to the same beacon and in case of several points finds a leastsquare estimation of the beacon position.

A jump distance greater than a threshold is the segmentation criterion andinitiates a new beacon cluster. A threshold on the number of points within acluster can also be applied and rejects candidate clusters with a weak support.This implementation assigns a constant default covariance matrix to each point.Readings on the same cluster are averaged using a multivariate weighted mean(an information filter).

The algorithm parameters in detail:

params.beaconthreshdist : jump distance for segmentation in [m]params.minnpoints : threshold on minimal number of points in

a cluster in order to be acceptedparams.cb : constant 2× 2 default covariance matrix of

raw beacon measurement

With the flag display = 1, the extraction result is plotted in a new figure win-dow.

Example: the figures below illustrate four point clusters and their extractedbeacon positions together with their 95% error ellipse (which is a circle).

-0.2 0 0.2 0.4 0.6 0.8 1

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

-0.2 0 0.2 0.4 0.6 0.8 1

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

52

See also extractlines, meanwm.

53

2.4 Line Extraction m-files(featureextr/lineextr folder)

2.4.1 calccompactness

Calculate relative model fidelity measure.

c = calccompactness(x) returns the sum of Mahalanobis distances of n α, r-lines from their common weighted mean in the α, r-model space. x is of dimen-sion 5 × n, where the rows are [alpha, r, saa, srr, sar]. The coordinateα is properly unwrapped.

If the covariance matrices are set to identity, the function returns the sum ofsquared Euclidian distances from the common arithmetic mean.

See also extractlines.

2.4.2 calcendpoint

Calculate segment endpoints.

pe = calcendpoint(p,x) returns the Cartesian coordinates of the segmentendpoint pe = [x; y] by perpendicularly projecting point p = [x; y] ontothe α, r-line with parameters x = [alpha; r].


2.4.3 extractlines

Probabilistic extraction of α, r-lines from range data.

L = extractlines(scndata,params,display) extracts line segments from un-certain range data given in polar coordinates. The lines, represented in theHessian α, r-model, and their associated covariance matrices are returned.

The structure scndata holds the raw range readings according to the outputof the read function readonestep. params is a structure with the algorithmparameters. With the flag display being 1, the extraction result is displayedin a new figure.

[L,segs,lines] = extractlines(scndata,params,display) returns also thematrices segs and lines. Their definition is given below.

The algorithm uses a sliding window technique where a window of constantsize sweeps over the (ordered) set of range readings. At each step the model(here: the line) is fitted to the points within the window and a model fidelitymeasure is calculated. The sought objects in the scan are found by a condition

54

on the model fidelity.

In many cases, this results in an oversegmented range image since a single out-lier can give rise to several small segments. Under the condition of collinearityon a significance level α, adjacent segments are fused in a final step.

For the fit, perpendicular errors from the points onto the line are minimized,which, in spite of being a non-linear regression problem with points in polarcoordinates, has an analytical solution.

The algorithm parameters in detail:

params.windowsize : size of the sliding window in number ofpoints (odd number, ≥ 3)

params.threshfidel : threshold on model fidelityparams.fusealpha : significance level for segment fusion,

between 0 and 1params.minlength : minimal length in [m] a segment must haveparams.cyclic : 1: range data are cyclic, 0: non-cyclicparams.compensa : heurististic compensation term to account for

correlations between the raw data. Is addedto all α’s after extraction, [rad]

params.compensr : same as above, for r values, in [m]

L is a map object according to the definition of the map class. It holds theinformation of the infinite lines of lines, treating the line parameters α, r asthe feature states.

segs is a matrix which holds information about the finite-length line segmentsand has the following elements:

1st row : identifier of segment2nd row : identifier of line the segment belongs to3rd row : number of contributing raw segments4th row : number of contributing data points

5,6th row : line parameters alpha, r7,8,9th row : covariance matrix elements saa, srr, sar

10th row : trace of covariance matrix11,12th row : begin- and end-index of segment13,14th row : Cartesian coordinates of segment start point15,16th row : Cartesian coordinates of segment end point

lines is a matrix which holds information about the infinite supporting linesand has the following elements:

1st row : identifier of line2nd row : number of contributing segments3rd row : number of contributing raw segments4th row : number of contributing data points

55

5-10th row : same as above: parameters, cov. matrix, trace≥11th row : identifiers of contributing segments

Reference: K.O. Arras, ”Feature-Based Robot Navigation in Known and Un-known Environments”, Ph.D. dissertation, Nr. 2765, Swiss Federal Institute ofTechnology Lausanne, Autonomous Systems Lab, June 2003.

See also fitlinepolar, readonestep, map.

2.4.4 findregions

Find homogeneous regions in array.

indices = findregions(x,id,op,cyc) searches in the array x all regions whereid has relation op. op is a string from the set of the following relational opera-tors:

'==' : equal'~=' : not equal'<' : less than'>' : greater than'<=' : less than or equal'>=' : greater than or equal

The functions returns a r× 3 matrix indices with three elements, region iden-tifier, start and end index, each region on a new row. If cyc is 1, x is interpretedto be a cyclic array. If id does not appear in x, -1 is returned, -2 if x is a matrixand -3 if relation op is not valid or cyc is not 0 or 1.

Examples:

findregions([-2 1 0 2 -9 -6 1 2 -5 -4 -3],0,'<',0)returns [1 1 1; 2 5 6; 3 9 11]findregions([5 0 1 5 5 0 5 5],5,'==',1)returns [1 7 1; 2 4 5]

See also find.

2.4.5 fitlinepolar

Fit α, r-line to points in polar coordinates.

[x,C] = fitlinepolar(p) fits a line in the weighted least squares sense topoints p in polar coordinates minimizing perpendicular errors from the pointsto the line. Uncertainties in rho are assumed and propagated through the fitexpressions yielding the parameter covariance matrix C. p is a n×3 matrix withn ≥ 3 with theta in the first column, rho in the second column and sigma rhoin the third column. The function returns the parameter vector x = [alpha;

56

r] and the covariance matrix C = [saa sar; sar srr].


P = [1.9 1 0.1; 1.6 1.1 0.1; 1.3 1.1 0.1; 1 1.4 0.1; 0.7 1.5 0.1];

polar(P(:,1),P(:,2),'b.')

[x,C] = fitlinepolar(P);

l = arlinefeature(1,x,C);

draw(l,0,'k');

generate the figure shown below. Note that we plot the line by first creatinga line object and then invoking its draw method. The uncertainties in ρ areshown by 2σ-intervals (not produced by the code example).

0 0.2 0.4 0.6 0.8 1 1.2 1.4

0

0.2

0.4

0.6

0.8

1

1.2

Reference (contains derivation of fit expressions): K.O. Arras, ”Feature-BasedRobot Navigation in Known and Unknown Environments”, Ph.D. dissertation,Nr. 2765, Swiss Federal Institute of Technology Lausanne, Autonomous Sys-tems Lab, June 2003.

See also regress.

2.4.6 isneighbour

Check if line segments are adjacent.

isneighbour(rawsegs,i,j,overlap,cyc) returns 1 if overlap = 0 and seg-ments i and j are adjacent or if overlap = 1 and segments i and j are adjacentand overlap. Otherwise, 0 is returned. Takes into account cyclic (when cyc =1) and noncyclic (when cyc = 0) scans. Uses the rawsegs matrix defined inextractlines.


57

2.4.7 mahalanobisar

Mahalanobis distance of two α, r-lines.

d = mahalanobisar(l1,l2) returns the mahalanobis distance of the two linesl1 and l2 unwrapping the α-dimension if necessary. l1 and l2 are 1× 5-arrayswith elements [alpha r saa srr sar].

See also mahalanobis, extractlines.

2.4.8 mod2

Matlab index-compatible modulus after division.

y = mod2(x,len) returns x modulus len such that y is always within the validindex range of Matlab-arrays.

Example:

len = 4, x: ... -2 -1 0 1 2 3 4 5 6 ...y: ... 2 3 4 1 2 3 4 1 2 ...

See also mod, rem

58

2.5 SLAM m-files (slam folder)

2.5.1 estimatemap

Apply Kalman filter on map.

G = estimatemap(G,nu,R,H) updates the map G by application of the extendedKalman filter (EKF). The function implements a stacked version of the EKFwith the following input arguments: The stacked measurement Jacobian H ofsize n×p, where p is the number of measurements (which in turn is the numberof observed features times their number of parameters), and n the size of themap state vector. The stacked p×1 innovation vector nu, and the stacked p×pobservation covariance matrix R.

See also slam, meanwm.

2.5.2 initmap

Initialize map for experiment.

M = initmap(mode,name,params) initializes the map M for a slam or local-ization experiment. With mode = 0 the map is prepared for a slam experiment:a map M with name name is created, a robot is created, added to M and set toits initial pose params.xrinit, params.crinit. With mode = 1, the map isprepared for a localization experiment: a map file with name name is opened,read in and the robot is set to the initial pose as above.

The params structure is defined in the setup file of the experiment.

See also map.

2.5.3 integratenewobs

Augment map by new features.

G = integratenewobs(G,L,associations) integrates new and unmatched ob-servations from the local map L into the global map G and returns the augmentedmap. associations is an array of structure with fields

associations(i).il : index of local pairing feature in Lassociations(i).ig : index of global pairing feature in G

For new observations, ig has a negative value (”star branch” match). Typeget(G) or get(L) for the data structure of G,L.

The function iterates over the new observations and calls the integrate methodof the feature objects.

59

See also pointfeature/integrate, arlinefeature/integrate.

2.5.4 matchnnsf

Perform data association using a NNSF.

[nu,R,H,associations] = matchnnsf(Gpred,Hpred,L,params) matches theobserved features in the local map L against the predicted features in the pre-dicted global map Gpred with a nearest neighbor strategy. The significance levelfor matching is given by params.alpha.

The function returns the stacked p×1 innovation vector nu, where p is the num-ber of matched observations (which in turn is the number of matched featurestimes their number of parameters), the stacked p×p observation covariance ma-trix R (which can contain non-zero entries in the off-diagonal), and the stackedp× n measurement Jacobian matrix H with non-zero entries in each row at therobot position and the position of the corresponding feature (n is the size ofthe map state vector). Successful data associations are in the array of structureassociations with fields:

associations(i).il : index of local pairing feature in Lassociations(i).ig : index of global pairing feature in G

The nearest neighbor standard filter (NNSF) is implemented in its most conser-vative variant: local or global pairing candidates which give rise to associationambiguities (i.e. a local feature in more than one validation gate or a globalfeature with more than one local features in its gate) are ignored.

See also predictmeasurements, estimatemap.

2.5.5 predictmeasurements

Transforms map features into the local frame.

[Gpred,Hpred] = predictmeasurements(G) applies the observation functionto all features in the map by transforming them from the global coordinateframe into the local frame given the current robot pose. Returns the predictedmeasurements in the map Gpred and the structure Hpred with the individualmeasurement Jacobians:

Hpred(i).Hr : measurement Jacobian of the i-th featurewith respect to the robot

Hpred(i).Hm : measurement Jacobian of the i-th featurewith respect to the map feature

The function iterates over the map features in G and calls the predict methodof the feature objects.

60

See also pointfeature/predict, arlinefeature/predict.

2.5.6 robotdisplacement

Predict global map after a robot displacement.

G = robotdisplacement(G,xr,Cr,Fxr) updates the global map G given thepredicted 3× 1 robot pose xr, its predicted 3× 3 covariance matrix Cr and the3× 3 process model Jacobian linearized around the prediction, Fxr.

The predicted map is returned, with the updated first row and column of themap covariance matrix.

See also slam.

2.5.7 slam

Simultaneous localization and mapping.

M = slam(setupfilename,startstep,nsteps,displaymode) builds a stochas-tic map M from the sensor data specified in the setup file setupfilename. Thesetup file contains the names of all sensor data files, the robot model, the sensormodels and the experimental and algorithmic parameters of the experiment.

Execution is started at startstep and iterates nsteps times.

displaymode is between 0 and 4. With 0 the algorithm runs through, with1 the global map is drawn at each step, and additionally paused if displaymode= 2. with displaymode = 3, the global and the local map is drawn, and pausedif displaymode = 4.

See also drawrawdata.

61

2.6 Map entity methods (@entity folder)

The map entity class is the parent class of robots and features in a map. Asit is a base class, there are only four methods, the constructor method entity,the get and set method, get, set, and the display method display.

2.6.1 display

Display method for map entity object. Gets invoked when the semicolon is leftaway.

display(e) displays the map entity object e.

Example: Given the second example entity object e of the constructor func-tion entity (section 2.6.2), typing display(e) or simply e without semicolonreturns

line segment id = 36

See also entity.

2.6.2 entity

Constructor function for map entity object.

e = entityCreates a new map entity object with default values

e = entity(e)Copies and returns generic map entity e

e = entity(typestr,id)Creates a new map entity object with values

typestr : type stringid : identifier, integer

Examples:e = entity('robot',1);e = entity('line segment',36);

62

See also class.

2.6.3 get

Get method for map entity object.

v = get(e,'PropertyName') returns the value v of the specified property forthe map entity object e.

get(e) displays all property names and their current values for the map en-tity object e.

Example:identifier = get(e,'id');

See also entity/set.

2.6.4 set

Set method for map entity object.

e = set(e,'PropertyName',PropertyValue) sets the value of the specifiedproperty for the map entity object e.

Note that the assignment to the output e is necessary since Matlab does notsupport passing arguments by reference. Hence the set method actually oper-ates on a copy of the object.

set(e) displays all property names and their possible values for the map entityobject e.

Example:e = set(e,'id',201);

See also entity/get.

63

2.7 Point feature methods (@pointfeature folder)

2.7.1 Definition

The model of a point feature is simply

p = (x, y)

where x and y are the Cartesian coordinates of the feature’s position in somereference frame. Accordingly, the 2× 2 associated covariance matrix

Cp =[

σ2x σxy

σyx σ2y

]holds the uncertainties in x, σ2

x, in y, σ2y, and the covariance between x and y,

σxy.

p = (x, y)

W

2.7.2 calcinnovation

Calculate innovation between two point features.

nu = calcinnovation(p1,p2) returns the difference of the state vectors ofpoint features p1 and p2. Note that for features with angles this is the place tocorrectly take angle differences.

See also pointfeature.

64

2.7.3 display

Display method for map entity object. Gets invoked when the semicolon is leftaway.

display(p) displays the point feature p.

Example: Given the example point feature p from the constructor functionpointfeature (section 2.7.7). Typing display(p) or simply p without semi-colon returns

point feature id = 201x = -1 C = 0.001 0

3 0 0.001


2.7.4 draw

Draw point feature.

draw(p,ids,color) draws the point feature p into the current figure with colorcolor. Enable and disable display of feature identifiers with the flag ids being1 or 0. color is a [r g b]-vector or a Matlab color string such as 'r' or 'g'.

h = draw(...) returns a column vector of handles to the line objects of thedrawing.

Example: In the code below we create three point features and plot themincluding their uncertainty (with drawprobellipse). Then the marker sizeand line width of p2 and p3 is changed using their handle vectors h2 and h3respectively.

p1 = pointfeature(201,[ 1; 2],0.003*eye(2));

p2 = pointfeature(102,[-2; 0],[0.02 0.01; 0.01 0.008]);

p3 = pointfeature(203,[ 0;-1],[0.004 -0.004; -0.004 0.008]);

drawprobellipse(get(p1,'x'),get(p1,'c'),0.99,[.3 .3 .3]);



draw(p1,1,'r');

h2 = draw(p2,1,[1 0.5 0]);

h3 = draw(p3,1,[1 0.7 0]);

set(h2,'MarkerSize',15);

set(h3,'LineWidth',2);

The result is shown in the figure below. Note how in the drawprobellipse callsthe first two moments are directly obtained by the get method.


65

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5

-1

-0.5

0

0.5

1

1.5

2

2.7.5 get

Get method for point feature object.

v = get(p,'PropertyName') returns the value v of the specified property forthe point feature object p.

get(p) displays all property names and their current values for the point fea-ture object p.

Example:state = get(p,'x');

See also pointfeature/set.

2.7.6 integrate

Add point feature to the map.

[pw,Gr] = integrate(p,xr,Cr) transforms the point feature p representedin the robot frame into the world frame given the uncertain 3 × 1 robot posexr and the 3 × 3 pose covariance matrix Cr. Returns p expressed in the worldframe, pw, and the Jacobian of the integration function, Gr.

See also pointfeature/predict.

2.7.7 pointfeature

Constructor function for point feature object.

p = pointfeatureCreates a new x, y-point feature object with default values

66

p = pointfeature(p)Copies and returns point feature p

p = pointfeature(id,x,C)Creates a new point feature object with valuesid : identifierx : 2× 1 feature parameter vector, interpreted as [x; y]C : 2× 2 parameter covariance matrix [σ2

x σxy; σyx σ2y]

The point feature class is a child class of the base class entity.

Example:p = pointfeature(201,[-1;3],0.001*eye(2));

See also entity, class.

2.7.8 predict

Measurement prediction of point features.

[pr,Hr,Hm] = predict(p,xr,Cr,Crp) transforms the point feature p repre-sented in the world frame into the robot frame given the uncertain robot posexr, Cr, and the cross-correlation between the robot and p, Crp. xr is a 3 × 1vector, Cr a 3× 3 covariance matrix and Crp a 3× 2 cross-correlation matrix.

The function returns the predicted point feature pr, the 2 × 3 measurementJacobian derived with respect to the robot, Hr, and the 2 × 2 measurementJacobian with respect to p, Hm.

See also pointfeature/integrate.

2.7.9 set

Set method for point feature object.

p = set(p,'PropertyName',PropertyValue) sets the value of the specifiedproperty for the point feature object p.

p = set(p,'PropertyName1',PropertyValue1,'PropertyName2',Property-Value2,...) sets multiple property values with a single statement.

Note that the assignment to the output p is necessary since Matlab does notsupport passing arguments by reference. Hence the set method actually oper-ates on a copy of the object.

set(p) displays all property names and their possible values for the point fea-ture object p.

67

Example:p = set(p,'x',[-1; 2],'C',zeros(2));

See also pointfeature/get.

68

2.8 (α,r)-line methods (@arlinefeature folder)

2.8.1 Definition

The model for infinite (α,r)-lines is

x sin(α) + y cos(α)− r = 0

where α is the angle of the perpendicular and r its length. A point is on theline if its coordinates (x, y) satisfy the above equation. If the point is given inpolar coordinates (ρ, θ), the model becomes

ρ cos(θ − α)− r = 0

In this model which is called Hessian line model, r is always positive. We canexploit this fact by using the sign of r to add visibility information. Given aline

` = (α, r)

we redefine the parameters (αv, rv) which encode visibility information as fol-lows:

(αv, rv) ={

(α, r) : ` visible from W(α + π,−r) : ` invisible from W

with 0 ≤ αv < 2π. W denotes the origin of the world frame.

r

α

αr

W

l2 :rv = –rαv = α + π

l1 :rv = rαv = α

69

The associated 2× 2 covariance matrix C` holds the uncertainties in α, σ2α,

in r, σ2r , and the covariance between α and r, σαr.

C` =[

σ2α σαr

σrα σ2r

]The corresponding ellipses are in the α, r-parameter space (see also figure insection ??).

2.8.2 arlinefeature

Constructor function for α, r-line feature object.

l = arlinefeatureCreates a new α, r-line feature object with default values

l = arlinefeature(l)Copies and returns α, r-line feature l

l = arlinefeature(id,x,C)Creates a new α, r-line feature object with valuesid : identifierx : 2× 1 feature parameter vector, interpreted as [α; r]C : 2× 2 parameter covariance matrix [σ2

α σαr; σrα σ2r ]

The line feature class is a child class of the base class entity.

Example:l = arlinefeature(10,[-pi/3;2],0.001*eye(2));


2.8.3 calcinnovation

Calculate innovation between two α, r-line features.

nu = calcinnovation(l1,l2) returns the difference of the state vectors of α, r-line features l1 and l2.

Note that for features with angle parameters this is the place to correctly takeangle differences.

See also arlinefeature.

2.8.4 display

Display method for α, r-line feature object.

70

display(l) displays the α, r-line feature l.

Example: Given the example α, r-line feature l from the constructor methodarlinefeature (section 2.8.2). Typing display(l) or simply l without semi-colon returns

alpha,r line feature id = 10alpha = -1.0472 C = 0.001 0

r = 2 0 0.001


2.8.5 draw

Draw α, r-line feature.

draw(p,ids,color) draws the α, r-line feature l into the current figure withcolor color. Enable and disable display of feature identifiers with the flag idsbeing 1 or 0. color is a [r g b]-vector or a Matlab color string such as 'r' or 'g'.

h = draw(...) returns a column vector of handles to the line objects of thedrawing.

Example: We create three different lines, call the draw method and changethe line width of l3 by using its handle vector, h3. We also adapt the fig-ure axes. The draw method tries to plot an infinite line which can result insomewhat unexpected axis scalings. Adapting the figure axes is almost alwaysrequired when plotting α, r-line features.

l1 = arlinefeature(11,[-pi/3;1],0.001*eye(2));

l2 = arlinefeature(12,[pi/6;1.1],[0.02 0.001; 0.001 0.008]);

l3 = arlinefeature(13,[-pi/3;-0.8],[0.001 0; 0 0.033]);

draw(l1,1,'r');

draw(l2,1,[1 0.5 0]);

h3 = draw(l3,1,[1 0.7 0]);

set(h3,'LineWidth',2);

axis([-2 2 -2 2]);

Note the l3 has a negative radius r. With the definition of the α, r-line feature(section 2.8.1) this is the way how to encode visibility information.


2.8.6 get

Get method for α, r-line feature object.

v = get(l,'PropertyName') returns the value v of the specified property forthe α, r-line feature object l.

71

-2 -1.5 -1 -0.5 0 0.5 1 1.5 2 -2

-1.5

-1

-0.5

0

0.5

1

1.5

2

get(l) displays all property names and their current values for the α, r-linefeature object l.

Example:state = get(l,'x');

See also: arlinefeature/set.

2.8.7 integrate

Add α, r-line feature to the map.

[lw,Gr] = integrate(l,xr,Cr) transforms the α, r-line feature l representedin the robot frame into the world frame given the uncertain 3 × 1 robot posexr and the 3 × 3 pose covariance matrix Cr. Returns l expressed in the worldframe, lw, and the Jacobian of the integration function, Gr.

See also arlinefeature/predict.

2.8.8 predict

Measurement prediction of α, r-line features.

[lr,Hr,Hm] = predict(L,xr,Cr,Crl) transforms the α, r-line feature l rep-resented in the world frame into the robot frame given the uncertain robotpose xr, Cr, and the cross-correlation between the robot and l, Crl. xr is a3×1 vector, Cr a 3×3 covariance matrix and Crl a 3×2 cross-correlation matrix.

The function returns the predicted line feature lr, the 2×3 measurement Jaco-bian derived with respect to the robot, Hr, and the 2×2 measurement Jacobianwith respect to l, Hm.

72

See also arlinefeature/integrate.

2.8.9 set

Set method for α, r-line feature object.

l = set(l,'PropertyName',PropertyValue) sets the value of the specifiedproperty for the α, r-line feature object l.

l = set(l,'PropertyName1',PropertyValue1,'PropertyName2',Property-Value2,...) sets multiple property values with a single statement.

Note that the assignment to the output l is necessary since Matlab does notsupport passing arguments by reference. Hence the set method actually oper-ates on a copy of the object.

set(l) displays all property names and their possible values for the α, r-linefeature object l.

Example:l = set(l,'x',[pi/6; 2],'C',zeros(2));

See also arlinefeature/get.

73

2.9 Map methods (@map folder)

2.9.1 addentity

Add map entity object to map.

m = addentity(m,e) appends map entity e to map m and extends the mapcovariance matrix by filling in zeros in the new row and column belonging to e.

See also entity, map.

2.9.2 display

Display method for map object.

display(m) displays the map object m.

Example: Given a map m which contains a differential drive robot and a pointfeature. Typing display(m) or simply m without semicolon returns

Map "global map":time = 7.321255e+05Robot:differential drive robot id = 0name = Piggytime = 7.321255e+05form type = 4x = 1 C = 1 0 0

2 0 1 03.1416 0 0 1

rl = 0.2rr = 0.2b = 0.5

Features:point feature id = 201x = -1 C = 0.001 0

3 0 0.001

See also map.

2.9.3 draw

Draw map.

draw(m,ids,ellis,newfig,color) plots the map m. Enable and disable dis-play of feature identifiers with ids = 0/1 and uncertainty ellipses with ellis =0/1. If newfig = 0, draw displays the map into the current figure, otherwise a

74

new figure window is opened. color is a [r g b]-vector or a Matlab color stringsuch as 'r' or 'g'. It determines the color of all map entities.

h = draw(...) returns a column vector of handles to all graphic objects ofthe map drawing. Remember that not all graphic properties apply to all typesof graphic objects. Use findobj to find and access the individual objects.

Example: See the mini-tutorial in section 1.2.3.

See also map/drawcorr.

2.9.4 drawcorr

Display map correlation coefficients.

drawcorr(m,colormode) displays the correlations of map m in the current fig-ure. If colormode is 1, the correlations are drawn as gray values where blackmeans fully correlated and white zero correlation. If colormode is 0, the defaultcolormap is used.

Example: Given the map G of the atrium experiment (section 1.3), the twocalls illustrate the difference between the color modes:

drawcorr(G,0);drawcorr(G,1);

5 10 15 20 25 30 35 40 45 50

5

10

15

20

25

30

35

40

45

50

5 10 15 20 25 30 35 40 45 50

5

10

15

20

25

30

35

40

45

50

See also map/draw, colormap.

2.9.5 get

Get method for map object.

v = get(m,'PropertyName') returns the value v of the specified property for

75

the map object m.

get(m) displays all property names and their current values for the map ob-ject m.

Examples:x = get(G,'x');C = get(G,'c');

See also map/getrobot, map/set, map/setstate.

2.9.6 getrobot

Get robot from map.

r = getrobot(m) returns the robot object r in map m. If m contains no robot,r is empty.

Example:r = getrobot(G);

See also map/get, map/getstate.

2.9.7 getstate

Get map state vector and state covariance matrix.

[x,C] = getstate(m) returns the stacked map state vector x and the stackedstate covariance matrix C of map m. The size of x is n× 1, the size of C is n× nwhere n is the number of features in m times their number of parameters plusthree from the robot.

See also map/setstate, map/get.

2.9.8 map

Constructor function for map object.

m = mapCreates a new map object with default values

m = map(m)Copies and returns map m

m = map(name,t)Creates a new map object with valuesname : map name as a string

76

t : discrete time index or timestamp

Map is a container class for the map entity base class entity and its childclasses.

Example:G = map('global map',now)


2.9.9 set

Set method for map object.

m = set(m,'PropertyName',PropertyValue) sets the value of the specifiedproperty for the map object m.

m = set(m,'PropertyName1',PropertyValue1,'PropertyName2',Property-Value2,...) sets multiple property values with a single statement.

Note that the assignment to the output m is necessary since Matlab does notsupport passing arguments by reference. Hence the set method actually oper-ates on a copy of the object.

set(m) displays all property names and their possible values for the map objectm.

Examples:m = set(m,'name','predicted map')m = set(m,'x',x)

See also map/setrobot, map/get, map/getstate.

2.9.10 setrobot

Set robot in map.

m = setrobot(m,r) sets the robot object in map m if m already contains arobot. Otherwise m is unchanged. Use addentity to add a robot to a map.

Note that the assignment to the output m is necessary since Matlab does notsupport passing arguments by reference. Hence the set method actually oper-ates on a copy of the object.

See also map/getrobot, map/get, map/addentity.

77

2.9.11 setstate

Set map state vector and state covariance matrix.

m = setstate(m,x,C) overwrites the map state vector and the state covari-ance matrix by x and C respectively. The size of x is n× 1, the size of C is n×nwhere n equals the number of features in m times their number of parametersplus three from the robot.

See also map/getstate, map/set.

78

2.10 Robot methods (@robot folder)

2.10.1 display

Display method for robot object.

display(r) displays the robot object r.

Example: Given the example robot r from the constructor method robot(section 2.10.3) and typing display(r) or simply r without semicolon returns

differential drive robot id = 0name = Piggytime = 7.321255e+05form type = 4

See also robot.

2.10.2 get

Get method for robot object.

v = get(r,'PropertyName') returns the value v of the specified property forthe robot object r.

get(r) displays all property names and their current values for the robot objectr.

Example:state = get(r,'name');

See also robot/set.

2.10.3 robot

Constructor function for robot object.

r = robot

79

Creates a new robot object with default values.

r = robot(r)Copies and returns the robot object r.

r = robot(typestr,name,formtype,t)Creates a new robot object with values

typestr : type stringname : name string

formtype : form type identifier according to drawrobott : discrete time index or timestamp

The robot class is a child class of the base class entity and in turn base classfor all robot classes.

Example:r = robot('differential drive robot','Piggy',4,now);

See also entity, robotdd, drawrobot.

2.10.4 set

Set method for robot object.

r = set(r,'PropertyName',PropertyValue) sets the value of the specifiedproperty for the robot object r.

r = set(r,'PropertyName1',PropertyValue1,'PropertyName2',Property-Value2,...) sets multiple property values with a single statement.

Note that the assignment to the output r is necessary since Matlab does notsupport passing arguments by reference. Hence the set method actually oper-ates on a copy of the object.

set(r) displays all property names and their possible values for the robot ob-ject r.

Example:r = set(r,'Name','Pluto','FormType',4);

See also robot/get.

80

2.11 Differential drive robot methods(@robotdd folder)

2.11.1 Definition

A differential drive robot is kinematically defined by three parameters, the wheelbase b, the radius of the left wheel rl and the radius of the right wheel rr (figure2.1). The robot reference frame R is attached such that the x-axis points in theforward direction of travel.

brR

rL

x

y

Figure 2.1: Differential drive robot with attached frame R

2.11.2 display

Display method for differential drive robot object.

display(r) displays the differential drive robot object r.

Example: Given the example differential drive robot r from the constructormethod robotdd (section 2.11.6) and typing display(r) or simply r withoutsemicolon returns

differential drive robot id = 0name = Piggytime = 7.321255e+05form type = 4x = 1 C = 1 0 0

2 0 1 0

81

3.1416 0 0 1rl = 0.2rr = 0.2b = 0.5

See also robotdd.

2.11.3 draw

Draw differential drive robot.

draw(r,ids,color) draws the differential drive robot r into the current fig-ure with color color. Enable and disable display of identifiers with the flag idsbeing 1 or 0. color is a [r g b]-vector or a Matlab color string such as 'r' or 'g'.

draw(r,ids,color,contour) draws the robot r in color color as a cross with-out contour when the flag contour = 0.

h = draw(...) returns a column vector of handles to all graphic objects ofthe robot drawing. Remember that not all graphic properties apply to all typesof graphic objects. Use findobj to find and access the individual objects.

Example: In the code below we create two robots and draw them in black.

r1 = robotdd('Donald Duck',[.5;1; pi/6 ],0.001*eye(3),0.2,0.2,0.5,4,now);

r2 = robotdd('Daffy Duck', [-1;0;-pi/10],0.001*eye(3),0.2,0.2,0.5,2,now);

draw(r1,1,'k');

h = draw(r2,0,'k');

A robot is drawn using Matlab line and patch objects to which different graphicproperties apply. The command set(h,'Color','r'); would return an er-ror message as the filled patch objects have no 'Color' property (but an'EdgeColor' and a 'FaceColor' property). Instead, the handle vector h has tobe decomposed into handles to line and patch objects by means of the findobjcommand. The findobj command takes a property name/property value pairand returns those handles where the pair matches. Let us now change the colorof robot r2 into red. The result in shown in the figure below.

h1 = findobj(h,'Color','k');

h2 = findobj(h,'EdgeColor','k');

h3 = findobj(h,'FaceColor','k');

set(h1,'Color','r');

set(h2,'EdgeColor','r');

set(h3,'FaceColor','r');

See also drawrobot.

2.11.4 get

Get method for differential drive robot object.

82

-1.2 -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8

-0.2

0

0.2

0.4

0.6

0.8

1

1.2

1.4

v = get(r,'PropertyName') returns the value v of the specified property forthe differential drive robot object r.

get(r) displays all property names and their current values for the differen-tial drive robot object r.

Example:state = get(r,'x');

See also robotdd/set.

2.11.5 predict

Apply odometry model for differential drive robot.

[r,Fxr,path] = predict(r,enc,params) calculates the final pose and finalpose covariance matrix given a start pose, a start pose covariance matrix andthe angular wheel displacements in enc. params contains the robot model andthe error growth coefficients.

Input arguments:

r : differential drive robot object with start pose r.x, r.Cenc : structure with fields

enc.params.kl : error growth coefficient of left wheelenc.params.kr : error growth coefficient of right wheel

with unit in [1/m].enc.steps(i).data1 : angular displacements of left wheelenc.steps(i).data2 : angular displacements of right wheel

in [rad] and monotonically increasingparams.b : wheelbase in [m]. Distance between the two wheel contact

pointsparams.rl : radius of left wheel in [m]

83

params.rr : radius of right wheel in [m]

Output arguments:

r : differential drive robot object with final pose r.x, r.CFxr : 3× 3 process model Jacobian matrix linearized with

respect to r.xpath : array of structure with fields path(i).x (3× 1) and

path(i).C (3× 3) which holds the poses and the posecovariance matrices over the path. path has the samearray length as enc.steps

The function implements an error model for differential drive robots which mod-els non-systematic odometry errors in the wheel space and propagates themthrough the robot kinematics onto the x, y, θ-pose level.

Reference: K.S. Chong, L. Kleeman, ”Accurate Odometry and Error Mod-elling for a Mobile Robot,” IEEE International Conference on Robotics andAutomation, Albuquerque, USA, 1997.

See also slam.

2.11.6 robotdd

Constructor function for differential drive robot object.

r = robotddCreates a new differential drive robot object with default values.

r = robotdd(r)Copies and returns the differential drive robot object r

r = robotdd(name,x,C,rl,rr,b,formtype,t)Creates a new differential drive robot object with values

name : name stringx : 3× 1 robot state vector, interpreted as

[x, y, θ] in units [m] [m] [rad]C : 3× 3 state covariance matrix

[σ2x, σxy, σxθ;σyx, σ2

y, σyθ;σθx, σθy, σ2θ ]

rl : radius of left wheel in [m]rr : radius of right wheel in [m]b : wheel base in [m]

formtype : form type identifier according to drawrobott : discrete time index or timestamp

r = robotdd(params)Creates a new differential drive robot object from the fields of the structureparams which holds the arguments as fields, namely .name, .x, .C, .rl, .rr,.b, .formtype, and .t. If params does not contain all required fields, an error

84

message is returned.

The robot reference frame is attached to the center of the wheel base suchthat the x-axis looks forward.

The differential drive robot class is a child class of the base class robot.

Example:r = robotdd('Piggy',[1;2;pi],eye(3),0.2,0.2,0.5,4,now);

See also entity, class, drawrobot.

2.11.7 set

Set method for differential drive robot object.

r = set(r,'PropertyName',PropertyValue) sets the value of the specifiedproperty for the differential drive robot object r.

r = set(r,'PropertyName1',PropertyValue1,'PropertyName2',Property-Value2,...) sets multiple property values with a single statement.

Note that the assignment to the output r is necessary since Matlab does notsupport passing arguments by reference. Hence the set method actually oper-ates on a copy of the object.

set(r) displays all property names and their possible values for the differentialdrive robot object r.

Example:r = set(r,'x',[0;1;pi/3],'C',zeros(3));

See also robotdd/get.

85

Bibliography

[1] R. Smith and M. Self and P. Cheeseman, “A Stochastic Map for Uncer-tain Spatial Relationships”, 4th International Symposium on Robotics Re-search, 1987.

[2] P. Moutarlier and R. Chatila, “Stochastic Multisensory Data Fusion ForMobile Robot Location and Environmental Modelling”, Proc. of the Inter-national Symposium on Robotics Research, p. 85–94, 1990.

[3] Kai O. Arras, Feature-Based Robot Navigation in Known and UnknownEnvironments, Doctoral Dissertation Nr. 2765, Swiss Federal Institute ofTechnology Lausanne, Autonomous Systems Lab, 2003.

86

Documents

The CAS Robot Navigation Toolbox - uni-freiburg.desrl.informatik.uni-freiburg.de/publicationsdir/CAS... · 2010-04-27 · The current toolbox version includes support for diﬀerential