Probabilistic Robotics: Review/SLAM

City College of New York

1

Dr. Jizhong XiaoDepartment of Electrical Engineering

CUNY City [email protected]

Probabilistic Robotics:

Review/SLAM

Advanced Mobile Robotics


• Prediction (Action)

• Correction (Measurement)

Bayes Filter Revisit

111 )(),|()( tttttt dxxbelxuxpxbel

)()|()( tttt xbelxzpxbel

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel


Probabilistic Robotics

Probabilistic Sensor Models

Beam-based ModelLikelihood Fields ModelFeature-based Model


4

Beam-based Proximity ModelMeasurement noise

zexp zmax0

bzz

hit eb

mxzP2

exp )(21

21),|(

otherwisezz

mxzPz

0e

),|( expunexp

Unexpected obstacles

zexp zmax0

Gaussian Distribution

Exponential Distribution


5

Beam-based Proximity ModelRandom measurement Max range

max

1),|(z

mxzPrand

otherwisezzif

mxzP01

),|( maxmax

zexp zmax0zexp zmax0

Uniform distribution

Point-mass distribution


6

Resulting Mixture Density

),|(),|(),|(

),|(

),|(

rand

max

unexp

hit

rand

max

unexp

hit

mxzPmxzPmxzP

mxzP

mxzP

T

How can we determine the model parameters?

Weighted average, and 1maxexp randunhit

System identification method: maximum likelihood estimator (ML estimator)

},,,,,{ maxexp randunhit


7

Likelihood Fields Model• Project the end points of a sensor scan Zt into the global

coordinate space of the map

• Probability is a mixture of …– a Gaussian distribution with mean at distance to closest

obstacle,– a uniform distribution for random measurements, and – a small uniform distribution for max range measurements.

• Again, independence between different components is assumed.

randrandhithittkt pppmxzp maxmax),(

)sin()cos(

cossinsincos

ksens

ksenskt

ksens

ksens

z

z zyx

yx

yx

kt

kt


8

Likelihood Fields Model

)sin()cos(

cossinsincos

ksens

ksenskt

ksens

ksens

z

z zyx

yx

yx

kt

kt

Distance to the nearest obstacles. Max range reading ignored


9

Example

P(z|x,m)

Example environment Likelihood fieldThe darker a location, the less likely it is to perceive an obstacleSensor

probability

O1

O2

O3

Oi : Nearest point to obstaclesZmax


10

Feature-Based Measurement Model• Feature vector is abstracted from the measurement:

• Sensors that measure range, bearing, & a signature (a numerical value, e.g., an average color)

• Conditional independence between features

• Feature-Based map: withi.e., a location coordinate in global coordinates & a signature • Robot pose:

• Measurement model:

Zero-mean Gaussian error variables with standard deviations

},,{}{)(2

2

2

1

1

1

21

t

t

t

t

t

t

ttt

s

r

s

rffzf

),,,(),)(( mxsrpmxzfp tit

i

it

ittt

},,{ 21 mmm

2

2

2

)),(2tan)()(

,,

2,

2,

s

r

j

xjyj

yjxj

it

it

it

sxmymaymxm

s

r

Tt yxx }{

Tjyjxjj smmm },,{ ,,

2r

r



Probabilistic Motion Models


Odometry Model

22 )'()'( yyxxtrans

)','(atan21 xxyyrot

12 ' rotrot

• Robot moves from to . • Odometry information .

,, yx ',',' yx

transrotrotu ,, 21

trans1rot

2rot

,, yx

',',' yx

Relative motion information, “rotation” “translation” “rotation”


Noise Model for Odometry• The measured motion is given by the true

motion corrupted with independent noise.

||||11 211

ˆtransrotrotrot

||||22 221

ˆtransrotrotrot

|||| 2143

ˆrotrottranstranstrans

2

2

221

221)(

x

ex

2

2

2

6||6

6|x|if0)(2

xx

),( 1ttt xuxp

How to calculate :


14

Calculating the Posterior Given xt, xt-1, and u

22 )'()'( yyxxtrans )','(atan21 xxyyrot

12 ' rotrot 22 )'()'(ˆ yyxxtrans )','(atan2ˆ

1 xxyyrot

12ˆ'ˆrotrot

)ˆ|ˆ|,ˆ(prob trans21rot11rot1rot1 p|))ˆ||ˆ(|ˆ,ˆ(prob rot2rot14trans3transtrans2 p

)ˆ|ˆ|,ˆ(prob trans22rot12rot2rot3 p

1. Algorithm motion_model_odometry (xt, xt-1, u)

2. 3. 4. 5. 6. 7. 8. 9. 10. 11. return p1 · p2 · p3

odometry values (u)

values of interest (xt-1, xt)

Ttt xxu 1

Tt yxx )(1

Tt yxx )(

An initial pose Xt-1

A hypothesized final pose Xt

A pair of poses u obtained from odometry

),( baprobImplements an error distribution over a with zero mean and standard deviation b),( 1ttt xuxp


Application• Repeated application of the sensor model for

short movements.• Typical banana-shaped distributions obtained for

2d-projection of 3d posterior.

x’ u

p(xt| u, xt-1)

u

x’

Posterior distributions of the robot’s pose upon executing the motion command illustrated by the solid line. The darker a location, the more likely it is.


16

Velocity-Based Model

v

ucontrol vr Rotation

radius


17

Equation for the Velocity ModelInstantaneous center of curvature (ICC) at (xc , yc)

sinrxx c cosryy c

Initial pose Tt yxx 1

Keeping constant speed, after ∆t time interval, ideal robot will be at Tt yxx

ttrytrx

yx

c

c

)cos()sin(

ttrrtrr

yx

)cos(cos)sin(sin Correcte

d, -90


18

Velocity-based Motion Model

With and are the state vectors at time t-1 and t respectively

t

tvv

tvv

yx

yx

t

tt

t

t

t

tt

t

t

t

ˆ

)ˆcos(ˆˆ

cosˆˆ

)ˆsin(ˆˆ

sinˆˆ

'

'

'

Tt yxx 1 Tt yxx '''

The true motion is described by a translation velocity and a rotational velocity

tv tMotion Control with additive Gaussian noise

),0(

ˆˆ

243

221 )(

tt

t

v

v

t

t

t

t Mvvv

tt

tt

Tttt vu )(

243

221

)(00)(

tt

ttt v

vM

Circular motion assumption leads to degeneracy ,2 noise variables v and w 3D poseAssume robot rotates when arrives at its final pose

tt ˆ

65

ˆ v


19

Velocity-based Motion ModelMotion Model:

tt

tvv

tvv

yx

yx

t

tt

t

t

t

tt

t

t

t

ˆˆ

)ˆcos(ˆˆ

cosˆˆ

)ˆsin(ˆˆ

sinˆˆ

'

'

'

243

221 )(

ˆˆ

tt

tt

v

v

t

t

t

t vv

65

ˆ v

1 to 4 are robot-specific error parameters determining the velocity control noise

5 and 6 are robot-specific error parameters determining the standard deviation of the additional rotational noise


20

Probabilistic Motion Model

Center of circle:

with

How to compute ?),( 1ttt xuxp Move with a fixed velocity during ∆t resulting in a circular trajectory from to

Tt yxx 1 Tt yxx

Radius of the circle:

2*2*2*2** )()()()( yyxxyyxxr

Change of heading direction: ),(2tan),(2tan **** xxyyaxxyya

tr

tdistv

*

ˆt ˆˆ

t

(angle of the final rotation)


21

Posterior Probability for Velocity Model

Motion error: verr ,werr and

Center of circleRadius of the circle

Change of heading direction


Examples (velocity based)


Map-Consistent Motion Model

)',|( xuxp

)',|()|(),',|( xuxpmxpmxuxp Approximation:

),',|( mxuxp)',|( xuxp

Map free estimate of motion model

)|( mxp“consistency” of pose in the map

“=0” when placed in an occupied cell

Obstacle grown by robot radius


24

Summary

• We discussed motion models for odometry-based and velocity-based systems

• We discussed ways to calculate the posterior probability p(x| x’, u).

• Typically the calculations are done in fixed time intervals t.

• In practice, the parameters of the models have to be learned.

• We also discussed an extended motion model that takes the map into account.



Localization

“Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]


26

Localization, Where am I??

• Given – Map of the environment.– Sequence of measurements/motions.

• Wanted– Estimate of the robot’s position.

• Problem classes– Position tracking (initial robot pose is known)– Global localization (initial robot pose is unknown)– Kidnapped robot problem (recovery)


27

Markov Localization

),,|( t:1t:1t:1 muzxp


• Prediction (Action)

• Correction (Measurement)

Bayes Filter Revisit

111 )(),|()( tttttt dxxbelxuxpxbel

)()|()( tttt xbelxzpxbel

111 )(),|()|()( tttttttt dxxBelxuxPxzPxBel


29

• Prediction:

• Correction:

EKF Linearization

)(),(),(

)(),(),(),(

1111

111

111

ttttttt

ttt

tttttt

xGugxug

xx

ugugxug

)()()(

)()()()(

ttttt

ttt

ttt

xHhxh

xx

hhxh

First Order Taylor Expansion


30

EKF Algorithm 1. Extended_Kalman_filter( t-1, t-1, ut, zt):

2. Prediction:3. 4.

5. Correction:6. 7. 8.

9. Return t, t

),( 1 ttt ug

tTtttt RGG 1

1)( tTttt

Tttt QHHHK

))(( ttttt hzK

tttt HKI )(

1

1),(

t

ttt x

ugG

t

tt x

hH

)(

ttttt uBA 1

tTtttt RAA 1

1)( tTttt

Tttt QCCCK

)( tttttt CzK tttt CKI )(


31

1. EKF_localization ( t-1, t-1, ut, zt, m):

Prediction:

2.

3.

4.

5.

6.

),( 1 ttt ug T

tttTtttt VMVGG 1

,1,1,1

,1,1,1

,1,1,1

1

1

'''

'''

'''

),(

tytxt

tytxt

tytxt

t

ttt

yyy

xxx

xugG

tt

tt

tt

t

ttt

v

yvy

xvx

uugV

''

''

''

),( 1

243

221

||||00||||

tt

ttt v

vM

Motion noise covariance

Matrix from the control

Jacobian of g w.r.t location

Predicted meanPredicted covariance

Jacobian of g w.r.t control


32


With and are the state vectors at time t-1 and t respectively

t

tvv

tvv

yx

yx

t

tt

t

t

t

tt

t

t

t

ˆ

)ˆcos(ˆˆ

cosˆˆ

)ˆsin(ˆˆ

sinˆˆ

'

'

'

Tt yxx 1 Tt yxx '''

The true motion is described by a translation velocity and a rotational velocity

tv t

Motion Control with additive Gaussian noise

),0(

ˆˆ

243

221 )(

tt

t

v

v

t

t

t

t Mvvv

tt

tt

Tttt vu )(

243

221

)(00)(

tt

ttt v

vM


33


),0()cos(cos

)sin(sin

'

'

'

t

t

tt

t

t

t

tt

t

t

t

RN

t

tvv

tvv

yx

yx

),0(),( 1 tttt RNxugx

)(),(),(

)(),(

),(),(

1111

111

111

ttttttt

ttt

tttttt

xGugxug

xx

ugugxug

Motion Model:


34


),0()cos(cos

)sin(sin

'

'

'

t

t

tt

t

t

t

tt

t

t

t

RN

t

tvv

tvv

yx

yx

,1,1,1

,1,1,1

',1

'

,1

'

,1

'

1

111

),(),(

tytxt

tytxt

tytxt

t

ttttt

yyy

xxx

xug

xG

Derivative of g along x’ dimension, w.r.t. x at

1t

xt

x

,1

Jacobian of g w.r.t location


35


),0()cos(cos

)sin(sin

'

'

'

t

t

tt

t

t

t

tt

t

t

t

RN

t

tvv

tvv

yx

yx

Derivative of g w.r.t. the motion parameters, evaluated at and

1t

tt

tt

tt

t

ttt

v

yvy

xvx

uugV

''

''

''

),( 1

t

ttvtvt

ttvtvt

t

tt

t

tt

t

t

t

tt

t

tt

t

t

0

)sin())cos((cos)cos(cos

)cos())sin((sin)sin(sin

2

2

Tttt

Ttttt VMVGG 1

Mapping between the motion noise in control space to the motion noise in state space

Jacobian of g w.r.t control

tu


36

1. EKF_localization ( t-1, t-1, ut, zt, m):

Correction:

2.

3.

4.

5.

6.

7.

8.

)ˆ( ttttt zzK

tttt HKI

,

,

,

,

,

,),(

t

t

t

t

yt

t

yt

t

xt

t

xt

t

t

tt

rrr

xmhH

,,,

2,

2,

,2atanˆ

txtxyty

ytyxtxt

mmmmz

tTtttt QHHS

1 tTttt SHK

2

2

00

r

rtQ

Predicted measurement mean

Pred. measurement covarianceKalman gain

Updated meanUpdated covariance

Jacobian of h w.r.t location


37

Feature-Based Measurement Model

2

2

2

)),(2tan)()(

,,

2,

2,

s

r

j

xjyj

yjxj

it

it

it

sxmymaymxm

s

r

)()(

)()( ttt

ttt x

xh

hxh

,

,

,

,

,

,),(

t

t

t

t

yt

t

yt

t

xt

t

xt

t

t

tt

rrr

xmhH

),0(),,( ttit QNmjxhz

• Jacobian of h w.r.t location

Is the landmark that corresponds to the measurement of

itzi

tCj


38

EKF Localizationwith known

correspondences


39

EKF Localizationwith unknown

correspondences

Maximum likelihood estimator


40

EKF Prediction StepInitial estimate is represented by the ellipse centered at 1t

Moving on a circular arc of 90cm & turning 45 degrees to the left, the predicted position is centered at t

High translational noise High noise in both translation & rotation

High rotational noiseSmall noise in translational & rotation


41

EKF Measurement Prediction Step

Innovations (white arrows) : differences between observed & predicted measurements

True robot (white circle) & the observation (bold line circle)

Measurem

ent Prediction


42

EKF Correction Step

Measurem

ent Prediction

Resulting correction

Update mean estimate & reduce position uncertainty ellipses


43

Estimation Sequence (1)EKF localization with an accurate landmark detection sensor

Dashed line: estimated robot trajectory

Solid line: true robot motion

Dashed line: corrected robot trajectory Uncertainty ellipses: before (light gray) & after (dark gray) incorporating landmark detection


44

Estimation Sequence (2)EKF localization with a less accurate landmark detection sensor

Uncertainty ellipses: before (light gray) & after (dark gray) incorporating landmark detection


45

Comparison to Ground Truth


46

UKF Localization?

• Given – Map of the environment.– Sequence of measurements/motions.

• Wanted– Estimate of the robot’s position.

• UKF localization


47

Unscented Transform

nin

wwn

nw

nw

ic

imi

i

cm

2,...,1for )(2

1 )(

)1( 2000

Sigma points Weights

)( ii g

n

i

Tiiic

n

i

iim

w

w

2

0

2

0

))(('

'

Pass sigma points through nonlinear function

Recover mean and covariance For n-dimensional Gaussianλ is scaling parameter that determine how far the sigma points are spread from the meanIf the distribution is an exact Gaussian, β=2 is the optimal choice.


48

UKF_localization ( t-1, t-1, ut, zt, m):

Prediction:

243

221

||||00||||

tt

ttt v

vM

2

2

00

r

rtQ

TTTt

at 000011

t

t

tat

QM00

00001

1

at

at

at

at

at

at 111111

xt

utt

xt ug 1,

L

i

Tt

xtit

xti

ict w

2

0,,

L

i

xti

imt w

2

0,

Motion noise

Measurement noise

Augmented state mean

Augmented covariance

Sigma points

Prediction of sigma points

Predicted mean

Predicted covariance


49

UKF_localization ( t-1, t-1, ut, zt, m):

Correction:

zt

xtt h

L

iti

imt wz

2

0,ˆ

Measurement sigma points

Predicted measurement mean

Pred. measurement covariance

Cross-covariance

Kalman gain

Updated mean

Updated covariance

Ttti

L

itti

ict zzwS ˆˆ ,

2

0,

Ttti

L

it

xti

ic

zxt zw ˆ,

2

0,

,

1, tzx

tt SK

)ˆ( ttttt zzK

Tttttt KSK

The predicted robot locations are used to generate the measurement sigma points

xt


50

UKF Prediction Step

Moving on a circular arc of 90cm & turning 45 degrees to the left, the predicted position is centered at

t

High translational noise

High rotational noise

High noise in both translation & rotation


51

UKF Measurement Prediction Step

Measurem

ent Prediction

Predicted Sigma points


52

UKF Correction Step

Measurem

ent Prediction

Resulting correction


53

Estimation Sequence

EKF UKF

Robot path estimated with different techniques, with UKF being slightly closer


54

Prediction Quality

EKF UKF Robot moves on a circle, estimates based on EKF prediction, & UKF prediction


55

• [Arras et al. 98]: • Laser range-finder and vision• High precision (<1cm accuracy)

Kalman Filter-based System

[Courtesy of Kai Arras]


56

Multi-hypothesisTracking


57

MHT: Multi-Hypothesis Tracking filter• Belief is represented by multiple hypotheses• Each hypothesis is tracked by a Kalman filter

• Additional problems:• Data association: Which observation corresponds to which

hypothesis?• Hypothesis management: When to add / delete hypotheses?

• Huge body of literature on target tracking, motion correspondence etc.

Localization With MHT


58

• Hypotheses are extracted from Laser Range Finder (LRF) scans• Each hypothesis has probability of being the correct one:

• Hypothesis probability is computed using Bayes’ rule

• Hypotheses with low probability are deleted.• New candidates are extracted from LRF scans.

MHT: Implemented System (1)

)}(,,ˆ{ iiii HPxH

},{ jjj RzC

)()()|()|(

sPHPHsPsHP ii

i

[Jensfelt et al. ’00]


59

MHT: Implemented System (2)

Courtesy of P. Jensfelt and S. Kristensen


60

MHT: Implemented System (3)Example run

Map and trajectory

# hypotheses

#hypotheses vs. time

P(Hbest)

Courtesy of P. Jensfelt and S. Kristensen



SLAM


62

SLAM Problem : Chicken or Egg

Fundamental problems for localization and mapping

The task of SLAM is to build a map while estimating the pose of the robot relative to this map.

Without a map, robot cannot localize itself

Without knowing its location, robot cannot build a map

Which needed to be done first? Localization or mapping?


63

Given:– The robot’s controls (U1:t)

– Observations of nearby features (Z1:t)

Estimate:– Map of features (m)

– Pose / Path of the robot (xt)

The SLAM ProblemA robot is exploring an unknown, static environment.


64

Why is SLAM a hard problem?

Uncertanties

• Error in pose

• Error in observation

• Error in mapping• Error accumulated


65

Why is SLAM a hard problem?SLAM: robot path and map are both unknown

Robot path error correlates errors in the map


66

Why is SLAM a hard problem?

• In the real world, the mapping between observations and landmarks is unknown

• Picking wrong data associations can have catastrophic consequences

Robot poseuncertainty


67

Data Association Problem

• A data association is an assignment of observations to landmarks

• In general there are more than (n observations, m landmarks) possible associations

• Also called “assignment problem”


Nature of the SLAM Problem

68

Continuous Location of objects in component the map

Robot’s own pose

Discrete Correspondence component

Object is the same or notreasoning


69

SLAM: Simultaneous Localization and Mapping

• Full SLAM:

Estimates Entire pose (x1:t) and map (m)

Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut)

),|,( t:1t:1t:1 uzmxp

Estimates entire path and map!


70

Graphical Model of Full SLAM:

),|,( :1:1:1 ttt uzmxp Compute a joint posterior over the whole path of robot and the map


71

SLAM: Simultaneous Localization and Mapping

• Online SLAM:

Estimates Most recent pose (xt) and map (m)

Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut)

),|,( t:1t:1t uzmxp

Estimates most recent pose and map!


72

Graphical Model of Online SLAM:

121:1:1:1:1:1 ...),|,(),|,( ttttttt dxdxdxuzmxpuzmxp

Integrations typically done one at a time

Estimate a posterior over the current robot pose, and the map


73

SLAM with Extended Kalman Filter

• Pre-requisites– Maps are feature-based (landmarks)

small number (< 1000)

– Assumption - Gaussian Noise

– Process only positive sightings

No landmark = negative

Landmark = positive


74

EKF-SLAM with known correspondences

Correspondence Data association problem

Landmarks can’t be uniquely identified

Correspondence variable (Cit)

between feature (fit) and real landmark

Tit

it

it

it Srf

True identity of observed feature

),|,,( t:1t:1t uzcmxp t

),|,,( t:1t:1:1t:1 uzcmxp t

Make correspondence variables explicit


75


Signature Numerical value (average color)

Characterize type of landmark (integer)

Multidimensional vector

(height and color)


76


Similar development to EKF localization

Diff robot pose + coordinates of all landmarks

Combined state vector

TNyNxNyxt

t SmmSmmyxmx

y ,,1,1,1 ...

(3N + 3)

),|( :1:1 ttt uzyp Online posterior


77

Motion update

Mean

Covariance

Iteration through measurements

Test for new landmarks

Initialization of elements

Expected measurement

Filter is updated



Observing a landmark improves robot pose estimate

eliminates some uncertainty of other landmarks

Improves position estimates of the landmark + other landmarks

We don’t need to model past poses explicitly

78


Example

79



80

Example:

• Uncertainty of landmarks are mainly due to robot’s pose uncertainty (persist over time)

Estimated location of landmarks are correlated


EKF-SLAM with unknown correspondences

• No correspondences for landmarks

• Uses an incremental maximum likelihood (ML) estimator

81

Determines most likely value of the correspondence variable

Takes this value for granted later on


EKF-SLAM with unknown correspondences

82

Motion update

Hypotheses of new landmark

Mean

Covariance


83


General Problem

Gaussian noise assumption Unrealistic

Spurious measurements Fake landmarks

Outliers

Affect robot’s localization

84


Solutions to General Problem

Provisional landmark list

New landmarks do not augment the map

Not considered to adjust robot’s pose

Consistent observation regular map

85


Solutions to General Problem

Landmark Existence Probability

Landmark is observed

Observable variable (o) increased by fixed value

Landmark is NOT observed when it should

Observable variable decreased

Removed from map when (o) drops below threshold

86


Problem with Maximum Likelihood (ML)

Once ML estimator determines likelihood of correspondence, it takes value for granted

always correct

Makes EKF susceptible to landmark confusion

Wrong results

87


Solutions to ML Problem

Spatial arrangement

Greater distance between landmarks

Less likely confusion will exist

Trade off:

few landmarks harder to localize

Little is known about optimal density of landmarks

88

SignaturesGive landmarks a very perceptual

distinctiveness(e,g, color, shape, …)


EKF-SLAM Limitations• Selection of appropriate landmarks

• Reduces sensor reading utilization to presence or absence of those landmarks

Lots of sensor data is discarded

• Quadratic update time

Limits algorithm to scarce maps (< 1000 features)

• Low dimensionality of maps harder data association problem

89


EKF-SLAM Limitations• Fundamental Dilemma of EKF-SLAM

It might work well with dense maps (millions of features)

It is brittle with scarce maps

90

BUT

It needs scarce maps because of complexity of the algorithm (update process)


91

SLAM Techniques

• EKF SLAM (chapter 10)

• Graph-SLAM (chapter 11)

• SEIF (sparse extended information filter) (chapter 12)

• Fast-SLAM (chapter 13)


92

Graph-SLAM• Solves full SLAM problem

• Represents info as a graph of soft constraints

• Accumulates information into its graph without resolving it (lazy SLAM)

• Computationally cheap

• At the other end of EKF-SLAM

Process information right away (proactive

SLAM)

Computationally expensive


93

Graph-SLAM• Calculates posteriors over robot path (not incremental)

• Has access to the full data

• Uses inference to create map using stored data

Offline algorithm


94

Sparse Extended Information Filter (SEIF)

• Implements a solution to online SLAM problem

• Calculates current pose and map (as EKF)

• Stores information representation of all knowledge (as Graph-SLAM)

Runs Online and is computationally efficient

• Applicable to multi-robot SLAM problem


95

FastSLAM Algorithm• Particle filter approach to the SLAM problem

• Maintain a set of particles

• Particles contain a sampled robot path and a map

• The features of the map are represented by own local Gaussian

• Map is created as a set of separate Gaussians

Map features are conditionally independent given the path

Factoring out the path (1 per particle)

Map feature become independent

Eliminates the need to maintain correlation among them


96

FastSLAM Algorithm• Updating in FastSLAM

Sample new pose update the observed features

• Update can be performed online

• Solves both online and offline SLAM problem

• Instances Feature-based maps

Grid-based algorithm


97

• Local submaps [Leonard et al.99, Bosse et al. 02, Newman et al. 03]

• Sparse links (correlations) [Lu & Milios 97, Guivant & Nebot 01]

• Sparse extended information filters [Frese et al. 01, Thrun et al. 02]

• Thin junction tree filters [Paskin 03]

• Rao-Blackwellisation (FastSLAM) [Murphy 99, Montemerlo et al. 02, Eliazar et al. 03, Haehnel et al. 03]

Approximations for SLAM Problem


98

Thank You

Documents

Probabilistic Robotics: Review/SLAM