Robot Localization Using Unconventional Sensors (PDFDrive)

Robot Localization using Unconventional Sensors
by
Robert Oliver
A Thesis
presented to
The University of Guelph
In partial fulfilment of requirements

for the degree of
Master of Applied Science
in
Engineering
Guelph, Ontario, Canada
© Robert Oliver, January, 2015

ABSTRACT
ROBOT LOCALIZATION USING UNCONVENTIONAL SENSORS
Robert Oliver Advisor:

University of Guelph, 2015 Professor Simon X. Yang
This thesis explores the problem of Simultaneous Localization and Mapping (SLAM)
with a focus on combining di↵erent sensors to build a more robust, accurate, and
reliable localization framework. Localization without prior knowledge of the envi-
ronment is considered to be one of the most challenging problems in the field of
robotics.
The core contribution of this thesis is the development of a high-level sensor fusion
solution, which enables simple integration of unconventional sensors not typically
used for robot localization. A capacitive sensor for sensing floor joists directly
under the robot is proposed as an example of an unconventional sensor. A neural
network is implemented to fuse multiple measurements and build a model of likely
joist locations in unexplored regions.
Two di↵erent sensor fusion approaches are proposed. The first solution explores
robot localization with a-priori map knowledge. Prior map knowledge removes the
requirement for map learning and focuses the problem on fusion of the di↵erent
sensor maps. With this focus a high-level scalable sensor fusion architecture is
implemented. Results show an improvement when using this algorithm to incor-
porate new sensors into the robot localization configuration. The approach also
resolves the problem where the map is known but the starting location is not.
The second fusion approach proposes a complete multi-sensor SLAM algorithm

without a-priori map knowledge. The capacitive sensor is incorporated into the
algorithm to demonstrate the scalability of the approach. After incorporating
multiple sensors information into the solution, the peak error and average error
of the estimated robot position are both reduced; while simultaneously enabling
greater robustness through redundant sensors.
Dedication
I would like to thank my family for their continued support in my

research. My mom for her persistent support in everything I do and
my father for talking through my research and providing valuable
advice.
Finally I need to thank my loving fiancée Melissa for her continued

support and encouragement through this work.
iii
Acknowledgements
I would like to thank my supervisor Simon Yang for his continued guidance and
experience throughout this research. His advice was invaluable in focusing my
work. Many of the ideas he taught me were explored in this research and I feel
blessed to have had him available for such quick assistance.
I would also like to thank Professor Radu Murresan who o↵ered me valuable advice
on the focus of this research. His assistance was greatly appreciated.
I would also like to collectively recognize and thank the faculty of Engineering
at Guelph. All of the professors and assistance are extremely approachable and
very responsive and helpful. This atmosphere made for an exceptional learning
environment.
I also appreciate the support my colleagues at BlackBerry provided throughout my

research. They continued to encourage me to continue my education and expressed
the importance of a graduate education. I hope to be able to return the favor with
new ideas and innovations.
iv
Contents
List of Tables ix
List of Figures x
List of Algorithms xiii
List of Abbreviations xiv
List of Symbols xv
1 Introduction 1
1.1 Objectives of This Thesis . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Methodologies of This Thesis . . . . . . . . . . . . . . . . . . . . . 3
1.3 Contributions of This Thesis . . . . . . . . . . . . . . . . . . . . . . 4
1.4 Organization of This Thesis . . . . . . . . . . . . . . . . . . . . . . 5
2 Background and Literature Survey 7

2.1 SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.1.1 Applications of SLAM . . . . . . . . . . . . . . . . . . . . . 8
2.1.2 Robot Localization . . . . . . . . . . . . . . . . . . . . . . . 10
2.1.2.1 Recursive State Estimation . . . . . . . . . . . . . 11
2.1.2.2 Map Representations . . . . . . . . . . . . . . . . . 13
2.1.2.3 Landmarks . . . . . . . . . . . . . . . . . . . . . . 13
2.1.2.4 Frames of Reference . . . . . . . . . . . . . . . . . 16
2.1.2.5 Robot Kinematic Models . . . . . . . . . . . . . . 17
2.1.3 Methods for SLAM . . . . . . . . . . . . . . . . . . . . . . . 17
2.1.3.1 Extended Kalman Filter . . . . . . . . . . . . . . . 17
2.1.3.2 FastSLAM . . . . . . . . . . . . . . . . . . . . . . 19
2.1.3.3 Other Methods . . . . . . . . . . . . . . . . . . . . 22
2.1.4 Advanced SLAM Concepts . . . . . . . . . . . . . . . . . . . 24
2.1.4.1 Multi-Robot SLAM . . . . . . . . . . . . . . . . . 24
2.1.4.2 Multi-Sensor SLAM . . . . . . . . . . . . . . . . . 25
2.1.4.3 SLAM in Dynamic Environments . . . . . . . . . . 25
2.2 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
2.2.1 Conventional Sensors . . . . . . . . . . . . . . . . . . . . . . 26
2.2.2 Unconventional Sensors . . . . . . . . . . . . . . . . . . . . . 31
v
Contents vi
2.2.3 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . 34

2.3 Intelligent Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
2.3.1 Bayesian Learning . . . . . . . . . . . . . . . . . . . . . . . 37
2.3.2 Neural Networks . . . . . . . . . . . . . . . . . . . . . . . . 37
2.3.2.1 Perceptron . . . . . . . . . . . . . . . . . . . . . . 38
2.3.2.2 Feed-Forward Neural Networks . . . . . . . . . . . 39
2.3.2.3 Backpropagation Training . . . . . . . . . . . . . . 40
2.3.2.4 Prior Use of Neural Networks in Mobile Robotics . 40
2.3.3 Genetic Algorithms . . . . . . . . . . . . . . . . . . . . . . . 41
2.3.4 Particle Filters . . . . . . . . . . . . . . . . . . . . . . . . . 42
2.3.4.1 Applications of Particle Filters . . . . . . . . . . . 43
2.3.4.2 Rao-Blackwellized Particle Filters . . . . . . . . . . 44
3 Experimental Setup 45
3.1 Matlab Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . 45
3.1.1 Input Abstraction . . . . . . . . . . . . . . . . . . . . . . . . 46
3.1.2 Algorithm Abstraction . . . . . . . . . . . . . . . . . . . . . 49
3.1.3 Output Abstraction . . . . . . . . . . . . . . . . . . . . . . . 50
3.2 Robot Platform . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
3.3 Simulator Configuration . . . . . . . . . . . . . . . . . . . . . . . . 56
3.3.1 Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
3.3.2 Sensor Model . . . . . . . . . . . . . . . . . . . . . . . . . . 57
3.3.3 Map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
4 Floor Joist Sensors for Robot Localization 61

4.1 Experimental Simulation Model . . . . . . . . . . . . . . . . . . . . 62
4.2 Neural Network Design . . . . . . . . . . . . . . . . . . . . . . . . . 63
4.3 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
4.3.1 Preliminary Encoding Comparison . . . . . . . . . . . . . . 66
4.3.2 Encoding Comparison with Di↵erent Maps . . . . . . . . . . 70
4.3.2.1 Tighter Spaced Map . . . . . . . . . . . . . . . . . 70
4.3.2.2 Rotated Map . . . . . . . . . . . . . . . . . . . . . 72
4.3.3 Testing Realistic Scenarios . . . . . . . . . . . . . . . . . . . 79
4.3.3.1 Robot Navigation Behavior . . . . . . . . . . . . . 79
4.3.3.2 Robustness to Position Errors . . . . . . . . . . . . 80
4.3.3.3 Robustness to Sensor Errors . . . . . . . . . . . . . 80
4.3.4 One-Dimensional Encoding . . . . . . . . . . . . . . . . . . . 83
4.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87
5 Multi-Sensor Localization Using Unconventional Sensors 89

5.1 Methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
5.1.1 Grid Based Localization . . . . . . . . . . . . . . . . . . . . 90
5.1.2 Sensor Classification . . . . . . . . . . . . . . . . . . . . . . 90
5.1.2.1 Reference . . . . . . . . . . . . . . . . . . . . . . . 91
5.1.2.2 Relative . . . . . . . . . . . . . . . . . . . . . . . . 92
Contents vii
5.1.2.3 Recall . . . . . . . . . . . . . . . . . . . . . . . . . 92
5.1.3 Sensor Models . . . . . . . . . . . . . . . . . . . . . . . . . . 93
5.1.3.1 Infra-Red Distance Sensors . . . . . . . . . . . . . 94
5.1.3.2 Capacitive Joist Sensors . . . . . . . . . . . . . . . 95
5.1.3.3 Light Source Tracking . . . . . . . . . . . . . . . . 98
5.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98
5.2.1 Complete Additive Algorithm . . . . . . . . . . . . . . . . . 99
5.2.1.1 Encoders . . . . . . . . . . . . . . . . . . . . . . . 101
5.2.1.2 Weighted Sensor Types . . . . . . . . . . . . . . . 101
5.2.2 Particle Filter Optimization . . . . . . . . . . . . . . . . . . 104
5.2.2.1 Improvements . . . . . . . . . . . . . . . . . . . . . 108
5.2.2.2 Heading . . . . . . . . . . . . . . . . . . . . . . . . 109
5.2.2.3 Unknown Start Position . . . . . . . . . . . . . . . 110
5.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114
6 Multi-Sensor FastSLAM Using Unconventional Sensors 116

6.1 Methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117
6.1.1 Algorithm Architecture . . . . . . . . . . . . . . . . . . . . . 118
6.1.2 Algorithm Specifics . . . . . . . . . . . . . . . . . . . . . . . 120
6.2 Experimental Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . 124
6.2.1 Distance Sensors . . . . . . . . . . . . . . . . . . . . . . . . 124
6.2.2 Capacitive Sensors . . . . . . . . . . . . . . . . . . . . . . . 125
6.3 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
6.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138
7 Conclusion and Future Work 142

7.1 Summary of Work . . . . . . . . . . . . . . . . . . . . . . . . . . . 142
7.2 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145
7.3 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145
References 148
Appendices 156
A Light Sensors for Robot Localization 157

A.1 Methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158
A.2 Turn Angle Detection . . . . . . . . . . . . . . . . . . . . . . . . . . 158
A.3 Real-time Turn Tracking . . . . . . . . . . . . . . . . . . . . . . . . 159
A.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162
B Sample Code of Core Components 163

B.1 Main Loop . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163
B.2 Output module . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165
B.3 Algorithm Module . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
B.4 Simulator Module . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172
Contents viii
C Improved Robustness Using High-Level Multi-Sensor SLAM 176

C.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 176
C.2 The Proposed Method . . . . . . . . . . . . . . . . . . . . . . . . . 179
C.3 Results and Discussion . . . . . . . . . . . . . . . . . . . . . . . . . 183
C.3.1 Normal Operation . . . . . . . . . . . . . . . . . . . . . . . 184
C.3.2 Sensor Failure . . . . . . . . . . . . . . . . . . . . . . . . . . 186
C.3.3 Kidnapped Robot . . . . . . . . . . . . . . . . . . . . . . . . 186
C.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 187
List of Tables
6.1 Summary of errors from di↵erent sensor configurations . . . . . . . 135
C.1 Performance Summary for Di↵erent Sensor Configurations During

Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184
C.2 Performance Summary for Di↵erent Sensor Configurations During
Sensor Failure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 186
ix
List of Figures
2.1 The EKF-SLAM flow diagram. . . . . . . . . . . . . . . . . . . . . 18

2.2 Occupancy grid map . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.3 Infrared sectoring . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
2.4 Three di↵erent sensors mounted on a robot . . . . . . . . . . . . . . 35
2.5 Sensor fusion between various sensor maps . . . . . . . . . . . . . . 36
2.6 Perceptron block diagram . . . . . . . . . . . . . . . . . . . . . . . 38
2.7 Feed-forward neurakl network topology . . . . . . . . . . . . . . . . 39
2.8 Particle filter state diagram. . . . . . . . . . . . . . . . . . . . . . . 43
3.1 Arduino microcontroller . . . . . . . . . . . . . . . . . . . . . . . . 48

3.2 Sensor algorithm UML diagram. . . . . . . . . . . . . . . . . . . . . 50
3.3 Output class definition. . . . . . . . . . . . . . . . . . . . . . . . . . 52
3.4 Complete robot platform used in research. . . . . . . . . . . . . . . 53
3.5 Robot frame used in research . . . . . . . . . . . . . . . . . . . . . 53
3.6 Robot schematic for core modules on robot platform. . . . . . . . . 54
3.7 BlueSmirf Bluetooth module . . . . . . . . . . . . . . . . . . . . . . 56
3.8 Basic sensor positions on robot . . . . . . . . . . . . . . . . . . . . 58
3.9 Default map configuration used in the research . . . . . . . . . . . . 60
4.1 Sample of joist layout in a house. . . . . . . . . . . . . . . . . . . . 63

4.2 Proposed neural network topology. . . . . . . . . . . . . . . . . . . 64
4.3 Scatter plot of capacitance values . . . . . . . . . . . . . . . . . . . 66
4.4 Complete test map. Red are joists, Blue are no joist. . . . . . . . . 67
4.5 Neural network training using continuous encoding. . . . . . . . . . 68
4.6 Neural network training error using binary encoding. . . . . . . . . 69
4.7 Neural network training error using bin encoding. . . . . . . . . . . 71
4.8 Scatter plot of capacitance values. High points are over joist, low
points have no joist, middle values are unexplored. . . . . . . . . . . 72
4.9 Neural network training error using binary encoding on a tighter
spaced map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
4.10 Neural network training error using bin encoding on a tighter spaced
map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
4.11 Scatter plot of capacitance values . . . . . . . . . . . . . . . . . . . 75
4.12 Neural network training error using binary encoding on rotated map 76
4.13 Di↵erent results of neural network training error using binary en-
coding on rotated map . . . . . . . . . . . . . . . . . . . . . . . . . 77
x
List of Figures xi
4.14 Neural network training error using bin encoding on rotated map . 78
4.15 Scatter plot of capacitance values. Red points are over joist, low
points have no joist, middle values are unexplored. . . . . . . . . . . 79
4.16 Neural network training error with binary encoding using robot
path behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
4.17 Neural network training error with bin encoding using robot path
behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
4.18 Scatter plot of capacitance values with noisy position. Red points
are over joist, Blue points have no joist, and Green values are un-
explored. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
4.19 Neural network training error using continuous encoding . . . . . . 84
4.20 Scatter plot of noisy capacitance values. red points are over joist,
blue points have no joist, and green values are unexplored. . . . . . 85
4.22 Scatter plot of capacitance values with both noisy sensor data and
noisy position. Red points are over joist, blue points have no joist,
and green values are unexplored. . . . . . . . . . . . . . . . . . . . 87
5.1 Simple test map, robot shown in center. . . . . . . . . . . . . . . . 94

5.2 IR probability map, scan location center of map. . . . . . . . . . . . 96
5.3 Capacitive sensor probability map . . . . . . . . . . . . . . . . . . . 97
5.4 Light probability map, scan location center of map. . . . . . . . . . 99
5.5 Simple additive algorithm probability map . . . . . . . . . . . . . . 100
5.6 Probability map from wheel encoder ignoring heading information. . 102
5.7 Probability map from wheel encoder included with other sensors. . 102
5.8 Probability map using di↵erent sensor weights. . . . . . . . . . . . . 103
5.9 demonstration of the prediction algorithms . . . . . . . . . . . . . . 106
5.10 Particle filter path compared to true path, using IR, Capacitive,
and light sensors. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
5.11 Error from robot navigation comparison . . . . . . . . . . . . . . . 107
5.12 demonstration of the prediction algorithms . . . . . . . . . . . . . . 110
5.13 Particle configuration at start of test with unknown start location . 111
5.14 Corresponding maps after 20 steps with unknown start location . . 112
5.15 End map configuration after test with unknown start location . . . 113
6.1 Basic UML architecture for sensor algorithms . . . . . . . . . . . . 119

6.2 FastSLAM Algorithm States. . . . . . . . . . . . . . . . . . . . . . 120
6.3 Flow chart of complete algorithm. . . . . . . . . . . . . . . . . . . . 121
6.4 Robot path of SLAM without sensors . . . . . . . . . . . . . . . . . 127
6.5 Robot path with IR only . . . . . . . . . . . . . . . . . . . . . . . . 129
6.6 Robot path with capacitive only . . . . . . . . . . . . . . . . . . . . 130
6.7 Robot path with IR and capacitive sensors . . . . . . . . . . . . . . 131
6.8 Robot path with light beacon only . . . . . . . . . . . . . . . . . . 133
6.9 Robot path using IR, light beacon, and capacitive with known map 134
List of Figures xii
6.10 Robot path using IR while collecting capacitive values for training . 137
6.11 Training data used to train the neural network . . . . . . . . . . . . 138
6.12 Neural network training error using bin encoding on a tighter spaced
map. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
6.13 Robot simulation with IR and trained capacitive neural network . . 140
A.1 TSL235 Light to frequency converter . . . . . . . . . . . . . . . . . 158

A.2 Light sensor scans before and after turn. . . . . . . . . . . . . . . . 161
A.3 Cross covariance of light scans. . . . . . . . . . . . . . . . . . . . . 161
A.4 Light sensor scans after being shifted to best match. . . . . . . . . . 162
C.1 Comparison of localization ability for di↵erent sensor configurations. 183

C.2 Robustness to sensor failure during robot localization . . . . . . . . 185
C.3 Results for kidnapped robot problem using all fused sensors. . . . . 188
List of Algorithms
1 Bayes Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2 FastSLAM 1.0 with Known Correspondences . . . . . . . . . . . . . 20
3 Joist Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
4 IR Probability Map . . . . . . . . . . . . . . . . . . . . . . . . . . . 95
5 Complete Additive Computation . . . . . . . . . . . . . . . . . . . 99
6 Complete Additive Computation With Weights . . . . . . . . . . . 104
7 Particle Filter Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . 104
8 Prediction Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . 105
9 Prediction Algorithm With Heading . . . . . . . . . . . . . . . . . . 110
10 Particle Filter Turn Tracking . . . . . . . . . . . . . . . . . . . . . . 160
xiii
List of Abbreviations
A2D Analog to Digital

ANN Artificial Neural Network
DSP Digital Signal Processing
EKF Extended Kalman Filter
FPGA Field Programmable Gate Array
GPS Global Positioning System
I2C Interface to Communicate
INS Indoor Navigation System
IO Input Output
IR Infra Red
IRID InfraRed IDentifiers
LASER Light Amplification by Stimulated Emission of Radiation
MEMS MicroElectroMechanical Systems
PMHT Probabilistic Multi Hypothesis Tracker
PWM Pulse Width Modulation
RANSAC RANdom SAmble Consensus
SLAM Simultaneous Localization And Mapping
SPI Serial Peripheral Interface
UDP User Datagram Protocol
UKF Uncented Kalman Filter
UML Unified Modeling Language
USB Universal Serial Bus
WIFI WIreless FIdelity
WMM World Magnetic Model
xiv
List of Symbols
A Acceleration
D Distance
E Expected value operator
Jd Joist o↵set
Js Joist separation
Jw Joist width
J✓ Joist heading
M Map
mi Magnetic interference vector
mz Magnetometer Z axis component
n new features
p Probability
P Particles
Q Control input noise
R sensor noise
S Sensor algorithm list
t time
u Robot control signal
v Vector
V Velocity
w Weights
WF WMM field strength
WI WMM inclination angle
xv
List of Symbols xvi
x, y Location
X Robot State
z Sensor scan
✓ heading
✓r turn rate of robot
⇥xy Cross Covariance of xy
Gaussian distribution
⌃ Covariance matrix
µ map feature
µm mean position of robot
O Big O notation
Chapter 1
Introduction
Simultaneous Localization and Mapping (SLAM) is a popular topic in the mobile

robotic community. It attempts to find the solution of how to build a map of
an unknown environment while simultaneously trying to localize within that map.
The problem is made more complex with imperfect data from sensors and imperfect
control from the robot actuators. The conventional approaches utilize high quality
laser range scanners. These sensors minimize the sensor noise allowing accurate
maps of the environment to be created. This is feasible for some applications; such
as the Mars Rover or self-driving cars, however it is too expensive to be practical
for any widespread consumer application. There is growing research exploring how
cheaper alternatives such as ultrasonic or infrared sensors could be implemented
to solve the SLAM problem.
The approach in this thesis focuses on using machine learning strategies to sift
through the high dimensionality map created by using many di↵ering sensors to
increase the confidence of where the robot is positioned. The concept is that lower
cost sensors that are measuring completely di↵erent features in the environment
will add new data and landmarks that can be tracked. The greater amount of
features could allow this technique to approach the quality of a single high quality
sensor that is only capable of tracking a single feature space.
1
The paradigm being suggested is partly inspired by nature. Looking for solutions
to engineering problems in nature is a technique called biomimicry. It is not an
uncommon ability for animals to localize themselves and travel both short and
long distances while still being able to return home. Nature also has examples of
various types of sensors that go beyond ssight that are also used for the purpose
of localization and navigation. For example, research has shown that many birds
sense the magnetic field to detect the direction of north when migrating. As well,
many animals use the sense of smell to track and recall locations. The use of mul-
tiple sensors allows animals to have a better ability to navigate their environment,
as well as some inherent redundancy in navigation. An animal is capable of utiliz-
ing other senses to help localize itself when vision is unavailable. Multiple senses
that each observe di↵erent environmental behaviours, but contribute together to
provide a better indication of the animals position, is the behavior that this work
is attempting to imitate in the field of robots.
In this work some new sensors for localization will be proposed, but the critical
concept of this research is that high-level data fusion can allow a wide range of
sensors to be used. Many new sensor types and ideas could be added in the future.
In many scenarios sensors already on board the robot for another task could be
used for localization at little to no extra cost.
1.1 Objectives of This Thesis
This thesis aims to demonstrate the potential of incorporating unconventional

sensor techniques for robot localization. The hypothesis being that providing new
information from other sources could enable a more accurate and robust location
estimate.
The objective is to develop an algorithm, supported by a software framework,

which allows high-level scalable sensor fusion, enabeling di↵erent sensors to be
incorporated into the robot localization solution.
2
The hypothesis being that by incorporating many di↵erent sensors into the local-
ization solution, a higher dimensional map can be achieved. The assumption being
that more features to be tracked at any given time should improve the localization
accuracy and precision. Furthermore the di↵ering sensors should enable greater
redundancy to sensor failure as well as allowing the system to handle a wider range
of environments.
1.2 Methodologies of This Thesis
A large Matlab framework was developed specifically for multi-sensor robot local-
ization research. This framework allowed abstraction of the input data and the
algorithms; allowing a wide range of algorithms to be developed and tested. Given
the complexity of the SLAM problem, a controlled environment was required.
To increase repeatability and reduce development complexity simulated data is

leveraged to investigate the problemt. A robot platform was built to better un-
derstand the problems encountered in mobile robotics and real world experiments
were conducted. Occasionally the robot platform was used for collecting data,
although most often the robot platform was not used in the actual results and
instead simulated data was used.
The reasons for utilizing simulations was for; repeatability, multi-sensor experi-
mentation, and algorithm-focused. A simulated environment has the advantage
of being highly repeatable, real-world sensors contain environmental and system
noise that result in each experiment producing new results and behaviors. Second,
the research had a heavy focus on experimenting with many sensor types, often
with unconventional sensors that have never been used in this fashion. The chal-
lenge of physically building and integrating the entire sensor was beyond the scope
of this work. Finally, the focus was on the algorithms, not on hardware, and for
that purpose it is enough to demonstrate the work in a simulated environment.
3
Each section describes the sensor models used for the creation of simulated data,
the simulator then typically adds noise to the samples to reproduce a more real-
world simulation.
In the capacitive sensor research, a neural network is trained. Matlab is leveraged

using the Neural Network Toolbox. The advantage of using this toolbox is the
inherent reliability of a published tool when compared to implementing your own
solution. Aside from this toolbox no other Matlab toolboxes were used in a major
way. Other algorithms were developed from the ground up, especially where they
were the focus of the research.
1.3 Contributions of This Thesis
This thesis investigated sensor fusion of di↵ering sensors for the purpose of robot
localization and mapping. The core contributions of this thesis are threefold;
the unconventional application of joist sensors for robot localization, multi-sensor
robot localization with a known map, and a complete multi-sensor SLAM solution
which is capable of fusing di↵ering sensors in a scalable way.
Sensors capable of detecting floor joists in close proximity are not new, the con-
tribution of this work is demonstrating the potential of utilizing this sensor in
combination with conventional sensors for robot localization. A method of learn-
ing the floor joist layout is developed and tested in a simulated environment. The
learning uses a neural network to develop a model of where the floor joists are
likely to be located, which allows the system to make predictions of floor joist
positions in unexplored regions. The joist sensor is also tested in both robot lo-
calization with a known map and an unknown map. It is shown that having a
trained floor joist model can improve the localization accuracy, and enhance the
robots robustness to various failures.
The second contribution performs a complete particle filter based implementation

of multi-sensor localization using unconventional sensor approaches incorporated
4
with conventional sensors. The advantage of this approach is using a probability
map as the fusion method, this enables vary di↵erent sensors to be fused together
in a simple scalable fashion. The scalability of this approach is demonstrated by
testing a variety of sensor configurations and showing the sensor data is incorpo-
rated into the location estimate. The approach taken is also proven to be able to
rapidely and easily solve conditions where the starting position is not known.
The final contribution assembles all of these core concepts to resolve the more
complex problem of multi-sensor SLAM. The challenge of complete SLAM solution
which incorporates unconventional sensors requires all sensors to learn a unique
map of their own feature space and contribute the information from this map into
the overall position estimate. A thorough solution is presented with a complete
scalable architecture that would enable many more sensors to also be included.
The approach taken provides the ability for sensors to learn the map, but does not
require it. The first tests investigated using the conventional FastSLAM algorithm
without any knowledge of the map and fusing known information about other
sensor sources, such as a known light source beacon or the floor joist layout. It
is possible that some environmental information would be known without having
all information available, this approach allows partial information to be provided
while allowing the other sensor maps to learn. The fusion is also demonstrated to
be able to train the floor joist model using the neural network solution developed
in this work.
1.4 Organization of This Thesis
This thesis is organized into 7 chapters. The first chapter Introduction examines
the overall structure of the thesis and the basic approach that will be used. The
overarching theme of the thesis is desgined to build up the complete multi-sensor
SLAM solution.
The Background chapter discusses the necessary background information to make

a reader comfortable with the problem and current research. It is a combination
5
of both the literature review of similar work in the field, while simultaneously
providing background knowledge of the core concepts used in the research.
The third chapter, Experimental Setup, discusses the various tools that were built
to facilitate the research. A comprehencive Matlab test-suite was developed specif-
ically for multi-sensor SLAM research. The core components are plugable algo-
rithm intyerface, a simulator, and a mobile robot that interfaces with the test-suite
in real-time.
The following chapter Capacitive Floor Joist Sensor for Robot Localization ex-
plores utilizing capacitive sensors for the purpose of robot localization. The ca-
pacitive sensor developed in this chapter works as an example of an unconventional
sensor. The algorithm is developed and tested in isolation from the rest of the fu-
sion system. Further work in future chapters will demonstrate how it is intergated
into the complete solution.
Chapter 5, Unconventional Sensors In Multi-Sensor Navigation, examines the sim-

pler problem of multi-sensor robot localization when the map is known. Solving
the robot navigation problem first, facilitated reducing the complexity of the over-
all problem, and allowed focus on developing a clean scalable software architecture.
Experimental results of various sensor configurations from simulation are demon-
strated and discussed in this chapter.
The previous two concepts are brought together in chapter 6, Unconventional

Sensors In Multi-Sensor FastSLAM, which expands on the previous work to resolve
the more complex problem of robot localization when the map is not known.
Various configurations of sensors are tested and using the simulator and the results
are presented. The chapter also integrates the capacitive training research from
chapter 4 to demonstrate how high-level sensor fusion can be used to incorporate
unconventional sensor types.
The final chapter Conclusion discusses the future work to be explored, and re-
iterates the core concepts discussed throughout the thesis.
6
Chapter 2
Background and Literature

Survey
This chapter will review the current relevant publications as well as provide back-
ground details on core concepts. Specific focus will be on robot localization, robot
SLAM, sensors, sensor fusion, and intelligent systems.
The general problem of localization is an old problem that has always required
novel solutions. In the past, ships would need to cross the oceans for long ex-
tended periods with no land in sight. These explorers developed methods of using
compasses, along with celestial objects as landmarks to navigate accurately. De-
spite poor weather that could easily throw them o↵ course. Similarly, robot local-
ization requires some new novel solutions to resolve these challenging problems.
My research focuses on fusing multiple di↵erent sensors together to increase the
confidence of the position estimate. This combines knowledge of many domains
such as; robotics, DSP (Digital Signal Processing), intelligent systems, statistics,
and computer algorithm.
This literature review will begin by reviewing the general problem of robotic local-
ization and the more complex problem of SLAM. It will demonstrate the current
state of the art approaches and expand on key concepts. A critical part of the
research is on incorporating sensors into navigation. This chapter will discuss the
7
current commonly used sensors as well as discussing some sensors that are not
typically used for the purpose of robot localization.
This chapter will also review some of the common filters and intelligent systems
used in robot localization, this is meant to give a generally overview not to teach
the topic. Similarly it will discuss the current approaches for resolving sensor fusion
problems at di↵erent levels. Finally it will provide an overview of the current work
in multi-robot and multi-sensor fusion.
2.1 SLAM
SLAM is a big problem in robotics (Beevers and Huang, 2006) and considered one
of the most challenging problems in mobile robotics (Lee, 2006). This is a result
of the large amount of uncertainty in both the sensory information and the control
system. The problem is essentially trying to make the best guess of the local map
given this uncertainty.
The SLAM problem was first well defined in 1986 at the IEEE Robotics and Au-
tomation Conference in San Francisco (Durrant-Whyte and Bailey, 2006). This
was the very beginning to approaching these problems with a probabilistic ap-
proach, rather than the more conventional absolute approach.
2.1.1 Applications of SLAM
Robot localization and mapping is of growing interest in many areas and with
growing precision of sensors and power of microchips many of these solutions are
beginning to be used in real world applications.
Planetary Rovers The mars rover and other planetary rovers have significant
lag between any control signals on earth and the rover being controlled. For this
reason SLAM algorithms allow the vehicle to handle basic obstacle avoidance and
finding safe modes of passage.
8
Household Robots There is also a growing industry for low cost household
robots especially for cleaning (Bloss, 2008). These robots traditionally have used
simple random navigation schemes but new products are now beginning to map
the environment allowing greater ability to ensure surface coverage in less time
as well as more advance features such as returning to the charging station when
batteries are low.
Military The military also has demonstrated considerable interest in drones and
robots in recent years. The benefit of autonomous drones that can recon areas
is highly advantageous, especially in hazardous environments. The drones being
autonomous allow them to work even when communications are being jammed;
it also enables more drones to be controlled by fewer operators. The military
is also exploring technologies for autonomous vehicles that search for dangerous
explosive devices (Bloss, 2008). These occupations are extremely dangerous and
it could save many soldiers’ lives in replacing this task with a robot.
Self-Driving Cars This technology has also been implemented by google in a

full size car that allows the vehicle to be completely autonomous and drive with
other cars in real streets. The ability of the system to handle extremely dynamic
environments is accomplished with the fusion of many sources of information, such
as laser range scanners, GPS (Global Positioning System), and cameras (Davis and
Stentz, 1995).
Fruit Picking Robots are also being developed that can autonomously navigate
a greenhouse and manage the garden. This technology enables the greenhouse to
be packed tighter allowing greater yield (Bloss, 2008). The robots are then able
to monitor and harvest the fruit when it becomes ripe.
Indoor Localization Indoor localization is becoming a growing field especially

in the domain of mobile phones. Many papers have investigated the fusion of
MEMS sensor data with WIFI and GPS (Chintalapudi et al., 2010). Although
9
this paper focuses on the application of robot localization many of the algorithms
and technological approaches are transferable.
Two key di↵erences between robot localization and mobile phone localization are
that robot localization is typically simplified to 2 dimensions; this drastically sim-
plifies the search space. Although the map of a mobile phone can be reduced to 2
dimensions the phones orientation can constantly rotate in 3 dimensions (Kunze
et al., 2009). Another di↵erence is that robot localization algorithms can take
advantage of knowledge of the control signals and the intended direction of move-
ment. Mobile phones must rely solely on sensory information to infer where the
robot is moving.
Despite these di↵erences many of the core algorithms are similar, for example a
common approach to combine MEMS (Microelectromechanical systems) sensors
data and Wi-Fi scans is to use a particle filters (Bistrovs and Kluga, 2011). This
is similar to the FastSLAM technique commonly used in robotic systems. It is the
intent that the discussions demonstrated in this thesis will be transferable to the
field of mobile phone localization.
2.1.2 Robot Localization
Robot localization is the task of determining the location of a robot within a known
map. This task is much simpler then the complete SLAM problem where the map
is unknown. Many of the same techniques are used for both SLAM and robot
localization, and for that reason robot localization techniques will be presented
first.
In its simplest form robot localization can be seen as a coordinate transformation

problem (Thrun et al., 2006). Where the algorithm must convert sensor readings
taken from the robots coordinate frame and place them within a global map. By
doing this the position of the robot can be determined.
10
2.1.2.1 Recursive State Estimation
State estimation is one of the key components to most robot localization algorithms
(Thrun et al., 2006). States track the estimated parameters that are not directly
observable, but can be inferred through sensor data. This is accomplished by
recursively improving these estimates the algorithm can iteratively improve its
general understanding of where it is within its environment. Since the exact state
is not known this problem is probabilistic in nature, where the algorithm tracks
the belief that the system is in a given state.
Recursive estimation is where the estimate of the state X(t) is derived from the
previous estimate X(t 1). In this way these algorithms tend to converge on the
solution iteratively.
Recursive Filters for Localization There are many filters for the purpose of
tracking recursive state estimates, a domain commonly referred to as Bayesian
filtering. The simple Bayes filter and common extension of Markov localization
will be discussed to explain the simplest forms of recursive state estimation for the
purpose of localization.
The filters used in this paper are simple extensions of these algorithms, and the
fundamental di↵erences will be presented. The first filter makes the assumption
that the error is Gaussian and tracks the mean and standard deviation of the
estimate. The second approach attempts to track many di↵erent estimates simul-
taneously and intelligently.
Bayes Filter Bayesian localization is considered one of the simplest techniques

of localization (Thrun et al., 2006). It works by iteratively improving a tracked
estimate of the belief of the state. This is shown in algorithm 1, where X(t) is the
state estimate at time t, u(t) is the control signal, and z(t) is the sensor input.
Line 3 of this algorithm is considered the prediction step, where the robots position
is predicted using the control input. Line 4 improves upon this prediction by
11
Algorithm 1 Bayes Filter
1: function Bayes Localization(bel(X(t 1)), u(t), z(t))
2: for all X(t) doR
3: ¯
bel(X(t)) p(X(t)|u(t), X(t 1)) bel(X(t 1)) dX(t 1)
4: bel(X(t)) ¯
⌘ p(z(t)|X(t)) bel(X(t))
5: end for
6: end function
including the sensor values. The robot predicts its new location, and then improves
on this estimate from sensory feedback of the environment.
The Bayes algorithm is simple yet costly. A state belief must be calculated and
tracked for every possible pose the robot could be in. The algorithm also requires
that the probability of sensor values given a state can be calculated; this is not
always simple.
Markov Localization Markov localization is a simple extension of Bayes local-

ization that incorporates the map into the algorithm (Thrun et al., 2006). This
algorithm utilizes a-priori knowledge of the map in both the prediction and update
step from the generalized Bayes algorithm. This allows a solution of the global
localization within the map.
This solution is also capable of tracking multiple hypotheses within the environ-
ment, but incurs the same cost as the Bayes filter in that it tracks all possible
states.
Kalman Filter The Kalman filter simplifies on the Markov localization algo-
rithm by tracking only the most likely estimate and tracking the probability of
being in that state (Thrun et al., 2006). This is accomplished by assuming a Gaus-
sian probability distribution around the estimate. With this assumption only the
mean and standard deviation need to be tracked.
This simplification has made the Kalman filter one of the most popular techniques
for robot localization especially on less powerful computers.
12
2.1.2.2 Map Representations
Two main classifications of maps; metric maps (also known as grid based) and
feature based maps (Botelho et al., 2010). Metric maps are grids of the entire
map space that contain a probability for each of those grids such as the likelihood
of an obstacle being present. This is typically referred to as a Occupancy Grid
Map. Feature based maps maintain a list of all the features and their respective
positions and sometimes a confidence. Often feature based methods maintain the
map as a set of geometric shapes.
Topological maps save sensory information and recall this later to relate previously
explored locations with the new position. This is typically represented by graphs
(Thrun, 1996).
Grid based maps are easier for path planning and simpler to display, but are less
favorable in real-time implementations due to the high computational cost and
memory cost. They also su↵er due to a maximum accuracy defined by the grid
cell size.
2.1.2.3 Landmarks
Landmarks, also referred to as features, are a common way to handle sensor al-
gorithms in robot localization. Landmarks factor the complex sensor stream into
fewer identifiable features. Attaching location to these features can create a map
of landmarks that can be matched to the sensor stream to provide a location
reference.
Landmarks should be re-observable, distinguishable, and stationary (Riisgaard

and Blas, 2004). Re-observable in the sense that multiple sensor streams taken
from di↵erent angles and positions of the same landmark would extract the same
feature. Distinguishable means that the features can be identified from each other
and stationary makes it easier to fix a landmark to the map. None of these are
rules but they all help simplify the algorithms.
13
In many algorithms the map is maintained as the set of features with matching
location estimates. This creates a convenient method to reduce the computational
complexity when compared to relating the entire map with every sensor value.
Utilizing features can be broken into two key algorithms; landmark extraction,
and landmark matching (Namoshe et al., 2010).
Landmarks Extraction Landmark extraction is the process of converting the

sensor stream into a feature list. As this needs to be run in real-time at sensor
speed it is import to find an efficient solution to this problem. The most common
sensors used for SLAM are distance based sensors such as laser or IR sensors.
Landmarks therefore typically extract features from streams of distance and angle
pairs (Abrate et al., 2007).
Corner Extraction Corners are commonly used as features as they are eas-
ily recognizable and repeatedly detected from the sensor stream (Namoshe et
al., 2010). This is also the approach my research uses. To help distinguish cor-
ners, properties of the corner can be calculated, such as the angle of the corner.
There exists a plurality of di↵erent algorithms for extracting corners from range
estimates. Essentially this is the problem of detecting slope changes in the polar
coordinate range data of ✓, and d.
Sliding Window The sliding window algorithm traverses the sensor stream with
a fixed window size and calculates two vectors as the vector between the leftmost
sample and the middle sample and the right most sample and the middle sample
(Namoshe et al., 2010). The algorithm then determines if the angle between these
two vectors is above a threshold, if it is it determines that a corner exists. The
angle of the corner can be calculated using the cosine rule defined as
✓ ◆
vi · vj
✓ = arccos , (2.1)
kvi k kvj k
14
where vi and vj are the two vectors and ✓ is the angle of the corner. Typically a
threshold of 120 degrees is selected.
After a corner is found an inward search is done to determine the exact sample
the corner exist on. This algorithm su↵ers from some computational complexity
since each sample must be scanned and compared, as well as the inward search
for each corner to determine the exact position of the corner. But this technique
is still relatively common (Abrate et al., 2007), likely as a result of the simplicity
of implementation.
Split and Merge Split and merge is one of the most popular line extraction
techniques (Nguyen et al., 2005; Namoshe et al., 2010). It originated from com-
puter vision work, but has been adapted to work with distance based measurements
for laser scanners. It works by recursively creating a line between two points and
determining if the furthest point away from that line is above a threshold. If the
line is split into two sub-lines around that point, and both the new lines are tested
against the same threshold. This process is repeated until every point is below the
distance threshold to its corresponding line.
RANSAC Landmarks Another popular technique that came from computer

vision is the RANSAC algorithm, which stands for RANdom SAmble Consensus
algorithm (Nguyen et al., 2005; Riisgaard and Blas, 2004). This works by itera-
tively choosing 2 points at random and computing the distance other points are
from the line formed between these two points. This technique is popular because
its generic to the type of features and it’s easy to implement.
With Sparse Data Beevers and Huang (2006) investigated handling sparse
data. Most of the above landmark extraction algorithms work best with dense
sensor information, most commonly using high-cost laser range scanners. They
demonstrate that this can be resolved using multi-scans of the same sensor. This
does cost more time but can give the illusion of denser sensor information.
15
Landmarks Matching Landmark matching, also referred to as data associa-
tion, is the process of relating features detected in the sensor frame to landmarks
in the map. This step is critical since even a perfect ability to extract features
from the sensor stream would result in large errors if those features are incor-
rectly matched to landmarks on the map. A common approach is to simply match
the sensor landmarks to the nearest map feature. Although this technique suf-
fers when there is uncertainty in the map. For that reason most approaches use
some indication of the confidence of the map feature in its weighting (Abrate et
al., 2007).
2.1.2.4 Frames of Reference
There exists many di↵erent ways of representing the location of the robot, often
referred to as di↵erent frames. A frame provides a definition of the reference of
the coordinate system. Common Frames are the world frame, navigation frame,
and the robot frame. The world frame uses the world as a reference, often using
a spherical based coordinate system such as latitude and longitude and a heading
relative to the north Pole.
In relatively small areas the spherical nature of the Earth can be sufficiently ap-
proximated with rectangular coordinates and a local frame of reference is created.
This is referred to as the navigation frame.
The robots frame is where everything is relative to the robot (Bai-fan et al., 2006),
this is often represented in polar coordinates, where all items are represented as a
distance and angle from the robot.
Every sensor and actuator can be thought to have its own frame of reference.
Knowing the coordinate transformations between these di↵erent frames and the
robots central frame will allow comparisons of sensor data taken from di↵erent
sensors.
16
2.1.2.5 Robot Kinematic Models
Robot kinematics is the process of modeling the robot while in motion (Matsebe et
al., 2010). Accurately modeling the kinematics of the robot system can help better
resolve the update equation p(X(t)|u(t), X(t 1)). This is di↵erent for every robot,
but the problem is often simplified by using wheel encoders and similar sensors for
use in a control system. This allows better prediction of the resulting displacement
as a result of a control input.
This paper generally ignores the complexity of full robot kinematics by using a
simulator that models a noisy control system.
2.1.3 Methods for SLAM
The SLAM algorithm has had a variety of di↵erent solutions proposed. The most
popular approaches are the Extended Kalman Filter and FastSLAM algorithms.
Both of these approaches track the robot and map features as a state estimate
and attempt to converge on the solution where error is reduced. These methods
will be discussed in detail here.
2.1.3.1 Extended Kalman Filter
The most popular of the SLAM solutions is the Extended Kalman Filter (EKF)
(Abrate et al., 2007; Myung et al., 2010; Durrant-Whyte and Bailey, 2006; Newman
et al., 2002; Matsebe et al., 2010; Myung et al., 2010; Rikoski et al., 2002). The
EKF is an extension of the Kalman filter that better handles non-linarites. Non-
linarites of the SLAM algorithm are present when tracking headings as well as
velocity. The math for the EKF is fairly simple (Matsebe et al., 2010), it is
essentially a recursive adaptive filter that approximates non-linarites.
The state is maintained in a variable X and the covariance matrix ⌃. There is

then a prediction phase that updates both given a control input u and the noise
of the control input Q.
17
Robot Moves
Control Signal (u)
Map Cleanup Predict
Map (M ) Scan Sensors (z)

Update
Figure 2.1: The EKF-SLAM flow diagram.
This requires calculation of the Jacobian to handle the non-linarites. After the
prediction phase the state and covariance is updated using the update equations
given the observation Z and noise of observation R.
The typical flow for a robot EKF implementation is shown in Figure 2.1. The
robot first moves tracking its movement with its dead reckoning sensors. The
system then uses the kinematic model and the dead reckoning sensors to predict
its new location. The third state then performs a sensor scan and updates its
location from re-observable features. Finally it cleans the map by removing and
adding landmarks that are newly observed or have not been re-observed.
One of the advantages of using the Kalman algorithm is that it handles the inac-
curacy of the estimate from each sensor. Therefore if a specific sensor has little
idea of the current location it just provides a high error and its input will be
mostly ignored. This has made the EKF e↵ective with even less accurate sensors,
as shown by Abrate et al. (2007) when they successfully implemented an EKF
algorithm with only IR sensors.
Unfortunately, this method is unimodal and takes a single prediction of the lo-
cation and cannot handle one sensor providing multiple possible estimates. For
18
example, the capacitive sensor could indicate the robot is over a floor-joists. Given
there are many floor-joists in the room this does not refer to a specific position
but does massively narrow down the solution space. This type of problem is not
easily handled by the Kalman solution.
Unscented Kalman Filter Using an uncented Kalman Filter rather than the
EKF allows handling of the non-linarites di↵erently (Choi and Lee, 2012). The
extended Kalman filter aims to linearize as an approximation but the UKF uses a
true non-linear model. This technique is less common as a result of the complexity.
2.1.3.2 FastSLAM
FastSLAM is a newer approach utilizing the Rao-Blackwell particle filters (Durrant-

Whyte and Bailey, 2006; Thrun et al., 2006). In this technique multiple possible
solutions are tracked through particles. Each particle is given a weight corre-
sponding to how well that particle’s map matches its sensors. Resampling removes
unlikely particles proportional to the particle’s weight. In this way more likely so-
lutions survive and less likely solutions disappear. One advantage of FastSLAM is
unlike the EKF multiple hypotheses can be simultaneously tracked. This is similar
to Monte Carlo localization.
The novel simplification of the Rao-Blackwell particle filter is that each particle
contains a state estimate of Gaussian likelihoods for features along with covariance
matrices, similar to the extended Kalman filter. Each particle is then individually
handled as a Kalman filter. This allows for the efficiency of the EKF, but using
particle filters allows for much more dynamic and complex situations to be handled.
FastSLAM 1.0 The FastSLAM 1.0 algorithm with known correspondences is

the simplest form of the algorithm. This algorithm contains a set of particle’s each
with a state estimate. The state estimate contains that particles prediction of the
current robot position and the location of the features. The features are repre-
sented as a set of means and standard deviations. Unlike the extended Kalman
19
filter each feature has its own covariance matrix with respect to x and y direction
rather than a complete covariance matrix relating it to every other feature. This
reduces the complexity of the matrix inversion problem to a simple 2x2 which is
trivial to solve (Thrun et al., 2006). The downside of this factorization is that it
fails to find the relationships between the features.
Algorithm 2 FastSLAM 1.0 with Known Correspondences

1: function fastslam 1.0(z(t), u(t), P (t 1))
2: for all k in 1 to M do
3: < X(t 1)k , < µ(t 1)k1 , ⌃(t 1)k1 >, . . . , < µ(t 1)kN , ⌃(t 1)kN >>
P (t 1)
4: X(t 1)k p(X(t)|X(t 1), u(t)) . Predict
5: if Feature is new then
6: initialize mean, covariance, weight
7: else
8: < µ(t)k1 , ⌃(t)k1 >=Kalman update(µ(t 1)k1 , ⌃(t 1)k1 , z(t))
9: wk =calculate weight(µ(t 1)k1 , ⌃(t 1)k1 , z(t))
10: end if
11: all unobserved features remain unchanged
12: end for
13: Resample
14: end function
Algorithm 2 shows the FastSLAM 1.0 algorithm with known correspondences.

Where z is the sensor scan, u is the control input, P is the particle set, X is the
state, µ is the map feature, ⌃ is the covariance matrix of the feature, and w is the
particle weight (Montemerlo et al., 2002).
FastSLAM 2.0 FastSLAM 2.0 is a common improvement on FastSLAM 1.0

discussed above (Montemerlo et al., 2003; Thrun et al., 2006). The improvement
works by taking the sensor measurement z(t) into account for the probability
distribution for pose X(t).
FastSLAM with Unknown Correspondence Nieto et al. (2003) improved

on FastSLAM’s typical data association technique. This works by calculating
the weight given each feature correspondence and taking the maximum likelihood
(Thrun et al., 2006).
20
Benefits of FastSLAM One of the major benefits of the FastSLAM approach
to robot localization is the ability to track multiple hypotheses at once. This
allows for the system to start with an unknown start location and a partial map
(Carlone et al., 2010). The particles can be uniformly distributed throughout the
possible positions, and through resampling will eventually converge on the true
position.
The complexity is O(K log N ), where K is the number of particles and N is the
number of landmarks (Lee, 2006). This demonstrates how the complexity increases
linearly in proportion to the number of particles used. This can be a convenient
way to increase performance by dynamically adjusting the number of particles
depending on the processing power available and the current error in the position.
The memory requirements are also reasonable, especially if the number of land-
marks is low (Miller et al., 2005).
Implementations of FastSLAM An e↵ective method for reducing the number

of particles required for grid based FastSLAM was demonstrated by Grisetti et
al. (2007). The task of reducing the number of particles is challenging as the
algorithm could su↵er from particle depletion. This paper focuses on adaptive
resampling techniques that improve the likelihood that a generated particle is
a likely candidate based on an improved proposal distribution. The improved
particle distribution is accomplished using information about the movement of
the robot as well as the most recent observations.
Geometric constraints to the conventional FastSLAM algorithm were succesfully

applied in the work by Lee (2006). Although this work was only demonstrated in
simulated environments, it showed promise in being able to apply virtually any
geometric constraint and maintain a stable map. This work resulted in comparable
efficiency to FastSLAM 2.0.
The incorporation of multi-rate sensors into the FastSLAM algorithm was demon-
strated by Schön et al. (2007). The problem presented in this paper was that
sensors working at di↵erent rates would not be treated equally by the particle
21
filter. Either some sensors are ignored by the fast running sensor or some samples
are repeated or completely unused by the slow running sensors. This paper derives
a multi-rate particle filter and demonstrates favourable simulated results.
There have also been interesting publications around real-world robots rather than
simulated models. Magnenat et al. (2010) demonstrated results from co-design of
the hardware with a FastSLAM algorithm. This paper aims to design a robot
hardware and software to work together. They adapted a FastSLAM algorithm to
work directly with there robot hardware.
A comparison of many SLAM algorithms for the use in radio controlled helicopters
was shown in the work by Miller et al. (2005). This provided a unique perspective
of mobile robotic mapping. The FastSLAM algorithm was completely implemented
within their 3D helicopter simulator.
2.1.3.3 Other Methods
EKF and FastSLAM are the two most popular techniques for resolving the SLAM
algorithm but many other techniques have been proposed. This section will give
a brief summary of these sections.
Occupancy Grid Mapping Occupancy grid maps store the features in a grid
map as probabilities of occupancy. This o↵ers a variety of advantages especially
with path planning. This breaks the space down into discrete cells (Elfes, 1989).
The disadvantage of this is that the obstacles are reduced to the resolution of
the cell size. This technique also easily integrates with user maps. Occupancy
grid maps usually range in probability from 0 to 1; where 1 is occupied and 0 is
unoccupied. Initialization spots are placed at an unknown state between the two
of 0.5. Sensor values can take advantage of both information that predicts the
occupancy of a grid and information that predicts no occupancy, see Figure 2.2.
This also makes visualization of the map simpler when the state based approaches
used for both EKF and FastSLAM.
22
Figure 2.2: Occupancy grid map. (From Elfes (1989))
SLAM can be implemented with occupancy grid maps using a particle filter sim-
ilar to FastSLAM (Thrun et al., 2006). This o↵ers many of the advantages of
FastSLAM in conjunction with the advantages of grid maps.
Genetic Algorithm A generic algorithm to resolve the SLAM problem was

developed by Duckett (2003). Tom defined the SLAM problem as a global opti-
mization problem and used the genetic algorithm to search all possible robot maps
and robot paths. Although genetic algorithm is a valid technique for resolving a
global optimization problem, they are computer intensive and are unlikely to work
in real-time with current technology.
Probabilistic Multi Hypothesis Tracker Davey (2007) demonstrated a tech-

nique of using a Probabilistic Multi Hypothesis Tracker (PMHT). This is a di↵erent
filter style similar to a particle filter in that it encodes the data in states. This pa-
per presents the entire algorithm, and provides a comparison to other algorithms.
This algorithm was then run with real world data collected in Victoria Park.
23
2.1.4 Advanced SLAM Concepts
The SLAM problem is often simplified for the purpose of research but specific tasks
often require more advanced algorithms. Multi-Robot SLAM examines the task of
fusing many robots information together to improve the robots position estimate.
Multi-Sensor SLAM utilizes many sensors on the robot; this is the focus of the
research in this thesis. Dynamic environments remove the common assumption
that the map is static.
2.1.4.1 Multi-Robot SLAM
Multi-robot slam is defined as the problem of using multiple robots feedback to

provide a better SLAM estimate then any individual robot. This has similar
problems as multi-sensor SLAM in that it requires data fusion for the purpose of
localization.
Multi-robot SLAM can be classified in two categories, collaborative or cooperative

(Matsebe et al., 2010). in a collaborative system each robot is updating the others
in real time and the robots work as a team. In a cooperative system the robots
share all information with a third party computer, which can then update the
robots of new information.
Cooperative robot behaviour for the purpose of robot localization was investigated
by Ranky (2007). This paper discussed the di↵erence between centralized and
decentralized control. Where centralized control allows one node to control all
robots, and decentralized control allows each robot to work to achieve their own
goals. This paper went beyond a simulation and constructed a working prototype
called the M-Tran III.
A sub-category of multi-robot SLAMN is coordinative SLAM, and is defined by

the robots sharing a common mapping framework and being coordinated by a cen-
tralized architecture (Choi and Lee, 2012). This paper uses aa unscented Kalman
filter to solve the CSLAM problem.
24
The use of a Rao Blackwellized particle filter, for the purpose of multi-robot SLAM,
was demonstrated by Carlone et al. (2010). This paper is interesting in that it
handles the problem of unknown starting location and over time builds a common
map that all robots can localize themselves within. This paper implements the
system within a simulated environment as well as performing real-world tests.
2.1.4.2 Multi-Sensor SLAM
A single sensing method can be a crippling limitation on successfully performing

robot SLAM, especially in more complex environments in the real-world (Davis
and Stentz, 1995). For this reason the field of multi-robot SLAM investigates how
multiple sensors could be used together to improve the SLAM solution.
The advantages of Multi-Sensor SLAM are that multiple sensors could increase ac-
curacy, precision, reliability, safety. As well fewer low cost sensors could approach
or surpass a more expensive sensor; resulting in overall cost savings. An exam-
ple of this was presented by Koval et al. (2007), where low cost ultrasound and
infrared sensors were fused together to accomplish the task of robot navigation.
A similar solution was proposed by Slušný et al. (2009). In this paper they pro-
posed a solution to developing a low-cost robot capable of SLAM. A key design to
their robot was multi-sensor fusion of vision and infrared. Low cost cameras with
the added depth information provided by infrared sensors produce a reliable and
robust SLAM platform. This is similar to the work presented by Lee et al. (2009)
that also fused infrared and vision data together. Although Lee focused more on
the particle filter algorithm required to fuse the data.
2.1.4.3 SLAM in Dynamic Environments
Most of the SLAM algorithms discussed up to this point made the assumption
that the environment was static. This greatly simplifies the SLAM problem as
trying to learn a map while the map is changing creates more unknowns and
increases the complexity. Despite this common assumption real environments are
25
continuously changing and for a robot to operate in these environments it must
be able to handle this.
In the paper presented by Alejandro Ramirez-Serrano a solution to robot localiza-

tion in quazi-dynamic environments was presented (Ramirez-Serrano et al., 2008).
Quazi-Dynamic environment relaxes the assumption without totally removing it.
In many environments this assumption is reasonable, a few obstacles move slightly
such as furniture or people, but the entire map would not change. This approach
uses a probability grid map, similar to an occupancy grid map. This technique ac-
tually tracks obstacles and creates a probability distribution of the likelihood that
the obstacle is in motion. Simulated results presented in this paper demonstrate
that this solution is robust to dynamic environments.
2.2 Sensors
One core component of this research is examining the sensors used for this problem.
This section will provide a cursory overview of some of the sensors.
2.2.1 Conventional Sensors
The conventional sensors used for this purpose are typically vision based or dis-
tance based sensors (Lee et al., 2009). The most common are Cameras, Ultrasonic,
Infrared and Laser scanners. The most accurate of these sensors is the laser range
scanner.
Laser Sensor Laser range scanners are one of the most popular sensors for
robot SLAM (Nguyen et al., 2005). These sensors have good range and a very
precise narrow beam making them ideal for SLAM. Typical laser scanners are also
omnidirectional, making them very fast and capable of working real-time. This
accuracy comes at a high financial cost and makes this solution impractical for
lower budget projects.
26
Infrared Sensor Infrared distance sensors are the common lower cost alterna-
tive to Laser range scanners (Abrate et al., 2007; Koval et al., 2007; Slušný et
al., 2009). They are not as accurate but have proven capability for SLAM.
These sensors typically interface with an analogue value proportional to the dis-
tance of the nearest obstacle. They shine infrared light and measure the angle
of the returning light. This angle can then be used to triangulate the distance
of the obstacle. These sensors are often not used as they don’t work in outdoor
environments where IR light contamination could be a problem.
Ultrasonic Sensor Another low-cost alternative to laser range scanners is the

ultrasonic sensor. It su↵ers from much larger noise then both laser and IR sensors
(Varveropoulos, 2005). It has a much wider beam then IR causing it to miss
objects that are too narrow.
Many solutions have been proposed to resolving the problems of ultrasonic sen-
sors. Moreno et al. (2002) used genetic algorithms to model the ultrasonic sensor.
Rencken (1993) used clustering algorithms.
Camera Camera sensors are growing in popularity with the decrease cost and
size of high quality sensors and the increased power of processors capable of real-
time image processing. Camera localization use similar techniques as laser based
localization, tracking features in the sensor space and mapping those features to
the map (Little et al., 1998).
Stereoscopic Vision Using a single camera and viewpoint makes feature ex-
traction difficult given the difficulty of determining depth from an image. For this
reason research in stereoscopic vision techniques have proven to help resolve the
depth using two cameras, with a known fixed separation providing two viewpoints
of the same object (Bloss, 2008). This works similar to human eyesight and is an
example of biomimicry applied to robot sensing.
27
Omnidirectional Camera The application of omnidirectional cameras for robot
navigation was demonstrated by Gamallo et al. (2013). This work demonstrated
the advantage of being able to have a wide field of view to track multiple land-
marks, especially in sparse environments. This work used the FastSLAM algorithm
implemented on a real robot.
Distributed Vision System Another interesting use of cameras for robot nav-
igation was presented by Menegatti et al. (2008). In this work they used a dis-
tributed camera network to track the robot moving through the environment.
This novel approach has the potential to reduce the size complexity of the physi-
cal robot while maintaining good performance. The disadvantage of this approach
is the initial environmental setup required.
Magnetometer A low cost MEMS magnetometer is capable of measuring the

intensity of magnetic fields. The conventional use of this is for the purpose of
compassing. Compassing is the process of using Earth’s magnetic field to deter-
mine your current heading relative to magnetic north. If your location is known
the magnetic declination will allow this to be converted into a heading relative to
true north. To track north a minimum of two axis magnetometer is needed. These
magnetometers should be mounted orthogonal to each other and placed parallel
to the ground. In this way the heading can be computed as the arctangent of the
magnetic field of the two axes. With the addition of third axis the inclination
(dip) of the magnetic field can be measured. Although this does not add critical
information for tracking north, it can be useful for distinguishing interference from
north.
Unfortunately, indoor environments are notoriously bad for magnetic interference

and distinguishing north from these interferences can be challenging. Some tech-
niques utilize models of the expected north magnetic field in a given position on
the planet. This is typically referred to as the World Magnetic Model (WMM).
The WMM provides an estimate for what the magnetic field intensity, inclination,
and declination should be at every position on the planet. By comparing these
28
values to the measured values from the sensor, some forms of interference can be
quickly recognized and discarded.
One advantage of magnetometers is that low cost MEMS versions are available
and easily integrated over common interfaces such as I2C, although these low-cost
sensors do require calibration for bias and gain values on each axis. The most
critical of these being the Bias o↵set. This is a static o↵set that needs to be
corrected for on each sample. It is not consistent and must be recalibrated every
time the device is powered. The calibration on a robot could be accomplished in
the X-Y plane simply by having the robot rotate in place allowing measurements
to be taken for an entire circle around the robot. The center of this circle can
then be easily calculated by mapping the points of the circle to the least square
solution of a circle.
The more challenging component to correct would be the Z axis that moves per-
pendicular to the ground. If the robot is only capable of moving through two
dimensions and without any outside interference, this axis will only see a constant
component of the north’s magnetic field. One solution would be to use assumptions
from the WMM to resolve what the o↵set is for this axis.
Magnetometers have been used for more than simply compassing. They can also
be used for tracking and mapping the magnetic interference. This concept will be
discussed in more detail below.
Dead Reckoning Dead Reckoning is the process of determining where a robot

is positioned relative to a known prior location. Typically dead reckoning utilizes
sensors to track movements of the robot and these individual movements are in-
tegrated from the last known position to obtain on estimate of your new position.
The challenge with dead reckoning is that any errors in the sensors integrate, and
can rapidly accumulate (Slušný et al., 2009).
Kalman filtering is a common approach for dead reckoning. This filter allows the
noisy dead reckoning sensors to be fused together with other information such as
control signals and position updates from other sensors (Myung et al., 2010).
29
Accelerometer Accelerometers measure acceleration. One common application
of accelerometers is to measure the acceleration from gravity providing a reference
of where down is (Steinho↵ and Schiele, 2010). Typically this is less important in
robotics when the two dimensional assumption is being made about the environ-
ment. Accelerometers can also measure linear accelerations from movement. In
theory double integrating acceleration can provide displacement using
Z Z Z
D= V = A. (2.2)
In practice though, the act of double integrating also double integrates errors in
the accelerometer. Meaning the accelerometer must be running at an extremely
fast rate to capture all movement signals without aliasing and must be extremely
well calibrated. In practice low cost accelerometers typically cannot achieve this
but Accelerometers have still shown use for detecting when unexpected movements
occur and occasionally even for detecting patterns of movement and extrapolating
likely displacements.
One example of this is pedestrian dead reckoning. For pedestrian dead reckoning
the accelerometer can be used to detect steps made by the user and use models
of likely stride lengths to determine likely displacement distances. This has been
shown to contain fewer errors then doubly integrating accelerometer for low cost
accelerometers.
Gyroscope Gyroscopes measure angular rotation rate (Myung et al., 2010).

This can be used to track orientation changes of a device such as tracking the size
of a turn made by the robot. Similar to accelerometer the rate of rotation can be
integrated to determine the new angle relative to a previous heading using
Z t
✓t = ✓rate + ✓t 1 , (2.3)
t 1
where ✓t is the new heading, ✓rate is the turn rate measured by the gyroscope, and
✓t 1 is the previous heading.
30
Low cost gyroscopes also contain significant error requiring calibration. One sim-
ple method of calibrating a robot is to ensure the robot remains stationary, and
measure the baseline of the gyroscope while not moving. This baseline can then
be removed from all future measurements as a fixed bias o↵set error.
Encoder Wheel encoders track the rotation of the wheel. For a robot, this can be
an accurate method of tracking the distance the robot has moved. Many di↵erent
styles of wheel encoders exist that provide di↵erent resolutions. Fundamentally
wheel encoders su↵er the same errors as other dead reckoning sensors, and over
time small errors accumulate and integrate into large errors in position if not
properly corrected for.
Common sources of error for wheel encoders are inaccuracies in wheel dimensions,
errors in sensor unit that cause misses or repeat ticks, and inability to measure
wheel slippage.
2.2.2 Unconventional Sensors
As discussed one of the critical concepts of this research is the inclusion of new
data sources that when supported by machine learning techniques would allow the
system to gain insight on its position from previously ignored data sources. Some
novel approaches of unconventional methods of localization are discussed here.
SurroundSense An interesting application of unconventional sensors for local-

ization was demonstrated by Azizyan et al. (2009). This work demonstrated a
technique to fingerprint logical locations based on sensor values; specifically this
work used light, sound, and colour. The idea of this work was to recognize log-
ical location rather than the physical locations. This is a di↵erent problem in
localization, but can be useful especially when defining robot behaviours.
This work was also interesting in that it was implemented on a mobile phone not
a robot platform. This meant it had no control or direct sensing ability on the
31
motion of the platform. It was continuously updating the estimate of the current
logical location.
Capacitive Sensor Capacitive sensors have been shown capable of detecting

the location of floor joists through the floor (Miller and Gatos, 2006; Franklin
et al., 1978; Baxter, 1997). This technology is commonly used for finding the
position of studs in walls using a handheld unit, although this is transferable to
floor joists. A capacitive sensor can measure the relative density of a mjaterial in
close proximity and through a simple detection circuit can determine the presence
of a wall stud or floor joist.
Robot Whiskers Biomimicry is the study of using solutions found in nature

and applying these solutions to solve problems. A good example of biomimicry is
the research in using whiskers on robots (Fox et al., 2011; Pearson et al., 2013).
This allows the robot to feel obstacles nearby without hitting the object.
Perhaps a more interesting application of this work is demonstrated by analyzing

the spectral pattern of the whiskers moving against a surface (Evans et al., 2009;
Fox and Evans, 2012). This work demonstrates an ability to discriminate di↵erent
flooring types. This simple low cost technique could be a valuable tool in localizing
a robot in a specific area in a building as the flooring type changes. This could
also be helpful in handling robot behaviours, for example cleaning robots could
change there cleaning method when moving between di↵erent flooring types.
Ambient Magnetic Fields A magnetometer can be used for interference de-

tection rather than simple heading estimation (IndoorAtlas Ltd, 2012). This is
made simpler when used in conjunction with a gyroscope. Especially indoors mag-
netic interference is very common and if measured accurately this could provide
a valuable signature for the current location. Utilizing a full 3 axis magnetome-
ter would provide information about the angle of the interference as well as the
strength and any dynamic signature it may contain (for examples motors would
32
(b)
(a)
Figure 2.3: Infrared sectoring: (A) Demonstrating the mounting technique;

(B) Handling overlapping regions to create new sectors. (From Lee (2008))
contain a dynamic signature). Single axis Hall E↵ect sensors would provide a
much simpler output by just providing the downward component of the magnetic
field. By removing the downward component of the earth’s magnetic field the
interference value can be determined as
mI = mz sin(WI )WF , (2.4)
where mI is the interference value mz is the Magnetometer Z axis value, WI is

the world magnetic inclination, and WF the world magnetic field strength. The
inclination and field strength can be determined for a given location through the
World Magnetic Model (WMM). For example in Waterloo the inclination is ap-
proximately 70 degrees downward with a strength of 54 µT . The use of this sensor
would allow features to be extracted from metallic hardware, duct work, piping,
wiring, as well as metallic furniture.
Infrared Landmark A technique of using infrared landmarks for the purpose of

mobile robot navigation was demonstrated by Lee (2008). This approach works by
attaching infrared identifiers (IRID) to the ceiling. These IRID’s are each encoded
with a unique signal that can be detected and decoded by the robot. Using this,
the robot can determine what sector the robot is in. This work also handles
overlapping IRID signals by creating new sectors in these places, see Figure 2.3.
33
Power Line Positioning Power line positioning is another interesting area of
research (Patel et al., 2006). This approach places multiple devices into power
outlets at di↵erent locations in the building. These devices apply a non-harmful
signal on top of the power line signal, this can then be detected using mobile
devices that can filter out the relative component of each device to characterize a
unique fingerprint for that location.
2.2.3 Sensor Fusion
Sensor fusion is a category of information fusion that works to combine sensor

data to produce a di↵erent perspective then the individual sensors can produce on
their own (Mitchell, 2007). Di↵erent types of fusion exist; Fusion across sensors,
fusion across attributes, Fusion across domain, and fusion across time.
Fusion algorithms are also commonly categorized as either low-level fusion or high-
level fusion, where low-level fusion represents fusion done on the data directly,
taking advantage of specific details of the data. High-level fusion more abstractly
deals with merging the data more generally.
Sensor configurations can be classified in 3 categories; complementary, competi-

tive, and cooperative. Complementary configurations combine to produce a new
perspective that each sensor is unable to provide on its own. Competitive is when
two sensors measure the same property possibly in di↵erent ways allowing in-
creased confidence in the measurement. Cooperative sensor configuration is when
data from multiple sensors combine to create information that would not otherwise
be possible.
Examples of Sensor Fusion Di↵erent fusion techniques for use in robotic lo-
calization were investigated by Kam et al. (1997). This paper investigated both
high-level and low-level fusion to fuse IR, ultrasonic, and tri-aural sensors together
34
Figure 2.4: Three di↵erent sensors mounted on a robot. (From Kam et al.
(1997))
for real time SLAM, see robot in Figure 2.4. This paper also discusses the advan-
tages of centralized vs decentralized architectures, and investigates implementing
intelligent systems such as neural networks to do the fusion.
Fusion of di↵erent sensors that each provides a di↵erent perception of the environ-
ment were examined by Botelho et al. (2010). Their investigation used growing cell
structures that placed each sensor in its own map that were then fused together
to create a better understanding of the environment, see Figure 2.5.
Particle Filter Sensor Fusion It has been shown that the particle filter has
great potential for scalable sensor fusion (Hightower and Borriello, 2004). As men-
tioned earlier particle filters are extremely flexible and therefore easily adaptable
to di↵ering data sources from di↵erent sensors.
An example of this is the publication by Lee et al. (2009). They e↵ectively used
a particle filter to combine vision and range data for the purpose of mobile robot
localization. This provided redundancy in there feature extraction algorithms.
35
Figure 2.5: Sensor fusion between various sensor maps. (From Botelho et al.
(2010))
Neural Network Fusion Another application of sensor fusion using neural

networks was demonstrated by Koval et al. (2007). In this work they successfully
fused infrared sensors with ultrasonic sensors to develop a fused distance sensor
that contained the advantages of both. The concept of using a neural network
allows the system to include all the characteristics of both sensors during the
training phase, and therefore needs much less manual configuration.
2.3 Intelligent Systems
Intelligent systems are generally defined as a system capable of perception, rea-

soning, and learning (Karray and De-Silva, 2004). Much of the work investigated
in this thesis is an attempt to apply techniques from soft computing into the con-
ventional location algorithms. Soft computing is a field that deals in probability
36
rather than absolutes; it is use full when handling learning systems that must
adapt over time. Data is a critical component to successful intelligent systems.
Bayesian techniques aim to filter through large amounts of data and learn statis-
tical patterns within. The difficulty is that often the desired e↵ect in nature is
not directly observable by the available sensors. Information fusion aims to ap-
ply intelligent fusion algorithms to take information from many sensor streams to
determine the likely state of the unobserved variable (Ahmed and Pottie, 2005).
2.3.1 Bayesian Learning
Bayesian based approach to learning by performing sample based approaches and

predictions of a desired state space (Candy, 2009). Much of Bayesian learning is
based around the common Baye’s equation defined as
p(x)p(y|x)
p(x|y) = . (2.5)
p(y)
This equations allows one to derive the probability of x given y from the probability
of x alone, y alone and the probability of y given x. This filter is commonly
adapted, for example one can figure out the probability of the sensor given the map,
knowing the probability of the sensor scan itself, the map itself, and the probability
of the map given the sensor. Ramirez-Serrano et al. (2008) is a demonstration of
Bayesian techniques applied to robot localization.
2.3.2 Neural Networks
Artificial Neural Networks (ANN) is a technique used in machine learning that

uses networks of nodes referred to a perceptrons connected by weights to learn
behaviours of systems. The ANN mimics the biological neural network in nature
that is the principal of how the brain functions (Karray and De-Silva, 2004). The
advantage of this approach is that it is capable of solving complex patterns in a
very generic sense.
37
Figure 2.6: Perceptron block diagram. (From
http://www.codeproject.com/Articles/16419/AI-Neural-Network-for-
beginners-Part-of)
There are many varieties of configurations and adaption of neural networks through-
out research but this research willonly utilize the feed forward neural network,
trained with backpropagation. This is one of the simplest and most common
neural networks.
2.3.2.1 Perceptron
An ANN is composed of a network of nodes call perceptrons. These nodes are

simple computation blocks that take the weighted sum of their input, and apply
an activation function to calculate the result. These nodes can have any number of
inputs each with a value and a weight associated and provides one output (Karray
and De-Silva, 2004). Often a bias value is also used to learn a fixed o↵set; the
block diagram for the perceptron can be seen in Figure 2.6
The activation function is a nonlinear (or linear) mapping from the weighted sum of
inputs to the output value. Di↵erent activation functions can change the learning
behaviour of the network. Linear, sigmoid and step activation functions are among
the most popular mappings.
The linear function creates a direct mapping between the input and output values,
shown as
linear(xm ) = xm (2.6)
38
Figure 2.7: Feed-forward neurakl network topology. (From
http://www.hindawi.com/journals/aai/2011/686258/fig1/)
In contrast the sigmoid function creates a nonlinear continuous function
1
sigmoid(xm ) = (2.7)
1 + exp( xm )
which approaches the behaviour of the step function defined as
8
< 1 if xm > 0
step(xm ) = (2.8)
: 0 otherwise
2.3.2.2 Feed-Forward Neural Networks
In this configuration the system consist of a series of layers of perceptrons with an

input layer and an output layer and typically at least one hidden layer (Karray
and De-Silva, 2004). The flow moves through the network unidirectionally starting
from the input node, through each hidden layer in series and finally ending at the
output nodes. The connections between nodes have an associated weight value
that is actually what is learned within the network, an example of a feed-forward
network topology is shown in Figure 2.7.
The connections between one network are typically configured to be fully connected
where each n ode in one layer is configured to be connected to each node of the next
39
layer. Although it is possible to have a topology that is not fully connected, this
could o↵er improvements in computationally efficiency as there are less weights to
train but might reduce the system’s ability to learn.
2.3.2.3 Backpropagation Training
Backpropagation is the typical technique use for training the feed-forward neural
network weights. It is a supervised learning technique that takes a known pair of
inputs and expected outputs and utilizes the errors to train the neural network.
Backpropagation implemented in this work requires a configurable training rate as

well as a momentum value. The training weight configures how much the system
corrects for any given training sample and the momentum parameter encourages
faster corrections in the same direction by using deltas from the last error.
2.3.2.4 Prior Use of Neural Networks in Mobile Robotics
The use of Neural Networks for SLAM for the purpose of modeling sensor values
from the sensor frame into the navigation frame was demonstarted by Guanshan
(2010). This work showed promising results when applied to ultrasonic readings,
and provides some hope for extending this approach into other sensors.
Other work investigated the use of Neural Networks to transform occupancy grids
(Dam et al., 1993). This paper tries to resolve the problem of converting samples
from one occupancy grid to another, while resolving the problem of map transla-
tions and rotations. Using neural networks allows this to be resolved by trained
example.
Similar work was demonstrated by Gupta et al. (2010), in their application of

neural networks to occupancy grid maps. But in this work they trained a neural
network to convert values form the sensor frame into the occupancy grid map.This
is made particularly challenging in the use of ultrasonic sensors that are much more
difficult to model.
40
In another paper presented by Kim et al. (2003), neural networks were shown to
be e↵ective at resolving the non-linarites of sensors. Proper sensor calibration has
a direct e↵ect on system performance for most algorithms. This paper resolves
the complexity of modeling these calibrations by using a neural network that can
learn that solution by example. This paper uses an infrared sensors, that require
calibration models to handle the non-linear characteristics.
The application of neural networks for sensor fusion for autonomous outdoor nav-
igation was researched by Davis and Stentz (1995). This work proposed a similar
statement as this thesis that using a single sensor was extremely limiting in the
ability to perform localization. This work used di↵erent sensors as inputs to the
neural network and attempted to train logic behaviours.
2.3.3 Genetic Algorithms
Genetic algorithms often discuss the problem in terms of genes rather than states
(Karray and De-Silva, 2004). In this case the state is considered as the combination
of the robotics estimated position as well as all sensor maps. Initially two genetic
algorithm techniques will be experimented: mutation and crossover. Other items
could also be added to the gene such as unsolved parameters, or the robotics path.
Employing mutation will allow new areas of the map to be explored and help
correct for local-minimum error. This will be relatively trivial to implement for
changing the robot’s position but applying mutations to the sensor map will be
more challenging. Modifying the sensor maps directly makes less sense and is
conceptually much more of a challenge. One possibility might be too occasionally
remove a landmark within a map; this could help when a single bad landmark is
forcing the system to converge on the incorrect solution.
Crossover could help by sharing the sensor maps between particles, some particles
may have a better map for some sensors rather than others. Although when
the maps are being altered the weighting/fitness function will need to have some
representation of the quality of the map.
41
The utilization of a genetic algorithm for robot localization using ultrasonic sensors
was shown by Moreno et al. (2002). This paper tries to imitate nature to allow
the filter to adapt to the di↵erent environmental positions. This paper proposed
the complete algorithm and showed experimental results integrated into a larger
solution.
2.3.4 Particle Filters
Particle filters are comparable to genetic algorithms in that they use fitness func-
tions to classify the fitness of a set of proposed solutions (Candy, 2009). Particle
filters are simpler in that they typically don’t have crossover or mutation of the
states directly. Instead they do rely on resampling to converge on the correct
states. One of the largest advantages of particle filters is that they handle non-
linarites and non-Gaussian distributions much better than the Kalman filter.
The particle filter functions by distribution candidate solutions across the solu-
tion space. A fitness function tests each of these proposals and provides a weight
estimate for each proposed solution. Resampling then selects a new population
based on the weight functions, larger weighted particles are chosen more read-
ily. Propagation functions move each particle according to known behaviour with
randomness applied proportional to the system noise.
The particle filter used for robot localization contains 3 basic states, Figure 2.8.
The first state is propagation; this moves the robot forward according to the control
input adding noise for the uncertainty in the control estimate. The system then
uses the available sensors and compares the values with the maps of each particle
to develop a weight estimate. These weight estimates are then used to resample
and select the next particle population. Typically the best estimate is taken as
where the particles are most dense.
42
Robot Moves
Resampling Propagation
Update
Figure 2.8: Particle filter state diagram.
2.3.4.1 Applications of Particle Filters
The particle filter is the core component of the FastSLAM algorithm, and so is
commonly used in robotic localization and mapping. This section will briefly
discuss some other studies of particle filters used in robotic localization.
An in-depth case study of particle filters for robot localization was done by High-
tower and Borriello (2004). They investigated di↵erent methods of solving the
density problem, comparing both performance and accuracy. They note some
of the main advantages of particle filters being their flexibility, practicality and
accuracy.
The flexibility of particle filters is demonstrated in the paper presented by Lee

(2006). They modified a recursive particle filter to include geometric constraints
of landmarks. This paper argued the benefit in geometric constraints in improving
map stability.
An improved particle filter resampling technique for distributed particle filters

was developed by Bolic et al. (2005). This paper also discusses the potential for
43
FPGA’s to o↵er significant processing improvements during resampling through
parallelism.
Particle filters for visual tracking was investigated by Perez et al. (2004). The
interesting contribution of there approach was that they employed the particle
filters in a data fusion capacity to fuse di↵erent data together. In their work they
fused three di↵erent visual cues together; color, motion, and sound. This again
emphasizes the flexibility of particle filters in handling di↵ering data streams.
2.3.4.2 Rao-Blackwellized Particle Filters
A Rao-Blackwellized particle filter is an optimization on the typical particle filter

(Grisetti et al., 2007). It works by factoring the robot localization probability
solution into
p(X(1 : t), M |z(1 : t), u(1 : t 1)) (2.9)
where X is the robot’s position, t is the time, M is the map, z is the sensor
samples, and u is the control signals. This equation asks the question what is the
likely map and robot path given the sensor value and control signals. This can be
factored into
p(X(1 : t), M |z(1 : t), u(1 : t 1)) =
p(M |x(1 : t), u(1 : t 1)) ⇥ p(X(1 : t)|z(1 : t), u(1 : t 1)). (2.10)
This factorization allows the map to be decoupled from the path, and in this
way the particle filter can be applied to resolving the path. This is ideal since
the robots position is a relatively small state and is deterministic, where the map
grows depending on the environment. The map can they be solved with a separate
filter treating the location as known. Commonly an EKF is used for this, this is
the principle behind FastSLAM.
44
Chapter 3
Experimental Setup
The complexity of researching multi-sensor SLAM imposed developing an entire

framework to study robotic localization in a controlled environment. The frame-
work was implemented in Matlab as it enabled rapid development of algorithms
and simplified the problems of visualization. The challenge was to create an ar-
chitecture within Matlab that enabled rapid experimentation with both real world
and simulated data across a multitude of algorithms. The focus of this chapter
will be to explain the core components that were developed for this research and
the purpose of the design decisions that were made. E↵orts are underway to open-
source this framework for other researchers in the field. The core API and sample
code is shown in the Appendix B.
3.1 Matlab Architecture
It was important to develop a framework that abstracted each algorithm and the
input data, as the research demanded multiple sensor configurations and algo-
rithms be tested. The framework that was developed was built in Matlab and has
4 main components:
1. Main Loop: The main loop is where the test is configured and run. It
selects which sensors are enabled and what control signals to give to the
45
robot. It is the plumbing of the system that passes the data between the
other three modules.
2. Data Input: The data input module is responsible for controlling the robot
based on the control signals received and for returning the sensor data from
the robot. The input system has been implemented such that the source
of the data can be from a plurality of orgins. The typical input sources
for this project were: simulated data, real data from a robot controlled
over Bluetooth, or logs from a previous execution. The important thing
to recognize is since the input source has been abstracted out switching
between these three very di↵erent data sources is as simple as changing the
configuration definition in the main loop.
3. Algorithms: The core of the research resides in the algorithm block. The
common algorithm module enforces that all algorithms be given the same
interface so they can be compared against each other. The algorithms are
self contained classes that contain their own required context information
and state, which allows multiple algorithms to be run in parallel.
4. Output Visualization: The final block abstracts the many di↵erent visu-
alizations that will be needed. For this a helper class was created that allows
a central way of choosing which plots to enable and at what frequency.
3.1.1 Input Abstraction
One core design criteria was to abstract the input from the algorithms being run.
Three main data sources were considered; a Real-time robot, Simulator, and Logs.
Real-time data implements the possibility of connecting an actual robot to the
algorithm in real-time to test the system with real data. The simulator allowed
for quick development with controlled data in a reproducible environment. Saved
logs allowed replaying of previous trials after tuning the algorithm. The framework
of this allowed switching of the input source by simply instantiating the data source
object in the configuration code.
46
Each input class must support 3 key operations; Drive(distance), Turn(angle), and
Scan. These operations can be accomplished in each of their own ways but they
must have the same interface. The standardized interface serves to e↵ectively ab-
stract the input source from the algorithms. Both the turn and drive functions are
designed such that they return the actual distance/angle and the calling parameter
is the requested distance/angle; which is useful to handel collisions. If a collision
occurs the rest of the algorithm is not required to work with incorrect control
information as it uses the actual data not the requested. Logs are then simply
implemented by always ignoring the requested command, and just returning the
next operation in the log.
Real-time Robot A simple robot was developed to understand the realworld

difficulties involved in mobile robotics. The details of the robot are further ex-
plained in section 3.2. The complexity of the algorithms meant that implementing
the wide range of sensors was beyond the scope for most experiments in the re-
search.
The robot is a simple Arduino based robot that communicates over Bluetooth to
the host computer. The host computer running Matlab uses the control interface
to send the commands to the robot to trigger and receive sensor scan results.
The Arduino that was used is shown in figure 3.1. Utilizing an Arduino encouraged
rapid development of the sensors, as most sensors have drivers pre-developed for
the arduino platform and the framework has many common interfaces; such as
I2C, SPI, AtoD. As well the Arduino has interrupt support and support for pulse
width counting. The cost of this development board was around $30, wich also
facilated the criteria of reducing cost.
Simulator The simulator keeps track of a true robot position as well as the true
maps for all sensor frames. The simulator then tries to replicate realistic real world
robot control behaviors and sensor scans. The advantage of the simulator is that
noise values and behaviors can be easily tuned and tested to test the robustness of
47
Figure 3.1: Arduino microcontroller used in this research. (From
www.robotshop.ca)
the system. The design also proved invaluable in the simplicity of bringing up new
sensors and to truly test the algorithms ability to handle a wide range of sensor
types and values.
The simulator was also designed in an abstracted object oriented fashion. Each
sensor is a new object but can inherit its behaviors from similar sensor types. The
simulator then contains a list of active sensor objects and when requested collects
a scan from each of those sensors. Each sensor object maintains its own map of
the world.
The simulator also allows controlling the seed value, this allows simple repeatabil-
ity of the experiments, while simultaneously maintaining the randomness required
for accurate testing. The framework would also support the simulator logging the
runs for playback with the log module.
48
Logs The third type of input is logs, and is the simplest of the three. The ability
to quickly replay logs allows for quick and easy debugging without requiring time
spent trying to reproduce specific corner cases. The log module is simply a file
parser that reads the operations and sends them to the algorithm. As mentioned
earlier the design of the input interface allows the logs control methods to return
the movement within the log regardless of the actual requested movements. The
requested movements sent by the calling functions are completely ignored.
3.1.2 Algorithm Abstraction
Analogous to the simulator, the algorithms have been abstracted from the rest
of the system; which allows many algorithms to be run concurrently for accurate
comparisons. Furthermore this enables rapid development and testing by enforcing
standardized interfaces. The efficiency of this approach is demonstrated by the
speed it allowed the two conventional algorithms, FastSLAM and EKF-SLAM, to
be developed. Which enabled a rapid comparison in a controlled environment, and
allowed deciphering of the strengths and weaknesses of both.
Each algorithm supported the same interface function: Step( control signal, sen-
sor data ). Where control signal is the control signals for how far the robot drove
and turned. Sensor data is a list of all the sensor scans; it is an array of structures,
which each contains the sensor type, timestamp, and data.
The algorithm layer was designed in much the same way the simulator was de-
signed. Each sensor type contains its own algorithm for interpreting the sensor
data and building a sensor map for that frame as shown in the sensor algorithm
UML diagram, Figure 3.2. The nature of the sensors often allows for some hier-
archical design paradigm to simplify development e↵ort. For example IR scanner
and ultrasonic scanner both run essentially the same algorithm with just some
di↵erent tuning behaviors.
49
Figure 3.2: Sensor algorithm UML diagram.
3.1.3 Output Abstraction
The final layer is an output abstraction layer. Instead of having plots and outputs
littered throughout the code, a controlled module handles everything. This allows
simple turning on and o↵ of the di↵erent visualizations to increase speed and al-
low rapid understanding of the behavior being studied. The output module also
contains all the output data from all other components, which allows easy imple-
mentation of interesting plots that contain information from di↵erent sections of
50
the code. Without this centralized architecture this would be much more complex
to implement.
The output module uses the singleton design paradigm. There exists only one
output class instance, in which every other class must contain a reference of.
Every time some data that could be outputted exist the relevent code simply calls
out.addData(type,data). Where type is a string representing the type of data such
as ”robotTruePosition”, the data is what will be visualized for that type. In this
way the various algorithms and simulators are abstracted from requiring code that
handles plotting data.
Another class type called Visualizations handles all the plots. A visualization is
a single plot behavior, it contains two main functions Draw() and newData(type,
data). The Output class keeps a list of all active visualizations every time addData
is called it calls each of the active visualizations. If the visualization requires
the new data type, it storesthe data in a local variable. When the main loop
calls out.draw() the output module calls the draw method of each of the active
visualizations. These visualization modules can then plot the new data.
Another advantage of centralizing the output behavior is that with a simple param-
eter change the location of the output can be modified. Four modes are currently
supported; real-time, file, movie, last-only. In real-time the plots are streamed to
the screen this is the best mode for development but is also the slowest. Some
throttling can still be done to improve speed such as skipping some visualizetions.
In File mode the images are not displayed to the screen but put in a file, logic has
been added so that only a certain part of the scan is put in the file and not every
sample. This is extremely valuable for report writing, since the system can create
all the plots for a given test and put them to a file. If a bug is later found the
test is just re-run in this mode to fix all the plots and each one doesn’t need to
be manually re-created. The third mode is movie this takes the stream of figures
and uses them as frames for a movie, the movie can then be played back after the
test multiple times much faster than having to rerun the complete test. It is also
51
Figure 3.3: Output class definition.
useful for re-examining specific parts of the test. Last-Only is the fastest and only
shows the plots of the final state, after the entire system is completed the test.
The other behaviour that’s handled by the output module is print logging. Cen-
tralizing the logginging behaviour allows added functionality such as hyperlinks
to the line within the module the print occurred from. As well as a simple inter-
face for changing the verbosity of prints from certain modules, and colouring more
critical prints and errors.
3.2 Robot Platform
Although the final results were presented from the simulator, a robot platform
was created to better understand the physical challenges in robot localization and
mapping. One of the core goals of the robot platform was to enable a scalable sys-
tem for quickly adding new sensors and incorporating that data into the solution.
The complete robot is shown in Figure 3.4
Hardware The platform was built on a commercially available robot frame. The
frame provided 4 wheels, each with their own electric motor. The multi-tier design
of the robot platform had many mounting points making it ideal for scalability.
The robot frame is shown in Figure 3.5
52
Figure 3.4: Complete robot platform used in research.
Figure 3.5: Robot frame used in research. (From www.robotshop.ca)
53
Figure 3.6: Robot schematic for core modules on robot platform.
The schematic of the system is shown in Figure 3.6. This is a simplified schematic
showing the core sensing components and communication. The complete robot
has 5 main components; the controller, drive system, sensors, communication, and
power.
Microcontroller The robot platform wass designed with a simple onboard mi-
crocontroller, with real-time communication to another more powerful control unit
that handles the larger control problems. The chosen controller was an Arduino
microprocessor. The Arduino was easy to integrate and had a functional c++
compiler. The board chosen also had plenty of IO for sensors, as well as a USB
controller that enabled programming and serial debugging. Another advantage
of the Arduino was its popularity, most drivers had already been written for the
Arduino platform; allowing sensors to be rapidly incorporated. The Arduino also
has a relatively low cost of around $30.
54
The Arduino is a family of boards that each contains an Atmel processor. The
specific board chosen was the Roboduino, predominately because of its dual motor
output. These outputs contain an H-Bridge on the board that allows for a sec-
ondary power source to control the motors, which will reduce the risk of brown-outs
while driving.
Drive System The drive system of the robot must be capable of controlling the
4 separate motors mounted to each of the robot wheels. The chosen platform’s
wheels remain parallel to the robot frame and don’t steer in the conventional way.
Instead the robot reverses direction of one side of the wheels to perform the turn.
This method reduces the complexity but makes it more difficult to track the turn
angle.
Sensors The Arduino platform allows easy integration of sensors which is critical
for this research. Most sensors already have working drivers for the Arduino, which
expediates development. As well, the Arduino has many interfaces which can be
used. Including SPI, I2C, A2D, PWM, Interrupts, Serial, and many Digital IO
pins.
The various input interfaces made mounting the numerous sensors quick and easy.
Furthermore, the Arduino chosen also included servo outputs, which allowed sen-
sors to be easily mounted to an actuator to perform scans of the environment.
Communication The robot is controlled through an external computer that

sends the commands, and collects the sensor data. Communication with the ex-
ternal computer is accomplished with Bluetooth serial profile. The Arduino has
many breakout boards that provide Bluetooth connectivity, and virtually any that
support the serial profile would work. The BlueSmirf module was chosen because
of its low cost, simplicity of integration, and long range. The BlueSmirf module is
shown in figure 3.7.
55
Figure 3.7: BlueSmirf Bluetooth module used in this research. (From
http://www.abra-electronics.com)
Power System A mobile robot requires a portable power source to operate.

The power source chosen for the robot is a 7.2 Volt lithium rechargeable battery.
The power circuit also contains a separate power isolator for the Motors and
microprocessor. Isolating the power lines helps reduce the risk of brown-outs
while the motors are in operation.
3.3 Simulator Configuration
The experimental test environment was designed to be scalable and support a va-
riety of experiments and simulations. For this research the test environment was
standardised to help in comparisons and avoid confusion. The details of the con-
figuration are presenbted in this section to provide context for the all the results.
Unless otherwise specified the tests used these sensor and map configurations.
The simulator itself does not have any idea of units as everything is just relative.
For these experiments a grid size was chosen as 1 inch, and all results can be
intrepreted in this unit.
56
3.3.1 Robot
The simulated robot was closely modeled from the physical robot that was built.
Approximate dimensions are 6 inches by 6 inches, although the algorithms used in
this work do not require knowledge of the size of the robot and therefore should
scale to other robot platforms.
The robot platform contains a wheel encoder for tracking movement distances.
The combination of the wheel encoder and wheel slippage results in a gaussian
error with a standard deviation of 1 inch for every movement interval. Standard
movement interval is 5 inches.
To track turns the robot platform uses a gyroscope, which can enable accurate
measurements of the relative angles. Over short durations this can be very ac-
curate, but accumulates error over time. The simulation was configured with a
gaussian error applied at every step with a gaussian error of 0.005 radians per step
interval.
3.3.2 Sensor Model
Three sensors were implemented in the simulator for this research; distance sensor,
joist sensor, and a light beacon sensor. The distance sensor is reprosentative of
the conventional approach, the joist sensor is a new sensor proposed for robot
lcoalization, and the light beacon sensor was implemented to demonstrate the
scalibility and flexibility of the localization algorithm. This section will define
these three sensors and how they were modeled for the experiments.
The layout of the sensors are shown in Figure 3.8. This figure shows the basic
model used for this research, the sensors are all located within a 1 inch square
which also coresponds to the grid square used in the simulations. In this way the
robot is shown with reference to the 1 inch square sensing block located at the
center of the robot. Robot collisions are not covered in this research and avoided
through design of the robot path. In this configuration the joist sensor is located at
57
Figure 3.8: Basic sensor positions on robot (units in inches).
the bottom of the robot moving against the floor. The other sensors are mounted
to a 180 degree servo at the top of the robot.
Distance Sensor The distance sensor was modeled to resemble the character-
istics of an infrared distance sensor. The model ignores calibration and environ-
mental interference related to infrared sensors but models the performance of the
sensor. The sensor was modeled with gaussian noise of a standard deviation of
1.0 inch and scans the environment at 1.0 degree intervals. There is no minimum
58
distance programmed into the sensor and the maximum range was programmed
to 100 inches. The sensor is located at the front of the robot with a field of view
of 180 degrees.
Joist Sensor The joist sensor is a new sensor proposed for robot localization,
this work investigates the potential advantage of including a sensor on the robot
platform. The joist sensor in the experiments is modeled as the capacitive stud
finders. It is capable of measuring directly under the robot the presense of a floor
joist. The work investigating the sensor itself experiments with adding noise while
learning the floor joist model, but other experiments assume an ideal joist sensor
that is able to accurately measure if a floor joist is located under the robot without
failure.
Light Direction Sensor The light direction sensor was designed as a simple
sensor to detect the direction of a light beacon. This sensor is mounted on a 360
degree servo motor with a light filter tuned to detect the light being emmited
from the light source of the beacon. Using this the robot can always know the
direction of a single beacon. Which is not enough to get a full position but could
agument other sensor information to improve the location estimate. This sensor
is located at the very top of the robot allowing clear line of sight. This sensor
was programmed to have gaussian noise with a standard deviation of 0.1 radians.
Maximum range for for light sensor is set to 100 inches.
3.3.3 Map
The map can be configured in many di↵erent ways but to simplify the work unless
otherwise specified this research uses one map configuration. The configuration
was chosen to reduce the complexity but also ensure a non-symetrical environment.
Figure 3.9, shows a map with all sensor landmarks overlayed. This map can be
referenced in other sections to understand the features being sensed by the robot.
59
Figure 3.9: Default map configuration used in the research (grid unit = 1
inch).
The map contains a path that the robot follows, this avoids the complexity of
requiring collision detection if the robot was ismply moving randomly throughout
the environment. Di↵erent paths were tested but all use the same map environ-
ment, unless otherwise stated.
60
Chapter 4
Floor Joist Sensors for Robot

Localization
This chapter will explore the use of an unconventional sensor for robotic localiza-
tion. The intent is to demonstrate one example of a currently unused environmen-
tal feature that could be employed to improve the robot’s location. The results
from these experiments will be used to help inspire the sensor fusion concepts
discussed in later chapters. The work presented here examines a unique method
of utilizing capacitive sensor data to train a neural network, and develop a model
of the capacitive environment of the device, for the purpose of robot localization
and mapping.
The approach takes advantage of the fact that typical indoor environments utilize
similar and predictable construction techniques. The capacitive sensor can be used
to measure the presence of floor joists under the robot. The locations of these floor
joists are often repeatable and predictable.
A neural network was selected to allow for greater flexibility in handling a wide
range of floor joist layouts and building constructions without requiring hard con-
straints on the exact build layout. As well, the neural network handles noisy data
e↵ectively and still converges on the solution.
61
It was also desirable to develop a sensor algorithm that was much more intelligence
based, rather than containing a predictable model. Fusion of a predictable model
can be done at a much lower level, but requires specific hooks to be developed for
each sensor algorithm. The intent of this research is to demonstrate an approach
that is not as tethered to the specific algorithm governing the sensors. Which
should allow the fusion algorithm greater potential to scale with other sensors.
One can better demonstrate the capability of the fusion system by incorporating
an algorithm as complex as a neural network. For example, microphones could
use neural network based classifiers to develop an environment based signature.
4.1 Experimental Simulation Model
4 A simplified version of the Matlab test system was developed to test the e↵ect
of the capacitive training in isolation from the rest of the system. The simplified
version uses the same core components for simulating the data, but ignores the
algorithm components. Instead this system bu↵ers all the input data for use with
the neural network training.
The system divides the input data into three data sets; training data, validation
data, and test data. Training data is used to train the neural network. The system
should rapidly learn to reduce the error in this data set. Validation data is not
used for training, but is used by the system to monitor the ability of the network
to abstract the general solution from the training data. Finally, the test data is
not used at all by the network and is only used to test the final behaviour against
a never examined data set.
The test data was created by developing a model for the floor joist in a room.
The model is defined as a function of joist separation(Js ), joist width(Jw ), joist
distance o↵set(Jd ), and joist angle o↵set(J✓ ). The algorithm is shown in Algorithm
3, where x and y are the robots position and ✓, of f set, spacing, width defines the
joist model.
62
Figure 4.1: Sample of joist layout in a house. (From
http://www.icreatables.com/framing/floor-build-house.html)
Algorithm 3 Joist Algorithm

1: function is joist(x, y)
2: if mod(x⇥sin(J✓ ) + y⇥cos(J✓ ) + Jd , Js + Jw ) < Jw then
return 1
3: else
return 0
4: end if
5: end function
Sensor noise can be added as Gaussian white noise to simulate sensor inaccuracies.
The noise can be controlled by adjusting the standard deviation and magnitude
of the noise, which allows the training model to be better tested. Position errors
can also be included to test a more realistic behaviour.
4.2 Neural Network Design
The input data is the x and y position where the joist sensor measurement was
taken. The output is the likelihood of a joist being at that location, where 1 is the
presences of a joist and -1 is no joist. Therefore an output of 0 indicates complete
uncertenty about the joist being present.
63
Figure 4.2: Proposed neural network topology.
The neural network was configured with 50 hidden nodes using a transfer function
of tan-sigmoid. The output node used a pure linear transfer function.
Di↵erent encoding schemes can be used to input x, y position into the neural
network. Three di↵erent approaches were tested and will be compared in detail;
continuous encoding, binary encoding, and bin encoding. The basic topology for
the feed-forward neural network used, is shown in Figure 4.2.
Continuous Encoding The first encoding method attempted was continuous

value encoding. This encoding used the x, y input values directly; resulting in a 2
input 1 output topology. Given the small size of this, it could potential be much
more efficient than other encoding methods. The numbers are still normalized to
keep the values between 1 and 0. The concern of this approach is that neural
networks typically treat each value as a continuous quantity and typically is less
able of finding subtle patterns within the input variable itself.
Binary Encoding The second encoding scheme that was used, converted the
locations into binary. This approach decomposes the continuous number into bins.
The intent is, that this would help the network find the relationship and patterns
64
within the location data. The process of encoding into binary also impose bound-
ary conditions on the values and could reduce the precision through rounding.
Bins The final encoding scheme that was tested was binning. This encoding
scheme takes all values within a specified range and places them into one bin. For
example, given a map 100 x 100 and a bin size of 1, there would be 200 inputs.
This encoding scheme has potential for better ability to distinguish the patterns
but at the cost of many more inputs; resulting in greater complexity.
4.3 Results
The three di↵erent input encoding approaches were compared using a variety of
tests, to determine each approaches benefits and weaknesses. The first test will
examine basic input data to ensure each encoding scheme is capable of converging
on a solution with good data. After that, more complex experiments will examine
di↵erent maps and di↵erent types of input data. Finally, more realistic data will
be examined to predict the algorithms feasibility when used in the complete fusion
system.
Most of the result plots in this chapter are shown as two figures; the first is a
recreation of the map the system has learned, the second is a plot of the learning
curve. The recreation of the map is created by feeding the coordinates of every
location into the map and plotting the output. The learning behaviour of the
system contain 4 pieces of information; the number of epochs for training, the
error on training data, the error on testing data, and the error on validation data.
All of these are shown against the epoch number on the x axis; which allows the
leaning behaviour to be visualized.
65
Figure 4.3: Scatter plot of capacitance values. Red are over joist, Blue have
no joist, Green are unexplored.
4.3.1 Preliminary Encoding Comparison
The first test examines the ability of the system to learn the model with perfect
data and 2000 samples randomly distributed within the map. The capacitance
values are shown in a scatter plot in Figure 4.3. In this figure peaks up in red are
joist measurements, peaks down in blue are no joist measurements, and all green
points are spots where no measurement was taken.
A neural network performance is drastically a↵ected by the encoding of the input

nodes. It is therfore expected that these three di↵erent approaches will have
di↵erent behaviours and characteristics.
The true map used in this test is shown in Figure 4.4. This map is representative
of a typical household joist layout, with joist separations of 16, and joist width of
2, making the grid size approximately 1 inch.
Continuous Encoding The continuous encoding method is the simplest tech-

nique. It requires 2 inputs; one for x and one for y. The input values were
normalized between 0 and 1.
66
Figure 4.4: Complete test map. Red are joists, Blue are no joist.
See figure 4.5 for the training error plot. It is clear from the predicted map
plot that the continuous encoding method does not learn the pattern. This is
not completely unexpected, as neural networks often find patterns easier between
inputs and not within a given input node.
Binary Encoding Binary encoding converts the input value into its binary
representation. The hope is that by decomposing the values, the system will more
readily determine the patterns. For this map, 7 bits are needed to represent the
map size of 100 x and another 7 bits for y, resulting in a total of 14 input nodes.
Therfore this encoding scheme results in a loss of precision, as the resolution of
the input is 1, which means all inputs are rounded to perform the encoding to
binary. See Figure 4.6 for the results.
The predicted map shows that the map was recreated near flawlessly, which is what
is expected when the system is using perfect data. The training plot shows that
it continued to run for 1000 epochs, which was the maximum for this experiment.
The majority of the error was removed after about 20 epochs, but the system
continues to train as long as the error in validation data is being reduced.
67
(a)
(b)
Figure 4.5: Neural network training using continuous encoding: (A) Predicted
map; (B) Training plot of neural network.
68
(a)
(b)
Figure 4.6: Neural network training error using binary encoding: (A) Pre-
dicted map; (B) Training plot of neural network.
69
Bin Encoding Bin encoding places all values in bins. In this experiment, the
input was configured in bin sizes of 1 unit. This can be easily accomplished by
rounding the x and y values. Which results in 200 input nodes; 100 for x and 100
for y. The results of this encoding are shown in Figure 4.7.
Similar to the binary encoding, the system successfully reproduces the map. The
error is also reduced within fewer training epochs then the binary encoding. Al-
though, given the much larger input size each training epoch did take longer with
this encoding scheme.
4.3.2 Encoding Comparison with Di↵erent Maps
The preliminary results demonstrated that each encoding scheme was capable of
converging on the correct solution when provided perfect data. From these results
it was clear that the continuous encoding scheme was unable to converge on the
solution and was therefore removed from any further experiments.
Another consideration was testing di↵erent models to ensure di↵erent joist spacing
would not have an e↵ect on the ability of the neural network to learn. Two
configurations were tested; first a tighter map with joists closer together, second,
a rotated map with the joists on angles. Similar to the previous section, these
experiments will again use 2000 randomly distributed perfect data points.
4.3.2.1 Tighter Spaced Map
The map model was modified to ensure the systems were capable of learning a
di↵erent joist model and not only the original map tested in the previous experi-
ments. One important reason for this test, was that the first model, with spacing
of 16 and joist width of 2, are both powers of two and easily represented in bi-
nary. Which could indicate they would be easier for the binary encoding scheme
to learn. The next experiment tested the binary and bin encoding schemes with
a model defined by joist separation of 9 and width of 1, shown in Figure 4.8.
70
(a)
(b)
Figure 4.7: Neural network training error using bin encoding: (A) Predicted
map; (B) Training plot of neural network.
71
Figure 4.8: Scatter plot of capacitance values. High points are over joist, low
points have no joist, middle values are unexplored.
Binary Encoding As mentioned, the tighter spaced map helps determine if

the binary encoding scheme is able to generalize to di↵erent maps. The results
for the binary encoding can be seen in Figure 4.9. This data shows that despite
the di↵erent spacing (that is not powers of two), the neural network successfully
converged on the map.
Bin Encoding The Bin encoding was also tested, it was expected that this
would have no problem learning the model. Figure 4.10 shows the results of the
test. The system had no difficulty converging on the correct solution.
4.3.2.2 Rotated Map
The next experiment examined the ability of these systems to learn a map that
was rotated relative to the robots frame of reference, see Figure 4.11. This required
the network to learn much more complex relationships between the nodes. In this
case, a map rotated by 45 degrees was tested. The spacing was configured to be
12 and a joist width of 2.
72
(a)
(b)
Figure 4.9: Neural network training error using binary encoding on a tighter
spaced map: (A) Predicted map; (B) Training plot of neural network.
73
(a)
(b)
Figure 4.10: Neural network training error using bin encoding on a tighter
spaced map: (A) Predicted map; (B) Training plot of neural network.
74
Figure 4.11: Scatter plot of capacitance values. High points are over joist,
low points have no joist, middle values are unexplored.
Binary Encoding Consecutive runs of the binary test resulted in very di↵erent
results. The best of these results are shown in Figure 4.12. This shows that the
system is capable of learning the general pattern of the map. There are gaps in the
stud lines, but these would likely be correct with more training data; especially
data in those areas. It is worth recognizing, that despite some ability at recognizing
the general pattern of the joist locations, this was with perfect data and it is likely
that this will become much worse when noise is added.
Di↵erent results for this same test are shown in Figure 4.13. Although, the gen-
eral pattern of the joist is observable in the data, it is not as prominent. Given
more data, the system could eventually converge, but this also demonstrates the
challenge of neural networks not always converging on the same solution.
Bin encoding The results for testing bin encoding with rotated maps are shown
in Figure 4.14. These results showed poor performance in converging on the solu-
tion. Consecutive runs on the same test yielded similar results, with the network
being unable to learn the more general pattern of where the joists were located.
75
(a)
(b)
Figure 4.12: Neural network training error using binary encoding on rotated
map: (A) Predicted map; (B) Training plot of neural network.
76
(a)
(b)
Figure 4.13: Di↵erent Results of neural network training error using binary
encoding on rotated map: (A) Predicted map; (B) Training plot of neural net-
work.
77
(a)
(b)
Figure 4.14: Neural network training error using bin encoding on rotated
map: (A) Predicted map; (B) Training plot of neural network.
78
Figure 4.15: Scatter plot of capacitance values. Red points are over joist, low
points have no joist, middle values are unexplored.
4.3.3 Testing Realistic Scenarios
After testing di↵erent map behaviours, the next step was to experiment with more
realistic behaviours. The first test explored data collected during a robot naviga-
tion of an environment, instead of randomly distributed data points. The following
two tests added noise for the position data and sensor data. The results of these
experiments helped predict the feasibility of this approach in real environments.
4.3.3.1 Robot Navigation Behavior
Using this system for robot SLAM requires that the data used for training is
received in a path and not randomly distributed. This was tested with both the
binary and bin encoding scheme. The path is shown in Figure 4.15. There is no
noise in this test on either the position or sensor values.
The joist model for this test is the same model used in the preliminary experiments,
a joist separation of 16 and joist width of 2, with no rotation. The challenge of
this experiment is that the test points are not very dispersed.
79
Binary Encoding The results for this experiment using binary encoding is
shown in Figure 4.16. The clear vertical lines demonstrate some insight into the
location of the floor joist, although, lack of samples around the right most joist
resulted in a model of the joist much wider than reality. The system also contains
a fair amount of noise in the estimates.
Bin Encoding The results for bin encoding is shown in Figure 4.17. This shows
a much more pronounced indication of where the joists are located. It is also no-
ticeable that only joists that were crossed in the training data are shown. Resulting
in the left most joist missing in the predicted map.
4.3.3.2 Robustness to Position Errors
Given the results of the previous test, the next two experiments will explore bin
encoding only. The first experiment will explore the robustness of the solution
to position errors. Robustness to position errors is important, as the position
estimate retrieved from a SLAM solution will have lots of error in the location.
For this test, Gaussian noise with a standard deviation of 1 was applied to both the
x and y position values. No noise was applied to the sensor values. The samples
are again following the same robot path on the same map used in the previous
experiment. The training data for this experiment is shown in Figure 4.18.
The results for this experiment using the bin encoding is shown in Figure 4.19.
The vertical bands show some knowledge of the joist positions. The thick band
on the right is where more samples were provided and insinuates that with more
training, this could improve throughout the map. The Error plot shows limited
convergence and still a lot of residual error.
4.3.3.3 Robustness to Sensor Errors
This experiment will test the systems robustness to noise in the capacitance values.
This experiment will add Gaussian noise with a standard deviation of 0.5 to the
80
(a)
(b)
Figure 4.16: Neural network training error with binary encoding using robot
path behavior: (A) Predicted map; (B) Training plot of neural network.
81
(a)
(b)
Figure 4.17: Neural network training error with bin encoding using robot
path behavior: (A) Predicted map; (B) Training plot of neural network.
82
Figure 4.18: Scatter plot of capacitance values with noisy position. Red points
are over joist, Blue points have no joist, and Green values are unexplored.
sensor values to indicate a confidence level in the samples. The test data for this
experiment is shown in Figure 4.20.
The results of this experiment using bin encoding is shown in Figure 4.21. The
dark vertical bands show some knowledge of the location of the floor joists. The
error plot shows little convergence, but would likely improve with more samples.
4.3.4 One-Dimensional Encoding
A final test was derived from the lessons learned from the previous experiments.
First, it was observed that the solution does not handle rotated maps as nicely as
vertical maps. As well horizontal lines occur as a result of the system trying to
resolve noise in the input data. A new encoding method will reduce the problem
into 1 dimension ignoring the y value.
Removing the y dimension could be made possible by using basic line tracking
techniques. When the robot reaches the first joist it could follow the joist briefly
to determine the direction the joist is traveling. The capacitive map would then
83
(a)
(b)
Figure 4.19: Neural network training error using continuous encoding: (A)
Predicted map; (B) Training plot of neural network.
84
Figure 4.20: Scatter plot of noisy capacitance values. red points are over joist,
blue points have no joist, and green values are unexplored.
have a fixed rotation applied, allowing a transformation between the robot’s map
frame and the capacitive map frame.
This final test will explore this method using bin encoding. The map chosen was
the same as the first map, with joist width of 2 and separation of 16. The training
samples were selected along the robot’s path to test against realistic training data.
As well position errors have been added with a standard deviation of 0.5 and sensor
error with standard deviation of 0.2. The training data is shown in Figure 4.22.
The results of this final experiment are shown in Figure 4.23. These results show
good indication of the location of the 3 right most joists and some indication of
the middle left joist. There is no indication of the left most joist. Which is a result
of the robot path not crossing that joist, and therefore having no data indicating
its location. The middle left joist was only crossed once and therefore only has
two samples indicating its position. It is likely that with more data covering other
areas of the map this could improve greatly.
85
(a)
(b)
86
Figure 4.22: Scatter plot of capacitance values with both noisy sensor data
and noisy position. Red points are over joist, blue points have no joist, and
green values are unexplored.
4.4 Summary
This chapter has explored a unique concept of utilizing capacitive sensors to de-
tect the presence of floor joists for the purpose of aiding robot localization and
mapping. An algorithm was developed using a trained neural network to learn the
joist positions within the environment. Di↵erent encoding schemes for the neural
network were tested and compared. From these experiments it was shown that
one dimensional bin encoding could be used to learn the joist model containing
both sensor noise and robot position noise. The one dimension encoding scheme
requires an algorithm to determine the angular rotation between the joists and
the robots map. The work demonstrated in this chapter will be incorporated into
a complete multi-sensor SLAM solution in Chapter 6.
87
(a)
(b)
88
Chapter 5
Multi-Sensor Localization Using

Unconventional Sensors
Before attempting to resolve the full SLAM algorithm, navigation with a known
map will be examined. Without first accomplishing this task, it is unlikely to
resolve the full SLAM solution. Robot Localization aims to determine the robot’s
localization relative to a given map (Thrun et al., 2006). Which is defined as
resolving the robot position Xt given the map M , control signals ut and sensor
values zt . In Robot Localization the map is known, therefore the sensor models
do not require any training and the role of the sensors is to predict the location
given a perfect map.
This chapter will examine the problem of robot localization using Multi-Sensor
Fusion, with a specific focus on sensors that are measuring di↵ering features.
Multi-sensor fusion has the potential to resolve many problems in robotics. Sensors
that measure very di↵erent features can result in a more robust solution; if one
feature space changes or is being interfered, the system can fall-back on the other
sensors. Multi-sensor fusion also tracks more features at a given time and therefore
has the potential to be more accurate. To better define the problem, this chapter
will begin by classifying sensors into three di↵erent categories. A naive fusion
89
method wil be implemented and tested with some unconventional sensors. The
naive technique will then be improved using a particle filter.
5.1 Methodology
Sensor fusion can be coarsely categorized into two approaches; high-level fusion,
and low-level fusion (Mitchell, 2007). High-level fusion is typically less aware of
the details of the specific sensors and instead focuses on more abstract fusion of
the data returned by the individual sensor algorithms. Low-level fusion typically is
much more specialized on the specific sensors being fused and often works directly
on the sensor data. This work examines a method of high level fusion that works
to abstract the di↵ering features, the various sensors are tracking, and fuse the
data in a simple centralized way.
5.1.1 Grid Based Localization
The high-level sensor fusion approch in this work expands on the common map
representation referred to as Grid localization (Thrun et al., 2006). Grid localiza-
tion decomposes the environment into distinct grid squares of possible states. The
system determines the probability the grid square is occupied.
Typically grid localization is approached with a histogram filter, which states the
likelihood that each square is occupied by an object. The approach in this research
is similar but instead of tracking the likelihood of objects, this algorithm tracks
the likelihood the robot is present in the given state.
5.1.2 Sensor Classification
Sensors used for the purpose of robot localization must be capable of determining
the probability of a given sensor scan zt , when the robot state Xt and map M are
90
known. This probability is defined as
P (zt |Xt , M ). (5.1)
Which is often referred to as robot perception (Thrun et al., 2006). For this research
the sensors are classified into 3 categories; Reference, Relative, and Recall. All
sensors used for the purpose of localization can be classified into at least one of
these three categories.
The advantage of categorizing the sensors in this way, is to define the expected
behaviour of the sensors and where errors are likely to occur. Furthermore this
approach also encourages some clear modularization and polymorphism in the
software architecture.
5.1.2.1 Reference
Reference based sensors are capable of measuring reference landmarks of objects

from many points in the navigation map. The advantages of reference based
sensors are that when you learn where the reference point is, you can use that
as a landmark to infer your position in unexplored regions. Reference sensors are
very important to robot localization since they provide some indication of your
position relative to the map.
The learning step can be skipped by assuming models for reference based sensors.
For example, using a magnetometer as a reference sensor allows the determina-
tion of the heading relative to magnetic north. Similarly, if exploring outside, a
light sensor could be used to determine the direction of the light source. Making
the assumption that this represents the solar position, allows the application of
a mathematical model that relates the solar heading, the time of day, and the
compass heading, to determine the robot’s position on the Earth.
91
5.1.2.2 Relative
Relative based sensors are integrators. These sensors are only capable of inferring
the robot’s position relative to a previous state. Often these sensors are very
accurate at first but accumulate error over time. Common examples of these
sensors are gyroscopes, accelerometers, and wheel encoders.
These sensors resolve the propagation problem defined as: the probability of the
new state Xt given the previous state Xt 1 and the sensor information zt . Which
is defined as
P (Xt |Xt 1 , zt ). (5.2)
These sensors are often handled internally to the robot drive control system and
the values are only returned as the robot control signal ut .
5.1.2.3 Recall
Recall based sensor have a limited range and are not considered useful for tracking
a reference feature. he benefit of these sensors are that they could be used to detect
the likelihood of being at a previously explored location. The di↵erences with these
sensors are that they have no knowledge of new regions, and the sensor’s readings
are only useful for the particular position they were taken from. These types
of sensors are the least common for robot localization, since they have a limited
range.
Although, the limited range of these sensors could potentially be improved by

applying models to the sensors; which allows the extrapolation of expected values
at unexplored regions. For example, lets define a capacitive sensor that scans the
density of the floor directly below the robot. By definition, this capacitive sensor
is only capable of reading what is directly below it and so cannot track its position
with reference to any feature. Although, when it returns to a previously explored
location, it can recall what the previous value was and determine the likelihood
that it has indeed returned to that location. This can further be improved by
92
assuming a model of a typical household floor joist layout. A few capacitive read-
ings from di↵erent parts in the room will allow a prediction of the household floor
joist model. The robot colud then extrapolate likely sensor values for unexplored
regions and predictions can be made in new unexplored regions depending on how
well it fits into the model.
5.1.3 Sensor Models
The high-level sensor fusion problem can be explained as solving the system as
though it were a Venn-diagram. The goal is to examine where in the search space
each of the sensor readings are plausible, and finding where all these sensors agree
and overlap.
The key di↵erentiators between a simple Venn-diagram plot and this research is
that a Venn-diagram plot usual considers absoluteness, whereas this research is
the statistical likelihood of the sensor reading at any position within the range 0
to 1.
A virtual robot was built to test the theoretical benefit of using a plurality of
di↵erent sensors for the purpose of robot localization. Phase one of these tests
will use very simple algorithms and examine the results. Based on these results,
more complicated algorithms were developed.
The first stage of this work will examine a simplified form of the problem by
ignoring heading determination. Which helps better visualize the search space as
finding the X-Y position given a-priori knowledge of the robots heading reduces the
dimensionality of the problem. Possible methods of resolving the robots heading
will be discussed in further detail later, but for now, one can assume that this can
be determined with enough reliability and accuracy that the algorithm must now
only resolve the robot’s X-Y location within the map.
An elementry map will be examined, shown in Figure 5.1, which will used to
explain the simplified algorithms for the various sensors used in this experiment.
93
Figure 5.1: Simple test map, robot shown in center.
Each sensor must be capable of returning the likelihood of being in every position
of the map given a specific sensor reading, and the known robot heading.
The robot being examined contains a variety of sensors to help it localize itself
within the map. The first sensor is an infra-red distance sensor, which is relatively
common in robot localization research. Other less common sensors include; a
capacitive joist sensor, and a light sensor. A wheel encoder is also included to
give the robot an indication of the approximate distance traveled. The intent is
to determine the robot’s position, utilizing all of these sensors.
5.1.3.1 Infra-Red Distance Sensors
The infra-red sensor is a conventional sensor used for robot localization. It is

relatively low cost and is capable of detecting the distance to an object with a
relatively narrow beam, especially when compared to an ultra-sonic sensor.
The basic algorithm is shown in Algorithm 4, where {m | x, y} is the list of known

map features, {s | ✓, d} is the sensor scan, {n | ✓, d} is the detected new features,
and p is the new probability map of size size, and ✓abs is the robot heading. This
94
Algorithm 4 IR Probability Map
1: function IR.calc map(s, ✓abs )
2: n ExtractCorner(s)
3: M ZeroMap(size)
4: for all n do
5: x n d ⇥ ⌃x
6: y n d ⇥ ⌃y
7: ✓ n✓ ⇥ ✓abs
8: for all m do
9: M M +Gaussian2d(size, x, y , ✓, mx , my )
10: end for
11: end for
12: return M . Return the entire map
13: end function
algorithm creates a likelihood between every corner in the map and every detected
corner. At locations where these overlap, the confidence will be higher.
The location is represented as a two dimensional Gaussian distribution defined by

two values; x, and y; where x is the distribution in the direction of the feature,
which represents the error of the sensor’s distance estimate. In comparison y is
the perpendicular distribution and represents the heading error within the sensor.
In this algorithm both of these values are linear functions of the distance from the
obstacle.
An example of the probability map for the robot which is located in the center
of the map is shown in the Figure 5.2. From this plot, it is clear what e↵ect this
relatively simple algorithm has. Probabilities occur in more than just one location,
but they are strongest where these events overlap and sum.
5.1.3.2 Capacitive Joist Sensors
A capacitive joist sensor is a simple low cost sensor that is capable of detecting
the relative capacitance of a surface, and is used by the robot to determine the
presence of a floor joist. It is the intent of this study to use this unconventional
sensor to increase the e↵ectiveness of localization, by providing knowledge of the
map, and joist positions.
95
Figure 5.2: IR probability map, scan location center of map.
The simulated capacitive sensor that were used are simplified by assuming perfect
data; with a value of 1 when a floor joist was detected and value of 0 when no
joist was present. The model of the capacitance within the room is defined by an
equation described by joist separation Js , joist width Jw , joist distance o↵set Jd ,
and joist angle o↵set J✓ . In the future, real world sensor testing will need to be
done to validate the legitimacy of this model, but this should be sufficient to test
the theory of sensor fusion demonstrated in this work. The algorithm works by
resolving
mod(x + Jd , Jw + Js ) < Jw , (5.3)
which will be resolve to 1 where a joist is present and 0 otherwise. Since the
probability map for this is static and only inverts when on or o↵ a floor joist, this
can be pre-calculated at every location and simply returned when requested.
The probability plots shown in Figure 5.3, demonstrate the likelihood of being in
96
(a)
(b)
Figure 5.3: Capacitive sensor probability map: (A) Robot over floor joist; (B)
Robot not over floor joist.
97
any position on the map. Figure 5.3a demonstrates the likelihood when the robot
is over a floor joist, and Figure 5.3b when not on over a floor joist. From these
plots it can be seen that although this would not be enough to directly determine a
location, it can be helpful in reducing the search space, especially when the entire
path X0 . . . Xt is examined.
5.1.3.3 Light Source Tracking
If the environment contains a strong unidirectional light source and the location of
this light source is known within the map, the information can be used to further
assist in localizing the robot. A simple light intensity sensor attached to a rotating
actuator can perform scans of the environment and determine the relative heading
with the strongest intensity. Assuming no interfering obstacles or light sources,
this should be the direction of the light source.
To reduce the complexity of this experiment, a simplified light tracking algorithm

was developed. The sensor knows the relative heading of itself and the light source
on the map. No shadowing e↵ect is taken into account at this time, and the map
currently being experimented with does not demonstrate a shadowing e↵ect.
The probability is mapped as an extruded Gaussian, which is trimmed at the

location of the light source. This approximates that the robot could be anywhere
along this with some side to side jitter based on the angle error of the sensor. An
example of the algorithm output can be seen in Figure 5.4.
5.2 Results
Using the defined sensor models in a simulated Matlab environment, one can begin
to explore some simple multi-sensor localization algorithms. The first algorithm
will examine the entire probability map, whch similar to the histogram filter com-
mon to grid based mapping. Later this approach will be improved upon using
particle filters.
98
Figure 5.4: Light probability map, scan location center of map.
5.2.1 Complete Additive Algorithm
In theory, the search space for this problem is in three-dimensions, X-Y and ✓,
which, when solved in a Venn-diagram style would be computationally intensive;
since every permutation must be accounted for. Instead, the first attempt will
assume a known heading.
Algorithm 5 Complete Additive Computation

1: function simple fusion
2: M ZeroMap(size)
3: for all S do
4: V S.read sensor( )
5: M M + S.calc map(V, ✓abs )
6: end for
7: end function
The first experiment simply requires running all three of the robot’s sensor algo-
rithms in a given state and summing the resulting probability maps. This simple
algorithm is shown in Algorithm 5; where m is a matrix representing the grid
squares of the map, S is the list of sensor algorithms. The resulting robot posi-
tion estimate can be taken as either the max value or through some method of
neighbour-weighted max values to remove outliers.
99
(a)
(b)
Figure 5.5: Probability map when using simple additive algorithm: (A) Robot
in center; (B) Robot on right side.
100
An example of the algorithm’s output is shown in Figure 5.5. This simple algo-
rithm already demonstrates the power of this approach. More likely positions have
increased probabilities and less likely positions have decreased probabilities. These
plots demonstrate the increased confidence of the robot’s position as all the sen-
sors begin to agree on a specific location. The advantage of this simple algorithm
is in its simplicity and scalability. Other sensors can easily be included by simply
returning a probability map given the observed sensor values. As shown, these
algorithms need not be overly complex since any information serves to enhance
the systems capability and increases overall confidence. The obvious disadvantage
of this method is its complexity and the lost information after the robot moves.
Solutions to this will be discussed later.
5.2.1.1 Encoders
The inclusion of a wheel encoder will serve to demonstrate how simple and scalable
the system is to di↵erent types of sensor information. As discussed earlier, the
wheel encoder is a relative sensor, where it’s only capability is determining where
you are relative to another point. For this algorithm, two values are needed;
the start point and the distance. The algorithm will ignore the robot’s heading
information. Essentially, the encoder knows only that the robot travelled a certain
distance in an unknown direction, shown in Figure 5.6.
Including this sensor in the simple additive fusion algorithm will improve the
confidence of the location; which is shown in Figure 5.7. The ability to rapidly
and easily add new sensors with very di↵erent behaviours to the fusion system at
a high-level is very powerful.
5.2.1.2 Weighted Sensor Types
A simple extension of the simple algorithm is to provide confidence weights for the
di↵erent sensor types, see Figure 5.8. Confidence weights allow some sensors to be
101
Figure 5.6: Probability map from wheel encoder ignoring heading information.
Figure 5.7: Probability map from wheel encoder included with other sensors.
102
Figure 5.8: Probability map using di↵erent sensor weights.
treated with a higher confidence then others. Which could either be implemented
through static weight definitions or dynamic weight definitions.
Static weight definitions are the simplest and allow the system designer to create
a basic table that defines how confident each sensor is related to each other. These
weights could be either developed through trial-and-error experiments or through
analysis of the algorithms and sensor performances. An important aspect of many
of these algorithms is the map they are using for localization. For this reason, an
important aspect of selecting the ideal sensor type weights is observing the quality
of map they are using to navigate. For example, how well do the joists conform to
the model definition? Is the light source actually observable from any location? If
these errors are high, the resulting weight confidence should be lowered, which is
shown in Algorithm 6.
Using dynamic weights can be useful if a greater understanding of the relative

sensor confidence can be determined. A common example of this might be when
the map is being learnt over time. When the map is untrained, the confidence
should be relatively low, as the map is explored and trained the relative confidence
103
Algorithm 6 Complete Additive Computation With Weights
1: function Dynamic Weights
2: M ZeroMap(size)
3: for all S do
4: V S.read sensor( )
5: M M + S.get weight( ) ⇥S.calc map(V, ✓abs )
6: end for
7: end function
should increase. An easy implementation would abstract this dynamic weight

within the sensor map calculations by multiplying it by a fixed scalar confidence.
5.2.2 Particle Filter Optimization
A particle filter can be used to traverse the probability map and track likely
routes. This approach is similar to the FastSLAM algorithm common in robotic
navigation, but ignores the problem of learning the map. Particle filters are an
increasingly popular technique of resolving non-linear problems. They work by
tracking a set of potential solutions, referred to as particles, and using the available
information to weight the quality of the particle’s solution. All these particles
then undergo resampling, which randomly selects a new set of particles, from the
previous set with a selection likelihood proportional to the particle’s confidence
weight. The major benefit of particle filters are their ability to track multiple
hypotheses and converge on the best solutions. The density of the particles can
demonstrate the confidence of the solution.
Algorithm 7 Particle Filter Sensor Fusion

1: function particle filter
2: M CompleteAdditiveMap(size)
3: for all X do
i i i i
4: Xx,t , Xy,t predict(Xx,t 1 , Xy,t 1 , E, ✓abs )
5: Xwi M (Xxi , Xyi )
6: end for
7: X Resample(X)
8: position Max Weight(X)
9: end function
104
In this solution each particle contains a unique state of {Xi | x, y, ✓, w} where x, y, ✓
define the robot’s position and heading and w represents the particle weight. u
and ✓abs are the wheel encoder value and robot heading respectively. Since the
additive algorithm (defined above) provided an estimate of the position of every
location on the map, one can simply update the weight as the position on the map
at the provided x, y position. Which results in a very simple method to convert
the previous algorithm into a particle filter. Although this method still results in
the assumption of a known ✓ value, which will be handled later. The particle filter
algorithm is shown in Algorithm 7. Where X is the particle filter and M is the
map.
Algorithm 8 Prediction Algorithm

1: function predict(Xt 1 , ✓abs , u, )
2: Xx,t Xx,t 1 +Rand( ) ⇥ u ⇥ sin ✓abs
3: Xy,t Xy,t 1 +Rand( ) ⇥ u ⇥ cos ✓abs
4: return Xt
5: end function
The prediction algorithm mentioned above is shown in Algorithm 8, where Rand( )

is a random value between 0 and that represents the wheel encoder error. Which
is what separates the solutions to track the various possibilities. The random num-
ber should ideally be chosen along a normal distribution, to better approximate
the real world behaviour. The result of this algorithm is shown in Figure 5.9. This
plot demonstrates how the particle density expands; which is expected since the
solution loses confidence without sensor information that applies a reference to the
map.
Running this algorithm for a fixed path is shown in Figure 5.10. In this plot the
blue dots are the actual positions and the red dots are the estimated positions.
Clearly the system was somewhat capable of tracking the position of the robot
through the path.
Figure 5.11 shows a comparison of the errors along the robot’s path when only the
IR sensor and all sensors are used. Which demonstrates the accuracy benefits of
sensor fusion.
105
Figure 5.9: A demonstration of the prediction algorithms e↵ect on particle
positions, ignoring heading.
Figure 5.10: Particle filter path compared to true path, using IR, Capacitive,
and light sensors.
106
(a)
(b)
Figure 5.11: Error from robot navigation comparison: (A) With all sensor;
(B) IR sensor only.
107
5.2.2.1 Improvements
The previous algorithm was intentionally kept simple. This foundation will now
be built on in an e↵ort to enhance the algorithm.
Grid Calculation Currently the algorithm works by calculating the entire prob-
ability map for each sensor and summing these values together. In reality this is
unnecessarily complex and wastes computation time. The particle filter will only
require the values where particles are actually present and all other grid square
probabilities are unnecessary.
The solution to optimize using the entire map, is to change the sensor algorithms
that calculate the map, from returning the entire map, to taking the grid square
as a parameter and only returning that squares probability. This optimization
will provide other advantages in handling headings; which will be shown in section
5.2.2.2.
Number of Particles The number of particles also has a direct relationship

between the complexity and accuracy of the algorithm. The more particles being
used, the more memory and computational cost required. In comparison, few
particles results in an increased likelihood of no particles existing at the robot’s
location and the system not finding the solution. Through trial and error, 100
particles appeared to be a reasonable trade-o↵.
Resampling Resampling too frequently can also be costly (Thrun et al., 2006),
to reduce the over-resampling e↵ect the weights can be filtered through multiple
iterations, and resampled at a specific interval. The weight update filter is defined
as 8
>
<1, if resampled,
wt = (5.4)
>
:P (zt |Xt )wt 1 , otherwise.
108
Particle Generation Another improvement to the particle filter would be to
generate particles randomly. Which is especially important after long run times
where the particles are likely very dense and not e↵ectively searching the entire
map. This is also useful for resolving the kidnapped robot problem (Thrun et
al., 2006), where the robot is unknowingly moved to a new location within the
map.
By randomly introducing new particles into the map, you increase the likelihood of
generating a particle in the area of the new robot’s position. Eventually, through
resampling the solution will converge on the correct location. A further improve-
ment to this approach is to generate particles using the probability map. If the
algorithm generates particles in highly probable locations on the map, it is more
likely to quickly find the robot location when lost.
5.2.2.2 Heading
The sensor algorithms resolve p{x, y, ✓ | V }, or what is the probability p that the
robot is at x, y, ✓ given the sensor reading V ? Now that the requirement of re-
solving the entire map at once has been removed, each particle can have di↵erent
heading information without the requirement of calculating a map for every pos-
sible direction.
Sensors can easily be added that provide weight estimates based completely on
the heading information and ignore the position. For this algorithm two sensors
will be used; first a gyroscope will track relative orientations similar to the wheel
encoder. Which is preferable to using the wheel encoder for rotations, since wheel
slippage makes this approach e↵ectively useless. A compass provides a reference
based sensor relative to magnetic north, although this sensor can be interfered by
magnetic objects.
The prediction algorithm is expanded to take the gyro estimate ✓g yro and includes
a heading error as well ✓. Which is shown in Algorithm 9 and results in the
109
Algorithm 9 Prediction Algorithm With Heading
1: function predict(Xt 1 , u, )
2: X✓,t X✓,t 1 +Rand( ✓ ) + u✓
3: Xx,t Xx,t 1 +Rand( d ) ⇥ ud ⇥ sin X✓,t
4: Xy,t Xy,t 1 +Rand( d ) ⇥ ud ⇥ cos X✓,t
5: return Xt
6: end function
Figure 5.12: A demonstration of the prediction algorithms e↵ect on particle

positions, ignoring heading.
e↵ect shown in Figure 5.12. It is clear, from these plots the distribution now
approximates more of a two dimensional Gaussian that over time will spread wider.
The particle filter algorithm can now include the compass and gyro. The weight
estimates will be representative of the more likely positions.
5.2.2.3 Unknown Start Position
A more challenging problem to resolve is solving the robot’s position without

knowledge of the starting location. To handle unknown starting locations, particles
are evenly distributed through the entire map as shown in Figure 5.13
110
(a)
(b)
Figure 5.13: Particle configuration at start of test with unknown start loca-
tion: (A) Initial particle position; (B) Initial probability map.
111
(a)
(b)
Figure 5.14: Corresponding maps after 20 steps with unknown start location:
(A) Particle position; (B) Probability map.
112
(a)
(b)
Figure 5.15: End map configuration after test with unknown start location:
(A) Particle positions; (B) Probability map.
113
After 20 steps, the particles begin to converge on the robot’s location through
resampling based on the sensor weights; which can be seen in Figure 5.14. This
plot clearly demonstrates the system’s ability to use all sensors. With the highest
concentration of particles existing where all sensors agree and strong concentration
of particles is observable on the line of the light source.
The particle convergence after the path is complete is shown in Figure 5.15. At
this time the particles have completely converged on the solution. This example
demonstrates this algorithms ability to e↵ectively fuse many di↵erent types of
sensor values together to resolve a robot location within a map.
5.3 Summary
This chapter has demonstrated a simple multi-sensor localization technique that

extends on the conventional grid based localization using particle filters. The
unconventional sensors, capacitive and light, were examined along with the con-
ventional IR sensor. Since all of these sensors measured very di↵erent features,
direct sensor fusion would have been impossible. By using high-level fusion where
each sensor provides a probability map representing its own belief of the robots
location, the algorithm could simply combine these probability maps together and
develop a fused estimate.
Very di↵erent sensors add an inherent robustness and thereby safety to a robot
system. Many conventional solutions are incapable of working without all sensors
being functional. Additionally, this algorithm is capable of operating despite sen-
sor failure. The system simply ignores those sensor values and resorts on using the
other sensors. The system likely will have degraded performance but will still be
capable of developing some indication of the robot’s location. Furthermore, the
di↵erent sensors will be inherently sensitive to di↵erent types of interference. So
when one senor is degraded from an environmental interference, the algorithm can
resort to the other sensors, which are likely to not also be hindered.
114
Further work will examine the extension of this algorithm into a complete SLAM
solution, capable of determining its location without a-priori knowledge of the
map.
115
Chapter 6
Multi-Sensor FastSLAM Using

Unconventional Sensors
This section will demonstrate the novel advancement to the SLAM algorithm
that adds simple scalable sensor fusion. The sensor fusion approach is done at
a high-level so the type of sensor and methodology of localization is abstracted
from the fusion algorithm. The intent of this concept, is that with very few
modifications to the conventional FastSLAM algorithm, the same elegant approach
can be maintain, while enabling much greater scalability of sensor choices.
The approach taken enables fusion of sensors with and without knowledge of the
map, which allows partial information of the environment to be included. For
example, if information about the location of obstacles was unknown but the
floor joists model was known this information could still be used. This allows for
partial SLAM where some map information is available but not a complete map.
No information of the map is required by the system, but it can be incorporated
to improve the performance. The initial sections leverage this to simplify the
problem, the distance sensor is always unknown and has no knowledge of the
obstacles in the environment. The floor joist sensor is considered to be already
trained for the map, this is done to show the potential benefits of the sensor. A
116
method of perform,ing this training using this algorithm is also demonstrated, to
develop a complete SLAM solution without any map knowledge.
Why FastSLAM? To understand the current state of the art SLAM approaches
both the FastSLAM and Extended Kalman Filter algorithms were implemented,
using the Matlab framework, and compared. The FastSLAM algorithm performed
better and was much simpler to tune.
The use of particle filter within the FastSLAM algorithm also provides a much
simpler way of interfacing multiple sensors. Since each sensor can run its own
algorithms, as long as they can provide a weight estimate, the confidences can be
combined. Another advantage of the FastSLAM algorithm is its ability to handle
non-Gaussian errors and track multiple hypotheses.
6.1 Methodology
The core concept of the algorithm is to create a population of particles that ex-
plore the map. Which is similar to the algorithm demonstrated in Chapter 5 but
this algorithm will also explore the problem of learning the map simultaneous to
localizing.
The algorithm works by providing each particle a sensor object containing each
sensor on the robot; the sensor object contains that particle’s current map es-
timate. For a sensor to be used for localization within the algorithm it must be
capable of two things; providing a weight estimate given a state estimate, and train
its map. Optionally each sensor can also provide a maximum likelihood estimate.
Weight Estimate The weight estimate approximates the probability of how

well the current sensor values fit into the map based on the current state of the
particle. Mathematically this is represented as
p(xt |X, M ), (6.1)

117
where xT is the sensor readings observed from the robot, X is the current particle
state x, y, ⇥ and M is the sensor map. Some sensors will not always be able to
provide a weight estimate all the time. Typically this occurs when the robot is
exploring an unexplored section of the map. If the system cannot make a weight
estimate it can return an undefined value (nan), and its measurement will be
ignored by the fusion algorithm. Which is useful if the sensor map is completely
untrained in a particular environment and can make no reasonable estimate.
Map Training Each sensor algorithm must be able to also learn its represen-
tation of the sensor map. There is no requirement for how this map information
is stored. Two typically methods of storing the map would be using the Kalman
filter state approach or an occupancy grid map.
Maximum Likelihood Another optional function that would be useful for a

sensor is an ability to extract a maximum likelihood position from the map. Which
could be used to generate new particles if either the particles are too dense or the
confidence is low. Generating new particles would work by requesting each sensor
to provide a list of candidate locations. All sensor algorithms that are capable
of providing an estimate would, otherwise an empty list would be provided. The
candidate location list would then be run through all other sensor algorithms to
determine a fused weight estimate and resampling would occur similar to the other
algorithms.
The maximum likelihood component would help resolve the kidnapped robot prob-
lem, where a robot is moved without knowledge to a di↵erent location within the
map.
6.1.1 Algorithm Architecture
As mentioned the initial algorithm attempts to extend the FastSLAM algorithm to

be capable of using an array of sensory inputs. Each sensor module is responsible
for maintaining its own map, and providing weight confidences based on that map.
118
Figure 6.1: Basic UML architecture for sensor algorithms. Diamond repre-
sents has-a, and triangle represents is-a relationship.
Implementation of this algorithm is assisted with an object oriented architecture.

Each sensor is implemented with its own algorithm that extends the abstract
sensor algorithm object. The simplified architecture is shown in Figure 6.1. This
diagram demonstrates the FastSLAM class that contains a list of sensor objects.
Each sensor object is of a sensor type that handles the data from that specific
sensor. The diamond shape indicates the has-a relationship, and the triangle
represents the is-a relationship.
The UML diagram shows the two core sensors being used in this chapter; the IR
sensor uses a Rao-Blackwatter implementation and stores the map as a Kalman
state and covariance, which is the conventional approach used in fastSLAM. There-
fore no specific function is required for training as training is done while calculating
the weight. The Kalman state approach also doesn’t map nicely to determining the
best solution within the entire map and so that function is also not implemented.
The capacitive sensor uses a model to store the likelihood at every position. It
therefore can predict likelihood positions within the entire map; it also uses a
separate training function. Later work will demonstrate incorporating the neural
network training algorithm for learning the capacitive model.
119
Robot
Moves
Resample Predict
Calculate
weights
Figure 6.2: FastSLAM Algorithm States.
6.1.2 Algorithm Specifics
This algorithm is similar to the other algorithms presented, it works by iteratively

moving through the states shown in Figure 6.2. The algorithm first performs a
movement and records the control signal. The control signal is used to predict
the new locations of the particles. After the movement, all sensors are scanned
and each particle uses the sensor data to calculate its new weight estimate. These
weights are combined to produce a fused weight estimate that should be propor-
tional to the overall state confidence. Finally the robot performs resampling to
remove the least likely particles and converge on the correct solution.
A More detailed breakdown of the flow of the complete system is shown in Figure
6.3. In this algorithm black squares represent the robot system external to the
localization algorithm, blue squares represent the core localization algorithm, and
red squares represent the individual sensor localization algorithms.
Initialization The first time the algorithm is run it initializes all the particles
and sensor objects. It will iterate through the list of all sensors provided and
create a sensor object that handles that sensor type. All further data from this
sensor will then be passed to the sensor object.
120
START
ROBOT MOVES (𝒅, ∅)
ROBOT SCANS ALL

SENSORS (𝒛) ROBOTS ESTIMATED
POSITION =
CLUSTER CENTER
PREDICT EACH PARTICLE
FROM MOVEMENT (𝒅, ∅)
AND ROBOT NOISE (𝑸) SOLVE BEST
CLUSTER
SET EACH PARTICLE

WEIGHT = 0 CLUSTER
PARTICLES
FOR EACH RESAMPLE

Done
PARTICLE
PARTICLES
(𝒑)
More
Done FOR EACH

SENSOR
(s)
More
particle(𝒑).weight +=
sensor(𝒔).get_weight(scan(𝒛),particle(𝒑).position)
Figure 6.3: Flow chart of complete algorithm.
121
This approach allows the algorithm to easily handle plug and play type function-
ality with sensors. When a new sensor is added an object can be easily created to
handle it, and the new sensor can be immediately used to assist with localization.
Prediction The prediction state functionally works the same as the algorithm
demonstrated for just navigation, shown in Algorithm 8. It updates each particle’s
location to include the new movement from the control signal and added noise.
Which creates the behaviour of reduced confidence as the cluster size will expand
outward.
Sensor Fusion The sensor fusion works by combining the weight estimates of
each sensor. The combination algorithm can be virtually any method, for this
research 3 techniques were examined; median, mean, and sum. The sum was
chosen as the final approach mainly because its simplicity for visualization.
Each algorithm must produce their own representation of the confidence, given
particle location, based on the sensor data provided. The confidence value is
typically in the range of 0 to 1, but the sensors can have their own map confidence
that could modify this normal value based on how well trained they believe the
map is. The sensor could even return a null value to indicate it has no knowledge
of the area.
There is also a centralized weighting map that scales each sensors weight by a
fixed amount, which allows some sensors to be treated with greater confidence
then others. The centralized weighting is useful when using redundant sensors, it
allows one sensor to have a much lower confidence but if a failure occurs with the
main sensor the redundant sensor is still fully trained.
It is important to recognize that there are two important aspects to the weight
values. The relative weight values produced from one sensor to another sensor, for
example if one sensors LOW confidence is equal to another sensor HIGH confidence
the first sensor will be virtually ignored. Also the variance in the weights is
important, if a LOW confidence is represented by 0.6 and a high confidence is 0.7
122
then there is not much variance in the measurements and resampling will not be
e↵ective. Some of this can be resolved by normalizing the weights.
Resampling Resampling is the process of selecting the new set of particles based
on the previous set. It works by selecting the new particles proportional to their
weight values.
The interval of resampling can either be each step, fixed interval, or dynamic
interval. Doing resampling at each step is the simplest but could result in one bad
step with poor data, removing many of the good particles. Resampling at fixed
intervals is also relatively simple, for this it was helpful to filter the weight values
with a simple IIR low-pass filter between resampling events. Which is important
so that the algorithm is resampling based on all steps, not just the current step.
Estimate Extraction The estimate extraction algorithm produces a population

of candidate solutions as particles and not a unique solution. For most applica-
tions a single position is needed to report the position to other modules such as
navigation. For this reason an estimate of the location needs to be extracted from
the particle density. Three methods were explored for extracting the location; best
particle, mean, K-Means.
Best Particle The first and simplest approach was to use the particle with
the highest weight as the location estimate. Although this approach is trivial
to implement it results in a very noisy measurement, and occasionally produces
extremely large errors when bad particles occasionally have large weights.
Mean The next method explored was to take the average of all the particles.
Which essentially filtered the results and ended up with a smoother output. Al-
though, occasionally this developed solutions that were not near any particles.
This behaviour can occur when there are two similar sized clusters of particles
123
tracking two candidate locations. If the average of these two groups are taken di-
rectly the resulting solution will be somewhere in the middle. Which is undesirable
since the system has no confidence that it is at the reported location.
Another simple improvement made to this approach was to perform a weighted

average, by weighting each particle by its particle weight. Applying the weights
to the mean, showed some improvement but could still result in the problem
mentioned above.
K-Means The final approach chosen was to use a clustering algorithm to extract
the di↵erent particle groups. After the di↵erent groups have been clustered the
best group is chosen as the cluster with the best cumulative weight. The algorithm
then uses the centroid of the cluster as the solution. This work used a fixed number
of clusters, and dropped cluster when required.
6.2 Experimental Sensors
For this work two very di↵erent sensors will be examined for the purpose of testing
the fusion algorithm. These sensors the conventional distance based sensor, tuned
to the behaviour of IR range base sensors, and the unconventional capacitive floor
joist sensor. These sensors measure very di↵erent landmarks and so are suitable
for the high-level fusion being presented here.
6.2.1 Distance Sensors
The distance based sensors will be done in the conventional way. Utilizing the
Rao-Blackwell approximation for separating the Kalman filter from the Map.
Landmark Extraction The conventional approach for SLAM uses landmarks.

Landmarks are considered as sensor features that are stationary, distinguishable,
and repeatable. These landmarks are then used as the map space. For this research
124
a corner extraction approach was chosen to capture landmarks for distance based
sensors such as IR, Ultrasonic, and Laser scanners.
Landmark Management Landmark matching is the method of taking the

landmarks from the map and matching them to the reading received from the
sensor. The first technique used a simple local search strategy with two threshold
values. If the distance between the measured landmark and the map landmark
were below a fixed defined threshold the values were considered a match, if the
distance to the closest landmark was above a fixed defined threshold the feature
was marked as a new landmark and added to the map. This technique was simple
to implement and understand, but lacked some intelligence about the confidence of
a given feature that would provide greater flexibility. This technique also requires
significant tuning for every sensor and possibly di↵erent environments.
Noisy sensor data can occasionally create features that don’t exist in the real
world. If left unmanaged the bad landmarks will cause bad matches and the map
will no longer function correctly. One technique to resolve this issue is to maintain
a count of the times the feature has been observed, and how many times it should
have been observed but was not. If this count falls below a threshold the feature
is removed from the map.
6.2.2 Capacitive Sensors
The capacitive sensor can provide an indication of the density and type of mate-
rial being observed. Many building environments have joists in the floors that can
be easily measured using capacitive sensors; which is commonly how stud-finders
work. Another method of using a capacitive sensor for localization is to develop
a model for the joists in the environment. The model would be a mathematical
representation that is capable of predicting where the joists would be in the en-
vironment. For example, if joists are separated by 16 inches, as the robot move
across the room the particles will spread apart as the dead reckoning error grows.
When the furthest particles begin reaching the next joist in the sensor model they
125
will disappear since this reading is not supported by the real world sensor measure-
ments. When the robot reaches the floor joist and measures the joist the particle
cluster will tighten around the correct position on the map.
6.3 Results
The algorithm was implemented in Matlab and run through similar simulations
used in the other chapters. One advantage of the high-level fusion is the flexibility
to what sensor data is provided. This ability is used here to better understand
the e↵ects of the algorithm and each sensor. First the results of having no sensors
available are demonstrated, than both IR and capacitive sensors alone, and finally
both sensors fused together.
Because of the flexibility of the algorithm’s implementation, switching between all

these tests requires only 1 line of code change. That line is where the simulator
is initialized to provide a list of sensor data, this list can be modified to be any
combination. The SLAM algorithm dynamically handles the list, without any
other configuration.
No Sensors The first test was to experiment with the algorithm without provid-
ing any sensor data. It is important the system can somewhat handle not having
sensor data for short periods, as this behaviour allows the robot to continue to
navigate with sensors that are broken or unable to work in the current environ-
ment. As expected the system loses confidence the longer it goes without a sensor
update.
The results for this test are shown in Figure 6.4. These figures show the error
in two ways, the first shows a plot of the distance between the true location and
the estimated location as well as the heading error. The second plot shows the
path of the estimated robot and the true path taken. These two plots provides
a simple visualization of what is happening. It is important to realize that this
126
(a)
(b)
Figure 6.4: Results of SLAM without sensors: (A) Error plot; (B) Path error.
127
is the estimated location and in fact there are many particles of possible location
estimates.
These results clearly demonstrate that the system is capable of continuing to

operate without sensors, but does result in large errors accumulating over time.
IR Only Navigation The conventional approach of FastSLAM is to use a dis-

tance only sensor, when only this data is provided the algorithm reduces to the
conventional FastSLAM algorithm. The results for this are shown in Figure 6.5.
These results show a large improvement over the no sensor case shown above, but
some residual error is still present. The intent is that by including other sensors
the error could be further reduced.
Capacitive Only Navigation The flexibility of this algorithm also allows the
examination of the benefits of using only the capacitive sensor. The results of
this are shown in Figure 6.6. Although this might seem like an unlikely method
of navigation it is a useful feature of the algorithm, as it will allow the system
to navigate without the IR sensor for short intervals. Which could occur when
the IR sensors are out of range of any usable features, or the IR sensor is for
another reason not functioning. For example the sensor might be saturated by
light causing it to malfunction.
These results show improvement over the no-sensor results, although not as good
performance as the IR sensors. The capacitive sensor used in this test has a trained
map for the floor joist locations.
Capacitive and IR Fusion Finally the algorithms were tested for the ability to
fuse the information from both of these sensors together. The results for this are
shown in Figure 6.7. The results shown here are the best of all other results shown.
Which is a good indication that the data from both sensors are being successfully
fused together to increase the confidence and knowledge of the position within the
environment.
128
(a)
(b)
Figure 6.5: Robot path with IR sensor only: (A) Error plot; (B) Path error.
129
(a)
(b)
Figure 6.6: Robot path with capacitive sensor only: (A) Error plot; (B) Path
error.
130
(a)
(b)
Figure 6.7: Robot path with IR and capacitive with known map: (A) Error
plot; (B) Path error.
131
Light Beacon Only Navigation To further test the flexibility of this system
the light beacon sensor will also be quickly used. Figure 6.8 shows the results of
using only a light sensor. This sensor can detect the angle to a known light source.
The light source is located at 80, 80 in the top right corner. and so helps correct
the left and right error in the downward section.
Since the exact position of the light beacon is known this sensor actually per-
forms relatively well but slowly accumulates error as it has difficulty correcting for
distance when moving directly towards or away from the light source.
Complete Sensor Fusion Now this algorithm will be tested with all three
sensors active at once, the di↵erent features all three sensors are measuring allows
the fusion algorithm to correct for the di↵erent errors. The capacitive sensor
assist with correcting distance while moving perpendicular to the floor joists and
correcting angle errors while moving parallel to the floor joist. The IR sensor
corrects both distance and angle but contains error while learning the map. The
light sensor tracks a known beacon, and helps correct the angle error while moving
towards it and distance error when moving perpendicular to the light source.
The results for the test are shown in Figure 6.9. Looking at the path it can be
seen that the robot closely tracks the true path.
Summary of Results All of the di↵erent sensor configurations are summarised

in Table 6.1. These results clearly show the ability of this algorithm to fuse very
di↵erent sensor information together in a scalable way. The errors in this table
are in simulator grid size units, which are configured to be 1 inch.
The conventional approach is represented as the results with distance sensor only.
In this configuration the algorithm reduces down to the conventional FastSLAM
algorithm. From these results it can be shown that this error can be further
reduced by incorporating a joist sensor. The error can be further reduced by
incorporating both the joist sensor and light sensor. This gives a good indication
that a variety of sensors could provide improved accuracy using this algorithm.
132
(a)
(b)
Figure 6.8: Robot path with light beacon sensor only: (A) Error plot; (B)
Path error.
133
(a)
(b)
Figure 6.9: Robot path using IR, light beacon, and capacitive sensors with
known map: (A) Error plot; (B) Path error.
134
Table 6.1: Summary of errors from di↵erent sensor configurations
Sensor Configuration Mean Squared Error (MSE) Max Error

No Sensors 64.84 16.30
Distance Sensor Only 10.50 8.91
Joist Sensor Only 21.80 9.45
Distance and Joist Sensors 8.68 9.60
Light Sensor Only 12.85 7.41
All Sensors 6.83 6.50
Furthermore using joist sensor or light sensor alone results in improved perfor-
mance when compared to not having any sensors, which is a good indication of
the ability for this system to improve the robustness to sensor failure. A sensor
could fail for a variety of reasons such as; software failure, hardware failure, or
environmental interference. Having redundent sensors allows the system to main-
tain accurate positioning during these times. Further research demonstrating the
improved robustness using this algorithm is shown in Appendix C.
Integrating Capacitive Neural Network Training The results above demon-

strate fusing an unknown IR map with known capacitive and light maps. This
section will explore incorporating the neural network capacitive map training from
Chapter 4 into the solution. The experiment will first run the system with IR only
and collect the training data from the SLAM output while simultaneously collect-
ing capacitive sensor readings. The data from this test will be used to train the
one dimensional bin neural network presented in Chapter 4. The simulation will
then be run again, this time using both the untrained IR and the trained neural
network output.
Training The Neural Network Training the neural network using the output
of the particle filter, could be done by simply taking the estimated position at each
step and the corresponding capacitive value. Instead we propose an approach that
takes advantage of the underlying particle filter to create a better training data
related to the confidence of each particle.
135
The training technique stores every particles location and confidence weight at
every step. The system then randomly selects the training data proportional to
the confidence of each particle. Which allows more confident particles to be used
more.
The simulation was configured to run with IR sensors only without any prior map
information. The simulation also collected capacitive data at each step but only
saved the data and didn’t use it. The map and error for the robot while collecting
the test data is shown in Figure 6.10.
The algorithm for selecting the samples picks a number between zero and the sum
of all particle weights. The algorithm then iterates through the list of particles
subtracting the weight of each until the value is less then 0. This particle is then
chosen and added to the data set. The process can be repeated until the desired
data size is achieved. For our experiment 2000 samples were selected shown in
Figure 6.11.
The data was encoded using the one dimensional bin encoding described in Chap-
ter 4. The encoded data was then used to train the neural network to develop a
capacitive map. The results are shown in Figure 6.12. Which shows clear indica-
tions of where the joists are located, with a Gaussian like distribution around the
joists. The NN error plot shows the system converged.
Testing the Trained Neural Network After training the capacitive model
the same simulation was re-run, this time combining the IR sensor data with the
trained capacitive model. The results for this experiment are shown in Figure
6.13. Which demonstrates an error reduction from the IR only experiment above.
The system accomplished the improvement without any prior knowledge of the
capacitive map.
136
(a)
(b)
Figure 6.10: Robot path using IR while collecting capacitive values for train-
ing: (A) Error plot; (B) Path error.
137
Figure 6.11: Training data used to train the neural network
6.4 Summary
This chapter has demonstrated a technique for multi-sensor SLAM that utilizes
high-level fusion architecture to allow quick and easy configuration of sensors.
The algorithm is closely modelled after the conventional FastSLAM approach but
allows new information to be easily incorporated into the system.
These experiments used a fixed weighting system for all sensors. In reality the
weights for the di↵erent sensors could be pre-configured based on a-priori knowl-
edge of the environment.
These experiments focused on the inclusion of high-level sensor fusion into the
FastSLAM algorithm. The IR distance sensor had no knowledge of the map prior
to the experiments, and used a Kalman filter to slowly converge on the feature
locations. The capacitive sensor did have knowledge of the joist model for the
environment in the first experiments, and the light sensor did have prior knowledge
138
(a)
(b)
Figure 6.12: Neural network training error using bin encoding on a tighter
spaced map: (A) Predicted joist map; (B) Neural network training plot.
139
(a)
(b)
Figure 6.13: Robot simulation with IR and trained capacitive neural network
(A) Error plot; (B) Path error.
140
of the light source location. Later results demonstrated how learning both the
capacitive and IR maps could be accomplished using the neural network based
system developed in Chapter 4.
141
Chapter 7
Conclusion and Future Work
This thesis has discussed various approaches of utilizing sensors for the purpose
of robotic navigation and map building. The focus was on incorporating uncon-
ventional sensors that could add new information to the map. The objective was
on generating a common framework and architecture that easily handles scalable
sensor fusion of very di↵erent sensors.
7.1 Summary of Work
The overarching theme of this work explored the possibilities of unconventional

sensors being incorporated into localization. Which was explored in three core
concepts; sensor classification, an example of an unconventional sensor, and multi-
Sensor Fusion.
Sensor Classification Three classification of sensors for the use of localization

were defined; relative sensors, reference based sensors, and recall. This method of
classification allows a higher level of discussion of fusion approaches. The exact
sensor hardware or technique doesn’t need to be known, only what classification
the sensor falls in. The expected errors and e↵ects it will have on the overall
location solution can then be inferred.
142
Relative based sensors are capable of determining their current state relative to a
previous state. These sensors are typically integrators such as gyroscopes, wheel
encoders or, accelerometers. They are typically fairly accurate for short periods
of time but accumulate error quickly.
Reference based sensors are able to determine an indication of a position given

a reference feature or landmark. These sensors are very useful since they enable
transferring the device map frame to a global map frame when the landmark
positions are known.
Finally recall based sensors have an extremely short range and are manly used
for identifying previously explored regions and recalling the previous sensor values
to determine the likelihood the robot has returned to the same position. These
sensors can be made more practical through the use of models that predict the
likely values of unexplored locations.
Unconventional Sensors Unconventional sensor is a term to refer to sensors

that would not typically be used for the purpose of localization. This thesis dis-
cussed the benefits of incorporating these sensors into the localization solution.
One key benefit is that often these sensors can be free, since they might already
be present on the robot serving another purpose. Another possible benefit is that
they might be a much lower cost solution. These sensors can also be used to tar-
get specific features within the environment being navigated. For example, this
thesis discussed the possible use of a capacitive sensor to detect floor joists within
a home. Which could be a useful technique to improve the localization results
where a blueprint that includes this information is available.
High-Level Multi-Sensor Fusion The final concept that was discussed, was
the idea of high-level multi-sensor fusion. The term high-level distinguishes this
approach from other approaches; in that the sensor data itself is not fused directly
together but rather the information is fused. Which allows fusion of very di↵erent
sensors together in a seamless construct.
143
This research built a comprehensive framework that developed a complete software
architecture that enabled extremely scalable sensor fusion. New sensors could be
easily included in the final solution, and the information from these sensors can
be used to improve localization.
The technique used in this thesis was an extension of conventional FastSLAM. It

is one of the most popular techniques for resolving the SLAM problem being used
today.
Biomimicry Biomimicry is the process of designing a system to model a be-

haviour observed in nature. This work has explored the idea of using all senses
available to help navigate, and is a common behaviour found in nature. People and
animals know their location from all information available to them, and not only
relying sight. Occasionally people can know generally where they are from smell
or taste as well as the sounds of the environment; some animals also possess other
sensors that they utilize for localization. Magnetoception allows some animals to
detect the magnetic field, and olfactory navigation is a proposed solution to how
pigeons navigate. Which work by detecting and remembering smells of particular
regions.
This behaviour makes nature extremely resilient to changing environments and

robust to damage. Sensor fusion in nature allows a blind person to learn to navigate
without sight, but to use their other senses to infer their location within the
environment.
This thesis has explored the concept of incorporating a variety of di↵erent sensors,
for the purpose of improving localization. The intent being that a variety of sensors
could have the same e↵ect of improving reliability, robustness and accuracy.
144
7.2 Conclusions
This work developed and tested a variety of sensor confiurations and sensor fusion
approaches, which allows a few conclusions to be proposed. First a joist sensor
could be a valuable sensor for robot localization, when incorporated with conven-
tional sensors the mean squared position error was reduced from 10.50 to 8.68.
O↵ering a simple way to improve accuracy. Furthermore the joist sensor alone
was capable of maintaining position with an error of 21.80. Although this is not
as good as the conventional distance sensor only configuration it is significantly
better then not haveing a sensor, which demonstrates the enhanced ability for the
system to operate after the primary distance sensor fails. These results demon-
strated the benefit of using the particle filter as the core fusion component in the
conventional FastSLAM algorithm, allowing very di↵erent sensors to be seamlessly
fused together.
A neural network was shown as a feasible method of learning the floor joist layout.
A one dimensional encoding scheme for the input location can allow the system to
learn likely floor joist locations despite position and sensor errors. This encoding
scheme can also be trained using the particle filter data to enable the floor joist
model to be learned as part of the SLAM algorithm.
A technique for scalable sensor fusion with di↵erent sensor was shown which used
a probability map as the core fusion component. This was shown to be scalable by
fusing together the conventional distance sensor as well as both light beacon sensors
and the floor joist sensor. This fusion approach was also capable of resolving the
global optimization problem, which could be expanded to solving the kidnapped
robot problem.
7.3 Future Work
Future research in this field would further build on the platform developed in
this research. The platform was designed to be extremely scalable with built in
145
components that would enable real-world testing of the current algorithms through
a Bluetooth connected robot.
Di↵erent Rates One interesting area of future research would be to investigate

how to handle sensors with varying rates. When dealing with very di↵erent sensors
di↵ering rates are likely to occur, and handling this properly is necessary to ensure
one sensor doesn’t get favoured simply because it is sampled more frequently. The
use of a simulator in this work allowed the sampling rates of all sensors to be
configured to the same rate, but real hardware typically is not so easily configured.
For example the sampling rate of a laser sensor would be much faster than that
of a capacitive sensor.
This problem could likely be handled using DSP filtering techniques. Which would
e↵ectively interpolate the less frequently sampled sensors to predict their values.
The issue with this is that a delay is incorporated into the system. Research
would need to be done to investigate the e↵ects of this delay and possible ways to
mitigate its e↵ects.
Sensor Fingerprinting Another area of future work is the concept of sensor

fingerprinting. Which creates a unique sensor snapshot of the environment using
all available sensors. This fingerprint can then be trained and used later to increase
the confidence of the location. The di↵erence between this approach and the
approach discussed in this thesis is that sensor fingerprinting would require fewer
sensor-specific algorithms; the sensor data itself creates the patterns. Which is
much more of a data-mining approach and would be an interesting area of research
to explore.
Other Sensors There are many other sensors that could be investigated for
the purpose of robot localization. In theory a greater variety of sensors could
increase the map’s density of information and fundamentally improve the quality
of navigation. Some ideas for further sensor research will be briefly discussed here.
146
An important concept is that these sensors do not need to be present only for the
purpose of robot localization, but rather could be present to assist with another
task and simultaneously be used to improve the localization.
Acoustic Localization Many environments have characteristic background noise

that could be used to assist in localization. This could be from a variety of sources
such as a fridge, computers, air ducts and furnaces. An array of directional mi-
crophones could detect these signatures and their approximate relative direction.
This information could then be used in two ways; first to determine a larger area
such as what room the robot is in, and secondly it could be used to correct the
robots location. It is conceivable that the same turn tracking technique that was
done with the light sensor could be applied.
Surface Characteristics The robot could also use information about the floor
surface to better determine where it currently is. This information could easily be
extracted by sensors; such as RGB colour sensors, reflectivity sensors, and whiskers
that could determine the floor texture. Knowing this information, the robot could
better determine what region it’s in within the environment.
Environmental Sensors The robot could also sense environmental e↵ects that
may help detect location specific signatures such as warm or cold spots in a build-
ing. Some ideas of environmental sensors would be temperature sensors, humidity
sensors, and air-flow sensors.
Other Sensors Other sensors that are less common but could be employed for
the purpose of localization would be a chemical sensor or Enose, as well as a
giger-counter. These sensors could detect characteristic properties that might be
localized to specific regions within the environment.
147
References
Abrate, F, B Bona and M Indri (2007). Experimental EKF-based SLAM for Mini-
rovers with IR Sensors Only.. In: Proc. of the 3rd European Conference on
Mobile Robots. Vol. Sept. Freiburg, Germany. pp. 1–6.
Ahmed, M and G Pottie (2005). Fusion in the Context of Information Theory.

CRC Press. Boca Raton.
Azizyan, M, I Constandache and R Choudhury (2009). SurroundSense: mobile

phone localization via ambience fingerprinting. In: Proc. of the 15th annual
international conference on Mobile computing and networking. Vol. 13. Bei-
jing, China. pp. 261–272.
Bai-fan, C, C Zi-xing and H De-wen (2006). Approach of simultaneous localiza-

tion and mapping based on local maps for robot. Journal of Central South
University of Technology 13(6), 713–716.
Baxter, Larry K. (1997). Capacitive Sensors: Design and Applications. Wiley-

IEEE Press. New York.
Beevers, K.R. and W.H. Huang (2006). SLAM with sparse sensing. In: Proc. IEEE
International Conference on Robotics and Automation. IEEE. Orlando, USA.
pp. 2285–2290.
Bistrovs, V and A Kluga (2011). MEMS INS / GPS Data Fusion using Particle
Filter. Electronics And Electrical Engineering 6(6), 3–6.
Bloss, R (2008). Simultaneous sensing of location and mapping for autonomous

robots. Sensor Review 28(2), 102–107.
148
References
Bolic, M, PM Djuric and S Hong (2005). Resampling algorithms and architec-

tures for distributed particle filters. IEEE Transactions on Signal Processing
53(7), 2442 – 2450.
Botelho, S, C da Rocha, P Drews and Oliveira G (2010). Growing Cell Structures

Applied to Sensor Fusion and SLAM. In: Robotics Symposium and Intelligent
Robotic Meeting. Sao Bernardo do Campo, São Paulo. pp. 79–84.
Candy, J. V. (2009). Baysian Signal Processing. John Wiley & Sons, Inc. New
Jersey.
Carlone, L, M Kaouk Ng, J Du, B Bona and M Indri (2010). Simultaneous Local-
ization and Mapping Using Rao-Blackwellized Particle Filters in Multi Robot
Systems. Journal of Intelligent & Robotic Systems 63(2), 283–307.
Chintalapudi, K, A Padmanabha Iyer and V N. Padmanabhan (2010). Indoor

localization without the pain. In: Proc. of the 16th annual international con-
ference on Mobile computing and networking - MobiCom. ACM Press. New
York, USA. p. 173.
Choi, K and S Lee (2012). An enhanced CSLAM for multi-robot based on un-
scented kalman filter. International Journal of Control, Automation and Sys-
tems 10(1), 102–108.
Choset, H, K M. Lynch, S Hutchinson, G Kantor, W Burgard, L. E. Kavraki

and S Thrun (2005). Principles of Robot Motion. Massachusetts Institute of
Technology. Cambridge.
Dam, J, J.A. Krose and F. Groen (1993). A neural network that transforms oc-
cupancy grids by parallel monte carlo estimation. Computer Science in the
Netherlands pp. 121–131.
Davey, S J. (2007). Simultaneous Localization and Map Building Using the Proba-
bilistic Multi-Hypothesis Tracker. IEEE Transactions on Robotics 23(2), 271–
280.
149
References
Davis, I.L. and A. Stentz (1995). Sensor fusion for autonomous outdoor navigation
using neural networks. In: Proc. of 1995 IEEE/RSJ International Conference
on Intelligent Robots and Systems. Human Robot Interaction and Cooperative
Robots. Vol. 3. IEEE Comput. Soc. Press. Pittsburgh, PA. pp. 338–343.
Duckett, T. (2003). A genetic algorithm for simultaneous localization and map-

ping. In: Proc. of IEEE International Conference on Robotics and Automa-
tion. Vol. 1. Ieee. pp. 434–439.
Durrant-Whyte, H. and T Bailey (2006). Simultaneous localization and mapping:

part I. IEEE Transactions on Robotics & Automation 13(2), 99–110.
Elfes, A (1989). Using occupancy grids for mobile robot perception and navigation.
IEEE Computer 22(6), 46–57.
Evans, M, C W Fox, M J Pearson and T J Prescott (2009). Spectral Template

Based Classification of Robotic Whisker Sensor Signals in a Floor Texture
Discrimination Task. In: Proc. of Towards Autonomous Robotic Systems.
pp. 19–24.
Fox, C and M Evans (2012). Tactile SLAM with a biomimetic whiskered robot. In:
Proc. of IEEE International Conference on Robotics and Automation. Saint
Paul, MN. pp. 4925 – 4930.
Fox, C W, M H Evans, N F Lepora, M Pearson, A Ham and T J Prescott (2011).

CrunchBot: a mobile whiskered robot platform. In: Proc. of Towards Au-
tonomous Robots. Sheffield, UK. pp. 102–113.
Franklin, R c., L Gatos and F I. Fuller (1978). Electronic wall stud sensor, US
Patent 4,099,118.
Gamallo, C, M Mucientes and C V Regueiro (2013). A FastSLAM-based Algorithm

for Omnidirectional Cameras. Journal of Physical Agents 7(1), 13–22.
Grisetti, G, C Stachniss and W Burgard (2007). Improved Techniques for Grid

Mapping With Rao-Blackwellized Particle Filters. IEEE Transactions on
Robotics 23(1), 34–46.
150
References
Guanshan, H (2010). Neural Network Applications in Sensor Fusion for a Mobile

Robot Motion. In: Proc. of WASE International Conference on Information
Engineering. Ieee. Beidaihe, Hebei. pp. 46–49.
Gupta, V, G Singh, A Gupta and A Singh (2010). Occupancy grid mapping using
artificial neural networks. In: Proc. of International Conference on Industrial
Electronics, Control and Robotics. IEEE. Orissa. pp. 247–250.
Hightower, J and G Borriello (2004). Particle filters for location estimation in

ubiquitous computing: A case study. Ubiquitous Computing 3205, 88–106.
IndoorAtlas Ltd (2012). Ambient magnetic field-based indoor location technology.
Kam, M., P. Kalata and Z. Xiaoxun (1997). Sensor Fusion for Mobile Robot Nav-
igation. Proceedings of the IEEE 85(1), 108–119.
Karray, F. O. and C. De-Silva (2004). Soft Computing and Intelligent System

Design. Pearson Education Limited.
Kim, H, Y. Ha and G. Jin (2003). A study on the enviromnental map building for
a mobile robot using infrared range-finder sensors. In: Proc. of IEEE/RSJ
International Conference on Intelligent Robots and Systems. Vol. 1. IEEE.
Las Vegas, USA. pp. 711–716.
Koval, V, O Adamiv and V Kapura (2007). The Local Area Map Building for
Mobile Robot Navigation Using Ultrasound and Infrared Sensors. In: Proc.
of IEEE Workshop on Intelligent Data Acquisition and Advanced Comput-
ing Systems: Technology and Applications. number September. Dortmund,
Germany. pp. 454–459.
Kunze, K, P Lukowicz, K Partridge and B Begole (2009). Which Way Am I Fac-

ing: Inferring Horizontal Device Orientation from an Accelerometer Signal.
In: Proc. of IEEE International Symposium on Wearable Computers. Linz,
Austria. pp. 149–150.
151
References
Lee, S (2006). Recursive Particle Filter with Geometric Constraints for SLAM. In:
Proc. of IEEE International Conference on Multisensor Fusion and Integra-
tion for Intelligent Systems. Heidelberg. pp. 395–401.
Lee, S (2008). Use of infrared landmark zones for mobile robot localization. In-
dustrial Robot: An International Journal 35(2), 153–159.
Lee, Y, B Yim and J Song (2009). Mobile robot localization based on e↵ective
combination of vision and range sensors. International Journal of Control,
Automation and Systems 7(1), 97–104.
Little, J.J., J. Lu and D.R. Murray (1998). Selecting stable image features for
robot localization using stereo. In: Proc. of IEEE/RSJ International Confer-
ence on Intelligent Robots and Systems. Innovations in Theory, Practice and
Applications. Vol. 2. Victoria, BC. pp. 1072–1077.
Magnenat, S, V Longchamp, M Bonani, P Retornaz, P Germano, H Bleuler and

F Mondada (2010). A↵ordable SLAM through the co-design of hardware and
methodology. In: Proc. of IEEE International Conference on Robotics and
Automation. Anchorage, USA. pp. 5395–5401.
Matsebe, O, M Namoshe, N Tlale, M Pretoria and S Africa (2010). Basic Ex-

tended Kalman Filter Simultaneous Localisation and Mapping. In: Robot
Localization and Map Building (Hanafiah Yussof, Ed.). pp. 39–59. Number 1.
Menegatti, E., G. Gatto, E. Pagello, T Minato and H Ishiguro (2008). Robot lo-
calisation enhanced by a distributed vision system in dynamic environments.
Sensor Review 28(3), 222–228.
Miller, J., H de Plinval and K. Hsiao (2005). Mapping contoured terrain: A

comparison of SLAM algorithms for radio-controlled helicopters. Cognitive
Robotics.
Miller, L F and L Gatos (2006). Auto-deup scan for capacitive sensing, US Patent
7,148,703.
152
References
Mitchell, H.B. (2007). Multi-Sensor Data Fusion. Springer Berlin Heidelberg.

Berlin, Heidelberg.
Montemerlo, M, S Thrun, D Koller and B Wegbreit (2002). FastSLAM : A Factored

Solution to the Simultaneous Localization and Mapping Problem. In: Proc. of
the AAAI National Conference on Artificial Intelligence. Edmonton, Canada.
pp. 593–598.
Montemerlo, M, S Thrun, D Koller and B Wegbreit (2003). FastSLAM 2 . 0

: An Improved Particle Filtering Algorithm for Simultaneous Localization
and Mapping that Provably Converges. In: Proc. of the Sixteenth Interna-
tional Joint Conference on Artificial Intelligence (IJCAI). Acapulco, Mexico.
pp. 1151–1156.
Moreno, L, J Armingol, S Garrido, A Escalera and M Salichs (2002). A genetic

algorithm for mobile robot localization using ultrasonic sensors. Journal of
Intelligent and Robotic Systems 34(2), 135–154.
Myung, H, H Lee, K Choi and S Bang (2010). Mobile robot localization with
gyroscope and constrained Kalman filter. International Journal of Control,
Automation and Systems 8(3), 667–676.
Namoshe, M, O Matsebe and N Tlale (2010). Corner Feature extraction : tech-

niques for landmark based navigation system, Sensor Fusion and it’s Appli-
cation. InTech.
Newman, P., J. Leonard, J.D. Tardos and J Neira (2002). Explore and return:
experimental validation of real-time concurrent mapping and localization. In:
Proc. of IEEE International Conference on Robotics and Automation. Vol. 2.
IEEE. Washington, USA. pp. 1802–1809.
Nguyen, V., A. Martinelli, N. Tomatis and R. Siegwart (2005). A comparison

of line extraction algorithms using 2D laser rangefinder for indoor mobile
robotics. In: Proc. of IEEE International Conference on Intelligent Robots
and Systems. Ieee. Edmonton, Canada. pp. 1929–1934.
153
References
Nieto, J., J. Guivant, E. Nebot and S. Thrun (2003). Real time data association
for FastSLAM. In: Proc. of IEEE International Conference on Robotics and
Automation. Taipei, Taiwan. pp. 412–418.
Patel, S, K Truong and G Abowd (2006). PowerLine Positioning : A Practical Sub-

Room-Level Indoor Location System for Domestic Use. In: Proc. of the 8th
international conference on Ubiquitous Computing. Springer-Verlag. Berlin,
Heidelberg. pp. 441–458.
Pearson, M J, C Fox, J C Sullivan, T J Prescott, T Pipe and B Mitchinson

(2013). Simultaneous localisation and mapping on a multi-degree of free-
dom biomimetic whiskered robot. In: Proc. of International Conference on
Robotics and Automation. Karlsruhe, Germany. pp. 586–592.
Perez, P, J Vermaak and A Blake (2004). Data fusion for visual tracking with
particles. Proceedings of the IEEE 92(3), 495–513.
Ramirez-Serrano, A, H Liu and G C. Pettinaro (2008). Mobile robot localization

in quasi-dynamic environments. Industrial Robot: An International Journal
35(3), 246–258.
Ranky, G (2007). Mobile robot sensor and networking design to enable cooperative
behavior. Industrial Robot: An International Journal 34(1), 21–25.
Rencken, W.D. (1993). Concurrent localisation and map building for mobile robots
using ultrasonic sensors. In: Proc. of the IEEE/RSJ International Conference
on Intelligent Robots and Systems. Tokyo, Japan. pp. 2192–2197.
Riisgaard, S and MR Blas (2004). SLAM for Dummies. A Tutorial Approach to

Simultaneous Localization and Mapping.
Rikoski, R.J., J.J. Leonard and P.M. Newman (2002). Stochastic mapping frame-
works. In: Proc. of IEEE International Conference on Robotics and Automa-
tion. Vol. 1. Washington, USA. pp. 426–433.
154
References
Schön, T, D Törnqvist and F Gustafsson (2007). Fast particle filters for multi-rate
sensors. In: Proc. of 15th European Signal Processing Conference. Pozna,
Poland.
Slušný, S, R Neruda and P Vidnerová (2009). Localization with a low-cost robot.

In: Proc. of the Conference on Theory and Practice on Information Tech-
nologies. Vol. 584. Kralova studna, Slovakia. pp. 77–80.
Steinho↵, U and B Schiele (2010). Dead Reckoning from the Pocket - An Ex-
perimental Study. In: Proc. of IEEE International Conference on Pervasive
Computing and Communications. Mannheim, Germany. pp. 162–170.
Thrun, S (1996). Integrating Grid-Based and Topological Maps for Mobile Robot
Navigation. In: Proc. of 13th National Conference on Artificial Intelligence.
number August. Portland, Oregon. pp. 944–950.
Thrun, S, W Burgard and D Fox (2006). Probabilistic Robotics. Massachusetts

Institute of Technology. Cambridge.
Varveropoulos, V (2005). Robot Localization and Map Construction Using Sonar

Data. The Rossum Project 10, 1–10.
Yang, X., M. Moallem and R.V. Patel (2006). a Sensor-Based Navigation Al-
gorithm for a Mobile Robot Using Fuzzy Logic. International Journal of
Robotics and Automation 21(2), 129 – 140.
155
Appendices
156
Appendix A
Light Sensors for Robot

Localization
Light sources are typically fairly static within a room . A low cost light intensity
sensor could be used to detect the direction of the light. This would help not only
correct position but also the robots heading. The system could be improved by
also scanning upwards in pitch. Technically these two angles would be able to
position the robot on a 2 dimensional map although in practice the noise is likely
too large to use reliably on its own. Another improvement to assist in identifying
the light source would be to use multiple sensors with di↵erent filters for di↵erent
components of the visible light spectrum. This would allow a signature to be
devised that would help di↵erentiate di↵erent light sources. As well as allowing
the robot to track more than one light source at a time.
The problem that this section will investigate, is the use of a light sensor to
handle robot turn angles. Many kinematic models for robots are not reliable for
predicting the turn angle of the robot. One reason for this is that wheel encoders
contain large errors as a result of wheel slippage. Common solutions to this are
to use magnetometers or gyroscopes to track the heading, but magnetometers
are commonly interfered by metallic objects, and low cost gyroscopes are difficult
to calibrate resulting in inaccuracies. As well gyroscopes only provide a relative
157
Appendix A. Light Source Tracking for Robot Localization
Figure A.1: TSL235 light to frequency converter used in this research. (From
www.digikey.ca)
heading. This proposed solution will provide both a reference based solution that
will not be interfered by metallic objects. Also integration error will be much less
then with gyroscope based solutions.
A.1 Methodology
The robot platform was utilized for these experiments. It was configured with a
low cost light intensity sensor mounted to a servo to perform light scans of the
environment. The chosen sensor was the TSL235 light to frequency converter, see
Figure A.1. This sensor was then interfaced with the robot’s microcontroller and
controlled over bluetooth in the custom matlab test suite.
A.2 Turn Angle Detection
One challenge in dealing with many robot designs is detecting the turn angle. This
is especially true with robots that turn by reversing directions of opposing wheels.
This technique results in lots of wheel slippage that makes it nearly impossible to
detect the turn angle using wheel encoders.
158
Treating the 360 degree light intensity maps as a discrete signal allows the use of
Digital Signal Processing DSP techniques to resolve the likely turn angle. This
approach works by performing one scan before beginning the turn and treating
this as the reference. The robot then performs the turn in place and does another
scan. This second scan can then be compared with the first scan to determine the
likely turn angle.
Where the signals best match will be the o↵set between the two signals representing
the turn angle. The technique used to compare these two signals is the cross
covariance. The cross covariance is the mean-removed cross-correlation defined as
⇥xy (m) = E{(x(n + m) µx )(y(n) µy )}, (A.1)
where ⇥xy is the cross-covariance of xy, E is the expected value operator, and
µmean is the mean of xy.
A.3 Real-time Turn Tracking
After determining that the angle can be captured using the light sensor an algo-
rithm was developed to make this work in real-time. This algorithm is shown in
Algorithm 10. This algorithm uses a simple particle filter to track the possible
rotations. This is useful, as the robot is turning wheel slippage can make the turn
rate inconsistent, and a particle filter is capable of capturing this.
The algorithm works by first performing a scan. This is accomplished by rotating

the servo mounted light sensor. It uses this as a light map of its environment and
compares this map to the readings taken while turning to correct for its likely
location. To perform the turn the robot points the light sensor directly ahead and
begins turning at a fixed rate. At specific intervals it performs the particle filter
steps shown in the algorithm. First predicting the particle filter new position, then
weighting all the particles by comparing the measured value against what is in the
159
Algorithm 10 Particle Filter Turn Tracking

1: for all ✓ in 1 to 360 do . Collect a Light Map
2: SensorSetAngle(✓)
3: Light M ap(✓) =SensorScan()
4: end for
5: P = zeros(N ) + randn(N )⇥
6: ✓estimate = 0
7: while ✓estimate < turn angle do . Perform the Turn
8: Perform turn(µ)
9: P = P + µ+randn(N )⇥ . Predict
10: W = Calc Weight(Light M ap(P ), SensorScan()) . Calculate Weight
11: P = Resample(P, W ) . Resample
12: ✓estimate = Extract Density(P, W ) . Calculate Estimate
13: end while
map at that position. Finally using these weights to resample. The estimated
value can be solved by extracting the center of particle density.
The weight calculation can be solved simply as the di↵erence between the actual
measured sensor value and the light map value at that particles predicted location.
Results of Turn Angle Detection Realworld data was collected using the
robot platform configured with a light sensor mounted to a servo. Two scans
were performed before and after the robot performed a turn, shown in Figure A.2.
This scan was performed in an office environment with natural light source from a
window. There was also a nearby computer monitor with minor dynamic lighting.
Performing the cross-covariance between these two light sensor streams is shown
in Figure A.3. The maximum value of this plot indicates the likely turn angle.
The total length is 720 two time 360. If the maximum value was a 360 the signals
are matched best in phase and no turn has occurred. In this plot the most likely
turn angle is shown at 288 which is 360 288 = 72 degrees.
After realigning these two signals the match plot is shown in figure A.4. Although
not a perfect match enough information was contained within the pattern to cap-
ture the approximate angle within 20 degrees .
160
Figure A.2: Light sensor scans before and after turn.
Figure A.3: Cross covariance of light scans.
161
Figure A.4: Light sensor scans after being shifted to best match.
A.4 Summary
This work investigated the unconventional use of a light sensor for the purpose of
tracking the turn angle of a robot. Some robots have difficulty tracking turns do
to wheel slippage. Using a low cost light sensor can be a simple way to provide
feedback on the turn angle.
This work demonstrated a simple method to achieve this. The particle filter used
could easily be adapted to include a range of light sensors each with di↵erent light
filters or other angular measurements such as a directional microphone to listen
to the ambient noise within the environment.
This work could be made faster by using many fixed light sensors pointing in
di↵erent directions rather then a single light sensor mounted on a servo.
162
Appendix B
Sample Code of Core

Components
This section will demonstrate some sample code of the core software components
in the custom Matlab framework developed in this work. E↵orts are currently
underway to open source this code for public use. The intent is to provide an
idea of how the interfaces work to help better understand the research, it is not a
complete copy of the code.
B.1 Main Loop
The main loop configures the test setup and runs the simulation.
1 %%
2 clear s a
3 clear
4
5 %% Configuration
6
7 %Setup Output
8 output = out();
163
Appendix B. Sample Code of Core Components
9 output.addVisual(visParticle());
10 output.addVisual(visMap());
11 output.addVisual(visError());
12 %output.addVisual(visFeaturesIr());
13 %output.addVisual(visFeaturesSonar());
14 output.addVisual(visTrue());
15
16 %Setup map source and simulator

17 m = map1();
18 s = simRobo(output,{ IR }, m);
19 output.addData( obstacles ,s.m obstacle); %Add map obstacles ...
to output
20
21 %Setup algorithm
22 a = venn(100, output, m);
23 %a = fastOccSlam(5,output);
24 %a = fastSlam(100,output);
25
26
27 %%Run Simulation
28 pos estimate = zeros(3,1);
29 RandStream.setDefaultStream(RandStream( mcg16807 , Seed ,0))
30 complete = false;% initial non-zero values
31 while (¬complete)
32 %Get data
33 [pos update, complete] = s.map drive(1);
34 scan = s.robot scan();
35
36 %Run algorithm
37 pos estimate = a.step(scan,pos update);
38
39 %handle output and plot results

40 output.addData( actual ,s.m pos);
41 output.addData( estimate ,pos estimate);
42 output.draw();
43 end
164
B.2 Output module
The output module simply creates a list of the visualization objects and passes
the data and draw commands. The simplicity of this module does not reflect
its utility, with this module a simple command anywhere in the code centralizes
the data collection. Which allows quick and easy visualization of data from very
seperate areas of the code.
1 classdef out < handle

2
3 properties
4 m list = {};
5 end
6
7 methods
8 %Call addData everytime data is produced that needs to be
9 %visualized.
10 function addData(this, type, data)
11 for i=1:length(this.m list)
12 this.m list{i}.addData(type,data)
13 end
14 end
15
16 %At configuration call this function to add a ...

visualization object
17 function addVisual(this, vis)
18 this.m list{end+1} = vis;
19 end
20
21 %Call Draw to force each visualization to plot ...

whatever data it
22 %currently has
23 function draw(this)
24 for i=1:length(this.m list)
25 this.m list{i}.draw();
26 end
165
27 drawnow;
28 end
29 end
30 end
Visulization This is a sample visualization module, this simply takes one pa-
rameter theta p and plots it every time draw is called.
1 classdef visTheta< handle

2 %UNTITLED Summary of this class goes here
3 % Detailed explanation goes here
4
5 properties
6 m figName = Heading probability ;
7 m map;
8 end
9
10 methods
11 function this = visTheta( )
12 fig(this.m figName);
13 end
14 function draw(this)
15 if(¬isempty(this.m map))
16 theta = [];
17 rho = [];
18 for t = this.m map.theta - ...
3⇤this.m map.std:0.1:this.m map.theta + this.m map.std
19 theta = t;
20 rho =
21 end
22 end
23 end
24 function addData(this, type, data)
25 if (strcmp(type, theta p ))
26 this.m map = data;
27 end
166
28 end
29 end
30 end
B.3 Algorithm Module
The algorithm module is the core of the research. The example algorithm here is
the SLAM algorithm used in the final chapter of the research.
1 classdef mySlam <handle

2 %SIM Summary of this class goes here
4
5 properties
6 m out
7 m map
8 m particles = [] %.x is the robot [x y] .t is the ...
theta, senorList is a list of senor thisects, each contains ...
its own map data. w is the particle weight
9 m prob move = [1, .005]
10
11 m best = 1;
12 m x = [0,0,0]
13
14 m resample count = 0;
15 m resample interval = 1;
16
17 m weighted out = false;

18
19 m cap save=[];
20 m light save=[];
21 end
22 methods
23 function this = mySlam( num, output, map )
24 this.m out = output;
167
25 this.m map = map;

26 for i = 1 : num
27 this.m particles(i).x = [0,0,0];
28 this.m particles(i).m = [];
29 this.m particles(i).p = [];
30 this.m particles(i).w = 0.5;
31 this.m particles(i).sensorList = [];
32 end
33 end
34 function X = position predict(this, x, u)
35 X(3) = mod((x(3) + u(2)+ ...
randn(1)⇤this.m prob move(2)),2⇤pi) ;
36 x(3) = X(3);
37 X(1:2) = sensor2nav([u(1) + ...
randn(1).⇤this.m prob move(1), 0],x);
38 end
39 function resample(this)
40 m = max([this.m particles(:).w]);
41 for i = 1:length(this.m particles)
42 this.m particles(i).w = this.m particles(i).w/m;
43 end
44 w = [this.m particles(:).w];
45 [keep, ¬] = stratified resample(w);
46 p saved = this.m particles;
47 for i = 1:length(keep)
48 this.m particles(i) = p saved(keep(i));
49 %copy the sensor objects
50 for j = 1:length(this.m particles(i).sensorList)
51 this.m particles(i).sensorList{j} = ...
this.m particles(i).sensorList{j}.copy();
52 end
53 end
54 end
55 function list = init sensorList(this, scan)
56 list = cell(length(scan) ,1);
57 leg = cell(length(scan) ,1); %legend of sensors
58 for i=1:length(scan)
59 leg{i} = scan(i).type;
168
60 if (strcmp(scan(i).type, ir ))
61 list{scan(i).id} = irAlg(this.m out);
62 elseif (strcmp(scan(i).type, sonar ))
63 list{scan(i).id} = sonarAlg(this.m out);
64 elseif (strcmp(scan(i).type, capacitive map ))
65 if(isempty(this.m cap save))
66 this.m cap save = ...
capAlgMap(this.m out, this.m map); %only init once
67 end
68 list{scan(i).id} = this.m cap save;
69 elseif (strcmp(scan(i).type, capacitive map NN ))
71 this.m cap save = ...
capAlgMapNN(this.m out, this.m map); %only init once
72 end
74 elseif (strcmp(scan(i).type, capacitive ))
76 this.m cap save = capAlg(this.m out, ...
this.m map); %only init once
77 end
79 elseif (strcmp(scan(i).type, light ))
80 if(isempty(this.m light save))
81 this.m light save = ...
lightAlgMap(this.m out, this.m map); %only init once
82 end
83 list{scan(i).id} = this.m light save;
84 else
85 fprintf( UNKNOWN SENSOLR TYPE: %s\n , ...
scan(i).type)
86 end
87 end
88 this.m out.addData( weights breakdown leg ,leg)
89 end
90 %step
91 function pos estimate = step(this, scan, pos update)
169
92 w breakdown = ...
zeros(length(this.m particles),length(scan));
93 for m = 1:length(this.m particles)
94 %setup
95 if (isempty(this.m particles(m).sensorList))
96 this.m particles(m).sensorList = ...
this.init sensorList(scan);
97 end
98
99 %predict
100 this.m particles(m).x = ...
this.position predict(this.m particles(m).x, pos update);
101
102 %Weight calculation / Fusion

103 w = zeros(1,length(scan));
104 for i=1:length(scan)
105 w(i) = ...
this.m particles(m).sensorList{scan(i).id}.calc weight(scan(i).data, ...
this.m particles(m).x, (m==this.m best));
106 end
107 w breakdown(m,:) = w;
108 this.m particles(m).w = sum(w(¬isnan(w)));
109 end
110 this.m out.addData( weights breakdown ,w breakdown)
111
112 %resampling
113 w = [this.m particles(:).w];
114 this.m out.addData( weights ,w./max(w));
115 this.m resample count = this.m resample count + 1;
116 if (this.m resample count ...
this.m resample interval && ...
117 sum([this.m particles(:).w]) > 0 )
118 this.resample();
119 this.m resample count = 0;
120 end
121
122 %select best particle

123 [ ¬, this.m best] = max([this.m particles(:).w]);
170
124
125 %weighted average

126 if(this.m weighted out)
127 x = [this.m particles(:).x];
128 this.m x(1) = sum(x(1:3:end) .⇤ ...
[this.m particles(:).w])/sum([this.m particles(:).w]);
129 this.m x(2) = sum(x(2:3:end) .⇤ ...
[this.m particles(:).w])/sum([this.m particles(:).w]);
130 this.m x(3) = this.m particles(this.m best).x(3) ;
131 else
132 % this.m x = this.m particles(this.m best).x;
133 x = [this.m particles(:).x];
134 X=reshape(x,3,length(x)/3);
135 num = 3;
136 [idx,c] = ...
kmeans(X(1:2,:) , num, emptyaction , drop );
137 best = 1;
138 for i = 1:num
139 if (sum([this.m particles(idx==i).w]) > ...
sum([this.m particles(idx==best).w]))
140 best = i;
141 end
142 end
143 this.m x(1:2) = c(best,:);
144 this.m x(3) = X(3,1);% meanangle(X(3,idx==best));
145 this.m out.addData( clusters ,idx);
146 this.m out.addData( cluster best ,best);
147 end
148 %this.m x = this.m particles(this.m best).x;
149 %density match to determine best estimate from all ...
particles
150
151 %fprintf ( Update %d (%f, %f, %f)\n , this.m best, ...

this.m x(1),this.m x(2),this.m x(3));
152 this.m out.addData( particles ,this.m particles);
153 this.m out.addData( best ,this.m best);
154 pos estimate = this.m x(1:3);
155 end
171
156 end
157 end
B.4 Simulator Module
The simulator module allows repeatable and configurable tests to be done quickly.
The core simulator module is shown here.
1 classdef simRobo <handle

2 %SIM Summary of this class goes here
4
5 properties
6 m out;
7 m map;
8
9 sensorList = {};
10
11 m pos = [50, 50, 0] %X Y DIR

12 %m prob move = [1, .005]
13 m prob move = [0.75, 0.0]
14
15 %for collision detection

16 m obstacle;
17 m fig = 1;
18 end
19
20 methods(Static)
21 function min dist = raycast(pos,dist, obs)
22 %return minimum distance to obstacle
23 min dist = inf;
24 for i=1:length(obs)
25 l = sensor2nav([dist 0],pos);
26 l1 = [pos(1) pos(2) l(1) l(2)];
27 l2 = obs(i,:);
172
28
29 %check parrallel
30 [x,y] = lineintersect(l1, l2);
31 if( max(isnan([x y])) == 0 && x < inf && y < ...
inf )
32 new dist = sqrt( (pos(1) - x)ˆ2 + (pos(2) ...
- y)ˆ2 );
33 if( new dist < min dist )
34 min dist = new dist ;
35 end
36 end
37 end
38 end
39 end
40
41 methods
42 %Constructor
43 function obj = simRobo(output, sensorConfig, map)
44 obj.m out = output;
45 obj.m map = map;
46 %append the boarder with 4 walls to obstacles
47 fprintf( construct\n )
48
49 %get map
50 [obj.m obstacle,joist, light source] = map.get map();
51
52 %build sensors
53 for i=1:length(sensorConfig)
54 switch(sensorConfig{i})
55 case IR
56 obj.sensorList{end+1} = ...
simIR(obj.m obstacle);
57 case SONAR
simSonar(obj.m obstacle);
59 case ...
{ capacitive , capacitive map , capacitive map NN }
173
simCapacitive(joist.spacing,joist.width,joist.offset,joist.axis, ...
joist.theta,map,obj.m out);
61 obj.sensorList{end}.m type = ...
sensorConfig{i};
62 case light
simLight(light source);
64 otherwise
65 error([ SENSOR UNSUPPORTED ...
sensorConfig{i}])
66 end
67 end
68 end
69
70 function scan = robot scan(this)

71 scan = ...
struct( type , , id ,num2cell(1:length(this.sensorList)), data ,[]);
72 for i = 1:length(this.sensorList)
73 scan(i).data = ...
this.sensorList{i}.scan(this.m pos);
74 scan(i).type = this.sensorList{i}.getType();
75 end
76 end
77
78 function pos = robot set position(obj, pos)

79 obj.m pos = pos;
80 end
81 function [pos dist] = robot move(obj, dist)
82 if( simRobo.raycast(obj.m pos, dist+5, ...
obj.m obstacle) == inf )
83 obj.m pos(1:2) = sensor2nav([dist+ ...
randn(1).⇤obj.m prob move(1),0],obj.m pos);
84 %obj.m pos(1) = obj.m pos(1) + ...
sin(obj.m pos(3))⇤dist + randn(1)⇤obj.m prob move(1);
85 %obj.m pos(2) = obj.m pos(2) + ...
cos(obj.m pos(3))⇤dist + randn(1)⇤obj.m prob move(1);
174
86 fprintf( ROBOT MOVED (%f, %f, ...

%f)\n ,obj.m pos(1),obj.m pos(2),obj.m pos(3));
87 else
88 dist = 0;
89 fprintf( COLLISION\n );
90 end
91 pos = obj.m pos;
92 end
93
94 function pos = robot turn(obj, theta)

95 obj.m pos(3) = mod((obj.m pos(3) + theta+ ...
randn(1)⇤obj.m prob move(2)),2⇤pi);
96 pos = obj.m pos;
97 end
98 function [pos update, complete] = map drive( obj, ...
drive distance )
99 %get control signals from map
100 [pos update(1), pos update(2)] = ...
obj.m map.get next control( drive distance );
101 if( pos update(1) == 0 && pos update(2) == 0)
102 complete = true;
103 else
104 complete = false;
105 end
106 obj.m pos = obj.m map.m pos;
107 %add movement noise
108 pos update(1) = pos update(1) + ...
randn(1).⇤obj.m prob move(1);
109 pos update(2) = mod(( pos update(2)+ ...
randn(1)⇤obj.m prob move(2)),2⇤pi);
110 end
111 end
112 end
175
Appendix C
Improved Robustness Using

High-Level Multi-Sensor SLAM
This paper explores the problem of Simultaneous Localization and Mapping (SLAM)
with a focus on combining di↵erent sensors to build a more robust, accurate, and
reliable localization framework. Localization without prior knowledge of the envi-
ronment is considered to be one of the more challenging problems in the field of
robotics.
The core contribution of the work is the development of a high-level sensor fusion
solution. Which enables simple integration of unconventional sensors not typically
used for robot localization. A joist sensor and light sensor are successfully fused
into the solution resulting in improved localization performance. Furthermore
the system is shown to improve robustness to sensor failure and help solve the
kidnapped robot problem.
C.1 Introduction
Simultaneous Localization and Mapping (SLAM) is often considered one of the

more challenging problems in mobile robotics (Beevers and Huang, 2006; ?). The
difficulty is a result of the large amount of uncertainty in both sensory information
176
Appendix C. Improved Robustness Using High-Level Multi-Sensor SLAM
and the control system. The challenge is essentially trying to make the best
guess of the local map, given all of the uncertainties. The SLAM problem was
first well defined in 1986 at the IEEE Robotics and Automation Conference in
San Francisco (Durrant-Whyte and Bailey, 2006). This was the beginning to
approaching these problems with a probabilistic approach, rather than the more
conventional absolute approach.
Robot localization is a growing requirement for mobile robots, both in industrial

and consumer applications. The Mars Rover and other planetary rovers have sig-
nificant lag between any control signals on Earth and the rover being controlled.
For this reason, SLAM algorithms allow the vehicle to handle basic obstacle avoid-
ance and finding safe modes of passage. There is also a growing industry for low
cost household robots, especially for cleaning (Bloss, 2008). These robots tradi-
tionally have used simple random navigation schemes, but new products are now
beginning to map the environment allowing greater ability to ensure surface cover-
age in less time as well as more advance features such as returning to the charging
station when batteries are low. Localizing the robot is di↵erent from robot naviga-
tion, although they are related. Robot navigation attempts to find optimal paths
through the environment while avoiding obsticles, one method is using fuzzy logic
(Yang et al., 2006). The map and robot position used for navigation algorithms
is the focus of this research.
The SLAM algorithm has had a variety of di↵erent solutions proposed. The most
popular approaches are the Extended Kalman Filter and FastSLAM algorithms.
Both of these approaches track the robot and map features as a state estimate
and attempt to converge on the solution where error is reduced. These methods
will be discussed in detail here.
FastSLAM is a newer approach utilizing the Rao-Blackwellized particle filters

(Durrant-Whyte and Bailey, 2006; Thrun et al., 2006; Nieto et al., 2003). In this
technique, multiple possible solutions are tracked through particles. Each particle
is given a weight corresponding to how well that particle’s map matches its sen-
sors. Resampling removes unlikely particles proportional to the particle’s weight.
177
In this way more likely solutions survive and less likely solutions disappear. One
advantage of FastSLAM of other SLAM solutions, are that multiple hypotheses
can be simultaneously tracked.
The novel simplification of the Rao-Blackwellized particle filter is that each particle
contains a state estimate of Gaussian likelihoods for features along with covariance
matrices, similar to the Kalman filter. In this way each particle contains a unique
map of the feature space. This allows for the efficiency benefits achivied through
kalman filter, but using particle filters allows for much more dynamic and complex
situations to be handled.
A single sensor can significantly reduce the performance of robot SLAM, especially
in more complex environments in the real-world (Davis and Stentz, 1995). For this
reason the field of Multi-Sensor SLAM investigates how multiple sensors could be
used together to improve the SLAM solution.
Fusion algorithms are also commonly categorized as either low-level fusion or hig-
level fusion (Mitchell, 2007). Low-level fusion represents fusion done on the data
directly, taking advantage of specific details of the data. High-level fusion more
abstractly deals with merging the data more generally. The work in this paper
explores a higher level fusion approach for FastSLAM that allows fusion of very
di↵erent sensors.
One example of a di↵erent sensor that could be incorporated to assist with localiza-
tion is floor joist sensors. Capacitive sensors have been shown capable of detecting
the location of floor joists through the floor (Miller and Gatos, 2006; Baxter, 1997).
This technology is commonly used for finding the position of studs in walls using
a handheld unit, but the technology is transferable to floor joists detection. A
capacitive sensor can measure the relative density of a material in close proximity,
and through a simple detection circuit can determine the presence of a wall stud or
floor joist. It will be demonstrated that incorporating this technology into mobile
robots could provide both increased accuracy and robustness to sensor failure.
178
Robustness to sensor failure ensures that the system can still operate when one or
more sensors fail. Another measure of robustness is the kidnapped robot problem.
Solving this problem is considered a more complex version of the global optimiza-
tion problem (Choset et al., 2005). In the kidnapped robot problem, the robot is
unknowingly moved to a new location, and the robot must detect this event and
correctly solve for the new position. In many scenarios this event may be consid-
ered unlikely, but this is still considered important as no localization algorithm
can be guaranteed to never fail. Being capable of resolving the kidnapped robot
problem provides an indication of the systems ability to recover from error(Thrun
et al., 2006).
C.2 The Proposed Method
The algorithm was implemented in Matlab and tested with simulated sensor data.
The intent of these experiments were to test the benefits of redundant sensors
that measure di↵ering feature spaces. The localization algorithm which was devel-
oped is an extension of the conventional FastSLAM algorithm; enabling high-level
scalable sensor fusion. The design of the algorithm is opportunistic and uses sen-
sor data which is provided, but does not fail if no sensor data is received. One
of the core reasons for selecting the FastSLAM algorithm is its ability to handle
non-Gaussian errors and track multiple hypotheses.
Algorithm The core component of the FastSLAM algorithm contains a particle

filter, which was used as the interface for the sensors. Each sensor is abstracted
into separate modules each containing unique implementations of the sensor lo-
calization algorithms. These sensor modules must be capable of interpreting the
sensor data and proposed robot state and providing a weight estimate proportional
to the likelihood that the robot is in that state. The sensor module should also
learn and adapt its feature map over time, but this is not strictly required. The
individual sensor weights are then combined to generate a fused weight estimate.
179
The algorithm contains the same fundamental flow as traditional FastSLAM, and
works by iteratively moving through 4 states; move, predict, scan, update. The
algorithm first performs a movement and records the control signal. The control
signal is used to predict the new locations of the particles. After the movement,
all sensors are scanned and each particle uses the sensor data to calculate its
new weight estimate. These weights are combined to produce a fused weight
estimate that should be proportional to the overall state confidence. Finally, the
robot performs resampling to remove the least likely particles and converge on the
correct solution.
The first time the algorithm is run it initializes all the particles and sensor objects.
The algorithm performs lazy initialization of the sensor objects. The advantage of
lazy initalization allows many sensor implementations to be implemented in soft-
ware and only the sensors currently available and operational will be started. This
approach also allows the algorithm to easily handle plug and play type functional-
ity with sensors. When a new sensor is added, an object can be easily created to
handle it, and the new sensor can be immediately used to assist with localization.
The sensor fusion works by combining the weight estimates of each sensor. The
combination algorithm can be virtually any method. For this research, the sum of
the weights was chosen as the fusion approach. Each algorithm must produce their
own representation of the confidence; given the particle’s location, and the sensor
data provided. The confidence value is typically in the range of 0 to 1, but the
sensors can have their own map confidence that could modify this normal value
based on how well trained they believe the map is. The sensor can also return
a null value to indicate it has no knowledge of the area, or as a result of sensor
failure.
Critical to this algorithm is the weight values provided by the sensor modules. It
is important to recognize that there are two important aspects to the weight val-
ues. The relative weight values produced from one sensor to another sensor. For
example, if one sensor’s LOW confidence, is equal to another sensor’s HIGH confi-
dence, the first sensor will be virtually ignored. Also the variance of the weights is
180
important, if a LOW confidence is represented by 0.6, and a high confidence is 0.7,

then there is not much variance in the measurements and resampling will not be
as e↵ective. Some of this can be resolved by normalizing the weights. Resampling
is the process of selecting the new set of particles based on the previous set. It
works by selecting the new particles proportional to their fused weight values.
The final component of the algorithm is to extract the location estimate from the
particles. The approach used in this work used a k-means clustering algorithm to
extract the di↵erent particle groups. After the di↵erent groups have been clustered,
the best group is chosen as the cluster with the best cumulative fused weight. The
algorithm then uses the centroid of the cluster as the solution. This work used a
fixed number of clusters of 3.
Sensors The conventional sensors used for robot localization are vision based or
distance based sensors (Lee et al., 2009). The most common of these are Cameras,
Ultrasonic, Infrared and Laser scanners. In this research a distance based sensor is
used to represent the conventional approach. This sensor is tuned to be comparable
in performance to an infrared sensor.
The robot also contains an encoder that helps track the distance travelled in
response to the control commands. This encoder is imperfect and introduces
position uncertainty. The encoders are not suitable for tracking turn angles, as
wheel slippage is difficult to predict. To track angles, a combination magnetometer
and gyroscope are used. The gyroscope provides accurate orientation tracking for
short periods, but the errors accumulate over time. The magnetometer contains
more error but provides a reference to north, which can help correct the long-term
error from the gyroscope.
Two addition unconventional sensors were also integrated into the algorithm; a
light sensor, and a joist sensor. The light sensor can measure the angle to a
known light beacon, and the joist sensor can measure the presence of a floor joist
directly below the robot. Both of these sensors are tested with known maps of
181
their environment, but the framework supports learning these maps and future
work will explore possible learning approaches.
Handling Sensor Failure The method used to test the robustness of the algo-
rithm to sensor failure was to artificially cause a sensor failure during a simulation.
A sensor failure could occur from a variety of sources, such as; environmental in-
terference, hardware failure, or software failure. In this case the sensor driver is
capable of detecting the failure, but not recovering and stops producing samples.
The algorithm must then attempt to coast without data from this sensor.
When a sensor failure event occurs, the algorithm stops retrieving the data from
that sensor, but is capable of seamlessly continuing using the other sensors. The
design of the algorithm means that it simply uses the fused weights of the sensors
that are available. For example, if the distance sensors fails, the algorithm will
calculate the fused particle weight as the sum of the joist sensor and light sensor
weights. If the distance sensor recovers and begins to provide data again, the
particle still contains the sensor object, and will resume using that data.
Kidnapped Robot Problem The kidnapped robot problem was tested by

moving the robot to a new location without providing any knowledge of the event
to the localization algorithm. The algorithm must detect the event occurred and
recover. The robot being studied is capable of global orientation tracking with an
onboard magnetometer and gyroscope unit. This provides some indication of the
angle of the robot, which reduces the search-space to two dimensions. The prob-
lem is further bounded in the known map space, but not necessarily the explored
regions.
Detecting the kidnapped robot event was accomplished with a simple algorithm
that monitored the ratio between observed features and expected observed fea-
tures. If the ratio increases above a threshold, a kidnapped robot event is de-
tected. To reduce false positives, a count can ensure consecutive trials are all
above a threshold before triggering the event.
182
Recovering from the kidnapped robot event requires essentially solving the global
search problem, but this can be simplified with the known sensor maps. Using the
floor joist map and the light beacon location, a probability map can be calculated
that infers the likelihood of being at every position in the map. Particles are then
randomly distributed throughout this map proportional to the likelihood values.
These new particles then converge over the next couple iterations based on the
sensor data observed. This technique could easily scale to support other sensors.
For a sensor to be used for this purpose, the sensor object must be able to provide
a probability map given the sensor data.
C.3 Results and Discussion
Figure C.1: Comparison of localization ability for di↵erent sensor configura-

tions.
183
Table C.1: Performance Summary for Di↵erent Sensor Configurations During

Navigation
Sensor Configuration Mean Square Error Max Error

No Sensors 55.14 16.32
Distance Sensor Only 16.19 8.4
All Sensors 7.69 6.57
Three experiments were run to test the performance of the algorithm. The first
test will examine the ability of the algorithm to combine the new sensor types
into the solution in typical scenarios. The next experiment will test the enhanced
robustness of the solution to sensor failure. The final test will examine the ability
of the new sensors to solve the kidnapped robot problem.
C.3.1 Normal Operation
The results for robot localization are shown in Figure C.1. Three di↵erent sensor
configurations were tested; no sensors, distance sensor only, and a fused solution,
which combines the joist sensor, light sensor and the distance sensor. The plot
clearly shows the accumulating error when no sensors are available to provide
weight estimates. Both the distance sensor only and fused solutions are capable of
significantly enhancing the robot’s location estimate. A summary of these errors
can be seen in Table C.1.
These results provide a good indication that the algorithm is successfully fusing
the data from both capacitive and light sources into the solution. The top most
section of the route travels on a floor joist, which had the desire e↵ect of reducing
errors in that area. This behaviour could be introduced into the navigational logic
of the robot. For example, the robot could choose to follow floor joists where
possible knowing that it will provide an increased ability to track its location.
The e↵ect of this is similar to following a line, but the line is already present in
the environment.
184
(a)
(b)
Figure C.2: Robustness to sensor failure during robot localization: (A)) Using
all sensors; (B)) Using distance sensor only.
185
Table C.2: Performance Summary for Di↵erent Sensor Configurations During

Sensor Failure
Distance Sensor Only All Sensors

Mean Square Error Before Failure 7.97 8.27
Max Error Before Failure 7.46 6.86
Mean Square Error After Failure 163.30 20.65
Max Error After Failure 22.89 11.58
C.3.2 Sensor Failure
Sensor failure could occur for a variety of reasons, and the robot’s ability to handle
this scenario could be critical. In this experiment, the distance sensor fails during
the route, the robot must complete the path. Two configurations were tested;
the conventional distance sensor only, and the fused configuration which combines
distance sensor with light and floor joist sensors.
The results for this experiment are shown in Figure C.2. It is clearly observable
from these plots that the configuration with only distance sensors rapidly lose track
of its position after the sensor failure event. The configuration with redundant
sensors continues with only a moderate degradation in performance. The detailed
breakdown of the performance both before and after the sensor failure is shown
in Table C.2. These numbers show that the system did have reduced performance
but was still capable of tracking its location.
The advantage of using very di↵erent sensing types is that they are less likely
too simultaneously fail. For example, if an infrared sensor were being used as the
distance sensor, this could be interfered with bright light. A redundant infrared
sensor would have the same problem, but using sensors that measure very di↵erent
feature spaces provides reliable redundancy that could handle this.
C.3.3 Kidnapped Robot
The kidnapped robot problem examines the robots ability to recover from being
moved to a new position within the environment without the robots knowledge.
186
Commonly this problem is used as an indication of the ability for the algorithm
to recover from failure. The experiment was conducted by having the robot spon-
taneously jump forward on the path without providing any indication to the lo-
calization algorithm. This event was successfully detected and a series of particles
were generated proportional to the combined probability maps from light and joist
sensor modules. The particle distribution and full path are shown in Figure C.3
These results demonstrate a clear ability of the system to recover from the kid-
napped robot event. The particles rapidly re-converge on the correct location
through resampling based on the fused weights from the sensors. The advantage
of using the joist map and light sensor map is to provide more particles in the
more likely locations of the map, which would result in a faster convergence.
C.4 Conclusion
This Paper has demonstrated a technique for multi-sensor SLAM that utilizes
high-level fusion architecture to allow quick and easy configuration of sensors.
The algorithm is closely modelled after the conventional FastSLAM approach but
allows new information to be easily incorporated into the system.
The results demonstrated show that utilizing this algorithm and incorporating
capacitive and light sensors could not only increase accuracy but also improve
robustness to sensor failure. Using di↵ering sensors allows robustness to environ-
mental interference which may cause all of a similar sensor type to fail at once.
The approach was also shown to help resolve the kidnapped robot problem through
proposing more likely positions the robot is in, which assists in particle generation.
Future work will explore training the capacitive map in real-time and investigate
real-world environments.
187
(a)
(b)
Figure C.3: Results for kidnapped robot problem using all fused sensors. (A)
After detection of kidnapped robot event; (B) At completion of route.
188

Robot Localization Using Unconventional Sensors (PDFDrive)

Uploaded by

Copyright:

Available Formats

Robot Localization Using Unconventional Sensors (PDFDrive)

Uploaded by

Document Information

Original Title

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

Robot Localization Using Unconventional Sensors (PDFDrive)

Uploaded by

Copyright:

Available Formats

Robot Localization using Unconventional Sensors

In partial fulfilment of requirements

Guelph, Ontario, Canada

© Robert Oliver, January, 2015

ROBOT LOCALIZATION USING UNCONVENTIONAL SENSORS

Robert Oliver Advisor:

The second fusion approach proposes a complete multi-sensor SLAM algorithm

I would like to thank my family for their continued support in my

Finally I need to thank my loving fiancée Melissa for her continued

I also appreciate the support my colleagues at BlackBerry provided throughout my

List of Algorithms xiii

List of Abbreviations xiv

2 Background and Literature Survey 7

2.2.3 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . 34

4 Floor Joist Sensors for Robot Localization 61

5 Multi-Sensor Localization Using Unconventional Sensors 89

6 Multi-Sensor FastSLAM Using Unconventional Sensors 116

7 Conclusion and Future Work 142

A Light Sensors for Robot Localization 157

B Sample Code of Core Components 163

C Improved Robustness Using High-Level Multi-Sensor SLAM 176

6.1 Summary of errors from di↵erent sensor configurations . . . . . . . 135

C.1 Performance Summary for Di↵erent Sensor Configurations During

2.1 The EKF-SLAM flow diagram. . . . . . . . . . . . . . . . . . . . . 18

3.1 Arduino microcontroller . . . . . . . . . . . . . . . . . . . . . . . . 48

4.1 Sample of joist layout in a house. . . . . . . . . . . . . . . . . . . . 63

5.1 Simple test map, robot shown in center. . . . . . . . . . . . . . . . 94

6.1 Basic UML architecture for sensor algorithms . . . . . . . . . . . . 119

A.1 TSL235 Light to frequency converter . . . . . . . . . . . . . . . . . 158

C.1 Comparison of localization ability for di↵erent sensor configurations. 183

A2D Analog to Digital

Simultaneous Localization and Mapping (SLAM) is a popular topic in the mobile

1.1 Objectives of This Thesis

This thesis aims to demonstrate the potential of incorporating unconventional

The objective is to develop an algorithm, supported by a software framework,

1.2 Methodologies of This Thesis

To increase repeatability and reduce development complexity simulated data is

In the capacitive sensor research, a neural network is trained. Matlab is leveraged

1.3 Contributions of This Thesis

The second contribution performs a complete particle filter based implementation

1.4 Organization of This Thesis

The Background chapter discusses the necessary background information to make

Chapter 5, Unconventional Sensors In Multi-Sensor Navigation, examines the sim-

The previous two concepts are brought together in chapter 6, Unconventional

Background and Literature

2.1.1 Applications of SLAM

Self-Driving Cars This technology has also been implemented by google in a

Indoor Localization Indoor localization is becoming a growing field especially

2.1.2 Robot Localization

In its simplest form robot localization can be seen as a coordinate transformation

Bayes Filter Bayesian localization is considered one of the simplest techniques

Markov Localization Markov localization is a simple extension of Bayes local-

Landmarks should be re-observable, distinguishable, and stationary (Riisgaard

Landmarks Extraction Landmark extraction is the process of converting the

RANSAC Landmarks Another popular technique that came from computer

2.1.2.4 Frames of Reference

2.1.3 Methods for SLAM

2.1.3.1 Extended Kalman Filter

The state is maintained in a variable X and the covariance matrix ⌃. There is