Implementation of SLAM algorithms in a small-scale vehicle ...1218791/FULLTEXT01.pdf · (SLAM) algorithm in a small-scale vehicle running the Robot Operating System (ROS). Which SLAM

Master of Science Thesis in Datorteknik, FordonssystemDepartment of Electrical Engineering, Linköping University, 2017

Implementation of SLAMalgorithms in a small-scalevehicle using model-baseddevelopment

Johan Alexandersson och Olle Nordin

Master of Science Thesis in Datorteknik, Fordonssystem

Implementation of SLAM algorithms in a small-scale vehicle usingmodel-based development

Johan Alexandersson och Olle Nordin

LiTH-ISY-EX–17/5101–SE

Supervisor: Peter A Johanssonisy, Linköpings universitet

Simon TegelidÅF Consulting

Examiner: Mattias Krysanderisy, Linköpings universitet

Department of Electrical EngineeringDepartment of Electrical Engineering

Linköping UniversitySE-581 83 Linköping, Sweden

Copyright © 2017 Johan Alexandersson och Olle Nordin

Abstract

As autonomous driving is rapidly becoming the next major challenge in the auto-motive industry, the problem of Simultaneous Localization And Mapping (SLAM)has never been more relevant than it is today. This thesis presents the idea ofexamining SLAM algorithms by implementing such an algorithm on a radio con-trolled car which has been fitted with sensors and microcontrollers. The softwarearchitecture of this small-scale vehicle is based on the Robot Operating System(ROS), an open-source framework designed to be used in robotic applications.

This thesis covers Extended Kalman Filter (EKF)-based SLAM, FastSLAM,and GraphSLAM, examining these algorithms in both theoretical investigations,simulations, and real-world experiments. The method used in this thesis is model-based development, meaning that a model of the vehicle is first implemented inorder to be able to perform simulations using each algorithm. A decision of whichalgorithm to be implemented on the physical vehicle is then made backed up bythese simulation results, as well as a theoretical investigation of each algorithm.

This thesis has resulted in a dynamic model of a small-scale vehicle which canbe used for simulation of any ROS-compliant SLAM-algorithm, and this modelhas been simulated extensively in order to provide empirical evidence to definewhich SLAM algorithm is most suitable for this application. Out of the algo-rithms examined, FastSLAM was proven to the best candidate, and was in thefinal stage, through usage of the ROS package gMapping, successfully imple-mented on the small-scale vehicle.

iii

Acknowledgments

First and foremost, we would like to thank Simon Tegelid and Magnus Carlssonat ÅF, for endless support whenever needed. Additional accolades go out to oursupervisor Peter Johansson at the Department of Electrical Engineering who wasvery helpful with technical support. We would also like to extend our gratitudeto our examiner Mattias Krysander, also at the aforementioned department, whoprovided an extensive amount of good insight about the subject and also lots ofuseful hints regarding academic writing in general.

Linköping, October 2017Johan Alexandersson and Olle Nordin

v

Contents

Notation xi

1 Introduction 11.1 The Company . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.3 Thesis Scope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.4 Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.5 Risk analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.5.1 General risks . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.5.2 Kinematic vehicle model . . . . . . . . . . . . . . . . . . . . 3

1.6 Equipment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.7 Thesis methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . 51.8 Related work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2 Recursive Bayesian Estimation 72.1 State-space representation . . . . . . . . . . . . . . . . . . . . . . . 72.2 Bayesian Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.3 Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.4 Extended Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . 102.5 Particle filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.5.1 Sampling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122.5.2 Resampling . . . . . . . . . . . . . . . . . . . . . . . . . . . 122.5.3 Rao-Blackwellized Particle Filter . . . . . . . . . . . . . . . 13

3 Simultaneous localization and mapping 153.1 Introduction to SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.1.1 Data association . . . . . . . . . . . . . . . . . . . . . . . . . 153.1.2 Loop closure . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.1.3 Full SLAM and online SLAM . . . . . . . . . . . . . . . . . 163.1.4 General models for SLAM problem . . . . . . . . . . . . . . 16

3.2 EKF SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.2.1 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173.2.2 Computational complexity . . . . . . . . . . . . . . . . . . . 19

vii

viii Contents

3.2.3 Data association . . . . . . . . . . . . . . . . . . . . . . . . . 193.3 FastSLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.3.1 Particle structure . . . . . . . . . . . . . . . . . . . . . . . . 203.3.2 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 203.3.3 FastSLAM 2.0 . . . . . . . . . . . . . . . . . . . . . . . . . . 213.3.4 Computational complexity . . . . . . . . . . . . . . . . . . . 213.3.5 Data Association . . . . . . . . . . . . . . . . . . . . . . . . 22

3.4 GraphSLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.4.1 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.4.2 Computational complexity . . . . . . . . . . . . . . . . . . . 253.4.3 Data association . . . . . . . . . . . . . . . . . . . . . . . . . 25

3.5 Theoretical evaluation of SLAM . . . . . . . . . . . . . . . . . . . . 26

4 Vehicle Dynamics 294.1 Longitudinal dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 29

4.1.1 Gear ratio . . . . . . . . . . . . . . . . . . . . . . . . . . . . 294.1.2 Wheel torque . . . . . . . . . . . . . . . . . . . . . . . . . . 294.1.3 Forward velocity . . . . . . . . . . . . . . . . . . . . . . . . 30

4.2 Lateral dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 304.2.1 Ackermann angle . . . . . . . . . . . . . . . . . . . . . . . . 314.2.2 Heading angle . . . . . . . . . . . . . . . . . . . . . . . . . . 31

4.3 Coordinates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

5 Sensor theory 335.1 Gyroscope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 335.2 Wheel encoders . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345.3 Laser range finder . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

6 Simulink Model Implementation 376.1 Input . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 386.2 Kinematic Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

6.2.1 Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 386.2.2 Servo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 396.2.3 Angle & Angular Velocity . . . . . . . . . . . . . . . . . . . 396.2.4 Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

6.3 Map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 396.3.1 Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . 396.3.2 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . 39

6.4 Odometry Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 416.4.1 Gyroscope . . . . . . . . . . . . . . . . . . . . . . . . . . . . 416.4.2 Wheel encoders . . . . . . . . . . . . . . . . . . . . . . . . . 44

6.5 LIDAR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 476.5.1 Description . . . . . . . . . . . . . . . . . . . . . . . . . . . 476.5.2 Operation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 486.5.3 Probability of error . . . . . . . . . . . . . . . . . . . . . . . 50

6.6 ROS Interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

Contents ix

6.6.1 /sensor_data . . . . . . . . . . . . . . . . . . . . . . . . . . . 556.6.2 /scan . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 556.6.3 /car_position . . . . . . . . . . . . . . . . . . . . . . . . . . 556.6.4 /clock . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

7 ROS Implementation 577.1 CAN Reader . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 587.2 Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 587.3 SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 587.4 rViz . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 597.5 Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 607.6 simulinkPosition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 607.7 ROS Control App . . . . . . . . . . . . . . . . . . . . . . . . . . . . 617.8 Joystick Input . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 617.9 CAN Writer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 617.10 CPU Recoder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

8 Simulations 638.1 Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

8.1.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 638.1.2 Maps . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 648.1.3 Tests . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

8.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 698.2.1 Mapping accuracy . . . . . . . . . . . . . . . . . . . . . . . . 698.2.2 Path accuracy . . . . . . . . . . . . . . . . . . . . . . . . . . 728.2.3 Average position error . . . . . . . . . . . . . . . . . . . . . 768.2.4 Average CPU load . . . . . . . . . . . . . . . . . . . . . . . . 77

8.3 Issues . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 788.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79

9 Real-world experiment 819.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 819.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 839.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86

10 Discussion 8710.1 Achievements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8710.2 Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8810.3 Complications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8810.4 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89

Bibliography 91

Notation

Abbreviations

Abbreviation Description

ROS Robot Operating SystemLIDAR Light detection and rangingSLAM Simultaneous localization and mappingIMU Inertial measurement unitGUI Graphical user interfaceAI Artificial intelligenceP DF Probability Density FunctionMEMS micro-electro-mechanical systemsLUT Lookup tableCPU Central Processing UnitP C Personal ComputerCAN Controller Area Network

xi

1Introduction

1.1 The Company

This thesis has been conducted at ÅF in Linköping. ÅF is a Swedish consultingcompany with over 8000 employees worldwide [6]. ÅF as a company focusesheavily on technology areas such as industry, infrastructure and energy [6]. Thethesis work was conducted at ÅF’s technology department.

1.2 Background

In recent times, many companies in the automotive industry have spent a lot oftime and funds on researching the possibility of producing autonomous cars. Theresearch has moved forward to the point that there are actually semi-autonomouscars available in the market today, and it is widely believed that at some point intime, almost all vehicles will be autonomous. In order to achieve this, there isobviously a need for advanced sensors and functions. One of the major issuesis the fact that the vehicle needs to be aware of its current position within anunknown environment. This is known as the Simultaneous Localization AndMapping problem, commonly abbreviated as SLAM.

As ÅF is a company that works a lot with autonomous driving, a competenceproject concerning this has been started. The goal with this project is to assemblea small-scale vehicle with embedded sensors and microprocessors, and imple-ment functions which will enable this vehicle to drive autonomously. As imple-menting a SLAM algorithm is a vital part of achieving this, this thesis will focuson finding the best SLAM algorithm for this purpose and implement it in thevehicle.

1

2 1 Introduction

1.3 Thesis Scope

The purpose of this thesis is to implement a Simultaneous-Location-And-Mapping(SLAM) algorithm in a small-scale vehicle running the Robot Operating System(ROS). Which SLAM algorithm to be chosen will be supported by a theoreticalinvestigation. Three different algorithms will be investigated, Extended KalmanFilter based SLAM (EKF)[12], particle filter-based SLAM (FastSLAM) [29], andgraph-based SLAM (GraphSlam) [11]. FastSlam is implemented in the ROS pack-ages gmapping [2] and coreslam [1], and Graphslam is implemented in the pack-age slam_karto [3]. In addition, simulations will be done on a model createdin Simulink representing the actual car and further information regarding theSLAM algorithm implemented in the model will be presented.The SLAM algorithm is supposed to simultaneously create a map of the vehicle’senvironment as well as calculating the position of the vehicle within this map.This data is to be broadcast to another ROS node on a PC, which will show themap and the vehicle’s position in real-time in a GUI.There will be two different levels of priority in this thesis. The first priority is toimplement passive SLAM, and the second priority is to implement active SLAM.Another second priority goal is to make the algorithm consider dynamic objects.

In order to reach the first priority goals, the following needs to be investigated:

• A theoretical investigation comparing different SLAM-algorithms in orderto make a valid decision on which one to use

• How to run a SLAM algorithm using limited resources (since it will be runon an embedded processor). This means first and foremost to pick a SLAMalgorithm which uses few resources. However, if this is not sufficient, mod-ifications to the algorithm (or the implementation of one) may be needed.

• Development of a model in Simulink and evaluating the model by compar-ing the output of the model to a theoretical position/direction within a map

In order to reach the second priority goals, the following needs to be investigated:

• A theoretical investigation comparing different pathfinding algorithms foractive SLAM, as well as simulating different algorithms in Simulink.

• Theoretical studies and simulations where dynamic objects are considered.

1.4 Limitations

The following limitations are set for this project:

• Ethical aspects of any kind will not be discussed in this thesis.

• There are no requirements of the environment of which the vehicle willoperate in.

1.5 Risk analysis 3

1.5 Risk analysis

There are several risks that certain parts of this thesis may be delayed or fail.These are explained in the following sections.

1.5.1 General risks

A major part of this thesis will be conducted during the summer, when super-visors may be on vacation leave. This will limit the amount of support that isavailable.

1.5.2 Kinematic vehicle model

There are several risks regarding the kinematic vehicle model:

• The sensors in the kinematic vehicle model should be able to represent theactual sensors in the car as well as possible. The sensors being modelledshould be validated by some measurements on the real sensors as well astheoretical values. However, that is not a guarantee that they actually rep-resent the actual sensors.

• The kinematic vehicle model should be validated using a measuring system.Currently, the measuring system is under development at Linköpings Uni-versitet. This basically implies that the testing phase of the model couldbe delayed. Alternative solutions for measuring could be by plain eye sightcomparing the difference in position within the maps.

4 1 Introduction

1.6 Equipment

Fig. 1.1 displays an overview of the system architecture used in this thesis.

Figure 1.1: System architecture overview

The hardware equipment and software components are further explained be-low:

• ROS

– A framework and operating system developed for robots. This thesiswill use the ROS Kinetic distribution

• Lubuntu 14.04

– Lightweight Ubuntu operating system, operating on the imx6 proces-sor on the Nitrogen6_MAX board.

1.7 Thesis methodology 5

• STM32 board

– A PCB containing a stm32f4 microcontroller, CAN transceivers, aswell as being connected to a RPLIDAR, magnet encoders and an IMU.

• Nitrogen6_MAX

– A motherboard containing CAN transceivers, Ethernet, Wi-Fi, and animx6 quad-core processor.

• Magnet encoders

– Hall effect sensors attached to the wheel axis of the physical vehicle,measuring each time a magnet mounted inside the wheel passes by.

• IMU 9250

– Inertial measurement unit, connected to a magnetometer, a gyro andan accelerometer .

• RPLIDAR

– A 2D laser scanner capable of producing scans in 360 different angles.

1.7 Thesis methodology

Fig. 1.2 illustrates the method workflow of this thesis. The first phase is a pre-study, where the basic theory of SLAM algorithms will be studied and ROS/Simulinktutorials will be conducted. This is just to get a good initial understanding of thesubject. The next phase of the thesis is to concurrently develop a model of thephysical vehicle in Simulink and doing a theoretical investigation on the SLAMalgorithms chosen for investigation in this thesis. Once the model is done, someROS implementation is needed to be able to perform simulations with the vehi-cle model. When the simulations are done, the results will be used to choose analgorithm to be implemented on the physical vehicle.

Figure 1.2: Thesis methodology

6 1 Introduction

1.8 Related work

This thesis, as stated above in Section 1.3, includes conducting a theoretical in-vestigation about three SLAM algorithms. Given that the SLAM problem hasbeen known for a long time, there exists papers related to each of the SLAM al-gorithms examined in this thesis. [4, p.6]. From the book Probabilistic robotics[28] by Sebastian Thrun, Wolfram Burgard and Dieter Fox, each SLAM algorithmhave been described thoroughly and it is being widely cited in this thesis. How-ever, given the specified thesis scope described in Section 1.3 the theoretical in-vestigation is also to be backed up by simulation results. Various aspects of theGraphSLAM and the FastSLAM algorithms have been compared to each other inthe paper "An evaluation of 2D SLAM Techniques available in Robot OperatingSystem" [24, p.1-6] and the master thesis "SmokeNav - Simultaneous Localizationand Mapping in Reduced Visibility Scenarios". [25, p.26-33]. Moreover, at pages33-36 in the master thesis [25], the reader is provided with real world resultsfrom GraphSLAM and FastSLAM implementations. Furthermore, real world re-sults of a GraphSLAM and FastSLAM implementation has been provided in [24,p.5-6].

2Recursive Bayesian Estimation

A key component of what enables a robot to successfully perform navigation issensing. In order to navigate properly, the robot needs information about its en-vironment. This information is supplied by sensors. A major problem, however,is that the sensor data will always be corrupted by noise to some degree. Thatmeans that if the robot just uses the raw sensor data as it is, the navigation willproduce erroneous results. To apprehend the fact that all measurements are inthe presence of noise, filtering is needed. One approach of doing this is throughRecursive Bayesian Estimation [28, p. 13], which uses probabilistic models inorder to filter out the noise.

2.1 State-space representation

In order to define states of a system, state-space representation is used. In general,a continuous-time system is said to be in state-space form [20, p. 19] if it can bedefined as

x(t) = f (x(t), u(t)) (2.1a)

y(t) = g(x(t), u(t)) (2.1b)

where x(t) is the system’s state vector, and u(t) and y(t) is the input and the outputof the system, respectively. The derivative of the state vector x(t) is a function ofthe current state x(t) and the current input u(t). Similarly, the output of thesystem y(t) is a function of the current state and the current input. For discrete-time systems, state-space equations [20, p. 19] are defined as

xt+1 = f (xt , ut) (2.2a)

yt = g(xt , ut) (2.2b)

7

8 2 Recursive Bayesian Estimation

For linear continuous-time systems, the state space model can be formulated as

x(t) = Ax(t) + Bu(t) (2.3a)

y(t) = Cx(t) + Du(t) (2.3b)

For linear discrete-time systems, the state space model can be formulated as

xt+1 = Axt + Bxt (2.4a)

yt = Cxt + Bxt (2.4b)

2.2 Bayesian Filtering

The pose of a robot is due to its current state xt . When the system enters a newstate xt+1, which is due to some control command of the robot ut , the new statewill produce a measurement zt+1. The relationship between controls, states, andmeasurements is shown in Fig. 2.1 below. A Bayes Filter is an algorithm which

Figure 2.1: Relationship between states, controls, and measurements.

2.3 Kalman filter 9

calculates the probability density function (PDF) for the state vector xt . This isdone in two steps, the prediction step bel(xt) and the correction step bel(xt) as

bel(xt) =∫p(xt | ut , xt−1) bel(xt−1) dxt−1 (2.5a)

bel(xt) = η p(zt | xt) bel(xt) (2.5b)

where in the prediction step (given by Eq. 2.5a), a new state xt is predicted us-ing only the control command ut and the previous state xt−1. This predictionis altered in the correction step by using sensor data in order to get a betterestimation[28, p.27]. A scaling factor η is introduced to scale the resulting PDFin order to make the integral sum 1 [28, p.17]. Pseudo-code for the algorithm isdescribed in Algorithm 1.

Algorithm 1 Recursive Bayes Filter

1: function bayes_filter(bel(xt−1), ut , zt)2: for all xt do3: bel(xt) =

∫p(xt | u1, xt−1) bel(xt−1) dxt−1

4: bel(xt) = η p(zt | xt) bel(xt)5: end for6: return bel(xt)7: end function

2.3 Kalman filter

A technique which implements a Bayes filter is the Kalman filter, which is usedin linear Gaussian systems. Basically, it computes a belief for a continuous state[28, p.40]. This state represents the information regarding the vehicle such as theodometry, and its surrounding environment for example landmarks[28, p.20].The belief of the state at a specific time instance t bel(xt), can be expressed by itsmean and its covariance.[28, p.40]

The Kalman filter needs three specific properties as prerequisites [28, p. 41-42], and these are:

• The current state xt , needs to be represented by linear arguments, meaningthat

xt = At(xt−1) + Btut + εt (2.6)

where At and Bt are constants. Moreover, the added noise source should beof Gaussian nature. The current state xt depends on the previous state xt−1,the control ut , and the noise εt .

• The current measurement zt requires its arguments to be of linear naturemeaning that

zt = Ctxt + δt (2.7)


where Ct is a constant and the noise source is Gaussian. The current mea-surement zt depends on the current state xt and the noise source δt .

• The last prerequisite is that the start belief bel(x0) has to be Gaussian dis-tributed.

The Kalman filter algorithm itself, shown below in Algorithm 2 expresses thebelief of the current state xt , at a specific time t. The bel(xt), is expressed bythe mean µt and the covariance Σt , which is also the output of the algorithm.[28,p.42]

Algorithm 2 Kalman filter algorithm

1: function Kalman_filter(µt−1,Σt−1, ut , zt)2: µt = Atµt−1 + Btut3: Σt = AtΣt−1A

Tt +Rt

4: Kt = ΣtCTt (CtΣtC

Tt +Qt)−1

5: µt = µt + Kt(zt − Ctµt)6: Σt = (I − KtCt)Σt7: return µt ,Σt8: end function

Basically, the algorithm calculates predictions of its mean µt and covarianceΣt at line 2 and 3. Whereas At and Bt are matrices which multiplied by the statevector xt−1 and the control vector ut represents the state transition probabilitywith linear arguments, which is one of the requirements for the Kalman filteralgorithm to be successful. These predictions are used in the correction step, lines4-6. The first step of the correction is the calculation of the Kalman gain denotedKt . Moreover, the calculation of the mean µt is performed with regards to thepredicted mean µt the measurement zt - the predicted measurement (2.7) Ctµttimes the Kalman gain. At last, the new covariance Σt is calculated with regardsto the Kalman gain factor as well as the predicted covariance Σt[28, p.43].

2.4 Extended Kalman filter

The Extended Kalman Filter is an approach of dealing with nonlinear models, ofwhich the traditional normal Kalman filter cannot handle. Basically, real worldmeasurements and state transitions are often not linear, thus a normal Kalmanfilter’s assumption of linear data is not accurate. The approach of how EKF pro-cesses this is by describing the state transition probability and the measurementprobability with nonlinear functions [28, p. 54]

x(t) = g(u(t), x(t − 1)) + ε(t) (2.8a)

z(t) = h(x(t)) + δ(t) (2.8b)

As stated above the EKF uses nonlinear functions. The output of the algo-rithm is an approximation in the form of a Gaussian distribution, with an esti-mated mean and covariance. The approximation part of the algorithm is due to

2.4 Extended Kalman filter 11

Figure 2.2: Linearization of the nonlinear function g(x). Dashed line indi-cates the Taylor approximation of g(x)

the fact that it uses nonlinear functions. These functions need to go through alinearization process. This process linearizes the functions by for example usinga first order Taylor expansion around a linearization point, this can be seen inFig. 2.2. The resulting Gaussian distribution can then be calculated in closedform. This enables the extraction of interesting characteristics of the distributionsuch as the mean µt and the covariance Σt . Given the mean and the covariance,the belief bel(xt) can be represented [28, p.56-58]. The EKF algorithm is shownbelow in Algorithm 3 [28, p.59].

Algorithm 3 Extended Kalman filter algorithm

1: function Extended_Kalman_filter(µt−1,Σt−1, ut , zt)2: µt = g(ut , µt−1)3: Σt = GtΣt−1G

Tt +Rt

4: Kt = ΣtHTt (HtΣtH

Tt +Qt)−1

5: µt = µt + Kt(zt − h(µt))6: Σt = (I − KtHt)Σt7: return µt ,Σt8: end function

As can be seen from Algorithm 3 it is slightly different compared to the Kalmanfilter Algorithm 2. This is due to the fact that the EKF uses nonlinear functionsto calculate the mean µt and the covariance Σt .


2.5 Particle filter

Another implementation of a Bayes filter is the particle filter. In contrast to theExtended Kalman filter, it does not use a parametric model for the probabilitydistributions. The main idea of the particle filter is that it instead keeps a set ofM samples, where each sample x[m]

t (known as a "particle") is a set of potentialstate variable values. The probability itself of these state variables is representedby how many particles contain similar state variable values, meaning that areaswith high probability will have a higher density of particles than the areas wherethe probability is low [19, p. 27].

The particle filter is described in Algorithm 4[28, p. 98], where Xt is thepredicted state vector, and Xt is the corrected state vector.

Algorithm 4 Particle Filter

1: function particle_filter(Xt−1, ut , zt)2: Xt = Xt = ∅3: for m = 1 to M do4: sample x[m]

t ∼ p(xt | ut , x[m]t−1)

5: w[m]t = p(zt | x

[m]t )

6: Xt = Xt + (x[m]t , w

[m]t )

7: end for8: for m = 1 to M do9: draw i with probability ∝ w[i]

t

10: add x[i]t to Xt

11: end for12: return Xt13: end function

2.5.1 Sampling

During sampling (lines 3-7 in Algorithm 4), new values are generated for eachparticle in the filter. These values are only temporary, and the particles them-selves will not be updated until the resampling stage. This, for each particle, newstate is due to the control ut and the previous state of the particle x[m]

t . After

this, the importance factor w[m]t is calculated. The importance factor is due to the

measurement zt and is used as a likelihood measure of the particle used in theforthcoming resampling of the particle filter. [28, p. 99]

2.5.2 Resampling

During resampling (lines 8-10 in Algorithm 4) the actual particles in the filter areupdated. This is done by randomly selecting a particle generated in the samplingstage, where the probability of selection for each particle is due to its importance

2.5 Particle filter 13

factor. The resampling is done with replacement, so each particle can be drawnseveral times. [28, p.99]

2.5.3 Rao-Blackwellized Particle Filter

There’s also another version of the Particle Filter called the Rao-Blackwellized par-ticle filter. This is an extension of the regular particle filter where some statevariables are represented by particles, and some with Gaussians. [28, p. 437]

3Simultaneous localization and

mapping

This chapter describes the basic features of a generic SLAM algorithm, as wellas introducing a few SLAM algorithms. The key features of these algorithms arethen summarized in Section 3.5, where each algorithm is a also evaluated in termsof being appropriate for implementation in the physical vehicle of this thesis.

3.1 Introduction to SLAM

Simultaneous localization and mapping is a problem where a moving object needsto build a map of an unknown environment, while simultaneously calculating itsposition within this map. [12, p. 229]There are several areas which could benefit from having autonomous vehicleswith SLAM algorithms implemented. Examples would be the mining industry,underwater exploration, and planetary exploration.[12, p. 229] The SLAM prob-lem in general can be formulated using a probability density function denotedp(xt , m|z1:t , u1:t), where xt is the position of the vehicle, m is the map, and z1:t isa vector of all measurements. u1:t is a vector of the control signals of the vehicle,which is either the control commands themselves or odometry, depending on theapplication.

3.1.1 Data association

Basically, the concept data association is to investigate the relationship betweenolder data and new data gathered. In a SLAM context it is of necessity to relateolder measurements to newer measurements. This enables the process of deter-mining the locations of landmarks in the environment, and thus this also givesinformation regarding the robots position within the map. [4, p. 26]

15

16 3 Simultaneous localization and mapping

3.1.2 Loop closure

The concept of loop closure in a SLAM context is the ability of a vehicle to rec-ognize that a location has already been visited. By applying a loop closure algo-rithm, the accuracy of both the map and the vehicles position within the mapcan be increased. [30, p.1] However, this is not an easy task to perform, dueto the fact that the operating environment of the vehicle could contain similarstructural circumstances as previously visited locations. In that case if the loopclosure algorithm performs poorly, it could lead to a faulty loop closure, whichcould corrupt both the map as well as the pose of the vehicle within the map.[30,p.1]

3.1.3 Full SLAM and online SLAM

There are two different types of SLAM problems. The online SLAM problem ofwhich only the current pose xt and the map m are expressed, given the controlinput u1:t and measurements z1:t . As well as the full SLAM problem which ex-presses the entire trajectory of the robot. The PDF of the full SLAM problem isdenoted as p(x0:t , m|z1:t , u1:t), where all the poses of the robot are considered, in-cluding its initial pose x0. [17, p. 21]

3.1.4 General models for SLAM problem

A motion model for the SLAM problem can be described by

xt = g(ut , xt−1) + δt (3.1)

where the current pose xt depends on the possible nonlinear function g(ut , xt−1),with ut being the control input, xt−1 the previous pose and δt being a randomGaussian variable with zero mean.[17, p. 19]

A measurement model for the SLAM problem can be described by:

zt = h(xt , m) + εt (3.2)

where the current measurement zt , depends on the possible nonlinear func-tion h(xt , m), where xt denotes the current pose, m represents the map, εt a ran-dom Gaussian variable with zero mean.[17, p. 19]

3.2 EKF SLAM

The EKF SLAM (Extended Kalman Filter) approach of solving the SLAM prob-lem is by using sensor data gathered from the movement and rotation of therobot. This can be done by for example using wheel encoders and a gyroscope.Furthermore, it is also necessary to gather information of the environment by,for example, using a Laser Range Finder (LIDAR). With this data, the algorithm

3.2 EKF SLAM 17

keeps track of where the robot is likely positioned within a map, as well as keep-ing track of specific landmarks observed.[4]

3.2.1 Algorithm

When the robot is turned on, the sensors on the robot such as the wheel encodersand the gyroscope will gather information of the positioning of the robot. More-over, landmarks from the environment are also extracted based on new observa-tions from the LIDAR which is mounted on the robot. These new observationsare associated with previous observations and updated in the EKF algorithm.However, if an observation of a landmark cannot be associated to a previousobservation the observation itself is presented to the EKF algorithm as a newobservation.[4, p. 29] This process is illustrated in Fig. 3.1.


Figure 3.1: EKF SLAM algorithm.[4, p. 10]

First and foremost, the robot localizes itself within the map being createdusing observed landmarks and information from the sensors. The landmarksshould preferably be observable from multiple angles, as well as not being sepa-rated from other landmarks. These prerequisites gives the EKF algorithm a goodpossibility to distinguish between landmarks at a later time. Moreover, the land-marks being used should also be stationary. [4, p. 18]

To describe the EKF SLAM process in short terms, it consists of two stages.

• Prediction step

– Predict the current state expressed by the predicted mean and the pre-dicted covariance. These are being calculated based on control input,matrices, the previous covariance and the previous mean. [27, p. 20-28]

• Correction step

3.2 EKF SLAM 19

– First and foremost, the idea behind the correction step is the data as-sociation. The main idea behind associating data in a SLAM context isto distinguish between an earlier observed landmark and a newly ob-served one. [27, p.15-16,30] When a new measurement has occurredthe procedure is as following:

* Store the locations of newly observed landmarks. [27, p.15-16,30]

* Associate previously observed landmarks with newly observed ones.[27, p.15-16,30]

Furthermore, the correction gain, also known as the Kalman gain iscomputed. Basically, it is a correction gain factor necessary to up-date the current mean and covariance of the current state. The cur-rent mean and covariance is not only dependent on the Kalman gainthough, but they are also taking the predicted mean and the predictedcovariance and the measurement into account. [27, p.37-38]

3.2.2 Computational complexity

The computational cost of the EKF SLAM algorithm is quite costly in comparisonto other SLAM algorithms. When new landmarks are detected, they are addedto the state vector of the filter, and the map of with N identified landmarks, willincrease linearily. [23, p.7765] In terms of numbers, the use of memory is O(N2).The computation cost of computing one step of the algorithm is approximatelyO(N2) [27, p.57] [5, p. 792], the complete cost for computing the whole algorithmis O(N3), heavily dependant on the size of the map[5, p.792].


A bottleneck when using the EKF algorithm is the computational cost which in-creases when new landmarks are observed. This leads to a prerequisite of that theenvironment should not contain too many landmarks. However, since the num-ber of landmarks in the environment are few, the localization within the mapand the data association is getting increasingly more difficult.[28, p. 330-331]Moreover, the data association is made by computing the maximum likelihoodand in case of a faulty association, the future results which are based on previousassociations by the algorithm will also be faulty.[28, p.331-332]


3.3 FastSLAM

Another way of solving the SLAM problem is using the FastSLAM algorithm,which makes use of the Rao-Blackwellized particle filter. FastSLAM takes ad-vantage of something that most other SLAM algorithms do not, namely the factthat each observation only concerns a small amount of the state variables. Thepose of the robot is probabilistically dependent on its previous pose, whereas thelandmark locations are probabilistically dependent on the position of the robot.[19, p. 27] This is not taken into account by the EKF SLAM algorithm, for exam-ple, which with each update has to recalculate its covariance matrix, resulting inpoor scaling in large maps. [19, p. 29] This is not a problem in FastSLAM, as itfactors out the calculations to simpler subproblems, as shown by

p(st , θ | zt , ut , nt) = p(st | zt , ut , nt)N∏n=1

p(θn | st , zt , ut , nt) (3.3)

which factors out the robot path st and each of the N landmarks θn. One majordifference here between FastSLAM and, for example EKF SLAM, is that the entirepath st is calculated, as opposed to just the current pose xt . This means thatFastSLAM is a full SLAM algorithm and EKF SLAM is an online SLAM algorithm,as described in Section 3.1.3.

3.3.1 Particle structure

The particle filter used in FastSLAM is comprised of M particles, where eachparticle is defined by

S[m]t = (st[m], µ1,t ,Σ1,t , ..., µN,t ,ΣN,t) (3.4)

Each particle contains N + 1 variables, one robot path st[m], and N landmarkposteriors, where each landmark is represented by an EKF with a mean µn,t anda variance Σn,t . During one iteration of the algorithm, the robot path for everyparticle st[m] is updated once, as well as each landmark filter µn,t ,Σn,t .

3.3.2 Algorithm

Each iteration of the algorithm can be summarized in four steps. These are pre-sented below.

1. For each of the M particles, sample a new robot path st from p(st | s[m]t−1, ut)

2. With the new measurement, update the landmark filters

3. Give each particle an importance factor

4. Resample the particles, with particles with a higher importance factor hav-ing a higher draw probability.

3.3 FastSLAM 21

In step 1, a new robot path is sampled for each particle. This path is drawnfrom the motion model, with the robot control as input. After this, the posteriorsof the landmarks are updated due to the new measurement. After this, eachparticle is given an importance factor, which is a measure on how realistic thevariables in the particle are. Later, the particles are resampled in order to get ridof the ones which do not have high probability.

[19, p. 10]

3.3.3 FastSLAM 2.0

The FastSLAM algorithm (and particle filters in general) performs more accu-rately when there is more diversity among the particles. Particles will not bediverse if, for example, the odometry sensors are noisy and the laser range finderis accurate, as this will render many of the motion hypothesises unlikely andmany particles will have a low importance factor. Because of this drawback ofthe FastSLAM algorithm, an updated one has been developed, named FastSLAM2.0. FastSLAM 2.0 is quite similar to its predecessor, with the exception thatwhen estimating the motion, it also considers the range sensors. This creates bet-ter guesses in general, and will result in more particles having higher importancefactors. [19, p. 63]


If the FastSLAM is directly implemented as described above, it has O(M ∗ N )computational complexity, where M is the number of particles and N is the num-ber of landmarks. [19, p. 48] To implement an algorithm like this would not bea good idea, since it would scale linearly with the number of landmarks. Thiswould render the algorithm unsuitable for large maps.

To apprehend this issue, FastSLAM is implemented using binary trees ratherthan the array structure shown in 3.4. The algorithm will then have O(MLog(N ))complexity [28, p. 465], which is illustrated in Fig. 3.2 below, which illustratessuch a tree in a single particle with N = 8.

Using this binary tree approach also signifcantly reduces the amount of mem-ory used by the algorithm. [19, p. 58]


Figure 3.2: Binary tree data structure of FastSLAM particle

3.3.5 Data Association

Another advantage of the FastSLAM algorithm is the fact that its data associa-tion is on a per-particle basis. This means that each particle will have its ownhypothesis on which landmark the measurement is associated with. If a measure-ment associates incorrectly with a landmark in a particle, this particle will havea low importance factor, thus making the impact of this wrong association verysmall. In other algorithms, such as EKF-based SLAM, once an incorrect data as-sociation has been made, it will cause the algorithm to produce incorrect resultsfrom that point. This is one of the main advantages of the FastSLAM algorithm incomparison to other algorithms, and contributes to the algorithm having a highrobustness. [19, p. 41]

3.4 GraphSLAM 23

3.4 GraphSLAM

GraphSLAM is another approach of solving the SLAM problem. The idea be-hind GraphSLAM is to address the SLAM problem by using a graph. The graphcontains nodes which represent the poses of the robot x0, ..., xt as well as land-marks in the map which are denoted as m0, ..., mt . However, this is not enough toaddress the SLAM problem, as there are also constraints between the positionsxt , xt−1, xt−2, ..., xt−n of the robot and the landmarks m0, ..., mt . [26, p.361] Theseconstraints represents the distance between adjacent locations such as xt−1, xt aswell as the distance between the locations to the landmarks m0, ...mt . These con-straints can be constructed due to information from the odometry source ut .[26,p. 361-362] The graph mentioned earlier can be converted to a sparse matrix.Which in contrast to dense matrices can be more efficiently used. [17, p. 1] Fig.3.3 illustrates how the landmarks m0, ..., mt , the locations x0, ..., xt and the con-straints are linked together.

Figure 3.3: Robot poses seen as circles, landmarks of the map seen assquares, dashed lines representing the motion of the robot based on controlcommands given to the robot as well as solid lines representing the distanceto the landmarks given by measurements.

3.4.1 Algorithm

The PDF for the full SLAM problem is denoted p(x0:t , m|z1:t , u1:t). For simplicity,all the poses x0:t and the landmarks m can be described by a state space vector y


in Fig. 3.4.

Figure 3.4: Statespace vector containing all poses and landmarks

Then in order to achieve the most likely pose of the robot within the map andthe positions of the landmarks in the map, an optimization algorithm has to becomputed. [17, p. 11]

Moreover, the PDF p(y|z1:t , u1:t) contains the possibly nonlinear functionsg(ut , xt−1) and h(xt , mi), whereas mi represents the i-th landmark observed attime t. [17, p.4,11] The optimization algorithm expects linear functions. Thus, anecessary step is to linearize these. This could be done by for example executinga Taylor linearization algorithm. [17, p.11]

As specified earlier, the GraphSLAM approach of solving the SLAM problemis by introducing a graph. This graph can be converted to a sparse matrix. Basi-cally, this sparse matrix contains all the information acquired by the robot whilemoving in an environment. For example, the robots movement through the mapand landmarks observed at different locations. Thus, as the robot moves throughthe environment the sparse matrix will increase in size and contain even moreinformation.[17, p. 14]

In the following Figs. 3.5,3.6,3.7 below, it is illustrated of how the robot posesillustrated as blue squares and landmarks illustrated as red squares observed areadded to the sparse matrix. Moreover, the blue squares are also adjacent to eachother due to their relationship as the new pose is dependant on the old pose. Thediagonal grey squares shows that the landmarks of each row m1, m2, m3, m4 andits corresponding column m1, m2, m3, m4 is the one and same landmark.

Figure 3.5: Robot poses and landmarks observed

3.4 GraphSLAM 25




Some interesting statistics when discussing the GraphSLAM algorithm is the com-putational load and the use of memory. In comparison with the EKF SLAM algo-rithm of which the use of memory grows by O(N2) when new landmarksN are in-troduced and the computational cost by O(N3), the GraphSLAMs use of memorygrows linearly when new landmarks N are introduced. As for the computationalcost of the GraphSLAM algorithm it is dependent on time, thus, if the path ofthe vehicle is very long time-wise the computational cost can be costly. However,in general the attributes of the GraphSLAM algorithm is very desirable when theenvironment contains a lot of landmarks, of which the EKF algorithm tends tofail.[26, p. 362]. [28, p. 330]


The data association part of the GraphSLAM algorithm is done using a vectorcontaining correspondences between landmarks observed in the map. [28, p.362]The algorithm performs a correspondence test to check the probability that dif-ferent landmarks observed within the map actually is the same landmark. If theresult of this correspondence test exceeds a certain value, the algorithm will de-termine the landmarks as the same landmark, otherwise it will determine these


as two separate landmarks. [28, p. 363] In comparison with the EKF SLAMalgorithm where the future data associations can be faulty due to earlier dataassociations, the GraphSLAM algorithm can reexamine older data associations,thus reducing the risk of introducing errors for future data associations. [28, p.339,363] The GraphSLAM algorithm solves the full SLAM problem [17, p. 15],in comparison to the EKF slam algorithm which only solves the online SLAMproblem.

3.5 Theoretical evaluation of SLAM

Given the specified thesis scope in Section 1.3, a theoretical investigation regard-ing three different SLAM algorithms, namely EKF, GraphSLAM and FastSLAMhas been conducted.

There are several parameters of interest when discussing SLAM algorithms,some of which have been investigated are the following: robustness, efficiency aswell as ROS compatibility. These parameters are evaluated and compared to eachother below.

• EKF

– Robustness

* Given what has been stated in 3.2.3, a faulty data association willresult in erroneous future results, thus the robustness of the EKFalgorithm is low. [28, p.331-332]

– Efficiency

* In Section 3.2.2 the total computational cost of executing the al-gorithm is O(N3), to a great degree dependant on the size of themap[5, p.792]. In conclusion this makes the EKF SLAM algorithmmore suitable for smaller maps containing fewer landmarks dueto the heavy computational load.[27, p. 57] However, as statedin 3.2.3 the localization and data association part can be difficultwhen the landmarks in the environment are few. [28, p. 330-331]

– ROS compatibility

* Implemented in the mrpt_ekf_slam_2d package.

• FastSLAM

– Robustness

* Given what has been stated in 3.3.5, the FastSLAM algorithm is sig-nificantly more robust than the EKF SLAM algorithm due to thefact that the data association from measurements to landmarks isparticle-based which reduces the impact of a wrong data associa-tion.

– Efficiency

3.5 Theoretical evaluation of SLAM 27

* In Section 3.3.4 the total computational cost is stated asO(MLog(N )),meaning that the complexity is due to the number of particles Mand the number of landmarks N .


* Implemented in the gMapping package.

• GraphSLAM

– Robustness

* Given what has been stated in 3.4.3, the robustness of the Graph-SLAM algorithm is higher than the EKF algorithm, partly due tothe fact that older data associations can be reexamined if a wrongdata association has been performed. Basically, this reduces therisk of producing erroneous future data associations thus increas-ing the robustness.

– Efficiency

* In Section 3.4.2 it is stated that the use of memory is linearly de-pendent in comparison to the EKF algorithm which uses O(N2)based on number of landmarks N Moreover, the computationalload is time dependent, thus if the path is long the algorithm canbe quite costly as stated in 3.4.2.


* Implemented in the slam_karto package.

From what can be read above, the EKF SLAM algorithm is not too robust,while both the GraphSLAM algorithm and the FastSLAM algorithm have a higherrobustness. Moreover, in terms of efficiency the EKF SLAM algorithm is verycostly. Given that all algorithms have been implemented in ROS packages, eachof these are viable choices. However, as the EKF SLAM algorithm lacks in bothrobustness and effeciency, the conclusion of using other more effective algorithmssuch as the FastSLAM or GraphSLAM instead of the EKF algorithm can be reached.These algorithms are able to efficiently deal with maps containing more land-marks, as well as larger maps in terms of sheer size. With maps containing a cou-ple of hundred landmarks, for example when trying to map the outside world,the computational complexity of the EKF algorithm could prove to be a hugeproblem to deal with. [18, p. 1], [28, p. 330]

4Vehicle Dynamics

This chapter describes some of the vehicle dynamics theory which was used todevelop the dynamic model of the small-scale vehicle. Some of the theory pre-sented in this chapter was ultimately not used in the final model, but is stillincluded here since it describes the initial modeling approach. This chapter onlyconcerns the movement of the vehicle, whereas the theory concerning the sensorsis presented in Chapter 5.

4.1 Longitudinal dynamics

Longitudinal dynamics describe the movement of a vehicle along the longitudi-nal axis. Thus, it can be said that longitudinal dynamics describe the speed ofthe vehicle in the forward and backward direction, due to how different forcesact upon the vehicle.

4.1.1 Gear ratio

In a vehicle the motor and the wheels rotate at different speeds. This is due to thefact that they are connected by different sized gears. The relationship betweenthe angular velocity ω of the motor and gear is known as the gear ratio [15, p. 43]and is defined as

ngear =ωmotorωwheels

(4.1)

4.1.2 Wheel torque

The torque τ at the wheel of the vehicle can be computed with

29

30 4 Vehicle Dynamics

P = τ ∗ ω (4.2)

where P is the power of torque and ω is the angular velocity.If it is assumed that there are no losses, the power at the motor and the wheels

will be equal, meaning thatPmotor = Pwheels (4.3)

and together with with (4.2) this implies

τmotor ∗ ωmotor = τwheels ∗ ωwheels (4.4)

If (4.1) is substituted into (4.4) we obtain the torque of the wheels as a functionof the gear ratio and torque of the motor

τwheels = ngear ∗ τmotor (4.5)

4.1.3 Forward velocity

To obtain the forward velocity of the vehicle, the longitudinal force of the drivingwheels needs to be calculated. This is obtained by

F =τr

(4.6)

and in the terms of the wheels it is denoted by

F =τwheelsrwheels

(4.7)

Newton’s second law of motion can then be used to determine the accelerationof the vehicle as

a =Fwheelsmvehicle

(4.8)

where mvehicle denotes the mass of the vehicle. The rotational inertia of thewheels and drive shaft can be neglected or included as mass equivalent. Finally,the velocity is obtained through integration as

v =

T∫t

adt (4.9)

4.2 Lateral dynamics

Lateral dynamics describe the movement of the vehicle along the lateral axis.Lateral movement is mainly determined by the heading angle, which needs tobe calculated in order to determine the lateral velocity of the vehicle.

4.2 Lateral dynamics 31

4.2.1 Ackermann angle

The Ackermann steering angle is defined as the wheelbase angle

δ = l/R (4.10)

which causes the vehicle to turn in a specific heading angle, or yaw. l is thelength of the wheelbase and R is the radius of the heading angle, i.e. the distancebetween the center of gravity and the turn center, as Fig 4.1 shows. [16, p .129]

Figure 4.1: The Ackermann angle

4.2.2 Heading angle

If the Ackermann angle, the length of the wheelbase, and the forward velocity isknown, the heading angle of the vehicle can be calculated. The forward velocityis defined as

v = R ∗ ψ (4.11)

Since the radius of the heading angle is related to the Ackermann angle, (4.10)can be substituted into (4.11) in order to get the yaw rate

ψ =vl∗ δ (4.12)

Finally, the heading angle can be obtained through integration as

ψ =

T∫t

ψdt (4.13)

32 4 Vehicle Dynamics

4.3 Coordinates

Coordinates of the vehicle can then be calculated by integrating the velocities inthe x and y direction over time as

x =∫vxdt (4.14a)

y =∫vydt (4.14b)

If the forward velocity v, vehicle length l, yaw rate ω = ψ, and yaw ψ of the ve-hicle are known, the velocities vx and vy can be calculated using a transformationmatrix [10] as [

vxvy

]=[v

l2 ∗ ω

] [cos(ψ) −sin(ψ)sin(ψ) cos(ψ)

](4.15)

5Sensor theory

The sensors used in this thesis are a gyroscope, wheel encoders, and a LIDAR.The measurements from these sensors provide the necessary input to the SLAMalgorithms. The SLAM algorithms will process this data in order to present a mapof the surrounding environment, including the pose of the vehicle positionedwithin the map. The following sections will provide a description of each sensormentioned.

5.1 Gyroscope

Gyroscopes have been widely used for military and civilian purposes the last cen-tury. Not only for aeronautical purposes but also by the marine. The traditionalgyroscopes used to be mechanical. However, what is being used in many ap-plications nowadays are MEMS gyroscopes (micro-electro-mechanical systems),which are being used in cell phones, robotic projects, and for military purposes.[14, p. 171] Gyroscopes in general can be described by the the formula of Coriolisforce

Fc = −2 ∗m ∗ (ω ∗ v) (5.1)

As can be seen from (5.1) the Coriolis force is depending on the mass m of anobject, the angular velocity ω as well as the velocity v of the object. This objectis a part of the gyroscope and the mass of the object as well as the velocity of theobject are known. Given that the object rotates, and by having information aboutthe mass and velocity of the object the angular velocity can be detected.[14, p.173] Given the angular velocity, the total angle that the vehicle has rotated canbe calculated as

33

34 5 Sensor theory

a =

t∫0

ψdt (5.2)

5.2 Wheel encoders

Wheel encoders are being used to provide the microcontroller with informationabout the velocity of the vehicle. There are several different types of wheel en-coders based on different technologies, such as, optical wheel encoders, mechan-ical as well as magnetic wheel encoders just to name a few. In this thesis a mag-netic wheel encoder is being used which consists of a Hall effect sensor and anumber of magnets. The Hall effect sensor outputs a high voltage level when-ever the magnetic field surrounding it is sufficiently strong, (a magnet passingby)[22, p.3-4]. The pulse outputted by the Hall effect sensor is being registeredby the microcontroller. By counting the number of pulses generated since theprevious sampling instance, the microcontroller is able to estimate the speed ofthe vehicle.

Below is some pseudo code for calculating the vehicle velocity in Algorithm5.

Algorithm 5 Vehicle car speed

1: function car_speed(()P ulsesavg ,T)2: Tnew = T − Told3: vcar = (P ulsesavg ∗ (wheelCircum/10))/Tnew4: Tnew = Told return vcar5: end function

The algorithm calculates the Vcar in cm/s. The average number of high pulsesregistered by the two Hall effect sensors is denoted P ulsesavg . Which is multi-plied by (1/10) of the circumference of the wheels, thus giving a distance trav-eled. By dividing with the time period Tnew the vcar resembles the well knownformula for velocity v = s/t. The wheel circumference is divided by ten due tothe fact that there are ten magnets attached on each wheel.

5.3 Laser range finder

Laser Range Finders (LIDAR) can be used to measure the distance to objects in thevicinity of the vehicle. A laser range finder consists of a laser emitter and receiver,and is sometimes mounted on a rotating motor in order to measure distancesin every direction of the vehicle. The basic operation principle is that a shortpulse of light is emitted, reflected on an object, and then received. The time twaitbetween the emitting and receiving can then be used to calculate the distance dto the object as

5.3 Laser range finder 35

d =12ctwait (5.3)

where c is the speed of light. [21, p. 31]

6Simulink Model Implementation

The vehicle, sensors, and the environment of the vehicle were modelled in Simulink.This chapter describes the implementation of all the subsystems of the model,which can be seen in Fig 6.1 below. The model is not able to be simulated inreal-time, due to the computation time being too high.

Figure 6.1: Block Diagram of Simulink Model

37

38 6 Simulink Model Implementation

6.1 Input

The input subsystem provides the input stimuli to the system. This is done in twodifferent modes, keyboard, and file. In keyboard mode, the steering angle and thevelocity of the vehicle is controlled using the arrow keys on the keyboard. In filemode, the velocity and steering angled is defined by a .mat file. These .mat filesare recorded when running the model in keyboard mode. A typical workflowwhile using this model would be to first record the instructions for the vehicle toa .mat file, and then use the same .mat file for repeated experiments.

6.2 Kinematic Vehicle

The kinematic vehicle, shown in Fig. 6.2 is comprised of five subsystems, each ofwhich is described in the sections below.

Figure 6.2: Model of Kinematic Vehicle

6.2.1 Velocity

6.2.1.1 Initial model

The initial velocity model was modelled after how velocity is created in the ve-hicle itself, with the motor being a torque source which is converted to force,and ultimately velocity. This is described in Section 4.1. However this modelwas never completed, and scrapped. This is because conception of such a modelproved to be too time-consuming, and might have required a master thesis on itsown to do.

6.2.1.2 Final model

The final model is considerably simpler than the initial model. In this model, theinput signal is the desired velocity of the vehicle. This is passed through a first-

6.3 Map 39

order Filter in order to model the acceleration and deceleration of the vehicle.The time it takes for the vehicle to go from a stationary state to be moving at fullspeed was measured in order to calculate the time constant τ of the first orderfilter.

6.2.2 Servo

A simple model was also used for the servo. The servo control signal is a valuefrom -100 to 100, which is then passed through a look-up table which ranges from-45°to 45°, which is the maximum steering angle of the vehicle. A first-order filteris used to model the delay of the steering of the vehicle, and the time constant τwas calculated by measuring the time it takes for the wheel of the vehicle to turnfrom 0°to 45°.

6.2.3 Angle & Angular Velocity

This subsystem calculates the yaw rate and the yaw angle for the vehicle, whichis due to the steering angle and the speed. This is calculated by using the Acker-mann angle, which is explained in Eq. 4.10 in Section 4.2

6.2.4 Position

The X-position and Y-position subsystems realize the transformation matrix de-scribed in Section 4.3 in order to determine the current position of the vehicle.

6.3 Map

6.3.1 Parameters

In order to emulate the environment of the vehicle, a map definition needed tobe created. These maps are defined by two parameters; dimensions and walls. Thedimensions array is a 1-by-2 matrix where the first index is the width of the mapand the second is the height of the map. The walls matrix is an n-by-4 matrixwhich describes the walls in the map, where each row contains the coordinates ofa wall. These can be either diagonal, vertical, or horizontal. The map parametersare stored in a .mat file which can then be read by the Simulink model file.

6.3.2 Implementation

The map is implemented in two S-functions called vehicleMap and drawMap. Theformer calculates all necessary values to display the current position and yaw ofthe vehicle within this map, and the latter draws this is in a MATLAB plot. Inthis plot, the position and yaw of the vehicle is represented by a triangle. This isshown in Fig. 6.3 below. The inputs of vehicleMap is shown in Table 6.1 below,and the Simulink model diagram is shown in Fig. 6.4.


Figure 6.3: Plot of map generated by vehicleMap S-function

Table 6.1: Inputs of vehicleMap

Inputs Direction Size DefinitionCoords In 2 Current coordinates of the vehicle.Yaw In 1 Current yaw of the vehicle.Triangle Coords Out 6 Coordinates of where to draw the triangleCollision Out 1 Boolean indicating if the vehicle has hit a wallInit Out 1 Boolean indicating if map has been initialized

6.4 Odometry Sensors 41

Figure 6.4: Map subsystem

6.4 Odometry Sensors

6.4.1 Gyroscope

6.4.1.1 Description

The model receives the actual angular velocity produced from the car model, andoutputs an estimation of what the sensor would have measured. These measure-ments are corrupted by noise and bias. The model has been designed with re-gards to actual measurements on the values from the gyroscope. By measuringthe output when the physical vehicle is standing still, an average error can be cal-culated and modelled. The pseudo code for deriving the bias is presented belowin algorithm 6

Algorithm 6 Gyroscope bias

1: for Gz in array do2: count = count + int(Gz)3: number = number + 14: end for5: ψ = (count ∗ 250 ∗ 3.14/(number ∗ 32768))

First and foremost, what is being derived above in algorithm 6 is the angularvelocity in rad/s. The bias output from the gyroscope’s z-axis has been measured


and an average bias output has been calculated. To transform this output toan angular velocity in rad/s, the average bias (count/number) is multiplied byDPS/ADC * pi/180. Where DPS = degrees per second of range specified, thereare three possible alternatives: 250 °/s, 500 °/s and 2000 °/s. ADC = Analog todigital converter, resolution of 16 bits. This division gives the angular rate in °/s,by multiplying with pi/180 the angular rate can be expressed in radians/s.

Moreover, the noise from the gyroscope has also been modelled using Simulink.The variance and the standard deviation has been derived from 21000 samplesof the gyroscope left stationary.

6.4.1.2 Implementation

Given the precondition that the vehicle is standing still, samples were obtainedfrom the physical sensor. These samples were converted into angular velocitiesin rad/s by algorithm 6. Given these converted samples, figures were created inMatlab which display the Gaussian nature of the values from the gyroscope. Fig.6.5 shows the angular velocities from the gyroscope, and how the curve resemblesa Gaussian distribution. This hypothesis is also in line with conclusions of [13,p.7-8,22] when measuring sensor errors.

Figure 6.5: Bar chart displaying the noise of the gyroscope, curve showing aGaussian distribution

The Gaussian noise as well as the bias offset was implemented into the Simulinkmodel. This can be seen in Fig. 6.6. This model outputs an angular rate expressedin rad/s corrupted by noise and bias with the aim of resembling the actual gyro-scope implemented in the physical car.


Figure 6.6: Gyroscope implementation in Simulink

6.4.1.3 Test phase

Evaluation of the model has been done by implementing a test environment inSimulink. Basically, a ROS subscriber block has been implemented in Simulinkwhich receives the actual values from the physical car. The values of the z-axis ofthe gyroscope are being measured and computed as an angular rate in rad/s aswell as an angle in rad. The comparison between the Simulink model of the sen-sor and the physical sensor values can then be done in the Simulink environment.Fig. 6.7 below shows the test environment.

Figure 6.7: Gyroscope test environment in Simulink

Moreover, Fig. 6.8 shows the angular difference between the gyroscope modeland real time measurements on the physical gyroscope.


Figure 6.8: Comparison between gyroscope model and real time measure-ments on the physical gyroscope. Top display shows the model output after129.2 seconds, and bottom display shows the total angle measured from thephysical gyroscope. (Notice, both are initialized to 1.507 rad, 90 degrees)

6.4.2 Wheel encoders

6.4.2.1 Description

A sensor model of the wheel encoders has been implemented into the model ofthe car in Simulink. They have been modelled with regards to measurementsof the output from the wheel encoders attached on the physical car. However,the data being received is actually an estimate of the speed of the physical carand not a number of magnets detected since the last timestamp. With that beingsaid, the sensor model implemented is using these measurements to estimateand mimic the transmitted car speed from the physical wheel encoders attachedon the car. These car speed measurements are not that precise though, whichcould be explained by a bias offset and noise. The bias consists of truncationerrors and the algorithm which converts magnet pulses to car speed in cm/s. Thetruncation error occurs when the algorithm truncates the floating number to anu_int8 which is to be transmitted on the CAN bus. The noise can be expressed interms of the variance around the mean velocity.


6.4.2.2 Test phase

The test phase of the wheel encoders was executed at our office on the floor. More-over, we used equipment such as a yardstick, rulers, as well as a mobile phonefor measuring time of which the vehicle traveled a certain distance. Each mea-surement conducted consisted of about 40 measured samples. Table 6.2 belowshows the measurements done from the test environment. The results on eachrow differs quite a bit, this is mostly due to the fact that the engine on our carrestarted randomly during runtime.

Table 6.2: Results of measurements on the wheel encoders

MI µ σ σ2 [s] [cm] [tq] [vq]5 4.1 0.98 0.96 6.59 58 0.83 0.965 4.1 1.1 1.2 4.3 33 0.83 0.955 4.4 0.79 0.62 4.6 40 0.83 0.945 4.1 0.31 0.1 4.15 34 0.44 0.7310 10.9 0.34 0.11 5.13 94 0.73 0.8910 10.7 0.94 0.89 6.4 122 0.76 0.8710 11 0.91 0.81 5.3 103 0.83 0.8710 10.7 0.47 0.22 3.55 50 0.38 0.6515 15.9 0.94 0.89 6.1 177 0.8 0.9315 15.7 1.24 1.56 3.6 103 0.74 0.8515 15.9 1.51 2.26 6.35 196 0.75 0.8815 15.9 1.1 1.21 5.2 144 0.72 0.8820 22.1 1.32 1.75 4.8 190 0.72 0.8420 21.5 1.02 1.05 4 142 0.56 0.7320 20.2 1.99 3.99 3.4 122 0.58 0.8020 19.8 0.95 0.9 4.65 167 0.61 0.79

Basically, what can be seen from Table 6.2 is the sent instruction MI to themotor, the average measured velocity µ, the standard deviation σ , the varianceσ2, the physical time [s] for the car to travel a distance expressed in seconds,the physical distance [cm] the car travelled during the measurement. Moreover,the factor [tq] denotes the portion of time of which the speed of the vehicle isabove a certain threshold, thus disregarding the portion of time of which thevehicle is accelerating. Furthermore, the factor [vq] corrects the actual distancethe car travelled during the performed test, by disregarding the distance travelledduring acceleration to a more steady velocity.

The Fig. 6.9 shows the speed received from the physical wheel encoders.

6.4.2.3 Implementation

The model was implemented based on the values in Table 6.2. The bias has beenmodelled through numerous measurements on the wheel encoder values and


Figure 6.9: Plot of the measured speed from the wheel encoders

physical measurements on the actual car driving a predefined distance of 2 m.From Table 6.3 below, the measured velocity from physical tests are significantlyhigher than the car speed which the sensors believe the car is driving in. The waythis is implemented in Simulink is by introducing a look up table, with an actualvelocity input scaled to the sensed value.

Table 6.3: Measured velocity compared to the sensed velocity from the phys-ical wheel encoders

Motor instruction 0 5 10 15 20Sensed velocity 0 3.9 10 15.7 21.3Measured velocity 0 9.82 23.3 36 49

In Fig. 6.10 below, the car speed from the sensor implemented in Simulink isplotted. The plot is showing the car speed in cm/s plotted over time. Moreover,the car speed is depending on user input in the model which can be seen as thesteps where a user has increased the car speed. In addition, the sensor modelimplemented includes a noise source which depends on the current speed of thecar model, this noise can be seen in the plot below.

Fig. 6.11 below shows the model implementation in Simulink. Basically, whatcan be seen is the conversion from m/s to cm/s, a LUT converting the velocityinput to what the sensor detects, as well as a Matlab function which adds noiseto the velocity depending on the current velocity.

6.5 LIDAR 47

Figure 6.10: Wheel encoder noise plotted over time in Simulink

Figure 6.11: Wheel encoder model implemented in Simulink

6.5 LIDAR

6.5.1 Description

The LIDAR is implemented as an S-function called lidar, and models the RPLI-DAR used in this thesis. The inputs of this function are shown in Table 6.4 below.


Table 6.4: Inputs of LIDAR S-functions

Inputs Direction Size DefinitionCoords In 2 Current coordinates of the vehicle.Yaw In 1 Current yaw of the vehicle.Lidar Offset In 1 Vertical Displacement of LIDARCollision Out 1 Boolean indicating if the vehicle has hit a wallDistances Out 360 Distances to objects in all angles

6.5.2 Operation

Before vehicleMap has initialized the map, the function waits in an idle state. Af-ter this, the distance is measured. Noise, range constraints, and an error PDF isthen applied to this value to make it resemble the RPLIDAR. This value is thenwritten to the state vector. Subsequently, the LIDAR is rotated one degree to pre-pare it for the next measurement, which will take place after a delay. This delayis equal to the time it takes for the RPLIDAR to rotate one degree. If the LIDARhas reached the end of its rotating cycle, the state vector is first written to theoutput and then flushed. A flowchart of the function is shown in Fig. 6.13 below,and a diagram of the subsystem is shown in Fig. 6.12.

Figure 6.12: Diagram of LIDAR subsystem

6.5 LIDAR 49

Figure 6.13: Flowchart of LIDAR S-function


6.5.3 Probability of error

Early on in the modeling process, it was observed that the LIDAR did not quiteperform as expected. When doing a full rotation, the LIDAR failed produce mea-surements for all 360 degrees. During one of the early tests, the LIDAR wasplaced in a cardboard box. It was observed that the amount of missed measure-ments was usually distributed among the same rotational points throughout re-peated experiments. This raised the hypothesis that the probability of the LIDARmissing a measurement is due to how the laser hits the object. When the LIDARis in a box, there are two conditions which will differ with every measurement,proximity to the wall, and the angle to the wall. Thus, it was decided to investi-gate how each of these conditions affected the probability of LIDAR misses.

In order to investigate the missed measurements, a test area was set up asshown in Fig. 6.16, where a cardboard box was placed in front of the vehicle.This cardboard box was then tilted in different angles and placed at differentdistances in order to determine what causes the LIDAR to miss a measurement.The results of this experiment can be seen in Fig. 6.14 and Fig. 6.15, respectively.No conclusive testing was done to see if different types of surfaces affected theprobability of error.

Figure 6.14: Percentage of misses due to wall incline

6.5 LIDAR 51

Figure 6.15: Percentage of misses due to distance to wall.

From Fig. 6.14 it can be seen that when the tilt of the reflection surface ex-ceeds 40°, the percentage of missed measurements increases dramatically. Hypo-thetically, this due to the tilt angle of the measured object causing a larger reflec-tion angle of the laser. This means that the reflected laser is more likely to missthe receiver. From observing Fig. 6.15 it can also be concluded that the LIDARdoes not operate probably when the distance to the measured object is gettingclose to its range constraints. In the model, these sets of error probabilities arerealized as two look-up tables. When a measurement is simulated, the angle anddistance to the measured object are passed to its respective look-up table, and aprobability of error is obtained. This probability is then used in a binomial ran-dom variable which is multiplied with the measured distance, meaning that themeasurement will be equal to 0 if the random variable generates a miss.


Figure 6.16: Measuring environment for investigating LIDAR misses

6.6 ROS Interface 53

6.6 ROS Interface

The ROS Interface subsystem, shown in Fig. 6.17 enables the model to com-municate with ROS, and makes heavy use of the Robotics Systems Toolbox forSimulink. The ROS Interface takes the LIDAR and Odometry data generated bythe model and it publishes in two ROS topics, /scan and /sensor_data. Moreover,it also publishes the current position of the car in the Simulink map on the topiccar_position. It also publishes the clock, since ROS needs to be run on the sameclock as Simulink if the two are to be used in the same system.


Figure 6.17: Block Diagram of ROS Interface

6.6 ROS Interface 55

6.6.1 /sensor_data

This topic contains the speed and the angular velocity of the vehicle. The messagetype is custom defined, and is comprised of a Header [9] and a Float64MultiArray[8]. This topic publishes the current odometry values periodically, with a clockperiod of 10 ms.

6.6.2 /scan

This topic contains the most recent LIDAR scan as well as a set of constants asrequired by the ROS message type sensor_msgs/LaserScan [7]. The /scan topicis published whenever the LIDAR has made a full 360°rotation. This means thatthe publishing frequency of the /scan message is effectively 5.5 Hz, as this is therotational frequency of the LIDAR model.

6.6.3 /car_position

This topic contains the position of the vehicle in the Matlab/Simulink generatedmap. This topic is solely for testing purposes to compare the x and y position ofthe vehicle at different time instances. The topic consists of a Float64MultiArrayand a header with the current time stamp. The publishing frequency of the topicis 10 Hz.

6.6.4 /clock

This topic contains the timestamp, expressed as a ROS timestamp. A ROS times-tamp constains two values, one value of how many seconds have passed, and onevalue of how many nanoseconds have passed since the last measured second. Forexample, if the time stamp is supposed to be 5.8, the value for seconds will be5 and the value of nanoseconds will be 0.8 ∗ 109 This topic was introduced dueto the fact that the model cannot run in real time. Basically, the clock topic con-tains the relative time being used in the model. The message type being used isros_graph/Clock, and is published at a frequency of 100 Hz.

7ROS Implementation

In order to control the vehicle and obtain its sensor data, a set of ROS nodes wasimplemented, as shown in Fig. 7.1 below. Everything within a dotted rectanglein the figure is a ROS node, where each dotted line represents a device runningthe node. Each node is described in the following sections.

Figure 7.1: Diagram showing the active ROS nodes while executing a SLAMalgorithm on the physical vehicle.

57

58 7 ROS Implementation

7.1 CAN Reader

The can_reader node is tasked with obtaining the sensor data sent by the STM32on the CAN bus, and making this data accessible to the other ROS nodes. TheCAN bus is being read continuously, and depending on the CAN ID of the re-ceived data, a function handling LIDAR data or a function handling odometrydata is called. These functions parse the data to a graspable format, an exam-ple of this would be the conversion of the z axis of the gyroscopes digital valueto an angular velocity. The LIDAR data is published using a topic called /scan,which is of the message type sensor_msgs/Laserscan. The sensor data topic iscalled /sensor_data, containing car speed, timestamp, gyroscope values to men-tion some, with the message type of the topic being a customized message calledcan/Sensor_data .

7.2 Odometry

The purpose of the odometry node is to present the angular rotation and the xand y position of the vehicle model as well as the physical vehicle. This is doneby subscribing to the sensor_data topic, described in Section 7.1. Basically, thedata is extracted from the topic in the form of a velocity, an angular velocity anda time stamp. With the help of this data, velocities in the x and y direction canbe calculated. By using these velocities and the difference between the currenttimestamp and the previous timestamp the position in x and y of the vehicleand the angle of which the vehicle has turned can be calculated. This data ispresented as a topic which contains the current coordinates of the vehicle as wellas the total angle rotated. Moreover, it is also presented to the TF tree, whichis a transform tree that relates coordinate frames to each other. The coordinatesof these frames depends on what kind of frame it is. For example, the base linkframe is the coordinate frame of the entire robot, so the coordinates for this frameis the actual position of the robot.The odometry node presents the transform fromthe odom frame to the base_link frame. This correlation shows the relative x andy position and orientation of the robot frame (base_link) to the odometry frame.

7.3 SLAM

The SLAM node is basically the implementation of the SLAM algorithm wrappedinto ROS. Basically, the SLAM algorithm requires information from the sensors,such as the LIDAR data as well as the odometry data. By acquiring this data it isable to generate a map topic which contains the information about its surround-ings. Moreover, it also presents a transform from map to the odom coordinateframe. With this information, the coordinate frames in the map can be relatedto each other and the positioning of the vehicle within the map can be calculatedand presented. Fig. 7.2, depicts the transform tree, which shows how the coordi-nate frames relate to each other.

7.4 rViz 59

Figure 7.2: An overview of the transform tree. The broadcaster is the nameof the node which presents the transform. How often the transform is up-dated can be seen in the average transform. The buffer length is how manyseconds of data is available in the tf buffer. Transform times are seen in themost recent and oldest transform, where the former contains the timestampof the first transform of that type, and the latter contains vice versa.

7.4 rViz

The rViz node is mainly a visualization tool which is able to provide a live updateof the map generated from the SLAM algorithm. Moreover, the trajectory of thevehicle in the map can also be visualized. An overview of the rViz window isshown in Fig. 7.3.

60 7 ROS Implementation

Figure 7.3: An overview of the rViz window. The main window shows themap generated by a SLAM algorithm. The little red square is the currentpose of the vehicle.

7.5 Position

The general idea behind the position node is to save the trajectory data of thevehicle. The position of the vehicle is obtained by looking up a transform fromthe fixed world frame to the base_link frame which represents the actual positionof the vehicle. The estimated position is generated by the SLAM algorithm. Thetopic was introduced for testing purposes in order to compare the actual positionof the vehicle with an estimated position to validate each SLAM algorithm. Thedata being saved to the text file is the x and y coordinates as well as a time stamp.The values are stored into a vector with a frequency of 10 Hz, basically, the sameas the frequency of the /car_position topic referred in Section 6.6.3.

7.6 simulinkPosition

The main idea behind the simulinkPosition node is to save the trajectory of thevehicle within the map generated from Simulink. The node subscribes to thetopic from simulink /car_position and saves the x and y coordinates as well asa time stamp into a text file. These coordinates represents the actual position ofthe vehicle in the map, thus, not distorted by the sensors in the model. This topicis described in Section 6.6.3.

7.7 ROS Control App 61

7.7 ROS Control App

The ROS Control App is an android mobile application, which is used to steer thephysical vehicle. This is done by moving a joystick in x and y directions, whichpublishes the angular and linear movement on a topic called /joystick.

7.8 Joystick Input

The purpose of the Joystick node is to interface the data from the ROS ControlApp and supply this to the physical vehicle. Basically, the node subscribes tothe joystick topic from the ROS Control App. The data from the application canbe seen as an instruction to rotate the vehicle as well as instruction to move thevehicle forward or backwards. This data is being being split up into two topics,/throttle as well as /steering which are both of the type std_msgs/Int8. The throt-tle topic contains instructions to the engine of the vehicle to move the vehicleeither forward or backwards, and the steering topic contains the instructions tothe servo on the vehicle.

7.9 CAN Writer

The task of the can_writer node is to send data over the CAN bus to the STM32.Basically, this data being sent is steering instructions to the servo on the vehi-cle, instructions to the motor to move the car forward or backwards as well asinstructions to the lights and the buzzer on the vehicle. This data is obtainedfrom various topics in the ROS environment, such as the /throttle 7.8 topic, the/steering 7.8 topic and the /carLights topic.

7.10 CPU Recoder

The task of the CPU recorder node is to periodically sample the CPU usage of aprocess and store it in a text file. It uses two different linux commands for this,top and ps, and stores the results from each method in a separate text file. Twodifferent methods are used as a security measure, since it would be very timeconsuming to have to do all simulations again if results from the other methodwas needed.

8Simulations

8.1 Method

8.1.1 Overview

In order to determine which algorithm was suitable to use on the vehicle, sim-ulations were performed on the model. Three maps were created, each of dif-ferent size and each map containing different amount of features in the envi-ronment. This is to check the performance of each algorithm during differentcircumstances. Control instructions were recorded for each map using the Inputblock in the Simulink model, so that an identical vehicle path could be used foreach simulation. After this, the simulation was run with this path and all thesimulated data was saved in a ROS bag. In this way, the real-time limitations ofthe model could be avoided while running the SLAM algorithms.

Fig. 8.1 below shows the active ROS nodes while simulations are taking place.The player node replays the data stored in the ROS bag and publishes the dataon the /scan, /sensor_data and /car_position topic. The Odometry node presentsodometry information to the SLAM node, the rViz node and the Position node.

The simulinkPosition, cpu_recorder, position, and rViz nodes were used fortesting purposes. Thorough descriptions of all these nodes can be found in Chap-ter 7.

63

64 8 Simulations

Figure 8.1: Diagram showing the active ROS nodes while simulating a SLAMalgorithm

8.1.2 Maps

Three maps were designed for the simulation. The main idea was to have threemaps which grow in size, as all SLAM algorithms scale with the map size, andthis might give an idea of how well a particular algorithm scales. The maps areshown in Figures 8.2, 8.3, and 8.4 below, where the axis tick labels show distancein meters.

8.1 Method 65

Figure 8.2: Plot of small map

Figure 8.3: Plot of medium map

66 8 Simulations

Figure 8.4: Plot of large map

8.1.3 Tests

Four different types of tests were performed on each algorithm, namely:

• Mapping accuracy

• Path accuracy

• Average position error

• Average CPU load

In the mapping accuracy test, the SLAM algorithm is used on a specific mapsimulation. When the map is obtained, this is saved, and presented next to theactual map in order to show how the generated map from the SLAM algorithmdiffers from the actual map.

The path accuracy test compares the generated path of each algorithm with theactual path the kinematic vehicle has travelled. This is presented in a plot whichincludes the actual path, and the generated path from each algorithm.

The average position error is a performance metric which shows how much the es-timated position from the SLAM algorithm differs from the actual position. Thisis calculated by computing the difference between the estimated position and ac-tual position at each time instance, and computing the mean.

In the average CPU load test, the CPU usage of each SLAM algorithm was recordedusing the CPU recorder node described in Section 7.10. For these results, the topmethod was used, with a sampling period of 10 ms.

8.1 Method 67

Each test is performed for each algorithm, for each map. Also, each of these testsis performed for simulations with ideal sensors, and simulations with added sen-sor noise.

After the theoretical investigation had been conducted, it was decided to per-form simulations with GraphSLAM and FastSLAM. This investigation is thor-oughly described in Section 3.5. These algorithms are implemented in the ROSpackages slam_karto and gMapping, which were used for the simulations. Theactive ROS nodes for the slam _karto and gMapping simulations are illustratedin Fig. 8.5 and Fig. 8.6, respectively.

Figure 8.5: ROS network during simulation of the slam_karto algorithm.

68 8 Simulations

Figure 8.6: ROS network during simulation of the gmapping algorithm

8.2 Results 69

8.2 Results

All simulation results are presented in the sections below. The tests themselveswere carried out as specified in Section 8.1.3.

8.2.1 Mapping accuracy

This section presents how well each algorithm performs in terms of generating anaccurate map. Each figure shows how one map is generated by both algorithms,as well as using both the ideal and the noisy sensor model for each algorithm.

8.2.1.1 Small map

Fig 8.7 below shows the results for the mapping of the small map. The results ofeach algorithm/sensor model appear to be fairly similar.

(a) gmapping with ideal sensor model (b) slam_karto with ideal sensor model

(c) gmapping with noisy sensor model (d) slam_karto with noisy sensor model

Figure 8.7: Mapping accuracy results on small map

70 8 Simulations

8.2.1.2 Medium map

Fig 8.8 shows the results of mapping of the medium map. In this case, the noiseappears to have an effect on the mapping.



Figure 8.8: Mapping accuracy results on medium map

8.2 Results 71

8.2.1.3 Large map

Fig 8.9 shows the mapping of the large map. Here, the performance of bothalgorithms varies greatly.



Figure 8.9: Mapping accuracy results on large map. When the noisy sensormodel is used, as shown in (d), the map produced by slam_karto is highlyinaccurate. This is because the loop closing fails.

72 8 Simulations

8.2.2 Path accuracy

The following sections contain the results of the path accuracy test for all threemaps and both the ideal and noisy sensor models. Each figure contains the theo-retical path as well as the generated path of slam_karto and gMapping.

8.2.2.1 Small map

Fig 8.10 below shows the generated path of both algorithms compared to thetheoretical path when running simulations on the small map. gMapping is nothandling the noise as well as slam_karto in this case.

8.2 Results 73

(a) Ideal sensor model

(b) Noisy sensor model

Figure 8.10: Estimated path on small map

74 8 Simulations

8.2.2.2 Medium map

Fig 8.11 below shows the generated path of both algorithms compared to thetheoretical path when running simulations of the medium map. The generatedpath of both algorithms does not deviate greatly from the theoretical path usingthe ideal model. Both algorithms however, especially slam_karto, show a signifi-cantly erroneous path with the noisy sensor model.



Figure 8.11: Estimated path on medium map

8.2 Results 75

8.2.2.3 Large map

Fig 8.12 below shows the generated path of both algorithms compared to thetheoretical path when running simulations on the large map. The loop closureproblems of slam_karto, described in Section 8.2.1.3, causes its path to drift awayduring the end of the simulation.



Figure 8.12: Estimated map on large map

76 8 Simulations

8.2.3 Average position error

8.2.3.1 Ideal sensor model

Table 8.1: Average position error of gMapping and slam_karto in differentmaps with ideal sensors.

Small Map Medium Map Large MapgMapping 0.08 0.3055 0.4308slam_karto 0.0913 0.1970 0.4012

As can be seen in Table 8.1, when an ideal sensor model is used, the algorithmsproduce similar results, although slam_karto slightly outperforms gMapping onthe medium map. A specific cause for this remains inconclusive. What can besaid regarding this in general is that given the same environment and sensordata available, slam_karto simply works better than gMapping under those veryparticular circumstances.

8.2.3.2 Noisy sensor model

Table 8.2: Average position error of gMapping and slam_karto in differentmaps with noisy sensors.

Small Map Medium Map Large MapgMapping 0.1983 0.3270 0.4260slam_karto 0.0783 0.5307 0.6829

In both the ideal and noisy simulation sets, the clear relationship is that the aver-age position error always seems to increase with the size of the map. This can beattributed to the fact that if the robot travels for a longer distance, the measure-ments may drift for a longer time until the algorithm has a chance to synchronize,i.e. through a loop closure. This is illustrated pretty well in Fig 8.12, where forexample gMapping drifts a lot while travelling through the long corridor until itis able to correct itself.

Fig 8.13 shows how the position error over time for one of the simulations.The figure shows that the error does not have any time-related behaviour, it’smagnitude does not for example increase nor decrease over time. The error is incontrast related to the algorithms ability (or disability) to predict the position ofthe vehicle. This ability can be due to the quality of sensor data the algorithmhas to work with, and how "difficult" the current estimation problem is to solve.

8.2 Results 77

Thus, the error is more related to the state of the vehicle and its environment,rather than the time elapsed.

Figure 8.13: Large map with ideal sensors- position error over time

8.2.4 Average CPU load

In the section below, the average CPU load is expressed as a percentage. Thispercentage is the average percentage of time that the SLAM algorithm utilizesthe CPU. This figure refers to the usage of one CPU core, so a number higherthan 100% is possible if the algorithm uses more than one core.

78 8 Simulations

8.2.4.1 Ideal sensor model

Small Map Medium Map Large MapgMapping 68.61% 99.19% 102.21%slam_karto 16.74% 39.03% 56.94%

Table 8.3: Average CPU load of gMapping and slam_karto in different mapsusing ideal sensor model.

8.2.4.2 Noisy sensor model

Small Map Medium Map Large MapgMapping 90.5% 97.93% 70.45%slam_karto 34.85% 46.45% 21.91%

Table 8.4: Average CPU load of gMapping and slam_karto in different mapsusing noisy sensor model.

Something which is somewhat surprising here is the the fact that for slam_karto,the CPU usage goes down significantly for the large map. If you cross-examinethis observation with the fact that this simulation produced a highly inaccuratemap (see Fig. 8.9 (d)) and a highly inaccurate path (see Fig 8.12 (b)), this mayindicate that the algorithm is not doing all computations necessary to work prop-erly.

8.3 Issues

When doing these experiments, there were some problems with running theslam_karto node. The problem was that when performing a loop closure, thenode crashed almost every time. A crash itself did not result in erroneous re-sults, but it meant that the SLAM was aborted, so no further mapping could beconducted from that point on. Also, since the crash resulted in a loop closurenot being executed, this means that the generated map was not as accurate as itwould have been following a successful loop closure.

The cause of the slam_karto crashes was never found. In order to still obtainresults which could be used in this thesis, the slam_karto experiments had toperformed over and over again until the loop closure algorithm managed to beexecuted without crashing.

8.4 Conclusion 79

8.4 Conclusion

The results show that both the gMapping and slam_karto algorithms are able tosolve the SLAM problem. The generated maps (seen in Fig. 8.7, 8.8, and 8.9)from the two algorithms are similar in the ideal case. Moreover, the position-ing error does not differ a lot between the two algorithms, with each algorithmoutperforming the other on at least one map.

However, when noise is introduced, it can clearly be seen that the loop closureof slam_karto starts failing. A good example of this can be seen in Fig. 8.8 d),where the robot has observed the lower room twice, but fails to recognize thefact that it as been there before. A similar issue can be seen in Fig. 8.11 b). Itshould also be noted that the loop closure of slam_karto mostly resulted in thenode crashing, or generated a map that was completely incomprehensible. Theresults shown in this report simply show the best result out of many trials. ThegMapping algorithm, however, produced similar results every time, and therewere no issues with the loop closure.

Another interesting observation can be seen in Fig. 8.12 b). In this simula-tion, gMapping continuously makes small position mistakes which are quicklycorrected. slam_karto, on the other hand, predicts the position really well un-til it reaches the lower corridor of the map, where the positioning error startsto increase. The algorithm never recovers from this error, and keeps on produc-ing false position estimates from that point. This, along with the fact that theslam_karto node almost always crashes when performing a loop closure, indi-cates that it may not be as reliable as gMapping.

Tables 8.3 and 8.4 show that in all simulations performed, gMapping has atleast twice as much CPU load as slam_karto. This large CPU load of the algo-rithm, which at times even occupies an entire core by itself, is an indicator thatthe algorithm might have problems being executed using limited resources. Re-gardless, it was still chosen to be the one to be implemented on the physicalvehicle, due to the fact that slam_karto was deemed too unreliable.

9Real-world experiment

9.1 Overview

Chapter 8 introduced simulation results on three different maps generated by twodifferent SLAM algorithms. From these simulations, it was decided that gMap-ping was the appropriate algorithm to use with the physical vehicle. This chap-ter describes a real-world experiment featuring a map generated by the physicalvehicle while running gMapping.

The real-world experiment of the gMapping algorithm was performed at theÅF office building in Linköping. The room chosen was a small office approxi-mately measuring 5.3x4.8 m. A sketch of this office can be seen in Fig. 9.1 below,and a photo can be seen in Fig 9.2

81

82 9 Real-world experiment

Figure 9.1: Sketch of test environment. The circles represent table legs, andthe squares two large cupboards which divide the room.

9.2 Results 83

Figure 9.2: Photo of test environment

9.2 Results

The experiment was conducted three times, and the generated maps are shownin Fig. 9.3 below. The computational load for each trial is shown in Table 9.1.


(a) Trial 1

(b) Trial 2

(c) Trial 3

Figure 9.3: Map of small office generated by gMapping

During the generation of each map, the robot was controlled manually, whichmeant that the robot drove differently at each trial. All maps are fairly accurate

9.2 Results 85

in the sense that they depict all landmarks correctly. This is evident if any of themaps in Fig. 9.3 are compared with the sketch of the test area shown in Fig. 9.1,as it can be seen that all features shown in the sketch are present. The algorithmdoes seem to struggle a little bit with the shape of the bottom cupboard as map(a) and (b) depict it not as a perfect rectangle, but slightly skewed. It probablywould have been easier for the algorithm to map this feature had it been ableto travel along the south side of the cupboard, in order to perform a better loopclosure. For the purposes of this thesis though, the mapping accuracy is highlysatisfactory.


Table 9.1: Computational load of the gMapping algorithm

Trial No. CPU load1 88.13%2 89.5%3 94.48%

9.3 Conclusion

The maps shown in Fig. 9.3 show that the gMapping algorithm is able to success-fully create a map of the office. All objects (the cupboards and the chair legs) aresuccessfully depicted in the map. The positioning part of the algorithm has notbeen evaluated in the real world experiment due to the lack of a sophisticatedenough test environment to do so.

The CPU load in this experiment, shown in Table 9.1 is similar to the one inthe simulations, meaning that almost an entire core is occupied while the algo-rithm is running. As the imx6 processor used on the physical vehicle is a quad-core, this is not a problem if only the SLAM algorithm is executed. However ifadditional functionality was to be added to the vehicle, for example a pathfind-ing algorithm, some performance issues may arise. These could be solved by, forexample, reducing the amount of particles in the filter, but may also result in thealgorithm producing more erroneous results.

10Discussion

10.1 Achievements

This thesis has yielded the following results:

• A Simulink model of the physical vehicle and its sensors

• A small-scale vehicle with a working SLAM algorithm

• A comprehensive study comparing different SLAM algorithms

The model-based development approach was proven to be beneficial for this the-sis. Comparing the estimated position with an actual position would have beensignificantly difficult if only the real-world experiment had been performed, asit would have required a highly sophisticated test apparatus. Without the model,the results section of this thesis would have been rather thin. The model wasprobably not necessary for the final implementation itself though, since the out-put of the algorithms could have been examined visually in order to evaluatewhich one should be implemented. The performance numbers could not havebeen obtained without it though, and the model-based approach can be reallyuseful in situations where testing is difficult and/or expensive.

The performance of the SLAM algorithm implemented were highly satisfac-tory. One of the main reasons for the ADEPT project is do develop somethingwhich could be demonstrated at job fairs, the SLAM algorithm works really wellin this scenario. One of the reasons for this is that it works seamlessly withoutany special kind of environment, as long as too many moving objects are avoided.The results are also very visual, and could easily appeal to a large audience.

The comparison of the SLAM algorithms was also successful. Several per-formance tests were devised, and clearly show how well the SLAM algorithmsperform under different circumstances. There’s something this thesis has which

87

88 10 Discussion

other similar works lack, and that is a noisy sensor model. Most other works as-sume ideal sensors. This is unrealistic, since there’s always sensors noise presentto some degree. A good study of SLAM algorithms needs to examine how wellthey perform in the presence of noise, otherwise the evaluation of the algorithmmay be overly optimistic.

10.2 Method

If this thesis was to be conducted again, there could be several improvements onits method. One is the fact that a lot of time was spent trying to create a realisticmodel of the vehicle itself. Since this proved to be too time-consuming, it wasabandoned in favor of a simple vehicle model, but nevertheless it consumed a lotof time which could have been better spent.

Another aspect which could be improved is the fact that the whole environ-ment model was created from scratch in Simulink. This was also probably unnec-essarily time-consuming, as ROS-compliant robot simulators already exist, suchas Stage or Gazebo. This was not known, however, until the model was almostcompleted. If these were to have been used (along with Simulink for the sensormodel), an equal or better model could probably have been implemented in ashorter amount of time. This time could have been spent completing some of thelower priority tasks, or do some further debugging on slam_karto in order to findout more explicitly why it had a tendency to crash.

10.3 Complications

During the thesis work complications arose regarding one of the specified ques-tions which was to be examined. The specified question was "Development ofa model in Simulink and evaluating the model by comparing the output of themodel to a theoretical position/direction within a map". As the modelling ofthe vehicle itself proved too time-consuming, most time was instead spent onmodelling the sensors of the vehicle. This resulted in a very simplistic vehiclemodel, which only approximately resembles the vehicle used in this thesis. Forexample, when it comes to acceleration and forward velocity, a generic model isused, which has little to do with the physical vehicle. Thus, comparing the modeloutput to a theoretical position/direction wouldn’t say much, due to the acceler-ation and forward velocity not being based on the physical vehicle. As a result,this evaluation had to be omitted from the thesis. However, since the thesis wasabout evaluating SLAM algorithms, a realistic motion model of the vehicle is notof high importance. What is of importance here is instead accurate modelling ofthe sensors, which was done successfully to some degree.

There were also some hardware issues with the physical vehicle. Some com-ponents broke, meaning that time had to be spent to make repairs. As some com-ponents had to be ordered, the physical vehicle was not available while waitingfor the new components to arrive. After gathering the results of the real-worldexperiment, the STM32 board broke. Since this board is hard to replace, it meant

10.4 Future work 89

that no additional experiments could be done, for example trying to make a mapof a larger environment.

10.4 Future work

Since the model of the vehicle has already been developed, it could be used to testany ROS-compliant SLAM algorithm with minor further implementation. Sev-eral newer algorithms which have not been covered in this thesis could be used,such as HectorSlam or Google Cartographer. HectorSlam would be of extra inter-est, as it uses no odometry, meaning that the inaccurate results provided by thewheel encoders in the physical vehicle would not affect the results in any way.

90 10 Discussion

Bibliography

[1] coreslam - ros wiki. http://wiki.ros.org/coreslam. Accessed: 24-03-2017. Cited on page 2.

[2] gmapping - ros wiki. http://wiki.ros.org/gmapping. Accessed: 24-03-2017. Cited on page 2.

[3] slamkarto - ros wiki. http://wiki.ros.org/slam_karto. Accessed:24-03-2017. Cited on page 2.

[4] EKF SLAM for dummies. https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-412j-cognitive-robotics-spring-2005/projects/1aslam_blas_repo.pdf, 2005. Accessed: 24-03-2017. Cited on pages 6, 15, 17,and 18.

[5] Fast large-scale SLAM with improved accuracy in mobile robot. 2010IEEE International Conference on Robotics and Biomimetics, Robotics andBiomimetics (ROBIO), 2010 IEEE International Conference on, page 791,2010. Cited on pages 19 and 26.

[6] About the company ÅF. http://www.afconsult.com/sv/om-af/historia/, 2016. Accessed: 2017-07-19. Cited on page 1.

[7] sensor_msgs/laserscan message. http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html, 2017. Accessed: 2017-06-27. Citedon page 55.

[8] std_msgs/float64multiarray message. http://docs.ros.org/jade/api/std_msgs/html/msg/Float64MultiArray.html, 2017. Ac-cessed: 2017-06-27. Cited on page 55.

[9] std_msgs/header message. http://docs.ros.org/jade/api/std_msgs/html/msg/Header.html, 2017. Accessed: 2017-06-27. Cited onpage 55.

[10] Mark Compere. Simple 2D kinematic vehicle steering model and animation.https://se.mathworks.com/matlabcentral/fileexchange/

91

http://wiki.ros.org/coreslam

http://wiki.ros.org/gmapping

http://wiki.ros.org/slam_karto

https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-412j-cognitive-robotics-spring-2005/projects/1aslam_blas_repo.pdf



http://www.afconsult.com/sv/om-af/historia/

http://www.afconsult.com/sv/om-af/historia/

http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html

http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html

http://docs.ros.org/jade/api/std_msgs/html/msg/Float64MultiArray.html

http://docs.ros.org/jade/api/std_msgs/html/msg/Float64MultiArray.html

http://docs.ros.org/jade/api/std_msgs/html/msg/Header.html

http://docs.ros.org/jade/api/std_msgs/html/msg/Header.html

https://se.mathworks.com/matlabcentral/fileexchange/54852-simple-2d-kinematic-vehicle-steering-model-and-animation


92 Bibliography

54852-simple-2d-kinematic-vehicle-steering-model-and-animation,1999. Accessed: 2017-06-27. Cited on page 32.

[11] A. Dine, A. Elouardi, B. Vincke, and S. Bouaziz. Graph-based simultaneouslocalization and mapping: Computational complexity reduction on a mul-ticore heterogeneous architecture. IEEE Robotics & Automation Magazine,(4):160, 2016. Cited on page 2.

[12] M. W. M. Gamini Dissanayake, Paul Newman, Steven Clark, Hugh F.Durrant-Whyte, and M. Csorba. A solution to the simultaneous localiza-tion and map building (slam) problem. IEEE Transactions on Robotics andAutomation, 17(3):229–241, 2001. Cited on pages 2 and 15.

[13] Manon Kok. Probabilistic modeling for sensor fusion with inertial measure-ments. Linköping Studies in Science and Technology. Dissertations: 1814.Department of Electrical Engineering, Linköping University, 2016. Citedon page 42.

[14] M. Laššák and K. Draganová. Improvement of Low-Cost MEMS GyroscopeCharacteristics by Data Filtering and Fusion. Advances in Military Technol-ogy, 16(2):171 – 178, 2016. Cited on page 33.

[15] Martin Meywerk. Vehicle dynamics. Automotive Series. West Sussex, Eng-land : Wiley, 2015. Cited on page 29.

[16] William F. Milliken and Douglas L. Milliken. Race Car Vehicle Dynamics.Warrendale, PA : Society of Automotive Engineers, 1 edition, 1995. Cited onpage 31.

[17] Curotto Molina and Franco Andreas. Graphslam algorithm implementationfor solving simultaneous localization and mapping. Master’s thesis, Univer-sidad de Chile, 2016. Cited on pages 16, 23, 24, and 26.

[18] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. Fastslam: A factoredsolution to the simultaneous localization and mapping problem. page 1,2013. Cited on page 27.

[19] Michael Montemerlo and Sebastian Thrun. FastSLAM - A Scalable Methodfor the Simultaneous Localization and Mapping Problem in Robotics.Springe Berlin Heildelberg New York, 1 edition, 2007. Cited on pages 12,20, 21, and 22.

[20] Erik Narby. Modeling and Estimation of Dynamic Tire Properties. Master’sthesis, Linköping University, Linköping, Sweden, 2005. Cited on page 7.

[21] Ulrich Nehmzow. Mobile Robotics - A Practical Introduction. Springer-Verlag London Limited 2003, 2 edition, 2003. Cited on page 35.

[22] Ed Ramsden. Hall-effect sensors - theory and applications. Amsterdam ;Boston : Elsevier/Newnes, 2 edition, 2006. Cited on page 34.



Bibliography 93

[23] S.B. Samsuri, H. Zamzuri, M.A. Abdul Rahman, and Ab Mazlan, S.A. Com-putational cost analysis of extended Kalman filter in simultaneous local-ization and mapping (EKF-SLAM) problem for autonomous vehicle. 2015.Cited on page 19.

[24] Machado J. Santos, D. Portugal, and P. R. Rocha. An evaluation of 2d slamtechniques available in robot operating system. 2013 IEEE InternationalSymposium on Safety, Security, and Rescue Robotics (SSRR), Safety, Secu-rity, and Rescue Robotics (SSRR), 2013 IEEE International Symposium on,(1), 2013. Cited on page 6.

[25] Pedro Machado João Santos. Smokenav - simultaneous localization and map-ping in reduced visibility scenarios. 2013. Cited on page 6.

[26] Roland Siegwart, R. Illah NourBakhsh, and Davide Scaramuzza. Introduc-tion to Autonomous Mobile Robots. Springer-Verlag London Limited 2003,2 edition, 2011. Cited on pages 23 and 25.

[27] Cyrill Stachniss. Robot mapping EKF SLAM. http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam04-ekf-slam.pdf, 2012. Cited on pages 18, 19, and 26.

[28] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabillistic Robotics.Cambridge, Mass. : MIT Press, 1 edition, 2005. Cited on pages 6, 7, 9, 10,11, 12, 13, 19, 21, 25, 26, and 27.

[29] Wei Wang, Li Dongying, and Yu Wenxian. Simultaneous localization andmapping embedded with particle filter algorithm. 2016 10th European Con-ference on Antennas and Propagation (EuCAP), Antennas and Propagation(EuCAP), 2016 10th European Conference on, page 1, 2016. Cited on page2.

[30] Yifan Xia, Jie Li, Lin Qi, and Hao Fan. Loop closure detection for visualslam using pcanet features. 2016 International Joint Conference on Neu-ral Networks (IJCNN), Neural Networks (IJCNN), 2016 International JointConference on, page 2274, 2016. Cited on page 16.

http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam04-ekf-slam.pdf



Documents

Implementation of SLAM algorithms in a small-scale vehicle ...1218791/FULLTEXT01.pdf · (SLAM) algorithm in a small-scale vehicle running the Robot Operating System (ROS). Which SLAM