mapeado y localización de robots autónomos ekf...

Mapeado y Localización de RobotsAutónomos

EKF SLAM

Domingo Gallardo

Robot Vision Grouphttp://www.rvg.ua.es

Doctorado Depto. CCIA - Robótica y Visión

http://www.rvg.ua.es

Contents

1. Kalman Filter

2. Mobile Robot Mapping

3. SLAM with Kalman Filter

Doctorado Depto. CCIA - Robots Autónomos 2/49

A very simple example

• How to combine recursively diferent measurements ofthe same object?

• Each measurement is done with a different device, andhave different variance

• Given: {(z1, σ2z1

), (z2, σ2z2

)}

• What is the best estimate (x, σ2) of the object


Recursive Solution

• Iniatilization:x1 = z1 σ2

1 = σ2z1

• New data arrives:

x2 = x1 + K2(z2 − x1) K2 =σ2

1

σ21 + σ2

z2

1σ2

2

=1σ2

1

+1

σ2z2


Graphical Example


Kalman Filter

• The system state is modeled by a multivariate Normaldistribution

• The change of state is a lineal model and a noise

• A lineal model predicts the measurements from thestate


What the Kalman Filter computes?

The Kalman Filter maintains the first two moments of thestate distribution.

The state in time k is estimated by the expected state ofthe distribution:

xk = E[xk]

The uncertainty in time k is modeled by the matrixcovariance of the distribution:

P k = E[(xk − xk)(xk − xk)T ]


Kalman Filter

Two phases:


Time Update (’Predict’)

Given:

A control input uk ∈ Rl at step k

A state estimate xk−1 ∈ Rn before uk

The state estimate matrix covariance P k−1

The time update phase of the Kalman Filter computes:

The state estimate x−k after uk

Its matrix covariance P−k


Process Model

The linear stochastic difference equation that govern theprocess:

xk = Axx−1 + Buk + wk

A is a n× n matrix

B is a n× l matrix

wk is the process noise with density p(w) ∼ N(0,Q)

Q is the process noise covariance


Time update equations

After executing the control uk the new state estimate andits matrix covariance are computed:

x−k = Axk−1 + Buk

P−k = AP k−1AT + Q


Measurement Update (’Correct’)

Given:

A state estimate x−k ∈ Rn at step k

Its matrix covariance P−k

A state measurement zk ∈ Rm at step k

The measurement update phase of the Kalman Filtercomputes:

The state estimate xk after the measurement zk

Its matrix covariance P k


Measurement Model

The measurements can be modeled with the equation

zk = Hxk + vk

zk ∈ Rm is the measurement at step k

H is a m× n matrix

vk is the measurement noise with densityp(v) ∼ N(0,R)

R is the measurement noise covariance


Measurement Update Equations (1)

xk = x−k + K(zk −Hx−k )

• Computes an a postori state estimate xk as a linearcombination of an a priori estimate and a weightedresidual between an actual measurement and ameasurement prediction (a magnitude called theinnovation or the residual.

• The n×m matrix K is computed to be the gain orblending factor that minimizes the a posteriori errorcovariance


Kalman Gain

• The Kalman Gain is the core of the Kalman filter. Oneequation for it is

Kk = P−k HT (HP−k HT + R)−1

• Interpretation:

limRk→0

Kk = H−1

limP−

k→0

Kk = 0

HP−k HT + R is the innoviation covariance matrix


Measurement Update Equations (2)

P k = P−k −KkHP−k

The state estimate covariance always decreases after ameasurement.


Kalman Filter Algorithm

[G. Welch and G. Bishop, 2004]


Estimating a Random Constant

Suppose we want to estimate a constant voltage (with avery small noise variance Q) and the measurements arethe same voltage with a noise variance R.

xk = xk−1 + w p(w) ∼ N(0, Q)

zk = xk + v p(v) ∼ N(0, R)

We initialize the filter with the values:

• x0 = 0

• P0 = 1


Time Update Equation

xk = xk−1

P−k = Pk−1 + Q


Measurement Update Equation

Kk =P−k

P−k + R

xk = x−k + Kk(zk − x−k )

Pk = (1−Kk)P−k


First Simulation

• The value to estimate is z = −0.37727

• The standard deviation of the measurements is 0.1.The variance R = 0.12 = 0.01


Second Simulation

• R = 1. The filter is slower to respond to themeasurements.


Third Simulation

• R = 0.0001. The filter responds to the measurementsquickly.


Kalman Filter Limitations

• The process and measurement model have to be lineal

• If not, linearize your model with Taylor expansion andyou’ll get an EKF

• The underlying distribution has to be Normal andunimodal

• If not, you’ll have to do data association and maintain anumber of simultaneous Kalman Filters


Key Issue: Representations

• A map is a global consistent environmentrepresentation

• It is used for :

– Robot (and target) localization– Motion Planning– Motion Control

• Unknown environment: incremental mapping

• SLAM: Simultaneous Localizaton and Mapping is thekeyword used in the Kalman Filter community


Mapping in AI Community


1 dof location

• Simplest SLAM problem

• Useful to understand the basis of SLAM

• MATLAB coding


Process Model (1)

xt = xt−1 + utv + wt

ut ={

1 if (xt−1 < xmax ∧ ut−1 = 1) ∨ (xt−1 < 0)−1 if (xt−1 > 0 ∧ ut−1 = −1) ∨ (xt−1 > xmax)

with u0 = 1, x0 = 0, wt ∼ N(0, q)


Process Model (2)

Integrating the landmark position in the state:

Xt =[

xt

mi

]

Q =[

q 00 0

]


Time update equations

X ← X +[

u

0

]+

[wt

0

]

P ← P + Q


Running example


Measurement Model

A landmark i is situated in the position mi of the X-axis.The distance from the robot to the landmark i at time t ismeasured as

zit = mi − xt + vt, vt ∼ N(0, r)

We consider a range maxrange for the sensor situatedon the robot. If the distance to the landmark is greaterthan that range, it is not measured.


Measurement Model

Xt =[

xt

mi

]

zt = HXt + v =

= [−1 1][

xt

mi

]+ v


MATLAB implementation

% Time updatex = x + v+randn(1)*q;Xest = Xest+[v 0]’;P = P + Q;...% Measurement update of z0if (z0 < maxrange)

innov = z0-H0*Xest;K = P*H0’*inv(H0*P*H0’+r);Xest = Xest+K*innov;P = P - K*H0*P;

end


SLAM Formulation


Solving SLAM with KF

• Assuming a global center of coordinates

• Robot pose: r = (x, y, θ)

• Landmark locations (map): {l1, . . . , lN} li = (x, y) inglobal coordinates

• Process State: {l1, . . . , lN , r}

• Measurements: {z1, . . . ,zK} zi = (x, y, θ) in robotcoordinates

• Control actions: motions ui that change the robotglobal position


System model


Covariance matrix

[Thrun, 2002]

(a) (b) (c)

Figure 3: (a) A map of landmarks obtained in simulation, (b) a correlation matrix after 278 iterations of Kalman filter mapping.

The checker-board appearance verifies the theoretical finding that in the limit, all landmark location estimates are fully correlated.

(c) The normalized inverse covariance matrix of the same estimate shows that dependencies are local, an effect that is exploited by

some algorithms that build local maps.

in Equation (7) can be calculated conveniently using the standard Kalman filter equations [49, 64, 103]:

µ!t"1 = µt"1 + But

!!t"1 = !t"1 + !control

Kt = !!t"1C

T (C!!t"1C

T + !measure)"1

µt = µ!t"1 + Kt(o! Cµ!

t"1)!t = (I !KtC)!!

t"1 (12)

As the reader may verify, these equations are equivalent to the standard Kalman filter update equations [64].

Their mathematical derivation from the Bayes filter is mathematically straightforward.

In practical implementations, the number of features in the map lies usually between a dozen and a few

hundreds. Sensor measurements are usually sparse (e.g., the robot observes one landmark at a time). The

most costly operations in updating the Kalman filter are matrix multiplications (and not matrix inversions),

which can be implemented in O(K2) time, where K is the number of features in the map—not to be

confused with the matrixKt in (12). Recent research has led to a range of extensions that can handle larger

number of features, by decomposing the problem into multiple smaller ones [56, 38]. Some techniques,

such as the FastSLAM algorithm described in [67], promise a reduction to O(log K) complexity for certainsituation, by using non-classical statistical sampling techniques for robot path estimation [29, 71] along with

efficient tree representations.

In practice, the number of features is not known a priori. State-of-the-art implementations often grow

this list dynamically. To do so, they maintain a list of candidate features, using a separate Kalman filter for

these candidates. If a feature is observed sufficiently often, it is permanently added to the list of features

in the map. Outliers, that is, measurements that do not correspond to any known feature with sufficient

likelihood, are usually ignored. These techniques work particularly well when features are scarce in the

environment.

Some of the key characteristics of the Kalman filter approach to robot mapping are summarized in

Table 1. The primary advantage of the Kalman filter approach is the fact that it estimates the full posterior

over maps m in an online fashion. To date, the only algorithms that are capable of estimating the full

posterior are based on Kalman filters or extensions thereof, such as mixture of Gaussian methods [30], or a

Rao-Blackwellized particle filter [29, 67, 71]. There are many advantages to estimating the full posterior:

In addition to the most likely map and robot poses, Kalman filters maintain the full uncertainty in the map,

which can be highly beneficial when using the map for navigation. Additionally, the approach can be shown

to converge with probability one to the true map and robot position, up to a residual uncertainty distribution

that largely stems from an initial random drift [73, 26, 27]. As is commonly the case with theoretical results,

10

(a) (b) (c)

Figure 3: (a) A map of landmarks obtained in simulation, (b) a correlation matrix after 278 iterations of Kalman filter mapping.

The checker-board appearance verifies the theoretical finding that in the limit, all landmark location estimates are fully correlated.

(c) The normalized inverse covariance matrix of the same estimate shows that dependencies are local, an effect that is exploited by

some algorithms that build local maps.

in Equation (7) can be calculated conveniently using the standard Kalman filter equations [49, 64, 103]:

µ!t"1 = µt"1 + But

!!t"1 = !t"1 + !control

Kt = !!t"1C

T (C!!t"1C

T + !measure)"1

µt = µ!t"1 + Kt(o! Cµ!

t"1)!t = (I !KtC)!!

t"1 (12)

As the reader may verify, these equations are equivalent to the standard Kalman filter update equations [64].

Their mathematical derivation from the Bayes filter is mathematically straightforward.

In practical implementations, the number of features in the map lies usually between a dozen and a few

hundreds. Sensor measurements are usually sparse (e.g., the robot observes one landmark at a time). The

most costly operations in updating the Kalman filter are matrix multiplications (and not matrix inversions),

which can be implemented in O(K2) time, where K is the number of features in the map—not to be

confused with the matrixKt in (12). Recent research has led to a range of extensions that can handle larger

number of features, by decomposing the problem into multiple smaller ones [56, 38]. Some techniques,

such as the FastSLAM algorithm described in [67], promise a reduction to O(log K) complexity for certainsituation, by using non-classical statistical sampling techniques for robot path estimation [29, 71] along with

efficient tree representations.

In practice, the number of features is not known a priori. State-of-the-art implementations often grow

this list dynamically. To do so, they maintain a list of candidate features, using a separate Kalman filter for

these candidates. If a feature is observed sufficiently often, it is permanently added to the list of features

in the map. Outliers, that is, measurements that do not correspond to any known feature with sufficient

likelihood, are usually ignored. These techniques work particularly well when features are scarce in the

environment.

Some of the key characteristics of the Kalman filter approach to robot mapping are summarized in

Table 1. The primary advantage of the Kalman filter approach is the fact that it estimates the full posterior

over maps m in an online fashion. To date, the only algorithms that are capable of estimating the full

posterior are based on Kalman filters or extensions thereof, such as mixture of Gaussian methods [30], or a

Rao-Blackwellized particle filter [29, 67, 71]. There are many advantages to estimating the full posterior:

In addition to the most likely map and robot poses, Kalman filters maintain the full uncertainty in the map,

which can be highly beneficial when using the map for navigation. Additionally, the approach can be shown

to converge with probability one to the true map and robot position, up to a residual uncertainty distribution

that largely stems from an initial random drift [73, 26, 27]. As is commonly the case with theoretical results,

10


Before closing the loopCerrando el ciclo (antes)


After closing the loopCerrando el ciclo (después)


Outdoor Mapping

[Juan Nieto, Jose Guivant, Eduardo Nebot]


EKF Solution

• Adventages

– Full Bayesian solution– Maintains all dependencies

• Disadventage

– Does not address data association problem– Linearized– Slow: O(N2)


References

Greg Welch and Greg Bishop: An Introduction to the Kalman Filter,TR 95-041, Department of Computer Science, University of NorthCarolina at Chapel Hill, 2006

[Smith, Self, Cheeseman]Randall Smith, Matthew Self, Peter Cheeseman, EstimatingUncertain Spatial Relationships in Robotics, 1990

[Dissanayake, 2001]M. Dissanayake, Paul Newman, Steven Clark, Hugh F. Durrant-Whyteand M. Csorba A Solution to the Simultaneous Localization and MapBuilding (SLAM) Problem, IEEE Transactions on Robotics andAutomation, vol. 17, no. 3, June 2001.

[Thrun, 2000]Sebastian Thrun, Robot Mapping: A Survey, 2002



Grid Map. Particle Filter. Monte CarloLocalization

Miguel Cazorla




Contents

1. Grid maps

Map buildingLocalization

2. Particle Filter

Implementation: localizing a robotImplementation: Four-legged leagueImplementation: tracking a person in 3D

3. Practical: Player/Stage

Doctorado Depto. CCIA - Robótica y Visión 2/29

Occupancy Grid Mapping1. In some cases we need a map of the environment where

a robot is moving on.

2. Furthermore, we could need a robot-point of view map:reduce efforts installing a robot and avoid human super-vision in changes.

3. Learning a map is not a trivial task: the hypothesis spa-ce is huge (maps are defined in a continuos space), itdepends on the size (the larger the size the more diffi-cult it is to acquire), perceptual ambiguity, there existsnoise in sensors and actuators and presence of cycles(SLAM).


Occupancy Grid Mapping: OGM

1. Addresses the problem of generating consistent mapsfrom noisy and uncertain measurement data.

2. We assume that the robot pose is known.

3. The key idea is to represent the map as a field of randomvariables, arranged in an evenly spaced grid.

4. Each random variable is binary and corresponds to theoccupancy of the location its covers.

5. OGM: implements approximate posterior estimation forthose random variable.


OGM: the algorithm

1. The goal is to calculate: p(m|z1:t, x1:t)


OGM: the algorithm

1. The map is partitioned in a set of grid cells: m = {mi}

2. mi indicates if the cell is occupied or not. p(mi = 1) orp(mi) refers to the probability that a grid cell is occupied.

3. For a map of M cells, the number of maps that can berepresented by this map is 2M : calculating the posteriorprobability for each single map is intractable.

4. Instead of calculating p(m|z1:t, x1:t), the problem is se-parated into a collection of separated problems, namelyestimating: p(mi|z1:t, x1:t) for each cell mi.


OGM: the algorithm

1. Now, the posterior over maps is approximated as theproducts of its marginals:

p(m|z1:t, x1:t) =∏

i

p(mi|z1:t, x1:t)

2. The algorithm loops through all grid cells i at each time,updating those cells inside the sensor range.

3. Of course, we need a (range) sensor. We assume a sen-sor like sonar or laser.


OGM: at last, the algorithmAlgorithm OGM ( p(mi|zt−1, xt−1)}, xt, zt)

for all cells mi doif mi in perceptual field of zt then

p(mi|zt, xt) = P (zt|mi,xt)P (mi){P (zt|mi,xt)P (mi)+(1−P (zt|mi,xt))(1−P (mi))}

elselt,i = lt−1,i

endifendfor


OGM: sensor model


OGM: sensor model

1. Let R be the distance to the sonar reading, α the anglefrom a cell to the center of the beam, and d the distanceof the cell to the sonar.

2. The sensor model provides a value in the range [0 : 1].

3. Region 1: |d − R| < ε and |α| < θ then P (zt|mi, xt) =0,5 + (0,5 ∗ (1 −

(αθ

)2) ∗ (1 −(

d−Rε

)2))

4. Region 2: d < R − ε and |α| < θ then P (zt|mi, xt) =

0,5 ∗ (1 − (1 − ((

αθ

)2) ∗ (1 −(

dR−ε

)2

))


OGM: examples


Grid Localization

1. The key idea is using a grid to approximate the posteriorusing a decomposition of the pose space.

2. The space is decomposed into cells of the same sizeand a third dimension to include angles.

3. The granularity used is critical: finer representations arecomputational expensive and less discretization provi-des less accuracy.

4. Mantains as posterior a collection of discrete probabilityvalues: bel(xt) = {pk,t}


Grid Localization: algorithmAlgorithm Grid localizacion ({pk,t−1}, ut, zt)

for all k dopk,t =

∑i pi,t−1motion model(ut,mean(xk),mean(xi))

pk,t = ηmeasurement model(zt,mean(xk))endfor


Grid Localization: example


Particle Filter

1. Particle filter is a nonparametric implementation of theBayes filter.

2. The key idea is to represent the posterior bel(xt) by aset of random state samples drawn from this posterior.

3. This representation is approximate, but it is nonparame-tric: can represent a much broader space of distribution(non Gaussians!).

4. The samples of a posterior distribution are called parti-cles: Xt = x

[1]t , x

[2]t , . . . , x

[M ]t


Particle Filter

1. A particle is a hypothesis as to what the true world statemay be at time t.

2. M is the number of particles (usually 1000), but can bea function of time.

3. The particle filter algorithm constructs bel(xt) recursivelyfrom bel(xt−1), i.e., the particle set at time t is construc-ted from the one at t − 1.


Particle filter: algorithmAlgorithm Particle filter (Xt−1, ut, zt)

Xt = Xt = ∅for m = 1 to M do

sample x[m]t ∼ p(xt|ut, x

[m]t−1)

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

t )Xt = Xt + 〈x[m]

t , w[m]t 〉

endforfor m = 1 to M do (importance sampling)

draw i with probability ∝ w[m]t

add x[i]t to Xt.

endfor


Monte Carlo Localization (MCL)

1. It is just the particle filter, substituting the probabilisticmotion and perceptual models (example).

2. It can solve the kidnapping problem.

3. It is easy to implement and works fine in a broad rangeof localization problems: widely used.


MCL: example


MCL: final considerations

1. Number of particles.

2. Kidnapping problem: generate a portion of samples fromperception.

3. Number of particles depending of time.

4. Videos.

5. MCL == Condensation. Example in 3D.


Pratical: Player/Stage

1. Let’s move to a practical sesion.

2. We’ll see Player/Stage software.

3. A practical implementation of MCL.


References

Probabilistic Robotics. S. Thrun, W. Burgard, D. Fox,MIT Press. 2005.

P.Barrera, J.M.Cañas, V.Matellán. Visual object trackingin 3D with color based particle filter. Int. Journal of Infor-mation Technology, Vol 2, Num 1, pp 61-65, 2005.



FastSlam algorithm

Miguel Cazorla




Contents

1. FastSlam algorithm.


FastSlam

1. Particle filter solution to the SLAM problem.

2. SLAM problem with known correspondences possessesa conditional independence between landmarks, giventhe robot pose.

3. This make possible to apply Rao-Blacwellized particlefilter: uses particle filters to represent posterior over so-me variables and Gaussians to represent other varia-bles.


FastSlam

1. Particle filters estimate the robot path.

2. For these particles, the individual map errors are condi-tionally independient.

3. The mapping problem can be factored into many sepa-rated problems, one for each feature.

4. To do that, an EKF for each landmark is used.

5. Data association is made in each particle.


SLAM problem

A solution for the SLAM problem maximizes

p(st,Θ|zt, ut) (1)

We can assume (without losing generality) that land-marks are observed once each time:

p(st,Θ|zt, ut, nt) (2)


Decomposition

The problem can be decomposed into K + 1 estimationproblems:

• Posteriori over the robot path: it is implemented with aparticle filter.

p(st|zt, ut, nt) (5)

• K problems estimating the location of the landmarks:they are estimated with Kalman filters.

p(Θnt|st, zt, ut, nt) (6)


FastSLAM with known dataassociation

FastSLAM uses a particle filter to estimate the posterior.

Each particle m in time t is denoted by st,[m] and it re-presents a guess of the particle path st.

At time t we have a population of particles St ={st,[m]

}M

m=1


FastSLAM with known dataassociation

A particle consists of the estimated position of the robotand K EKF estimating the landmarks positions:


How the whole thing works: robotposition

Remember we are in the known data association case.We have K landmarks.

We suppose that the initial set of particles can be gene-rated arbitrarily.

In each initial particle, the robot position s0 is (0, 0, 0).

The robot moves ut and we use the motion model togenerate (for each particle):

st ∼ p(st|ut, st−1) = h(ut, st−1) + δt (7)


How the whole thing works:landmarks position

One EKF (non-linear) for each landmark.

Landmark positions are represented by the mean µ[m]n,t−1

and the covariance Σ[m]n,t−1.

We assume that only one landmark is seen in each ob-servation. At time t, every landmark not seen is not up-dated, so:

〈µ[m]n,t ,Σ

[m]n,t 〉 = 〈µ[m]

n,t−1,Σ[m]n,t−1〉 (8)


How the whole thing works:landmarks position

The update is:

p(Θnt|st, zt, nt) = ηp(zt|st,Θnt, nt)p(Θnt|st−1, zt−1, nt−1)(9)

where the second term is represented by a Gaussian(the previous mean and variance) and the first term islinearized as EKF does.


How the whole thing works:landmarks position (cont.)

We have to linearized the measurement function g by afirst degree Taylor expansion:

g(Θnt, s[m]t ) ≈ z

[m]t + G

[m]t (Θnt − µ

[m]nt,t−1) (10)

where

z[m]t = g(µ[m]

nt,t−1, s[m]t ) (11)

G[m]t = g′(µ[m]

nt,t−1, s[m]t ) (12)


How the whole thing works:landmarks position (cont. II)

The new mean and covariance are obtained using thestandard EKF measurement update:

K[m]t = Σ[m]

nt,t−1G[m]t (G[m]T

t Σ[m]nt,t−1G

[m]t + Rt)−1 (13)

µ[m]nt,t

= µ[m]nt,t−1 + K

[m]t (zt − z

[m]t )T (14)

Σ[m]nt,t

= (I −K[m]t G

[m]Tt )Σ[m]

nt,t−1 (15)


How the whole thing works: weightfor each particle

The weight indicates the goodness of this particle. It isnecessary due to the new robot position is calculatedbased only on ut, paying no attention to zt.

This weight, w[m]t :

w[m]t ≈ η det(2πQ

[m]t )−1/2 exp

n−1

2(zt−z[m]t )T Q

[m]−1t (zt−z

[m]t )

o

with the covariance

Q[m]t = G

[m]Tt Σ[m]

n,t−1G[m]t + Rt


FastSLAM 2.0

In the previous algorithm, the path posterior (the robotposition) is calculated based only on the movement ut.This is problematic when the accuracy of control is lowrelative to the accuracy of the robot’s sensors.

This version of FastSLAM takes into account the obser-vations zt.


Reformulation: robot poseNow, the pose is drawn from (including observations):

s[m]t ∼ p(st|st−1,[m], ut, zt, nt) (16)

Rewritten 16 (with a lot of calculus)

p(st|st−1,[m], ut, zt, nt) =

η[m]∫

p(zt|Θnt, st, nt)p(Θnt|st−1,[m], zt−1, nt−1)dΘnt

p(st|s[m]t−1, ut) (17)

Convolution of two Gaussians multiplied by a third


Reformulation: robot poseHow g is not linear so we need to replace g by a linearapproximation.

g(Θnt, st) ≈ z[m]t +Gθ(Θnt−µ

[m]nt,t−1)+Gs(st− s

[m]t ) (18)

where

z[m]t = g(µ[m]

nt,t−1, s[m]t ) (19)

s[m]t = h(s[m]

t−1, ut) (20)

GΘ = ∆Θntg(Θnt, st) |st=s

[m]t ;Θnt=µ

[m]nt,t−1

(21)

Gs = ∆sg(Θnt, st) |st=s[m]t ;Θnt=µ

[m]nt,t−1

(22)


Reformulation: robot poseNow the robot pose is sampled by a particle filter withmean and variance:

Σ[m]st

=[GT

s Q[m]−1t Gs + P−1

t

]−1

(23)

µ[m]st

= Σ[m]st

[GT

s Q[m]−1t (zt − z

[m]t + Gss

[m]t )

+P−1t s

[m]t

]= Σ[m]

stGT

s Q[m]−1t (zt − z

[m]t )

+Σ[m]st

[GT

s Q[m]−1t Gs + P−1

t

]s[m]t

= Σ[m]st

GTs Q

[m]−1t (zt − z

[m]t ) + s

[m]t (24)


Reformulation: landmark EKF

K[m]t = Σ[m]

nt,t−1GTΘQ

[m]−1t (25)

µ[m]nt,t

= µ[m]nt,t−1 + K

[m]t (zt − z

[m]t ) (26)

Σ[m]nt,t

= (I −K[m]t GΘ)Σ[m]

nt,t−1 (27)


Reformulation: weight for eachparticle

w[m]t ≈ det(2πL

[t]t )−1/2 exp

n−1

2(zt−z[m]t )T L

[t]−1t (zt−z

[m]t )

o(28)

L[t]t = GsPtG

Ts + GΘΣ[m]

nt,t−1GTΘ + Rt (29)


FastSLAM with Unknown dataassociation

Let’s move to the next challenge: we do not have the nt

We have a new landmark observation zt but we do notknow to which θk owns this observation.


FastSLAM with Unknown dataassociation

We can first choose nt so that it maximizes the likelihoodof the sensor measurement zt:

nt = arg maxnt

p(zt|nt, nt−1, st, zt−1, ut) (30)

p(zt|nt, nt−1, st, zt−1, ut) =

det(2πQ[m]t )−1/2

expn−1

2(zt−g(µ[m]nt,t−1,s

[m]t )T Q

[m]−1t (zt−g(µ

[m]nt,t−1,s

[m]t )

o(31)


Unknown Data association

A single data association can be easily obtained (nea-rest neighbor method, using Mahalanobis distance).

This is the approach followed by EKF-style methods andcan be followed here.

Simply calculate which is the most likely landmark andif the probability in 30 is below a fixed threshold, say p0,consider the observation as a new landmark


Data association IIThis can be done using an additional variable (i[m]

n,t ). Itcounts the number of times a landmark has been seen.

First, it calculates the wn for each landmark, given thecurrent observation.

If all the wn are below p0 then the current observation isa new landmark: It generates a new EKF and initializesthe counter to 1. The weight for this particle is p0.

If not, we associate the current observation to the mostlikely landmark, updating its EKF and incrementing thecounter.


Data association II

For the rest of landmarks, we check for the landmarkswe would see in our current position. If a landmark isin our observation space and it has not been observed,decrement the counter. If it has, increment the counter.

When a landmark has its counter below 0, discard thislandmark.


Results


Results II


Implementation detailsKDtree


ConclusionsTested in small real-world and huge simulated environ-ments.

It needs a small number of particles (M = 100) even fora large number of landmarks (K >> M ) (in simulationK = 50000 and M = 250).

It’s easy to implement (but it requires a lot of assump-tions).

It’s able to manage a lot of landmarks (where other ap-proaches don’t)


References

Probabilistic Robotics. S. Thrun, W. Burgard, D. Fox,MIT Press. 2005.

S. Thrun, M. Montemerlo, D. Koller, B. Wegbreit, J. Nietoand E. Nebot. FastSLAM: An efficient solution to the si-multaneous localization and mapping problem with unk-nown data association. 2004. Journal of Machine Lear-ning Research.


mapeado y localización de robots autónomos ekf...

Documents