Chapter 3 3D EKF-SLAMbibing.us.es/proyectos/abreproy/70437/fichero/5_Capitulo...of a mobile robot...

Chapter 3

3D EKF-SLAMDelayed initialization

As a first approach, the details of how to implement a common non-parametric Bayesian filter for the simultaneous localization and mapping(SLAM) problem is presented in this chapter. The approach detailed, isbased on a sequential Monte Carlo algorithm, commonly known as ParticleFilter, to solve the initialization stage of landmarks. The vehicle localizationis estimated with an Extended Kalman Filter (EKF) which is dynamicallyincreased with those nodes whose particle filter has converged into a singleGaussian distribution. Thus, at the end, the complete SLAM problem iscarried out as a centralized EKF-SLAM.

3.1 Overview of the method

The first part of this work was focused on learning how to implement the classicapproaches presented at the introduction of this document, so that the maindrawbacks of the RO-SLAM problem could be detected. For that purpose, thischapter propose a RO-SLAM approximation which extends the mapping solutionpresented in [9], adding the localization of the robot. The proposed SLAMsolution is mainly based on an EKF which initially contains only the estimationof a mobile robot (ground or aerial vehicle) position. On the other hand, a set ofrange sensors are used as the main sensor to correct the estimation of the robotlocalization. However, the location of these sensors (or landmarks) is completelyunknown and hence, their position must also be estimated by means of mappingalgorithms.

To map the position of these sensors with an extended Kalman filter, it is nec-essary to have an initial position estimation of them. Due to the low-informativemeasurements provided by these devices, (i.e. only distance between robot andone landmark), it is necessary to employ an initialization strategy which allows to

3.2 Localization approach

Figure 3.1: Initial uniform distribution of a range sensor position when only onerange measurement is received from a robot. The estimated position from whichthe robot detected the landmark the first time (i.e. first range measurementreceived) is depicted with a yellow point and represent the center of the uniformspherical distribution in which the landmark might be located. The real positionof the landmark is depicted with a green point.

get an initial estimation of landmarks which deals with the complex observationmodel of such kind of sensors. Here, it is considered the use of range sensors suchas radio emitters or ultrasonic sensors which provides the identification of eachlandmark, so that the data association problem is directly solved by the use ofthis unique identifiers. The method presented in this chapter proposes a delayedinitialization based on a sequential Monte Carlo filter (or Particle filter) which,as stated above, consist on sampling the state space of the sensor location usingan initial probability distribution with which particles/samples of the particlefilter are drawn.

In this case, the initialization stage takes place when the first range mea-surement ri is received from beacon i. This stage consist on initializing a newparticle filter Pi,t which particles are drawn according to the probability distri-bution of the range observation model, like the one depicted in Figure 3.1; thegeneral observation model of this kind of sensors is detailed in Appendix A.3.Then, once the particle filter converges into a single Gaussian distribution, theparticle set Pi,t can be integrated in the same EKF where the robot localizationtakes place. By following this procedure with every landmark detected, the EKFvector state increase dynamically. At the end, the complete SLAM problem issolved in a centralized EKF .The framework

which uses the EKF

to solve the SLAM

problem is usually

called EKF-SLAM

framework

The framework

which uses the EKF

to solve the SLAM

problem is usually

called EKF-SLAM

framework

40 | 3D EKF-SLAM Delayed initialization

3.3.1 Particle filter: Update stage

3.2 Localization approach

In the SLAM strategy presented in this chapter, the localization of the vehicle isdirectly performed by the EKF, so that the first parameters of the state vectorare the vehicle parameters. Thus, an initial value of these parameters must bedefined, indicating the associated variance of the initial robot position in thecovariance matrix of the EKF . Depending on the motion model and the vehicle Initially, the pa-

rameters of the

vehicle position are

often considered

completely uncorre-

lated.

Initially, the pa-

rameters of the

vehicle position are

often considered

completely uncorre-

lated.

used, the number of robot parameters might differ from 3 to more parameters.The motion models used in this work for simulation and real experimentationare described in Appendix A.

As the vehicle moves, depending on the motion model employed, new mo-tion measurements ut are received. The motion measurements are integrated inthe EKF during the prediction stage to get a prior distribution of the vehicleposition. On the other hand, the correction stage of this localization problem,consist on integrating the range measurements between the vehicle and the differ-ent landmarks detected. However, this correction stage cannot be applied untilthe landmarks have been integrated in the EKF, i.e. until the landmarks areinitialized in the EKF. The delay on the integration of landmarks into the EKF,implies that the localization estimation is not corrected until, at least, 3 land-marks are integrated in the EKF and hence, the localization of the vehicle mightdiverge making the EKF unstable numerically. For this reason, when employingdelayed initialization methods, it is mandatory the use of anchors, i.e. landmarkswhich position is known, so that the robot position estimation can be correctedwith the measures of this anchors. Thus, anchors must be deployed throughoutthe environment taking into account the area coverage of each transmitter . At least 3 anchors

are required to per-

form a good trilat-

eration of a land-

mark when a 2D

representation is

considered, and at

least 4 anchors for

3D representations.

At least 3 anchors

are required to per-

form a good trilat-

eration of a land-

mark when a 2D

representation is

considered, and at

least 4 anchors for

3D representations.

3.3 Hybrid Mapping approach

This section explains how to initiate and update the particle filters used duringthe initialization stage of each landmark, and how to determine the convergenceof the particle filter so that, finally, these particle filters can be integrated intoa centralized extended Kalman filter trading off between the precision of thisestimation and the convergence velocity.

3.3.1 Landmark initialization

The particle filter is initiated when the first range measurement from the robot isreceived. The first range measurement defines the radio of the uniform annulussphere distribution in which the real position of the landmark is located. Thecenter of this sphere is directly related with the estimated position of the robotat this time xr,t. This uniform distribution is depicted in Figure 3.1, in thispicture two radio values are depicted, r1 and r2, they represent the thickness ofthe annulus, which depends on the deviation σ(rt). Particles are then initiated

3D EKF-SLAM Delayed initialization | 41


as 3D points represented in Cartessian coordinates x = [XY Z]T and distributedaccording to that annulus spherical uniform distribution.


The objective of the range-only mapping algorithm is to estimate the position ofthe landmarks from data provided by range sensors. The information about thestate is updated with the set of measurements z1:t received up to time t. Thisset of measurements consists of range measurements ri generated between onerange sensor i and the aerial/ground robot (the algorithm considers a movingnode, and thus the time subscript t for the robot position).

In the Bayesian framework employed during the initialization stage, the infor-mation about the state is represented by the conditional probability distributionp(xt|z1:t). This distribution (the posterior) can be estimated online while newmeasurements are received. Indeed, the position will be estimated and updatedrecursively.

In that sense, the likelihood function p(zt|xt) plays a very important role inthe estimation process. In this case, this function expresses the probability ofobtaining a given range value ri from a range sensor i given the position of therobot xr,t.

The model used here considers that p(zt|xt) follows a Gaussian distribution

centered on the range value of r[n]i with a standard deviation proportional to the

standard deviation of the range measurement received.

ri = N (µ(r[n]i ), σ(r

[n]i )) (3.1)

A detailed explanation of the observation model used along this document ispresented in Appendix A.3.

Once the parameters of the Gaussian observation model have been identified,the weights of the particles are updated considering the likelihood of the receiveddata and the current likelihood of each particle (lines 1 to 5 of Algorithm 3.1).

In detail, the update procedure is as follows: for each particle p[n]t , the dis-

tance r[n]t = ||p[n]

t − xr,t|| is obtained. From this distance, the mean and vari-

ance of the conditional distribution p(rt|p[n]t ) are obtained, so that p(rt|p[n]

t ) =

N (rt;µ(r[n]t ), σ(r

[n]t )).

The probability of the actual range value under this distribution is finallyemployed to update the weight of the particle ω

[n]t as:

ω[n]t =

ω[n]t−1

σ(r[n]t )√2π

e−

(rt−µ(r[n]t

))2

2σ(r[n]t

)2 (3.2)

When the filter is running, the weights of the particles with high likelihoodincrease, while most of the particles rest at places of very low likelihood on thestate space.


3.3.3 Switching from PF to EKF

Algorithm 3.1: Particle filter for mapping WSN

Input: Pt−1 and rtOutput: Pt

1 for n = 1 to N do

2 Compute r[n]t = ||p[n]

t − xr,t||3 Determine µ(r

[n]t ) and σ(r

[n]t )

4 Update weight of particle n as

ω[n]t = ω

[n]t−1p(rt|p[n]

t ) = ω[n]t−1N (rt;µ(r

[n]t ), σ(r

[n]t ))

5 end for

6 Normalize weights ω[n]t

7 Compute Neff

8 if Neff < Nth then

9 Resample with replacement N particles from Pt.10 end if

As the number of particles is limited, a resampling algorithm (line 9 of Al-gorithm 3.1) is included to compute the posterior probability bel(xt). The algo-rithm duplicates particles with high weights and eliminates those with very lowweights according to the probability p(rt|p[n]

t ), i.e. according to the previouslyupdated weights . The resampling method employed in this chapter is known It is very impor-

tant to normalize

the weights updated

with p(rt|p[n]t) be-

fore resampling

particles.

It is very impor-

tant to normalize

the weights updated

with p(rt|p[n]t) be-

fore resampling

particles.

as low-variance resampling method, which is described in Algorithm 3.2. Thisalgorithm allows to spread the particles over the maximum likelihood areas. AsAlgorithm 3.2 shows, this algorithm differs from the one described in [30] in that,instead of replacing the most probable particles, they are drawn with a normaldistribution centered in one of the most likely particles and with variance σlvr.

In order to overcome some of the known problems with the resampling stage,an additional consideration is taken into account: resampling only takes placewhen the effective number of particles Neff is below a threshold Nth. The effec-tive number is computed as follows:

Neff =

[

N∑

n=1

(ω[n]t )2

]

−1

(3.3)


As new measurements are received, each particle filter tends to converge to asolution which can be compared with a Gaussian distribution. Then, in orderto determine the convergence of the solution, the algorithm developed uses theaveraged standard deviation σPt

as a measurement of convergence of the particlefilter into a Gaussian distribution. The averaged standard deviation is computedfrom the covariance matrix of the Gaussian distribution followed by the particlefilter at current time t.



Algorithm 3.2: Low variance resampling

Input: P̄t

Output: Pt

1 Pt = ∅;2 r = rand([0, N−1]);

3 c = ω̄[1]t ;

4 i = 1;5 for n = 1 to N do

6 u = r + (n− 1)N−1;7 while u > c do

8 i = i+ 1;

9 c = c+ ω̄[i]t ;

10 end while

11 draw pt → Pt according to N (p̄[i]t , σlvr) with ωt = ω̄

[i]t ;

12 end for

13 Normalize weights ω[n]t ;

The mean µt and variance Σt of the Gaussian distribution followed by theactual particle filter can be computed as follows:

µt =N∑

n=1

p[n]t ω

[n]t (3.4)

Σt =N∑

n=1

(p[n]t − µt)

2ω[n]t (3.5)

If the value σPtis lower than a certain threshold σth, then the particle set

follows a distribution similar to the Gaussian distribution defined by parametersµt and Σt. Another convergence criteria tested in this algorithm was the use ofthe Kullback Leibler divergence factor. The Kullback Leibler divergence factorcompares two distributions and gives a value of divergence which is near to0 when there is a high similarity between both distribution. This divergencefactor was used to compare the Gaussian distribution N (µt,Σt), with the realdistribution of the current particle set, using equation (3.6). This divergencefactor was finally discarded as a convergence criteria due to the computationaltime required to compute this divergence factor, which is very high when thereis a huge number of particles.

KL =

∫

ω[n]t ln(

ω[n]t

N (p[n]t ;µt,Σt)

)dpt (3.6)

When the PF has converged into a Gaussian distribution, the GaussianN (µt,Σt) can be integrated in the extended Kalman filter. The procedure to


3.4.0 Characterization of Nanotron WSN

3x33

x

1Ø

Øm

x

1EKF

t

mxm

EKF

t

Figure 3.2: This figure shows how the state vector of a EKF can be extendedwith µt and Σt extracted from a particle filter. The initial state vector of theEKF is supposed to havem parameters before incorporating the 3x1 mean vectorof the particle filter.

include this Gaussian distribution into the current state vector and covariancematrix of the centralized EKF is quite simple: the mean value is appended atthe end of the state vector and the covariance matrix obtained from the particlefilter is also included at the end of the EKF covariance filter, making the corre-lations with other elements of the actual EKF equal to zero. Figure 3.2 showsthe procedure with a simple scheme; the schemes assumes that µt is a columnvector of 3 elements X, Y, Z and that the actual size of the EKF state vector ism.

Once a particle filter has been integrated in the centralized EKF, new mea-surements are incorporated in this EKF according to the range observation modeldescribed in Appendix A.3 for Cartesian coordinates.

3.4 Experimental results

To validate the SLAM framework proposed in this chapter, different experimentswhere carried out. The experiments where performed in a simulated environmentand with a real data set. The range sensor used for real experiments, and hencethe one considered in simulations, is composed by a radio emitter and a setof receivers of a Wireless Sensor Network. These radio beacons are able to



measure the distance between each pair of devices by processing the ToF of thetransmitted signal.

The first section characterize the Wireless Sensor Network used during theexperiments and then some simulation and real experiment results are presented.


The Wireless Sensor Network employed for the experiments of this work consiston a set of radio beacons specially designed for Real Time Localization Systems(RTLS). The WSN model employed is nanoPAN 5375 DK, developed by Nan-otron. This model is shown in Figure 3.3 and has the following characteristics:

❼ ATMega 1284P microcontroller at 20MHz.

❼ Radio transceiver 2.4 GHz ISM band. Up to 20dB transmission power.

❼ Ranging accuracy of 2 m indoors / 1 m outdoors.

❼ 128KB flash memory for programs and retrieved data.

❼ Distance measurements computed with SDS-TWR method based on ToFmethod without needing any clock synchronization between nodes.

These radio beacons implement a special protocol derived from the 802.15.4protocol called 802.15.4a. This protocol extends the low consumption character-istics of the previous protocol with a physic layer specially designed for RTLS.Despite the protocol reduces the bandwidth for data transmission, it improvesthe physic layer to reduce interferences with other devices working on the ISMband (2.4GHz) by adding two channels specially designed for the modulationtechnique called Chirp Spread Spectrum (CSS). This modulation technique re-duces some interference with other existing frequencies by working on two newchannels and by employing a robust modulation (CSS modulation) which reducesthe transmission errors with respect the modulation used in 802.15.4.

On the other hand, radio beacons employed for these experiments implementa ToF based technique which improves the accuracy of common RSSI techniquesfor distance computation. Although the main drawback of ToF technique isthe required synchronization between beacons, nanoPAN 5375 DK uses a mea-surement technique known as SDS-TWR (Symmetrical Double-Sided Two WayRanging) which avoids the synchronization between nodes by performing a sym-metrical double-sided two way ranging measurement which works as follows (seeFigure 3.4):

1. Emitter node (node A) sends a range request message to node B and savesthe local time stamp tr1.



Figure 3.3: WSN nodes employed during experiments: nanoPAN 5375 DB. Thefigure shows the essential parts of the nanoPAN 5375 radio beacon architecture.The upper layer represents the hardware of this device, mainly composed by aWireless interface (omnidirectional antenna), the transmitter microchip devel-oped by Nanotron and the microcontroller used to control this transmitter. TheAPI layer represents the drivers provided by the manufacturer to program themicrocontroller to start ranging with other devices, interchanging data messagesand to configure some parameters. The last layer represents the host controller,i.e. the computer in which range measurements are processed (in this case thehost is the CPU of the robot which has a node attached; beacon nodes doesn’thave a host computer).



2. Receiver node (node B) process the request message and sends an acknowl-edgement message to node A adding information about the time tp1 usedto process the received request message.

3. Node A saves the time stamp tack1 in which received the ACK message.

4. Steps 1, 2 and 3 are repeated starting from node B.

5. Node B sends the results tr2, tack2 and tp2 of the second ranging to node A.

Once the process has finished, the node which started the ranging cycle com-putes the distance between both radio beacons as follows:

ra,b =(tack1 − tr1 − tp1) + (tack2 − tr2 − tp2)

2(3.7)

Despite this ranging method is more accurate than other existing rangingmethods, it is necessary to perform a characterization of the range measurementsprovided by the Nanotron’s WSN. Thus, the first experiments performed duringthis work were focused on characterizing the noise of range measurements whenthe emitter and the receiver are both static and placed at different distances.The setup employed for this experiment is shown in Figure 3.5, as this figureshows, experiments were carried out in indoor and outdoor environments, usinga metric tape to measure the real distance between both nodes.

Using these setups, the characterization of the range sensor was at first per-formed with two static nodes separated by 1, 2, 4, 8, . . . , 30 meters. At firt,nodes were configured with the default configuration. The results of these exper-iments are depicted in Figure 3.6, where green dots represent the mean value ofthe measures received and the pink line represents the associated standard devi-ation at each tested distance. The results were quite similar in both scenarios,indoor and outdoor environments. Due to a constant deviation detected in themeasures received, red points with the cyan line represents the same informationbut with an added offset.

Even adding this offset correction, the measures were very noisy with severaloutliers, so a second experiment was performed where nodes were configuredto emit with a higher transmission power and increasing the CSS pulse widthto 4µs. The results are shown in Figure 3.7, where green dots represent thereceived range measurements and red dots represent the mean value at eachdistance tested, cyan and red lines represents the associated standard deviationof real measurements and mean values respectively. The results show a deviationof about 25cm for indoor and outdoor environments which is quite better thanthe precision specified by the manufacturer.



Node A Node B

ACK

RangeRequest

TW

R-

2nd

rangin

g

ACK

RangeRequestTi

me

proc

essi

ng t

p1

tr2

tack2

tr1

tack1

TW

R-

1st ra

ng

ing

Tim

e pr

oces

sing

tp2

ToF 2

Figure 3.4: Symmetric Double-Sided, Two Way Ranging (SDS-TWR): methodto measure the distance between a pair of radio-based devices employing ToFmeasurements. The method performs the operation in both directions to correcterrors related to the local clock of devices, the method is asynchronous, i.e. itdoes not need any synchronization between emitter and receiver to compute theToF.


3.4.1 Simulations

(a) Outdoor setup

Receiver

Emitter

Metric tape

(b) Indoor setup (c) Host connected tobase node (emitter)

Figure 3.5: Setup of nanoPAN 5375 characterization for outdoor and indoorenvironments. (a) shows the setup for outdoor experiments and, (b) and (c)shows the same setup for indoor experiments.

Finally, a last characterization was performed employing the same configu-ration as in the second experiment. In this case, the experiment was performedin order to test the effect of other phenomenon which appears when one of theradio beacons moves, like doppler shifting and others. Most of these effects arevirtually suppressed by using the CSS modulation of 802.15.4a protocol but, asthe results shown in Figure 3.8, the motion of a beacon still affects the signalpropagation, and hence the ranging precision. The characterization experimentswith motion where carried out with a Pioneer-3AT with a beacon on-board thismobile robot.

In this experiment the groundtruth of the robot was computed with a preciselocalization algorithm using laser data and a map of the environment in whichthe robot was moving through. In this case, the results in Figure 3.8 show anon constant behaviour of the standard deviation (cyan line) with respect themean value (green line) of samples registered (blue dots) from the moving robotto 5 different static nodes. The standard deviation follows a linear function (3.8)which increase with the distance between radio beacons.

σ(ri) = 1.025ri + 0.025 (3.8)

This characterization was only performed for indoor environments, a newdataset for outdoor environments is going to be gathered in the near future tocharacterize the WSN of Nanotron with a quadrotor where the the velocity ishigher than the one used with the Pioneer-3AT. This increase in velocity can be


3.4.1 Simulations

Real distance (m)

Measure

d d

ista

nce

(m

)Avg. received distance

Std received distance

Std received distance + offset

Avg. received distance + offset

Figure 3.6: First characterization of nanoPAN 5375 with static beacons. X axisrepresent the real distance between beacons and Y axis represent the measureddistance. Green dots represent the mean value of the range measures receivedat each distance tested, and the pink line represents the associated standarddeviation at each distance. Red points represent the mean value of the rangemeasures with an added offset, while the cyan line represents the associatedstandard deviation at each tested distance.

Real distance (m)

Measure

d d

ista

nce

(m

)

Measures received

Std. averaged mesures

Std. measures received

Avg. measures received

Figure 3.7: Second characterization of nanoPAN 5375 with static beacons. X axisrepresent the real distance between beacons and Y axis represent the measureddistance. Green dots represent the range measures received at each distancetested, and the cyan line represents the associated standard deviation of all mea-surements received. Red points represent the mean value of the range measures,while the red line represents the associated standard deviation of all mean values.


3.4.2 Simulations

0 2 4 6 8 10 12 145

0

5

10

15

20

25

30

Distancia real (m)

Dis

tancia

medid

a (

m)

Samples

Real model

Error. Avg: 0.057555

Error. Std = 1.0203x + 0.055667

Figure 3.8: Characterization of nanoPAN 5375 with one beacon moving. X axisrepresent the real distance between beacons and Y axis represent the measureddistance. Blue dots represent the range measures received while robot was mov-ing. The green line represents the regression function of the mean values of rangemeasurements received, while the cyan line represents the associated standarddeviation of all mean values.

compensated with the benefit of working on outdoor environments, where themultipath effect and other interference effects are lower than in indoor environ-ments, specially when employing aerial robots.

3.4.2 Simulations

To validate the algorithms developed in this work, different simulations areshown. This simulations have been performed using Matlab as the main pro-gramming language and framework. For this purpose, a simulator has beendeveloped, which simulates not only the motion model of the vehicle but alsothe observation model of range sensors based on the static characteristic gottenin the previous section. Other sensors are also simulated, like a GPS and IMU.

A Matlab toolbox has been used for the visualization of the simulated en-vironment, while the other simple elements of the algorithms are plotted withthe common plot3 function of Matlab. These visualization tools are used in thisdocument to show the results of the different algorithms. A pair of examplesof the visualization tool used to show the environment simulated can be seen inFigure 3.9a and Figure 3.9b.

In this section several simulation experiments are presented, where an aerialrobot (Quadrotor) is simulated together with a set of radio beacons with the


3.4.2 Simulations

(a) Simulated beacon in an outdoor environment

(b) Simulated quadrotor in an outdoor environment

Figure 3.9: Examples of the simulator implemented.

same characteristics obtained above. The mapping algorithm (i.e. the casewhere the position of the robot is well known) is not tested in this work sincethe mapping method implemented in this SLAM solution is the same that theone implemented in [9] and because this article shows the result of applying thismapping algorithm with an aerial robot.

The first simulation considers the case where there is only one beacon to bemapped which real position is the center of a circular trajectory. The Z coordi-nate of this circular trajectory follows a sinusoidal function and the model of thequadrotor employed during the prediction stage is the one described in AppendixA. The use of a circular trajectory avoids the lack of state observbility caused forexample in linear trajectories. An example of the observability problem effectwill be shown in the next chapter. In this first simulation, 4 anchors are used asthe unique sensors to correct the estimated position of the aerial robot by theEKF while the beacon position is not initialized by the PF used. The results ofthis experiment are shown in Figure 3.10, where the real position of the beaconand anchors are represented with a blue diamond an the real position and trajec-


3.4.2 Simulations

Figure 3.10: This figure shows the final results of a delayed RO-SLAM employinga particle filter to get a initial estimation of the beacon position for the EKF. Inthis experiments 4 anchors are used as the unique sensor to correct the estimatedposition of the aerial robot while the beacon has not been initialized yet.

tory of the robot is depicted with a red cross (the red line is the trajectory untilthe current timestamp). The estimated position of the robot is represented witha light blue cross (the light blue line represents the estimated trajectory until thecurrent timestamp). The estimated position of the beacon is represented with apink cross, this estimation corresponds to the estimation made by the EKF oncethe beacon has been initialized by the PF.

For the PF, the standard deviation σlvr used during the resampling algorithmof all simulations is 0.1 meters. This value was selected after trying differentvalues in different simulations, being σlvr = 0.1 the best value which makes theresampled particles not diverge so much from the most probable prior particlesand at the same time makes the new set of particles to cover properly the mostprobable areas where the real position of the beacon is located. An exampleof convergence of a particle filter is shown in Figure 3.11. In this figure thecyan diamond represents the current position of the robot, while the cyan dotrepresents the real position of the beacon to be initialized. The circles representthe particle set Pt, where blue circles are the most probable particles (particleswith higher weight), the green circles represent those particles with a probabilitybetween the most probables and the less probable particles, this particles arerepresented with red circles. The concentration of particles in different areasis caused by the resampling algorithm, where the sparse distribution around an


3.4.2 Simulations

4020

020

40

60

40

20

0

2020

0

20

40

KL:4.8337 PF STD Noise: 0.13126 Min. KL: 4.5647 Max. KL: 4.5647 Estimation error: 19.975

Robot

Tracked object distance to robot: 19.9759

High weight higher than 0.00063012

(a) Initial uniform distribution

3020

100

1020

30

5040

3020

100

1020

10

0

10

20

30

40

KL:4.0602 PF STD Noise: 0.13126 Min. KL: 4.5647 Max. KL: 4.5647 Estimation error: 22.9535

Robot


Low weight less than 0.00183

Medium weight between 0.00183 and 0.00366


(b) After 25sec

3020

100

1020

5040

3020

100

1010

12

14

16

18

20

KL:Inf PF STD Noise: 0.13126 Min. KL: 4.5647 Max. KL: 4.5647 Estimation error: 7.4819

Robot



(c) After 50sec

3020

100

1020

5040

3020

100

1010

12

14

16

18

20

KL: 3.6869 PF STD Noise: 0.13126 Min. KL: 4.5647 Max. KL: 4.5647 Estimation error: 3.7056

Robot





(d) After 59sec

Figure 3.11: Example of particle filter convergence. The cyan diamond representsthe current position of the robot, while the cyan dot represents the real position ofthe beacon to be initialized. The circles represent the particle set Pt, where bluecircles are the most probable particles (particles with higher weight), the greencircles represent those particles with a probability between the most probablesand the less probable particles, which are represented with red circles.

area is the result of employing the standard deviation σlvr when drawing the newparticles instead of replacing an existing particle from the prior distribution.

The convergence threshold σth used is 3 meters, which is the same thresholdused in [9]. Figure 3.12 represents the initial estimation of a beacon in the EKF,once the PF ha converged. After several simulations the convergence delay forthis trajectory and the velocity of the simulated aerial robot is near to 1 minute(around 30 meters).

As Figure 3.12 shows, beacons are not always properly initialized (in thisexample the initial estimation error is near to 4 meters), this bad initializationmakes difficult the convergence of the beacon estimation in the EKF and mightmake diverge the estimation of the vehicle position.

The second simulation experiment was intended to see the effect of fusingother sensor data into the EKF. The sensors fused with the range sensors are


3.4.3 Real dataset

Figure 3.12: Example of the convergence of a PF into a Gaussian distribution.The picture shows the initial estimation of the EKF once the PF has converged.After several simulations the convergence delay for this trajectory and the ve-locity of the simulated aerial robot is near to 1 minute (around 30 meters).

a GPS and an IMU. The results of the delayed RO-SLAM solution using othersensors are shown in Figure 3.13.

The results show how the inclusion of new sensor smooths the estimation ofthe robot trajectory. On the other hand, the estimation of the beacon position isquite worst compared with previous results, but this is due to a bad initializationof the beacon estimation with the PF (near to 5 meters of initial error).

The last simulation experiment was focused on checking the effect of removingthe anchor nodes of the WSN which are the most accurate sensors for robotpositioning considering the three sensors used in the previous experiment. Theresults of this experiment are shown in Figure 3.14.

The final results show how the estimation of the robot position is quite worst,this is mainly related to the error associated with the GPS sensor (near to 3meters). In this case, the estimation of the beacon position is quite better dueto a better initialization of the EKF after the convergence of the PF.

3.4.3 Real dataset

For real experiments, the Pioneer 3-AT ground robot was used together with theNanotron WSN composed by 4 anchor nodes, 1 beacon node and 1 base node


3.4.3 Real dataset

Figure 3.13: This figure shows the final results of a delayed RO-SLAM employinga particle filter to get a initial estimation of the beacon position for the EKF.In this experiments 4 anchors are used to correct the estimated position of theaerial robot together with a GPS and an IMU sensor.

Figure 3.14: This figure shows the final results of a delayed RO-SLAM employinga particle filter to get a initial estimation of the beacon position for the EKF. Inthis experiments only the GPS and the IMU are used to correct the estimatedposition of the aerial robot.


3.4.3 Real dataset

(a) Pioneer 3-AT

Beacon 2 Beacon 1Beacon 3 Beacon 4

Base

Beacon 5

(b) Setup of radio beacons

(c) Nanotron radio beacon

Figure 3.15: Setup used in CONET testbed for real experiments. Pioneer 3-ATground robot was used together with the Nanotron WSN composed by 4 anchornodes, 1 beacon node and 1 base node (node attached to the robot).

(node attached to the robot). The experiments were performed in the CONETtestbed at the Engineering School (University of Seville). Figure 3.15 shows thesetup used for this experiment.

The motion and observation models used for this experiments are the WSNobservation model and the Pioneer 3-AT model explained in Appendix A. Forthe groundtruth registration of the robot, an MCL (Monte Carlo Localization)algorithm was used employing a LIDAR sensor (Hokuyo UTM-30LX laser sen-sor). The real position of beacons was measured with a metric tape, taking intoaccont the global frame of the CONET testbed. In this experiment, the pa-rameters of the robot position are the 2D position and orientation of the robot[xr, yr, φr], while the parameters of the beacon estimation are represented as 3DCartesian coordinates [xb, yb, zb].

The results of this experiment employing the delayed RO-SLAM algorithmare shown in Figure 3.16. In this figure the groundtruth of the robot is repre-sented with a red line and a red cross is used to represent its real position, the


3.5 Summary and conclusions

Figure 3.16: 3D representation of the final results gotten with the delayed 3D RO-SLAM developed in this chapter. The groundtruth of the robot is representedwith a red line and a red cross for the current real position, the result of the deadreckoning algorithm is represented with the green line and the green cross. Thelocalization result of the RO-SLAM algorithm employed is presented with a lightblue line and cross. The real position of the anchors and beacon is representedwith a blue diamond, while the estimated position of the beacon is representedwith a pink cross, the represented estimation is the estimation of the EKF.The number next to the identifier of each blue diamond, represents the distancemeasurement received in that instant from robot to this radio beacon.

result of the dead reckoning algorithm is represented with the green line andthe green cross. The localization result of the RO-SLAM algorithm employedis presented with a light blue line and cross. The real position of the anchorsand beacon is represented with a blue diamond, while the estimated position ofthe beacon is represented with a pink cross, the represented estimation is theestimation of the EKF. The convergence of the PF is represented in Figure 3.18,in the figure it is depicted the final estimation of the PF from which the EKFestimation is initialized with an initial estimation error of 1.82 meters. On theother hand, the final estimation of the EKF filter has an error of 75cm, whereasthe final estimation error of the robot localization is 1.13 meters. The differ-ence between the dead reckoning algorithm and the RO-SLAM developed in thischapter is represented in the 2D representation of the final results in Figure 3.17.



Figure 3.17: 2D representation of the final results gotten with the delayed 3D RO-SLAM developed in this chapter. The groundtruth of the robot is representedwith a red line and a red cross for the current real position, the result of thedead reckoning algorithm is represented with the green line and the green cross.The localization result of the RO-SLAM algorithm employed is presented witha light blue line and cross. The real position of the anchors and beacon isrepresented with a blue diamond, while the estimated position of the beacon isrepresented with a pink cross, the represented estimation is the estimation of theEKF. The number next to the identifier of each blue diamond, represents thedistance measurement received in that instant from robot to this radio beacon.The legend of Figure 3.16 is the same of this figure.

15

10

5

0

5

15

10

5

0

5

10

5

0

5

10

Y(meters)X(meters)

Z(m

ete

rs)

Robot





Figure 3.18: Convergence of the PF used to initialize the position of Beacon 1in the EKF. The most probable particles are depicted with blue circles, the lessprobable ones are represented with red circles and other intermediate particlesare represented with green particles. The cyan point represents the real positionof the radio beacon to be mapped and the real position of the robot is representedwith a cyan circle.




This chapter has presented an implementation for the RO-SLAM problem ap-plied to 3D environments. The solution presented in this chapter is a delayed3D RO-SLAM which uses a particle filter (PF) to initiate each landmark of themap in a centralized EKF where the localization of the robot takes place. Thenthe EKF presented increase dynamically the size of the state vector when eachlandmark is initialized. Different simulations have been shown and a real experi-ment too. The main problem of this solution is that is a delayed solution so, onlythose measures received after the initialization of each beacon are integrated inthe EKF loosing the first ones.

The results of the experiments show that the main problem of the delayedinitialization with a PF is the convergence of the particle filter into a Gaussiandistribution. The results show that some times the initialization is quite farfrom the real solution, making the convergence of the landmarks estimationin the EKF very difficult and some times, this estimation might even diverge.Additionally the main drawback of the PF employed to initiate the landmarksis its computational complexity when the number of particles are very large.

On the other hand, this chapter has also shown how different measures canbe fused in a single filter, making the estimation of the robot position smootherwhen a GPS is employed together with the range sensors (WSN) and the IMU.

In order to estimate properly the position of each landmark, a characteri-zation of the WSN employed in this work as range sensors is presented in thischapter. The characterization of the WSN showed a linear relation between theerror of the range measured by the WSN and the received range measurement.


Date post:	20-Feb-2021
Category:	Documents
Upload:	others
View:	1 times
Download:	0 times

Chapter 3 3D EKF-SLAMbibing.us.es/proyectos/abreproy/70437/fichero/5_Capitulo...of a mobile robot...

Documents