SLAM

60475032
2008 2
A Thesis Applying for Master Degree of Shanghai Jiao Tong University
MOBILE ROBOT SIMULTANEOUS LOCALIZATION

AND MAPPING IN LARGE AREA
Supported by the Natural Science Foundation of China under grant

60475032 and Program for New Century Excellent Talents in University
Department:
Department of Automation
Major:
Control Theory and Control Engineering
Author:
Dong Haiwei
Advisor:
Assoc. Researcher Li Ning

Prof. Chen Weidong
Shanghai Jiao Tong University

February, 2008

SLAM SLAM
SLAM
SLAM
Bayes
SLAM
Eustice SLAM
Thrun
Bayes
SLAM SLAM
SLAM
SLAMExtended Information Filter SLAMEIF-SLAM
SLAM
SLAM
SLAM
SLAM
Frontier-
SIFT
SLAM
ICNN
SLAM
II
ABSTRACT
MOBILE ROBOT SIMULTANEOUS LOCALIZATION

AND MAPPING IN LAGRE AREA
ABSTRACT
Computational efficiency has become quite crucial while SLAM
technology becomes much more mature. Recent research concerning
information sparsification for Simultaneous Localization and Mapping
(SLAM) has become quite popular. By classifying landmarks into different
types, various Bayes topological network structures are built which could
make information matrix exactly sparse. However, although the accuracy
remains, the efficiency of SLAM algorithm is ruined during landmark
classification. Compared to the research work shown before, our new
algorithm which is based on Eustice's work is much more efficient and
practical while maintaining high accuracy.
Focusing on sparsing information matrix, geometrical meaning of
Extended Information Filter SLAM (EIF-SLAM) and related formula are
analyzed respectively. The conclusion is drawn that normalized information is
sparse. Moreover the structure of information matrix in EIF-SLAM is put
forward for the first time. According to the structure of information matrix, an
improved algorithm based on EIF is introduced. Owning to the high
efficiency of the special rules in sparse operation, the new algorithm solves
SLAM very efficient by sparsing information matrix directly. The errors that
come from sparsification decrease apparently due to loop-closure. In a word,
the new algorithm realizes efficiency with consistent estimate of SLAM. At
last, the relationship between sparsification and SLAM accuracy is analyzed
theoretically.
The new algorithm is simulated in a large scale environment.
Information matrix sparsification, algorithm efficiency, relocalization, error
III
ABSTRACT
and covariance are analyzed respectively after setting up parameters of

motion model and observation model. Indoor two-wheel robot with camera
and outdoor four-wheel robot with laser are taken into account in experiments.
The two experiments are of great difference. Experiment one accomplishes by
Frontier-II robot which is developed by our lab. Pictures represent landmarks
that are associated by Scale Invariant Feature Transform (SIFT). Experiment
two adopts standard Car Park Dataset which is famous in SLAM field. The
environment of experiment two is outdoor car park and the robot is a
four-wheel mobile car mounted with laser sensor. Cylindrical objects
represent landmarks that are associated by Individual Compatibility Nearest
Neighbor (ICNN). The results of simulation in large scale environment as
well as indoor and outdoor experiments testify validity of the algorithm.
Keywords: Mobile roboticsSLAMExtended information filterInformation

matrixSparsification
IV
.................................................................................................................................................................. I
ABSTRACT ................................................................................................................................................... III
........................................................................................................................... VII
............................................................................................................................. IX
..................................................................................................................................................... 1
1.1 ........................................................................................................................................................ 1
1.2 .................................................................................................................................... 2
1.3 ................................................................................................................ 3
SLAM ........................................................................................................ 6
2.1 ................................................................................................................................................ 6
2.2 ................................................................................................................................................ 9
2.2.1 ................................................................................................................... 10
2.2.2 ........................................................................................................... 14
2.2.3 ............................................................................................................................................... 17
SLAM .............................................................................................................. 20
3.1 .................................................................................................................................. 20
3.2 .................................................................................................................................. 21
3.3 ...................................................................................................................................... 23
3.4 ...................................................................................................................................... 25
3.4.1 1-D, 1-DOF ....................................................................................... 25
3.4.2 2-D, 2-DOF ....................................................................................... 27
3.4.3 2-D, 3-DOF ....................................................................................... 32
............................................................................................................................... 38
4.1 .............................................................................................................................. 38
4.2 .......................................................................................................................... 41
4.3 .................................................................................................................. 45
4.4 .......................................................................................................................................... 47
4.5 .............................................................................................................................................. 48
....................................................................................................................................... 51
5.1 .............................................................................................................................................. 51
5.2 .................................................................................................................................. 52
5.3 .................................................................................................................................. 54
................................................................................................................................................... 60
6.1 .............................................................................................................................................. 60
6.2 .......................................................................................................................................... 63
........................................................................................................................................... 66
7.1 INTEL MATH KERNEL LIBRARY ............................................................................................................ 66
7.2 .............................................................................................................................................. 68
........................................................................................................................................... 71
8.1 .............................................................................................................................................. 71
8.2 .............................................................................................................................................. 72
................................................................................................................................................... 74
1 MATLAB ........................................................................................................................... 77
2 VISUAL C++ ...................................................................................................................... 78
........................................................................................................................................................... 79
..................................................................................................................... 80
..................................................................................................................... 80
VI
2.1 ................................................................................................... 7
2.2 ........................................................................................................................... 7
2.3 .................................................................................................................................... 10
2.4 ICNN .......................................................................................................................... 14
2.5 JCBB .......................................................................................................................... 16
2.6 ............................................................................................................................. 17
2.7 ICNN .......................................................................................................................... 18
2.8 JCBB .......................................................................................................................... 19
3.1 1-D, 1-DOF EIF-SLAM ............................................................................................ 26
3.2 2-D, 2-DOF EIF-SLAM ................................................................................................ 27
3.3 ............................................................................................. 29
3.4 ..................................................................................................................... 29
3.5 EIF-SLAM ................................................................................................. 30
3.6 2-D, 2-DOF SLAM ............................................................................................. 31
3.7 ............................................................................................................................................ 31
3.8 ..................................................................................... 33
3.9 SLAM ......................................................................................................... 33
3.10 2WB, 2-D, 3-DOF SLAM ........................................................................... 35
3.11 2WB, 2-D, 3-DOF SLAM ................................................................................ 37
4.1 BAYES ........................................................................................ 38
4.2 BAYES ................................................................................ 40
4.3 BAYES ........................................................................................ 40
4.4 ......................................................................................................................... 42
4.5 ......................................................................................................................... 44
4.6 ......................................................................................................................... 45
4.7 ..................................................................................... 46
4.8 ......................................................................................................... 47
5.1 ............................................................................................................................................ 51
5.2 ................................................................................................. 54
5.3 SLAM ................................................................................ 55
5.4 -SLAM ................................................................................... 57
5.5 EIF-SLAM ................................................................. 58
5.6 EIF-SLAM EIF-SLAM ............................................. 59
6.1 FRONTIER- ........................................................................................................... 60
6.2 .................................................................................................................................... 61
VII
6.3 .................................................................................................................................... 62
6.4 ................................................................................................................. 63
6.5 ................................................................................................................................ 64
6.6 ............................................................................................................. 65
7.1 ............................................................................................................................. 69
7.2 VC++....................................................................................................................... 70
2
2.1 d , ........................................................................................................................................ 12
2
2.2 2, ......................................................................................................................................... 12
3.1 ............................................................................................. 26
4.1 EIF .............................................................................................. 42
4.2 BAYES ......................................................................................... 43
7.1 MKL ........................................................................................................................... 67
7.2 MKL ........................................................................................................................... 67
VIII
FIG. 2.1 DENSITY COMPARISON OF VARIANCE MATRIX & INFORMATION MATRIX ................................................. 7
FIG. 2.2 ALGORITHM DIAGRAM OF INFORMATION FILTER SLAM ........................................................................ 7
FIG. 2.3 PRINCIPLE OF DATA ASSOCIATION ........................................................................................................ 10
FIG. 2.4 RESULTS OF ICNN DATA ASSOCIATION ................................................................................................ 14
FIG. 2.5 RESULTS OF JCBB DATA ASSOCIATION ................................................................................................. 16
FIG. 2.6 SIMULATION ENVIRONMENT OF DATA ASSOCIATION ............................................................................. 17
FIG. 2.7 ANALYSIS OF ICNN DATA ASSOCIATION ............................................................................................... 18
FIG. 2.8 ANALYSIS OF JCBB DATA ASSOCIATION ............................................................................................... 19
FIG. 3.1 SIMULATION RESULTS OF EIF-SLAM IN 1-D, 1-DOF CASE ................................................................. 26
FIG. 3.2 SIMULATION RESULTS OF EIF-SLAM IN 2-D, 2-DOF CASE ................................................................. 27
FIG. 3.3 STATISTIC HISTOGRAM OF ELEMENTS DISTRIBUTION IN NORMALIZED INFORMATION MATRIX .............. 29
FIG. 3.4 LINK STRENGTH BETWEEN ROBOT & FEATURES ................................................................................... 29
FIG. 3.5 STATISTIC ANALYSIS OF EIF-SLAM ALGORITHM ON COMPUTATION TIME............................................ 30
FIG. 3.6 ERROR AND COVARIANCE OF ESTIMATION IN 2-D, 2DOF CASE ............................................................ 31
FIG. 3.7 MAP ERROR ......................................................................................................................................... 31
FIG. 3.8 COMPARISON OF MOTION TRAJECTORY BETWEEN 2DOF & 3DOF .......................................................... 33
FIG. 3.9 GRAPHICAL USER INTERFACE OF SLAM SIMULATION ENVIRONMENT.................................................. 33
FIG. 3.10 ESTIMATION RESULTS OF 2WB, 2-D, 3-DOF SLAM BEFORE AND AFTER LOOP CLOSURE .................. 35
FIG. 3.11 ERROR AND COVARIANCE OF ESTIMATION IN 2WB, 2-D, 3-DOF CASE............................................... 37
FIG. 4.1 EVOLUTION OF BAYES NETWORK AND INFORMATION MATRIX IN PROJECTION ..................................... 38
FIG. 4.2 EVOLUTION OF BAYES NETWORK AND INFORMATION MATRIX IN ADDING FEATURES ........................... 40
FIG. 4.3 EVOLUTION OF BAYES NETWORK AND INFORMATION MATRIX IN UPDATE ............................................ 40
FIG. 4.4 ELEMENT DISTRIBUTION OF INFORMATION MATRIX ............................................................................. 42
FIG. 4.5 RESULTS OF INFORMATION MATRIX SPARSIFICATION ............................................................................ 44
FIG. 4.6 RELATIONSHIP BETWEEN SPARSIFICATION RATIO & THRESHOLD .......................................................... 45
FIG. 4.7 EFFICIENCY COMPARISON OF MATRIX INVERSION BETWEEN DENSE & SPARSE METHOD ....................... 46
FIG. 4.8 RELATIONSHIP BETWEEN SPARSE RATIO & EFFICIENCY IN MATRIX INVERSION ..................................... 47
FIG. 5.1 SIMULATION ENVIRONMENT ................................................................................................................ 51
FIG. 5.2 LOG STATISTIC HISTOGRAM OF ELEMENT DISTRIBUTION IN INFORMATION MATRIX .............................. 54
FIG. 5.3 EFFICIENCY COMPARISON BETWEEN EIF-SLAM & IMPROVED EIF-SLAM ......................................... 55
FIG. 5.4 EIF-SLAM ESTIMATION RESULTS BEFORE & AFTER LOOP-CLOSURE ................................................... 57
FIG. 5.5 IMPROVED EIF-SLAM ESTIMATION RESULTS BEFORE & AFTER LOOP-CLOSURE.................................. 58
FIG. 5.6 COMPARISON OF ERROR AND COVARIANCE BETWEEN EIF-SLAM & IMPROVED EIF-SLAM ............... 59
FIG. 6.1 FRONTIER- AUTONOMOUS MOBILE ROBOT ...................................................................................... 60
FIG. 6.2 CORRIDOR EXPERIMENT ENVIRONMENT .............................................................................................. 61
IX
FIG. 6.3 CORRIDOR EXPERIMENT RESULTS ........................................................................................................ 63

FIG. 6.4 LOCALIZATION ERROR OF ROBOT IN CORRIDOR EXPERIMENT .............................................................. 63
FIG. 6.5 CAR PARK EXPERIMENT RESULTS......................................................................................................... 64
FIG. 6.6 LOCALIZATION ERROR OF ROBOT IN CAR PARK EXPERIMENT............................................................... 65
FIG. 7.1 COMPARISON OF MATRIX STORAGE RULES ........................................................................................... 69
FIG. 7.2 INVOKING RELATION OF VC++ PROGRAMS .......................................................................................... 70
2
TAB. 2.1 d , DISTRIBUTION .......................................................................................................................... 12
2
TAB. 2.2 d , DISTRIBUTION .......................................................................................................................... 12
TAB. 3.1 MARK NOTATION IN NORMALIZED INFORMATION MATRIX .................................................................. 26

TAB. 4.1 STRUCTURE ANALYSIS OF INFORMATION MATRIXEIF FORMULA ................................................. 42
TAB. 4.2 STRUCTURE ANALYSIS OF INFORMATION MATRIXBAYES NET ...................................................... 43
TAB. 7.1 ILLUSTRATION OF DATA TYPES IN MKL .............................................................................................. 67
TAB. 7.2 ILLUSTRATION OF MATRIX TYPES IN MKL .......................................................................................... 67
1.1
Simultaneous Localization And MappingSLAM Smith

Self Cheeseman
[1, 2] SLAM SLAM
SLAM
Smith Extended Kalman Filter, EKF SLAM
SLAM
EKF-SLAM EKF-SLAM
EKF-SLAM
SLAM
SLAM SLAM
SLAM SLAM
SLAM Feature-based SLAM[3, 4]
SLAM Grid-based SLAM[3, 4]
- SLAM Topological-Metric SLAM[5-7]
SLAM Dense SLAM[8, 9]
SLAM
-
SLAM SLAM
Extended Kalman Filter,

EKF[10, 11]Compressed Extended Kalman Filter, CEKF
[12]
Rao-Blackwellized SLAM[13,
14]
SLAMFastSLAM[15]DP-SLAM[16, 17]
Extended Information Filter,

EIF-Thin Junction-Tree Filter, TJTF[18]
Scan Matching[19-21]
SLAM
SLAM
SLAM
SLAM EIFEKF
EKF-SLAM
SLAM EIF-SLAM [22-28]
1.2
SLAM
SLAM SLAM
SLAM
SLAM EIF
SLAM
EIF-SLAM
Thrun Frese Hiringer EIF-SLAM

[25,
29]
Thrun SLAM
Gaussian
Markov Random FieldGMRF[30]
Bayes Bayes Paskin
Bayes SLAM Paskin
Frese Thin Junction Tree FilterTJTF[18]
Treemap Filter[31-34] O(n) O(log n)
Maximum Likelihood EstimateMLE

Batch Algorithm[35-39]
SLAMTrajectory-Oritened SLAM
SLAMDelayed Mapping SLAM
[38,
40, 41]
SLAM [42]
Wang SLAM SLAM
Decoupled SLAMD-SLAM[17,
43-45]
naturally
sparse
1.3
SLAM SLAM
SLAM SLAM
SLAM
Thrun
SLAM [25]Eustice [46]
Bayes
[22,
41, 46, 47]
SLAM
Eustice
-SLAM [41]
Thrun
Bayes
SLAM
SLAMExtended Information Filter SLAMEIF-SLAM
SLAM
Simultaneous Localization and

MappingSLAM SLAM
SLAM SLAM
Eustice Thrun
SLAM
SLAM
SLAM
ICNN
JCBB, ICNN JCBB
SLAM
Bayes
SLAM
SLAM
SLAM
SLAM
SLAM
FrontierSIFT
SLAM
Intel Math Kernel LibraryMKL MKL

SLAM Math Library
VC++
SLAM
SLAM
SLAM SLAM SLAM
SLAM
SLAM
SLAM
EQUATION SECTION 2
2.1
t
x xt xt xt M
xt xt xt
1
p( xt , M | z t , u t ) N t ,
N ,
M Mx MM
M Mx
t
t
1
N ( t , t ) N (t , t )
xt M
MM
(2.1)
xt
M
z t
u t
(2.1) u t z t xt M
t t t
t
t t
[41]
t t
t t1
t t t
(2.2)
(2.2)
t t 2.1SLAM
0 SLAM
SLAM
b t
a t
2.1
Fig. 2.1 Density comparison of variance matrix & information matrix
Time Projection
Get Observation
Data Association
Add Features
New Feature?
Do Update
2.2
Fig. 2.2 Algorithm diagram of information filter SLAM
SLAM
2.2
[22, 23, 41, 46, 47]
a.
xt 1 f ( xt , ut 1 )
(2.3)
f ( xt , ut 1 ) F ( xt xt )
F f xt
N (0, Q)
p( xt 1 , M | z t , u t 1 ) N 1 (t 1 , t 1 )
t 1
1 T 1
Mxt F Q
Q 1 F 1 xt u
, t 1
Mxt 1 xt M
M Mxt 1 xt F T Q 1u
Q 1 F 1 xt M
MM
(2.4)
Q F xt1xt F T
xt xt
F T Q 1 F
u F T Q 1 f ( xt , ut 1 ) F xt
b.
lt g ( xt , zt )
g ( xt ) G ( xt xt )
(2.5)
lt G g xt
N (0,R )
SLAM
p( xt 1 , xt , M | z t , u t 1 ) N 1 (t 1 , t 1 )
xt xt GT R 1G xt M
t 1
Mxt
MM
R 1G
0
u g ( xt , zt ) G xt
xt GT R 1u
GT R 1
0 , t 1
M
1
R u
R
(2.6)
c.
zt h( t )
(2.7)
h( t ) H ( t t )
N (t , t ) N 1 (t , t )
t t
N (0, R)
H h t
t 1 t H T R 1 H
t 1 t H T R 1u
(2.8)
u zt h( ) H
2.2
SLAM
SLAM
{E1 ,
z {z1 ,
, Em }
, zm } H m { j1 ,
, jm }
Ei F ji zi
ji 0
SLAM
2.3
2.3
Fig. 2.3 Principle of data association
i Ei
Ei {F1 ,
, Fn }
n 1
(n 1)m SLAM
Individual Compatibility Nearest NeighborICNNJoint

Compatibility Branch and BoundJCBB[48, 49]
2.2.1
x R {F1 ,
, Fn } x x
x R
x
F1
x

x Fn
10
(2.9)
PR
T
PRF
P 1
T
PRF
n
SLAM
PRFn
PF1Fn
PFn
PRF1
PF1
PFT1Fn
(2.10)
m {E1 , , Em }
y y
y y u
(2.11)
u 0 S Gaussian u
N (0, S )
y E1

y
y E
m
S E1
S
S ET E
1m
(2.12)
S E1Em
S Em
(2.13)
Ei F ji [49]
fiji ( x, y) 0
(2.14)
(2.14)
fiji ( x, y) hiji Hiji ( x x) Giji ( y y )
(2.15)
hiji fiji ( x, y );
H iji
fiji
x
;
( x , y )
Giji
fiji
y
( x , y )
hiji Ei F ji (2.14)(2.15)
Ciji Hiji Cov( x x ) HijTi Giji Cov( y y )GijTi H iji PH ijTi Giji SGijTi
(2.16)
Mahalanobis Diji
Dij2i hijTi Ciji1hiji
11
(2.17)
SLAM
Ei F ji Individual Compatibility TestIC_Test
if Diji d2,1
j
Hi
(2.18)
else
2
2
d , d dim( fiji ) d , 2
2.1
2.1 d ,
2
Tab. 2.1 d , distribution
2
d2,
50
0.45
1.39
2.37
3.35
4.35
5.43
80%
1.64
3.21
4.64
5.99
7.29
8.56
90%
2.71
4.61
6.25
7.78
9.24
10.64
95%
3.84
5.99
7.81
9.49
11.07
12.59
99%
6.63
9.21
11.34
13.28
15.09
16.81
2,
2
2.2
2.2 2,
2
Tab. 2.2 d , distribution
2
2
2,
16
40
86
99
99.9%
IC_TEST GATE_REJECT
GATE_AUGMENT GATE_REJECT
Dij2i GATE_REJECT GATE_AUGMENT

2
Diji >GATE_AUGMENT z Mahalanobis

ICNN
12
SLAM
GATE_REJECT=4.0 GATE_AUGMENT=16.0
Nearest NeighborNN
Mahalanobis SLAM
2.1 NN
O(mn)
2.1
Individual Compatibility Nearest Neighbor ( ICNN )
for i 1 to m do {measurement Ei }
2
Dmin
mahalonobis 2( Ei , F1 )
nearest 0
outer 0
for j 2 to n do {measurement F j }
Dij2 mahalanobis 2( Ei , F j )
2
if ( Dij2 GATE_REJECT) & ( Dij2 Dmin
) then {withint the gate, store nearest neighbor}
nearest j
2
Dmin
Dij2
else if outer Dij2

outer Dij2
end if
end for
if nearest 0
H i nearest
else if outer GATE_AUGMENT {Ei too far to associate, but far enough to be a new feature}
Hi 0
end if
end for
return H i
2.4a
bc ICNN
13
SLAM
GATE_REJECT
ICNN
ICNN
b
c
2.4 ICNN
Fig. 2.4 Results of ICNN data association
2.2.2
Joint CompatibilityJC
Hi { j1 , , ji } f Hi ( x, y) 0 ( x, y )
Taylor
f1 j1 ( x, y )
f Hi1 ( x, y ) f 2 j2 ( x, y )
f H i ( x, y )
hHi H Hi ( x x ) GHi ( y y )
fiji ( x, y )
fiji ( x, y )
14
(2.19)
SLAM
h1 j1
hHi1 h2 j2
hHi f Hi ( x, y )
hiji
hiji
H Hi
f Hi
x
( x , y )
H1 j1
H Hi1 H 2 j2
;
H iji
H iji
GHi
f Hi
y
( x , y )
(2.20)
G1 j1
GHi1 G2 j2
;
Giji
Giji
(2.21)
hH i Em (1 m i) F j H Hi
GHi ( x, y ) x y Jacobian ICNN
P S H i CHi
CHi H Hi PH HT i GHi SGHT i
(2.22)
H i Mahalanobis DHi
DH2 i hHT i CH1i hHi d2,
(2.23)
Ei F j
true
Hi
false
DH2 i d2,1
(2.24)
else
d d dim( f Hi )
Branch and
BoundBB
BB
Joint Compatibility Branch and BoundJCBB 2.2

2.2
Joint Compatibility Branch and Bound ( JCBB )
Best [ ]
JCBB([ ],1) {recursive function}
return Best
15
SLAM
JCBB( H , i ) { find pairings for observation Ei }

if i m then
if pairings ( H ) pairings ( Best ) then {the last landmark ?}
Best H
end if
else
for j 1 to n do
if individual_compatibility(i, j ) & joint_compatibility( H , i, j ) then {( Ei , F j ) accepted}
JCBB([ H j ], i 1) {recursive recall function JCBB towards Ei 1}
end if
end for
JCBB([ H 0], i 1) {no features can paired with Ei }
end if
JCBB
2.5abc JCBB
b
2.5 JCBB
Fig. 2.5 Results of JCBB data association
16
SLAM
c ICNN 2.4
hH i CHi H i
JCBB
JCBB
2.2.3
ICNN JCBB
2.6
88
SLAM
10
9
8
7
Y m
6
5
4
3
2
1
0
-2
10
12
X m
2.6
Fig. 2.6 Simulation environment of data association
ICNN
2.7
GATE_REJECT
17
SLAM
GATE_AUGMENT
Ture positives Ture nagatives
False positives False nagatives
ICNN
150
True positives
True negatives
0.5
0.5
50
100
150
False positives
1
0.5
0.5
50
100
100
150
False negatives
50
150
50
100
150
2.7 ICNN
Fig. 2.7 Analysis of ICNN data association
ICNN JCBB
2.8 False positives False nagatives
True nagatives 180 True positives
2.2.2
18
True positives
True negatives
0.5
0.5
50
100
150
False positives
1
0.5
0.5
50
100
50
100
150
False negatives
SLAM
150
50
2.8 JCBB
Fig. 2.8 Analysis of JCBB data association
19
100
150
SLAM
SLAM
EQUATION SECTION 3
3.1
a.
x (k 1) xv (k ) Vx dt
f v

yv (k 1) yv (k ) Vy dt
(3.1)
T
T
[ xv (k 1) yv (k 1)] [ xv (k ) yv (k )] k 1 k
Vx Vy x y dt
0 2
Jacobian
f 1 0
xv 0 1
(3.2)
b.
xv (k 1) xv (k ) Vdt cos(v (k ))
f yv (k 1) yv (k ) Vdt sin( v (k ))
v (k 1)
v (k ) wdt
(3.3)
T
[ xv (k 1) yv (k 1) v (k 1)] k 1
[ xv (k ) yv (k ) v (k )]T k V w
20
SLAM
k dt 0 2
Jacobian
1 0 Vdt sin(v (k ))
f
F
0 1 Vdt cos( v (k ))
xv
0 0
1
c.
(3.4)
xv (k ) Vdt cos(G v (k ))
xv (k 1)
f yv (k 1) yv (k ) Vdt sin(G v (k ))
v (k 1)
Vdt sin(G )
v (k )
WB
(3.5)
T
[ xv (k 1) yv (k 1) v (k 1)] k 1
[ xv (k ) yv (k ) v (k )]T k V k
G k WB dt
0 2 Jacobian
1 0 Vdt sin(G v (k ))
f
F
0 1 Vdt cos(G v (k ))
xv
0 0
(3.6)
3.2
Jacobian
a.
360
21
SLAM
x xv z x
g i

yi yv z y
(3.7)
T
T
[ xi yi ] [ xv yv ]
T
[ z x z y ] 0
2 Jabobian
g 1 0
xv 0 1
(3.8)
SLAM (3.7)
zx x x
h i v
z y yi yv
(3.9)
Jacobian
h 1 0
0 1
1 0
0 1
(3.10)
b.
x x r cos( v )
g i v

yi yv r sin( v )
(3.11)
T
T
[ xi yi ] [ xv yv v ]
[r ]T
0 2 (3.11) Jacobian
g 1 0 r sin( v )
xv 0 1 r cos( v )
(3.12)
SLAM (3.11)
( x x (k )) 2 ( y y (k )) 2
i
v
i
v
r
h
yi yv (k )
atan 2
v
xi xv (k )
22
(3.13)
SLAM
Jacobian
xi xv (k )
h
r
H
yi yv (k )
r2
yi yv (k )
r
x x (k )
i 2v
r
xi xv (k )
r
y y (k )
i 2v
r
0
1
yi yv (k )
r
xi xv (k )
r2
(3.14)
3.3
a.
x Vx dt 1 0 x Vx
u f ( xt , ut 1 ) F xt

y Vy dt 0 1 y Vy
(3.15)
x Vdt cos(G ) 1 0 Vdt sin(G ) x
y Vdt sin(G ) 0 1 Vdt cos(G ) y

Vdt sin(G ) 0 0
1

WB
Vdt cos(G ) sin(G )
Vdt sin(G ) cos(G )
Vdt sin(G )
WB
23
(3.16)
SLAM
x Vdt cos( ) 1 0 Vdt sin( ) x
y Vdt sin( ) 0 1 Vdt cos( ) y
0 0

wdt
1
Vdt cos( ) sin( )
Vdt sin( ) cos( )
wdt
(3.17)
b.
u
x z x 1 0 x z x

y z y 0 1 y z y
(3.18)
x
x r cos( ) 1 0 r sin( )
cos( ) sin( ) (3.19)
r

y
sin( ) cos( )
y r sin( ) 0 1 r cos( )

c.
u zt h( ) H
z x fxi x 1 0

0 1
fy
y
i
24
1 0
0 1
x

y
zx
z
fxi y

fyi
(3.20)
SLAM
u zt h ( ) H
( )2 ( )2
fxi
x
fyi
y

y
atan 2 fyi

fx x
fxi x
fyi y
r2
fy y
i
r
fxi x
r
fx x
fy y
fy y
r
fxi x
r2
r2
x

y

fxi
fyi
(3.21)
3.4
3.4.1 1-D, 1-DOF
1-D, 1-DOF EIF-SLAM

EIF-SLAM
3.1a
[2 2]
SLAM EIF-SLAM
3.1b
0 1 [0, 1]
25
0
0
2
2
v+
v+
1
1
2
2
3
3
4
4
5
5
6
6
4
4
6
6
8
8
a
v+
v+
1
1
2
2
3
3
4
4
SLAM
5
5
10
10
6
6
3.1 1-D, 1-DOF EIF-SLAM

Fig. 3.1 Simulation results of EIF-SLAM in 1-D, 1-DOF case
3.1
3.1
3.1a
Tab. 3.1 Mark notation in normalized information matrix
V+
(v, v) (v,1) 1
1 0
26
SLAM
1 Hermit
2
1
3
3.4.2 2-D, 2-DOF
waypoint SLAM 3.2a
v+ 2 3 6 7 8 9 10 36 11
100
13
15
31
Y m
24 11
14
50
20
16
17
18
28
29
-50
0
X m
33
3
19
-100
-150 -100
27
26 8
22
32
23
-50
36 9
25 12
30 21
v+
2
3
6
7
8
9
10
36
11
10
4
34
35
50
100
150
3.2 2-D, 2-DOF EIF-SLAM

Fig. 3.2 Simulation results of EIF-SLAM in 2-D, 2-DOF case
EIF-SLAM
27
SLAM
2
normalized information matrix
1
C R
R(i, j )
C (i, j )
C (i, i) C ( j , j )
(3.22)
2 2
(3.23) I
R(1, n 1) R(1, n)
R(2, n 1) R(2, n)
R(3, n 1) R(3, n)
R(4, n 1) R(4, n)
(3.23)
R (1,1) R(1, 2) R(1,3) R (1, 4)
R (1, n 1) R (1, n)
R(2, n 1) R(2, n)
R(2,1) R( 2, 2) R(2,3) R(2, 4)
R (3,1) R(3, 2) R(3,3) R(3, 4)
R(3, n 1) R(3, n)
I
R(4, n 1) R(4, n)
R(4,1) R(4, 2) R(4,3) R(4, 4)
3 I 3.4.1
R(1,1) R(1, 2) R(1,3) R(1, 4)
R (2,1) R(2, 2) R(2,3) R(2, 4)
R(3,1) R(3, 2) R(3,3) R(3, 4)
R (4,1) R(4, 2) R(4,3) R(4, 4)
Bayes
3.2b
1 [0, 1]
3.3 0
Bayes
28
SLAM
350
300
Statistic result
250
200
150
100
50
0
-0.2
0.2
0.4
0.6
Range of distribution
0.8
1.2
3.3
Fig. 3.3 Statistic histogram of elements distribution in normalized information matrix
2
3.4
landmark 2
landmark 3
landmark 6
10
-20
Link strength
10
-40
10
-60
10
-80
10
100
200
300
Steps
400
500
3.4
Fig. 3.4 Link strength between robot & features
29
600
SLAM
2 3 6
3
3 3
3
3
3.5
SLAM
0.016
Projection time
Update time
0.014
Computation time s
0.012
0.01
0.008
0.006
0.004
0.002
0
10
15
20
25
Current size of state vector
30
35
40
3.5 EIF-SLAM
Fig. 3.5 Statistic analysis of EIF-SLAM algorithm on computation time
4 SLAM
SLAM SLAM
SLAM 3.6
30
SLAM
SLAM 200
400
Error
Covariance
Covariance
-5
100
200
300
400
500
600
100
200
300
Steps
400
500
600
-5
3.6 2-D, 2-DOF SLAM

Fig. 3.6 Error and covariance of estimation in 2-D, 2DOF case
1.6
1.4
1.2
Map error
1
0.8
0.6
0.4
0.2
0
100
200
300
Steps
3.7
Fig. 3.7 Map error
31
400
500
600
SLAM
5 EM
EM | ei |
(3.24)
iM
M ei i
EM 200 3.7
4
SLAM
SLAM
3.4.3 2-D, 3-DOF
SLAM
3.8SLAM
SLAM
100
80
60
40
Y m
20
0
-20
-40
-60
-80
-100
-100
-50
0
X m
50
32
100
SLAM
100
80
60
40
Y m
20
0
-20
-40
-60
-80
-100
-100
-50
0
X m
50
100
3.8
Fig. 3.8 Comparison of motion trajectory between 2dof & 3dof
SLAM Graphical
user interfaceGUIlandmarkwaypoint
3.9
3.9 SLAM
Fig. 3.9 Graphical user interface of SLAM simulation environment
33
SLAM
p q GUI
.mat
L1, x
lm
L1, y
L2, x
L2, y
Lp , x
W1, x W2, x
, wp
Lp , y
W1, y W2, y
Wq , x
Wq , y
(3.25)
T
lm wp [ Li , x Li , y ] (1 i p) i
T
[W j , x W j , y ] (1 j q) j
SLAM 3.4.2
3.10
SLAM
SLAM
3.4.2 2 (3.26)
R(1,1)
R(2,1)
R(3,1)
R(4,1)
R (5,1)
R(1,1)
R (2,1)
R (4,1)

R(5,1)
R(1, 2) R(1,3) R(1, 4)

R(2, 2) R(2,3) R(2, 4)
R(3, 2) R(3,3) R(3, 4)
R(4, 2) R(4,3) R(4, 4)
R(5, 2) R(5,3) R (5, 4)
R(1, 2)
R(2, 2)
R(1,3) R (1, 4)
R(2,3) R(2, 4)
R(4, 2)
R(5, 2)
R(4,3) R(4, 4)
R(5,3) R(5, 4)
34
R(1, n 1) R (1, n)
R(2, n 1) R(2, n)
R(3, n 1) R(3, n)
R(4, n 1) R(4, n)
R (5, n 1) R (5, n)
R (1, n 1) R (1, n)
R(2, n 1) R(2, n)
R(4, n 1) R(4, n)
R(5, n 1) R(5, n)
(3.26)
SLAM
80
60
40
Y m
20
0
-20
-40
-60
-80
-100
-50
50
100
50
100
X m
80
60
40
Y m
20
0
-20
-40
-60
-80
-100
-50
0
X m
b
3.10 2WB, 2-D, 3-DOF SLAM
Fig. 3.10 Estimation results of 2WB, 2-D, 3-DOF SLAM before and after loop closure
35
SLAM
3.4.2 x y SLAM
950
30
Error
Covariance
20
10
-10
-20
-30
200
400
600
800
1000
Steps
1200
1400
1600
1800
a x
40
Error
Covariance
30
20
10
0
-10
-20
-30
-40
200
400
600
800
1000
Steps
b y
36
1200
1400
1600
1800
SLAM
0.25
Error
Covariance
0.2
0.15
0.1
0.05
0
-0.05
-0.1
-0.15
-0.2
-0.25
200
400
600
800
1000
Steps
1200
1400
1600
1800
c
3.11 2WB, 2-D, 3-DOF SLAM
Fig. 3.11 Error and covariance of estimation in 2WB, 2-D, 3-DOF case
37
EQUATION SECTION 4
4.1
SLAM Bayes
a.
xt 1 xt
Bayes 4.1
Bayes
m5
m1
xt
m2
xt 1
m1
m5
xt
m4
m1 m2 m3 m4 m5
xt 1 xt
m4
m2
m3
m3
m3
xt
m4
m2
xt 1
m1
m5
m1 m2 m3 m4 m5
xt 1 m1
xt
xt 1
xt 1
m1
xt
m1
m2
m2
m3
m4
m1
m2
m3
m3
m4
m5
m4
m5
m2 m3 m4 m5
m5
b
c
4.1 Bayes
Fig. 4.1 Evolution of Bayes network and information matrix in projection
38
xt m1 m2 m3 m4
m5 4.1a
xt 1 xt
4.1b xt
xt 1 xt 4.1c
xt 1 m1 m2 m3 m4
(4.1) t 1 (2, 2)
MM Bayes
Q 1 F 1 xt M
t 1
1 T 1
MM Mxt 1 xt M
Mxt F Q
(4.1)
Q F xt1xt F T
xt xt
F T Q 1 F
u F T Q 1 f ( xt , ut 1 ) F xt
b.
4.2
(4.2) t 1 (2,3) t 1 (3, 2) 0
xt xt GT R 1G xt M GT R 1
t 1
Mxt
MM
0
(4.2)
R 1G
0
R 1
39
m5
m1
xt
xt
m1
m2
m4
m2
m3
xt
m5
m1
m4
m2
m3
m1
xt
m3 m4
xt
xt
m1
m1
m2
m2
m3
m3
m4
m4
m2 m3
m4
m5
m5
4.2 Bayes
Fig. 4.2 Evolution of Bayes network and information matrix in adding features
c.
xt 4.3
xt m1 xt m2 xt m3 xt m4
m1 m2 m3 m4 m5
xt
m5
m1
xt
m2
m4
m3
xt
xt
xt
m1
m1
m2
m2
m3
m3
m4
m4
m5
m5
m1 m2 m3 m4 m5
4.3 Bayes
Fig. 4.3 Evolution of Bayes network and information matrix in update
(4.3) H H T R1 H
[ xv yv v ]T [ x fi
y fi ]T
t 1 t H T R1H
H
h h
xv
h
yv
h
v
h
x fi
40
h
y fi
(4.3)
4.2
Eustice [23,
41, 47]
Bayes
4.4 row column

value
1
2
3
800
600
value
400
200
0
-200
-400
-600
100
80
100
60
40
50
20
column
row
41
value
-2
80
80
60
60
40
40
20
20
column
row
b
4.4
Fig. 4.4 Element distribution of information matrix
4.1 4.2 EIF Bayes
4.1 EIF
Tab. 4.1 Structure analysis of information matrixEIF formula
Hermit [27, 41, 47]
(4.1) t 1 (2, 2) MM
(4.3) H H T R1 H
T
[ xv yv v ]T [ x f y f ]
i
42
4.2 Bayes
Tab. 4.2 Structure analysis of information matrixBayes net
xt
Bayes
C R
R(i, j )
C (i, j )
C (i, i) C ( j , j )
(4.4)
C (i, j ) R(i, j ) C R i j
R I
2 2
I
R(1, n 1) R(1, n)
R(2, n 1) R(2, n)
R(3, n 1) R(3, n)
R(4, n 1) R(4, n)
R (1, n 1) R (1, n)
R(2, n 1) R(2, n)
R(3, n 1) R(3, n)
R(4, n 1) R(4, n)
R(1,1) R(1, 2) R(1,3) R(1, 4)

R (2,1) R(2, 2) R(2,3) R(2, 4)
R(3,1) R(3, 2) R(3,3) R(3, 4)
R (4,1) R(4, 2) R(4,3) R(4, 4)
R (1,1) R(1, 2)
R(2,1) R( 2, 2)
R (3,1) R(3, 2)
R(4,1) R(4, 2)
R(1,3) R (1, 4)
R(2,3) R(2, 4)
R(3,3) R(3, 4)
R(4,3) R(4, 4)
43
(4.5)
(4.4) R (4.5)
I
I
Hermit
Eustice [41]
4.5
4.5
Fig. 4.5 Results of information matrix sparsification
4.5
4.6ab
44
0.65
simulation data
0.6
Sparsification ratio
0.55
0.5
0.45
0.4
0.35
-11
-10
10
-9
10
-8
10
-7
10
Threshold
-6
10
-5
10
10
a
simulation data
-0.3
Sparsification ratio
10
-0.4
10
-0.5
10
5
Threshold
10
-6
x 10
b
4.6
Fig. 4.6 Relationship between sparsification ratio & threshold
4.3
(2.2)
45
(4.6)
Tim Davis [50]

4.7 0.5
1/60
0.5
0.7
35
Sparse
30
Dense
0.5
25
0.4
20
0.3
15
0.2
10
0.1
0
500
1000
1500
2000
2500
Dimension of matrix
3000
3500
Time based on dense method s
Time based on sparse method s
0.6
0
4000
4.7
Fig. 4.7 Efficiency comparison of matrix inversion between dense & sparse method
4.8 2000
46
0.21
0.2
0.19
Time s
0.18
0.17
0.16
0.15
0.14
0.13
0.12
0.5
0.55
0.6
0.65
0.7
0.75
0.8
Sparse ratio
0.85
0.9
0.95
4.8
Fig. 4.8 Relationship between sparse ratio & efficiency in matrix inversion
4.4
SLAM
4.1
4.1
while k N
0
k (i, j )
(i, j )
k
| k (i, j ) |
1
dim( k ) 2 k11
others
i, j 1 i r ( k ) 1 j c( k )
end
47
0.5 1 N 1 k k
dim( k ) k r ( k ) c( k ) k
N SLAM
[23]
SLAM
E E
E 1 1
(4.7)
1
E E 1SLAM
1
E 1
1
E 1 SLAM
4.1
[22,
23]
SLAM
SLAM
4.1 SLAM
4.5
4.1 SLAM
4.1 SLAM
[51]
4.1
nn E
1
s ( E )
48
(4.8)
( E ) s
(4.9)
s s
0 E E
E 1/ 1
s
E
()
1 ()( E / )
(4.10)
s
E
()
1 ()( E / )
(4.11)
E E
( E)1 1 ( I 1E)1 1 ( 1E ) 1 E
1
( I 1E )1 1E
k 0
k 1
1 ( E )1 1 (1)k ( 1E )k 1 (1) k 1 ( 1E ) k 1
1
1
1
E 1/ ( E) E
(4.12)
1
E 1
( 1E ) 1(4.11)
1 ( E ) 1
(1)
k 1
( 1 E ) k 1
k 1
1 E
k 1
1 E
1
1 E
1 ( E / )
(4.13)
1 1 ( E / )
1 ( E ) 1
()
E
, E 1 1
1 ()( E / )
(4.14)
()
E ( 1E ) 1(4.11)
49
s 1 ( E )1 [ 1 ( E )1 ]
k 1
k 1
(1)k 1 ( 1 E )k 1 (1) k 1 ( 1 E ) k
1
A E 1
s E
1
k 1
(4.15)
1 E
1 1 E
1
(4.13) E 1
(4.16)
s
E
()
1 ()( E / )
(4.17)
4.1
50
EQUATION SECTION 5
5.1
SLAM
SLAM
5.1 400
(0, 0)
100
80
60
landmark
waypoint
40
Start Point
Y m
20
-20
-40
-60
-80
-100
-50
0
X m
50
5.1
Fig. 5.1 Simulation environment
51
100
5.2
-
xv (k 1) xv (k ) Vdt cos(v (k ))
f ( xt , ut ) yv (k 1) yv (k ) Vdt sin(v (k ))
v (k 1)
v (k ) wdt
(5.1)
T
[ xv (k 1) yv (k 1) v (k 1)] k 1
[ xv (k ) yv (k ) v (k )]T k V w
k dt 0 2
(5.1) Jacobian
1 0 Vdt sin(v (k ))
f
F
0 1 Vdt cos( v (k ))
xv
0 0
(5.2)
u
Vdt cos( ) sin( )
u f ( xt , ut ) F xt Vdt sin( ) cos( )
wdt
(5.3)
x x r cos( v )
g ( xt , zt ) i v

y
y
r
sin(
)
v
i v
(5.4)
T
T
[r ]T
0 2
(5.4) Jacobian
52
g 1 0 r sin( v )
xv 0 1 r cos( v )
(5.5)
cos( ) sin( )
u g ( xt , zt ) G xt r
sin( ) cos( )
c
(5.6)
(5.4)
( x x (k )) 2 ( y y (k )) 2
i
v
i
v
r
h
(5.7)

yi yv (k )
atan 2
v
xi xv (k )
T
T
[r ]T
0 2
Jacobian
xi xv (k )
h
r
H
yi yv (k )
r2
yi yv (k )
r
x x (k )
i 2v
r
xi xv (k )
r
y y (k )
i 2v
r
0
1
yi yv (k )
r
xi xv (k )
r2
(5.8)
u zt h ( ) H
( )2 ( )2
fxi
x
fyi
y

fyi y
atan 2

fx x
fxi x
fyi y
r2
fy y
i
fx x
i
r2
fx x
53
fy y
i
fy y
fx x
r2
r2
x

y

fxi
fyi
(5.9)
5.3
EIF-SLAM
(1)
5.2
14
x 10
12
Statistic result
10
0
-140
-120
-100
-80
-60
-40
-20
Range of distribution in log scale
20
5.2
Fig. 5.2 Log statistic histogram of element distribution in information matrix
(2)
EIF-SLAM
SLAM
EIF-SLAM EIF-SLAM
5.3
EIF-SLAM
54
Do projection
1.5
EIF-SLAM
Improved EIF-SLAM
0.5
0
Add features
50
100
150
200
250
300
350
400
50
100
150
200
250
300
350
400
50
100
150
200
250
300
Current size of state vector
350
400
1.5
1
0.5
0
Do update
1.5
1
0.5
0
5.3 SLAM
Fig. 5.3 Efficiency comparison between EIF-SLAM & improved EIF-SLAM
(3)
EIF-SLAM [41]
EIF-SLAM
EIF-SLAM 5.4 5.5
(4)
SLAM
EIF-SLAM
EIF-SLAM EIF-SLAM 5.6a
5.6b
55
250 250
5.6b
4.1
EIF-SLAM 3
56
100
80
60
Y m
40
20
0
-20
-40
-60
-80
-100
-50
0
X m
50
100
100
80
60
Y m
40
20
0
-20
-40
-60
-80
-100
-50
0
X m
50
100
b
5.4 -SLAM
Fig. 5.4 EIF-SLAM estimation results before & after loop-closure
57
100
80
60
Y m
40
20
0
-20
-40
-60
-80
-100
-50
0
X m
50
100
100
80
60
Y m
40
20
0
-20
-40
-60
-80
-100
-50
0
X m
50
100
b
5.5 EIF-SLAM
Fig. 5.5 Improved EIF-SLAM estimation results before & after loop-closure
58
Error
Covariance
10
-10
50
100
150
200
250
300
350
400
450
500
50
100
150
200
250
300
350
400
450
500
50
100
150
200
250
Steps
300
350
400
450
500
20
-20
0.1
-0.1
aEIF-SLAM
Error
Covariance
10
-10
50
100
150
200
250
300
350
400
450
500
50
100
150
200
250
300
350
400
450
500
50
100
150
200
250
Steps
300
350
400
450
500
20
-20
0.1
-0.1
b EIF-SLAM
5.6 EIF-SLAM EIF-SLAM
Fig. 5.6 Comparison of error and covariance between EIF-SLAM & improved EIF-SLAM
59
EQUATION SECTION 6
6.1
Frontier-
6.1
6.1 Frontier-
Fig. 6.1 Frontier-autonomous mobile robot
500mm
SLAM
6.2
Scale Invariant Feature Transform, SIFT
60
SIFT
Frontier--
2500
2000
Y mm
1500
1000
500
Sta rt Point
-500
-500
500
1000
X mm
1500
2000
2500
6.2
Fig. 6.2 Corridor experiment environment
N 2 0.8 SLAM
6.3a 6.3b
6.3a
SLAM
6.4
X Y SLAM
61
3000
Waypoint
Landmark
2500
2000
Y mm
1500
1000
500
-500
-1000
-1000
-500
500
1000
X mm
1500
2000
2500
a
3000
2500
2000
Y mm
1500
1000
500
-500
-1000
-1000
-500
500
1000
X mm
1500
bSLAM
6.3
62
2000
2500
3000
Fig. 6.3 Corridor experiment results

200
x (mm)
100
0
-100
-200
500
1000
1500
2000
2500
3000
3500
500
1000
1500
2000
Steps
2500
3000
3500
200
y (mm)
100
0
-100
-200
6.4
Fig. 6.4 Localization error of robot in corridor experiment
6.2
SLAM [52]
GPS
EIF-SLAM SLAM
SLAM
[50]
Individual Compatibility Nearest Neighbor, ICNN
63
xv (k 1) xv (k ) Vdt cos(G v (k ))
f yv (k 1) yv (k ) Vdt sin(G v (k ))
v (k 1)
Vdt sin(G )
v (k )
WB
(6.1)
T
[ xv (k 1) yv (k 1) v (k 1)] k 1
[ xv (k ) yv (k ) v (k )]T k V G
k WB dt
0 2
Longtitude m
15
10
-5
-25
-20
-15
-10
-5
0
Lantitude m
10
15
10
15
aGPS
Longitude m
15
10
-5
-25
-20
-15
-10
-5
0
Latitude m
bSLAM
6.5
Fig. 6.5 Car Park experiment results
64
6.5
a
GPS b SLAM
3
SLAM
6.6
GPS
[50]
Latitude m
-2
100
200
300
400
500
600
700
800
900 1000 1100
100
200
300
400
500 600
Steps
700
800
900 1000 1100
Longitude m
2
0
-2
-4
-6
6.6
Fig. 6.6 Localization error of robot in Car Park experiment
65
EQUATION SECTION 7
7.1 Intel Math Kernel Library

Intel Math Kernel LibraryMKL Intel
MATLAB
---

Fourier Fourier

MKL
MKL
MKL
<character code> <name> <mod> ( )
66
<character code> 7.1<name>

7.2<mod>
sgemm Hermit
dhemm
7.1 MKL
Tab. 7.1 Illustration of data types in MKL
real, single
precision
complex,
single
precision
real, double
precision
complex,
double
precision
7.2 MKL
Tab. 7.2 Illustration of matrix types in MKL
ge
general matrix
gb
general band matrix
sy
symmetric matrix
sp
symmetric matrixpacked storage
sb
symmetric band matrix
he
Hermit Hermitian matrix
hp
Hermit Hermitian matrixpacked storage
hb
Hermit Hermitian band matrix
tr
triangular matrix
tp
triangular matrixpacked storage
tb
triangular band matrix
MKL Fortran VC++

VC++
67
Fortran
MKL &
A B
A coln B rown A B
dgemm(& A rown, & A colnA pdata, \
& B rown, & B colnB pdata );
(7.1)
A rown A coln A B rown

B coln B A pdata B pdata
A B
7.2
SLAM
MKL VC++
(1)
MKL
#define SLAM_int int

#define SLAM_double double
struct mat
// struct definition of matrix
{
SLAM_int rown;
SLAM_int coln;
SLAM_double *pdata;
}
rown coln pdata
C SLAM
68
SLAM_int SLAM_double
(2)
VC++ Fortran 7.1

1
aC bFortran
7.1
Fig. 7.1 Comparison of matrix storage rules
MKL
m n Mat Arr Mat i j
Mat (i, j ) Arr ( j m i)
i 1,
, m j 1,
,n
(3)
MKL
SLAM MKL SLAM
SLAM Math LibraryInformation Filter Module
Data Association Module SLAM 7.2
69
Information Filter
Module
Data Association
Module
SLAM Math
Library
Intel Math Kernel

library
7.2 VC++
Fig. 7.2 Invoking relation of VC++ programs
70
8.1
EIF-SLAM EIF-SLAM
SLAM
SLAM
EIF-SLAM
EIF-SLAM
SLAM
1 SLAM
SLAM Bayes
SLAM SLAM
2 SLAM
1
E E 1SLAM
1
E 1
1
E 1 SLAM
1
E 1
71
SLAM
3 SLAM
EIF-SLAM
SLAM
8.2
SLAM
SLAM SLAM
SLAM
SLAM
1 SLAM
SLAM
SLAM
Nieto Dense SLAM
[9]
2 SLAM
SLAM
SLAM
SLAM Newman
[11] SLAM
72
3 SLAM
SLAM SLAM
SLAM
Gaussian
SLAM -Auto
Regression-Moving Average, ARMA
SLAM
SLAM SLAM
73
[1]
Smith, R, M. Self, and P. Chesseman. Estimating uncertain spatial relationships in robotics. in

Proceedings of Conference on Uncertainty in Artificial Intelligence. 1988.
[2]
Hugh, D.W. and B. Tim, Simultaneous localization and mapping: part. IEEE Robotics and
Automation Magazine, 2006. 13(2): p. 99-110.
[3]
Choi, Y.H. and S.Y. Oh, Grid-based visual SLAM in complex environments. Journal of Intelligent
and Robotic Systems: Theory and Applications, 2007. 50(3): p. 241-255.
[4]
Stachniss, C., D. Hahnel, W. Burgard et al., On actively closing loops in grid-based FastSLAM.
Advanced Robotics, 2005. 19(10 SPEC. ISS.): p. 1059-1079.
[5]
Choset, H. and K. Nagatani, Topological simultaneous localization and mapping (SLAM): Toward
exact localization without explicit localization. IEEE Transactions on Robotics and Automation,
2001. 17(2): p. 125-137.
[6]
Kouzoubov, K. and D. Austin. Hybrid topological/metric approach to SLAM. in Proceedings - IEEE

International Conference on Robotics and Automation. 2004.
[7]
Blanco, J.L., J.A. Fernandez-Madrigal, and J. Gonzalez. A new approach for large-scale localization
and mapping: Hybrid metric-topological SLAM. in Proceedings - IEEE International Conference on
Robotics and Automation. 2007.
[8]
Ellekilde, L.P., S. Huang, J.V. Miro et al., Dense 3D map construction for indoor search and rescue.
Journal of Field Robotics, 2007. 24(1-2): p. 71-89.
[9]
Nieto, J., J. Guivant, and E. Nebot, DenseSLAM: Simultaneous localization and dense mapping.
International Journal of Robotics Research, 2006. 25(8): p. 711-744.
[10]
Castellanos, J.A., J. Neira, and J.D. Tardos, Map Building and SLAM Algorithms. Autonomous
Mobile Robots: Sensing, Control, Decision-Making and Applications, 2006.
[11]
Newman, P.M., EKF Based Navigation and SLAM. 2006, Oxford University. p. 46-55.
[12]
Folkesson, J. and H. Christensen. Outdoor exploration and SLAM using a compressed filter. 2003.
Taipei, Taiwan: Institute of Electrical and Electronics Engineers Inc.
[13]
Li, M.H., B.R. Hong, and R.H. Luo, Improved Rao-Blackwellized particle filters for mobile robot
simultaneous localization and mapping. Jilin Daxue Xuebao (Gongxueban)/Journal of Jilin
University (Engineering and Technology Edition), 2007. 37(2): p. 401-406.
[14]
Dong, J.F., W.S. Wijesoma, and A.P. Shacklock. An efficient rao-blackwellized genetic algorithmic
filter for SLAM. in Proceedings - IEEE International Conference on Robotics and Automation.
2007.
[15]
Kim, C., R. Sakthivel, and W.K. Chung. Unscented FastSLAM: A robust algorithm for the
simultaneous localization and mapping problem. in Proceedings - IEEE International Conference on
74

[16]
Eliazar, A.I. and R. Parr. DP-SLAM 2.0. in Proceedings - IEEE International Conference on
[17]
Wang, Z., S. Huang, and G. Dissanayake, D-SLAM: A decoupled solution to simultaneous

localization and mapping. International Journal of Robotics Research, 2007. 26(2): p. 187-204.
[18]
Paskin, M., Thin junction tree filters for simultaneous localization and mapping. 2002, University of
California, Berkeley.
[19]
Bosse, M. and J. Roberts. Histogram matching and global initialization for laser-only SLAM in
large unstructured environments. in Proceedings - IEEE International Conference on Robotics and
Automation. 2007.
[20]
Nuchter, A., K. Lingemann, J. Hertzberg et al., 6D SLAM - 3D mapping outdoor environments.

Journal of Field Robotics, 2007. 24(8-9): p. 699-722.
[21]
Cole, D.M. and P.M. Newman. Using laser range data for 3D SLAM in outdoor environments. in
Proceedings - IEEE International Conference on Robotics and Automation. 2006.
[22]
Walter, M.R., R.M. Eustice, and J.J. Leonard, Exactly sparse extended information filters for
feature-based SLAM. The International Journal of Robotics Research, 2007. 26(4): p. 335-359.
[23]
Eustice, R.M., M.R. Walter, and J.J. Leonard, Sparse extended information filters: insight into
sparsification, in IEEE/RSJ International Conference on Intelligent Robots and Systems. 2005. p.
3281-3288.
[24]
Thrun, S., D. Koller, Z. Ghahmarani et al, A. Ng, Simultaneous localization and mapping with
sparse extended information filters: theory and initial results. 2002.
[25]
Thrun, S., Y. Liu, D. Koller et al, Simultaneous localization and mapping with sparse extended
information filters. International Journal of Robotics Research, 2004. 23(7-8): p. 693-716.
[26]
Walter, M.R., R.M. Eustice, and J. Leonard, A provably consistent method for imposing sparsity in
feature-based SLAM information filters, in Springer Tracts in Advanced Robotics. 2007.
[27]
Walter, M.R., R.M. Eustice, and J.J. Leonard, Exactly sparse extended information filters for
feature-based SLAM. International Journal of Robotics Research, 2007. 26(4): p. 335-359.
[28]
Liu, Y. and S.Thrun. Results for outdoor-SLAM using sparse extended information filters. in
Proceedings of IEEE International Conference Robotics Automation. 2003.
[29]
Frese, U. and G. Hirzinger. Simultaneous localization and mapping - a discussion. in Proceedings of

the IJCAI Workshop on Reasoning with Uncertainty in Robotics. 2001.
[30]
Speed, T.P. and H.T. Kiiveri, Gaussian Markov distributions over finite graphs. Annals of Statistics,
1986. 14(1): p. 138-150.
[31]
Frese, U. Treemap: An O(log n) algorithm for simultaneous localization and mapping. in Lecture
Notes in Artificial Intelligence (Subseries of Lecture Notes in Computer Science). 2005.
[32]
Frese, U., Treemap: An O(log n) algorithm for indoor simultaneous localization and mapping.
Autonomous Robots, 2006. 21(2): p. 103-122.
[33]
Frese, U. Efficient 6-DOF SLAM with treemap as a generic backend. in Proceedings - IEEE
International Conference on Robotics and Automation. 2007.
[34]
Frese, U., Treemap: An O(logn) algorithm for simultaneous localization and mapping, in Spatial
75
Cognition IV. 2005, Springer Verlag. p. 455-476.

[35]
Duckett, T., S. Marsland, and J. Shapiro. Learning globally consistent maps by relaxation. in
Proceedings of the IEEE International Coference on Robotics and Automation (ICRA). 2000.
[36]
Folkesson, J. and H. Christensen. Graphical SLAM - a self-correcting map. in Proceedings of the

IEEE International Conference on Robotics and Automation (ICRA). 2004.
[37]
Frese, U., P. Larsson, and T. Duckett, A multilevel relaxation algorithm for simultaneous localization
and mapping. IEEE Transactions on Robotics, 2005. 21(2): p. 196-207.
[38]
Dellaert, F. Square root SAM. in Proceedings of Robotics: Science and Systems (RSS). 2005.
[39]
Thrun, S., W. Burgard, and D. Fox, Probabilistic Robotics. 2005: MIT Press.
[40]
Newman, P.M., D.M. Cole, and K. Ho, Outdoor SLAM using visual appearance and laser ranging,
in IEEE International Conference on Robotics and Automation. 2006.
[41]
Eustice, R.M., H. Singh, and J.J. Leonard, Exactly sparse delayed-state filters, in IEEE International
Conference on Robotics and Automation. 2005. p. 2428-2435.
[42]
Deans, M. and M. Hebert, Experimental comparison of techniques for localization and mapping
using a bearing-only sensor., in International Symposium on Experimental Robotics. 2000.
[43]
Huang, S., Z. Wang, and G. Dissanayake. Mapping large scale environments using relative position
information among landmarks. in Proceedings - IEEE International Conference on Robotics and
Automation. 2006.
[44]
Wang, Z., S. Huang, and G. Dissanayake, Implementation issues and experimental evaluation of
D-SLAM, in Springer Tracts in Advanced Robotics, P. Corke and Sukkariah, Editors. 2006. p.
155-166.
[45]
Wang, Z., S. Huang, and G. Dissanayake, D-SLAM: Decoupled localization and mapping for
autonomous robots, in Springer Tracts in Advanced Robotics. 2007.
[46]
Eustice, R.M., S. Hanumant, J.J. Leonard et al, Visually navigating the RMS Titanic with SLAM
information filters. Robotics: Science and Systems, 2005. 25(12): p. 1223-1242.
[47]
Eustice, R.M., S. Hanumant, and J.J. Leonard, Exactly sparse delayed-state filters for view-based
SLAM. IEEE Transactions on Robotics, 2006. 22(6): p. 1100-1114.
[48]
Cooper, A.J., A Comparison of Data Association Techniques for Simultaneous Localization and
Mapping, in Aerospace Engineering and Mechanics. 2003, University of Minnesota. p. 45-63.
[49]
Neira, J. and J.D. Tardos, Data association in stochastic mapping using the joint compatibility test.
IEEE Transactions on Robotics and Automation, 2001. 17(6): p. 890-897.
[50]
Davis, T.A., Direct Methods for sparse linear systems. 2006: Society for Industrial and Applied
Mathematics.
[51]
Roger, A.H. and R.J. Charles, Matrix Analysis. 1986, Cambridge: Cambridge University Press.
[52]
Car park dataset, http://www.acfr.usyd.edu.au/homepages/academic/enebot/experimental_data

/car_park.zip.
76
1 MATLAB
if_sim.m
SLAM
configfile.m
frontend.fig
GUI
frontend.m
GUI
example_webmap.mat
information_doprojection.m
information_addfeature.m
information_doupdate.m
vehicle_model.m
observe_model.m
compute_control.m
compute_steering.m
get_observations.m
get_obs_jacs.m
Jacobian
do_observation_model.m
add_control_noise.m
add_observation_noise.m
data_associate.m
data_associate_known.m
distance_mahalanobis.m
mahalanobis
pi_to_pi.m
[-,]
transform_to_global.m
rhomatrix.m
plot_distribution.m
plot_time.m
readm.txt
77
2 Visual C++
mat_int
mat_pre
mat_tran
mat_add
mat_sub
mat_mul
mat_inv
mat_sol
mat_sca
mat_sel
mat_ext_right
mat_ext_down
mat_ass
cre_eye
cre_zeros
pi_to_pi
[-,]
get_obs
info_do_pro
info_add_feat
info_do_upda
data_ass
obs_mod
comp_ass
Mahalanobis
78
79
[1] , , -SLAM . EI.

[2] , , EIF-SLAM . EI.
1
(60475032) 2006 9 -2007 12
2
2006 9 -2006
12
3 2007 3 -2008 2
4 863 /2006AA040203
2007 10 -2008 2
80

SLAM

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

SLAM

Uploaded by

Copyright:

Available Formats

60475032

A Thesis Applying for Master Degree of Shanghai Jiao Tong University

MOBILE ROBOT SIMULTANEOUS LOCALIZATION

Supported by the Natural Science Foundation of China under grant

Control Theory and Control Engineering

Assoc. Researcher Li Ning

Shanghai Jiao Tong University

SLAMExtended Information Filter SLAMEIF-SLAM

MOBILE ROBOT SIMULTANEOUS LOCALIZATION

and covariance are analyzed respectively after setting up parameters of

Keywords: Mobile roboticsSLAMExtended information filterInformation

FIG. 6.3 CORRIDOR EXPERIMENT RESULTS ........................................................................................................ 63

TAB. 3.1 MARK NOTATION IN NORMALIZED INFORMATION MATRIX .................................................................. 26

Simultaneous Localization And MappingSLAM Smith

SLAM Feature-based SLAM[3, 4]

SLAM Grid-based SLAM[3, 4]

- SLAM Topological-Metric SLAM[5-7]

SLAM Dense SLAM[8, 9]

Extended Kalman Filter,

Extended Information Filter,

Thrun Frese Hiringer EIF-SLAM

Maximum Likelihood EstimateMLE

41, 46, 47]

Simultaneous Localization and

Intel Math Kernel LibraryMKL MKL

Individual Compatibility Nearest NeighborICNNJoint

Dij2i hijTi Ciji1hiji

Ei F ji Individual Compatibility TestIC_Test

Dij2i GATE_REJECT GATE_AUGMENT

Diji >GATE_AUGMENT z Mahalanobis

Individual Compatibility Nearest Neighbor ( ICNN )

else if outer Dij2

CHi H Hi PH HT i GHi SGHT i

DH2 i hHT i CH1i hHi d2,

Joint Compatibility Branch and BoundJCBB 2.2

Joint Compatibility Branch and Bound ( JCBB )

JCBB( H , i ) { find pairings for observation Ei }

x Vdt cos(G ) 1 0 Vdt sin(G ) x

y Vdt sin(G ) 0 1 Vdt cos(G ) y

Vdt cos(G ) sin(G )

Vdt sin(G ) cos(G )

Vdt sin( ) cos( )

3.4.1 1-D, 1-DOF

1-D, 1-DOF EIF-SLAM

3.1 1-D, 1-DOF EIF-SLAM

3.4.2 2-D, 2-DOF

waypoint SLAM 3.2a

3.2 2-D, 2-DOF EIF-SLAM

R(3,1) R(3, 2) R(3,3) R(3, 4)

R (4,1) R(4, 2) R(4,3) R(4, 4)

3.6 2-D, 2-DOF SLAM

R(1, 2) R(1,3) R(1, 4)

4.4 row column

4.1 4.2 EIF Bayes

Hermit [27, 41, 47]

R(1,1) R(1, 2) R(1,3) R(1, 4)

R(3,1) R(3, 2) R(3,3) R(3, 4)

R (4,1) R(4, 2) R(4,3) R(4, 4)

Tim Davis [50]

Time based on dense method s

Time based on sparse method s

u f ( xt , ut ) F xt Vdt sin( ) cos( )

Fig. 6.3 Corridor experiment results

Individual Compatibility Nearest Neighbor, ICNN