Marginal Particle and Multirobot Slam: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS SLAM=‘SIMULTANEOUS

LOCALIZATION AND MAPPING’LOCALIZATION AND MAPPING’

By Marc SobelBy Marc Sobel

(Includes references to Brian Clipp(Includes references to Brian Clipp

Comp 790-072 Robotics)Comp 790-072 Robotics)

The SLAM ProblemThe SLAM Problem

Given Given Robot controlsRobot controls Nearby measurementsNearby measurements

EstimateEstimate Robot state (position, orientation)Robot state (position, orientation) Map of world featuresMap of world features

SLAM ApplicationsSLAM Applications

Images – Probabilistic RoboticsImages – Probabilistic Robotics

Indoors

Space

Undersea

Underground

OutlineOutline

SensorsSensors SLAMSLAMFull vs. Online SLAMFull vs. Online SLAMMarginal SlamMarginal SlamMultirobot marginal slamMultirobot marginal slamExample AlgorithmsExample Algorithms

Extended Kalman Filter (EKF) SLAMExtended Kalman Filter (EKF) SLAMFastSLAM (particle filter)FastSLAM (particle filter)

Types of SensorsTypes of Sensors

OdometryOdometry Laser Ranging and Detection (LIDAR)Laser Ranging and Detection (LIDAR) Acoustic (sonar, ultrasonic)Acoustic (sonar, ultrasonic) RadarRadar Vision (monocular, stereo etc.)Vision (monocular, stereo etc.) GPSGPS Gyroscopes, Accelerometers (Inertial Gyroscopes, Accelerometers (Inertial

Navigation)Navigation) Etc.Etc.

Sensor CharacteristicsSensor Characteristics

NoiseNoise Dimensionality of OutputDimensionality of Output

LIDAR- 3D pointLIDAR- 3D point Vision- Bearing only (2D ray in space)Vision- Bearing only (2D ray in space)

RangeRange Frame of ReferenceFrame of Reference

Most in robot frame (Vision, LIDAR, etc.)Most in robot frame (Vision, LIDAR, etc.) GPS earth centered coordinate frameGPS earth centered coordinate frame Accelerometers/Gyros in inertial coordinate frameAccelerometers/Gyros in inertial coordinate frame

A Probabilistic ApproachA Probabilistic Approach

Notation:Notation:

1:t

1:t

1:t

t 1:t

t

(1:N)1:t

p(x ,m | z ,u ) = 'joint probability of pose and map'

x = State of the robot at timet

m = Map of the environment from times 1 to t at landmarks 1, ...,N

z = Sensor inputs from time 1 to t

u = Co

1:t

(1) (N)

ntrol (odometry) imputs from time 1 to t;

θ = rotations from one pose to another;

L = (L ,...,L )

Full vs. Online classical SLAMFull vs. Online classical SLAM

Full SLAM calculates the robot pose over Full SLAM calculates the robot pose over all time up to time t given the signal and all time up to time t given the signal and odometry:odometry:

Online SLAM calculates the robot pose for the Online SLAM calculates the robot pose for the current time tcurrent time t

1:t1:t 1:tp(x ,m | z ,u )

1:tt 1:t 1:t 1:t 1:t 1 tp(x ,m | z ,u ) = ..... p(x ,m | z ,u )dx ....dx

Full vs. Online SLAMFull vs. Online SLAM

),|,p( :1:1:1 ttt uzmx

Full SLAM Online SLAM

121:1:1:1:1:1 ...),|,(),|,p( ttttttt dxdxdxuzmxpuzmx

Classical Fast and EKF SlamClassical Fast and EKF Slam

Robot Environment: Robot Environment: (1) N distances: (1) N distances: mmtt ={d(x ={d(xtt,L,L11)) ,….,d(x,….,d(xtt,L,LNN)) };};

m measures distances from landmarks at m measures distances from landmarks at time t.time t. (2) Robot pose at time t: x(2) Robot pose at time t: x t.t.

(3) (Scan) Measurements at time t: z(3) (Scan) Measurements at time t: z tt

Goal: Determine the poses xGoal: Determine the poses x1:T 1:T

given scans zgiven scans z1:t1:t,odometry u,odometry u1:T1:T, and map, and map

measurements m.measurements m.

EKF SLAM (Extended Kalman EKF SLAM (Extended Kalman Filter)Filter)

As the state vector moves, the robot pose moves As the state vector moves, the robot pose moves according to the motion function, g(uaccording to the motion function, g(utt,,xxtt). This can be ). This can be

linearized into a Kalman Filter via:linearized into a Kalman Filter via:

The Jacobian J depends on translational and rotational The Jacobian J depends on translational and rotational velocity. This allows us to assume that the motion and velocity. This allows us to assume that the motion and hence distances are Gaussian. We can calculate the hence distances are Gaussian. We can calculate the mean mean μμ and covariance matrix and covariance matrix ΣΣ for the particle x for the particle xt t at time at time

t.t.

t t-1 t t-1 t-1 t-1 tg(u ,x ) = g(u ,μ ) + J(x - μ ) + N(0,R )

Outline of EKF SLAMOutline of EKF SLAM

By what preceded we assume that the map By what preceded we assume that the map vectors m (measuring distance from landmarks) vectors m (measuring distance from landmarks) are independent multivariate normal: Hence we are independent multivariate normal: Hence we now have:now have:

(1) (1,k) (1,k) (1)1:t 1:t 1:t 1:t

(M) (M,k) (M,k) (M)1:t 1:t 1:t 1:t

Particle 1 : x ,m μ ,Σ ;

......................

Particle M : x ,m μ ,Σ ;

N

N

Conditional IndependenceConditional Independence

For constructing the weights associated with the For constructing the weights associated with the classical fast slam algorithm, under moderate classical fast slam algorithm, under moderate assumptions, we get:assumptions, we get:

We use the notation,We use the notation, And calculate that:And calculate that:

1:t 1:t 1:t 1:t 1:t 1:t 1:t 1:tP x ,m | z ,u = P(m | z ,u )P(x | z ,u )

' function of g, , ';

' on the EKF expansion'

a

depending

(1,k)1:t

(1)1:t

μ

Σ

M

1:t 1:t j 1:t 1:tj=1

P(m | z ,u ) = P(m | z ,u )

Problems With EKF SLAMProblems With EKF SLAM Uses uni-modal Gaussians to model non-Gaussian Uses uni-modal Gaussians to model non-Gaussian

probability density functionprobability density functionP

roba

bilit

y

Position

Particle Filters (without EKF)Particle Filters (without EKF)

The use of EKF depends on the odometry The use of EKF depends on the odometry (u’s) and motion model (g’s) assumptions (u’s) and motion model (g’s) assumptions in a very nonrobust way and fails to allow in a very nonrobust way and fails to allow for multimodality in the motion model. In for multimodality in the motion model. In place of this, we can use particle filters place of this, we can use particle filters without assuming a motion model by without assuming a motion model by modeling the particles without reference to modeling the particles without reference to the parameters.the parameters.

Particle Filters (an alternative)Particle Filters (an alternative)

Represent probability distribution as a set of Represent probability distribution as a set of discrete particles which occupy the state spacediscrete particles which occupy the state space

Particle FiltersParticle Filters

For constructing the weights associated with the For constructing the weights associated with the classical fast slam algorithm, under moderate classical fast slam algorithm, under moderate assumptions, we get: (for x’s simulated by q)assumptions, we get: (for x’s simulated by q)

1

t

t

W

W

1:t 1:t

t 1:t 1:t-1 1:t-1 1:t

t 1:t 1:t-1

t t-1 tt t t-1

t t-1

P(x | z )

P(x | z ,x )P(x | z )

P(x | z ,x )

P(x | x ,z ) P(z | x ) W

q(x | x )

ResamplingResampling

Assign each particle a weight depending Assign each particle a weight depending on how well its estimate of the state on how well its estimate of the state agrees with the measurementsagrees with the measurements

Randomly draw particles from previous Randomly draw particles from previous distribution based on weights creating a distribution based on weights creating a new distributionnew distribution

Particle Filter Update CycleParticle Filter Update Cycle

Generate new particle distribution Generate new particle distribution For each particleFor each particle

Compare particle’s prediction of Compare particle’s prediction of measurements with actual measurementsmeasurements with actual measurements

Particles whose predictions match the Particles whose predictions match the measurements are given a high weightmeasurements are given a high weight

Resample particles based on weightResample particles based on weight

Particle Filter AdvantagesParticle Filter Advantages

Can represent multi-modal distributionsCan represent multi-modal distributionsP

roba

bilit

y

Position

Problems with Particle FiltersProblems with Particle Filters

Degeneracy:Degeneracy: As time evolves particles As time evolves particles increase in dimensionality. Since there is increase in dimensionality. Since there is error at each time point, this evolution error at each time point, this evolution typically leads to vanishingly small typically leads to vanishingly small interparticle (relative to intraparticle) interparticle (relative to intraparticle) variation.variation.

We frequently require estimates of the We frequently require estimates of the ‘marginal’ rather than ‘conditional’ particle ‘marginal’ rather than ‘conditional’ particle distribution. distribution.

Particle Filters do not provide good methods Particle Filters do not provide good methods for estimating for estimating particle features.particle features.

Marginal versus nonmarginal Marginal versus nonmarginal Particle FiltersParticle Filters

Marginal particle filters attempt to update the X’s Marginal particle filters attempt to update the X’s using their marginal (posterior) distribution rather using their marginal (posterior) distribution rather than their conditional (posterior) distribution. The than their conditional (posterior) distribution. The update weights take the general form,update weights take the general form,

t 1:tt

t 1:t

P x | zW =

q(x | z )

Marginal Particle updateMarginal Particle update

We want to update by using the old weights rather We want to update by using the old weights rather than conditioning on the old particles.than conditioning on the old particles.

t 1:t t t t 1:t-1

t-1 1:t-1t 1:t t t t-1 1:t-1 t t-1 t-1

t-1 1:t-1

(j) (j)t t tt-1 t-1

P(x | y ) P(y | x )P(x | y )

P(x | y )P(x | y ) P(y | x ) q(x | y )P(x | x ) dx

q(x | y )

P(y | x ) w P(x | x )

Marginal Particle FiltersMarginal Particle Filters

We specify the proposal distribution ‘q’ via:We specify the proposal distribution ‘q’ via:

k (j) (j)

t 1:t t tt t-1j=1

q(x | y ) = w q x | y ,x

Marginal Particle AlgorithmMarginal Particle Algorithm

(1)(1)

(2) Calculate the importance weights:(2) Calculate the importance weights:

ˆ (kernel density)k(i) (j) (j)

t 1:t t tt t t-1j=1

x p(x | y ) = w P(x | x ,y )

k(i) (j) (i) (j)t t tt-1 t-1

j=1(i)t k (j) (i) (j)

ttt-1 t-1j=1

P y | x w P x | x

w =w q x | y ,x

Updating Map features in the Updating Map features in the marginal modelmarginal model

Up to now, we haven’t assumed any map features. Up to now, we haven’t assumed any map features. Let Let θθ={={θθtt} denote the e.g., distances of the robot at } denote the e.g., distances of the robot at

time t from the given landmarks. We then write time t from the given landmarks. We then write

for the probability associated with scan Zfor the probability associated with scan Ztt

given the position xgiven the position x1:t1:t. .

We’d like to update We’d like to update θθ. This should be based, not on . This should be based, not on the gradient , but rather on the gradient, the gradient , but rather on the gradient, . .

θ t 1:tP (Z | X )

θ t 1:tP (Z | X )

θ t 1:t-1P (Z | Z )

Taking the ‘right’ derivativeTaking the ‘right’ derivative

The gradient, is highly The gradient, is highly non-robust; we are essentially non-robust; we are essentially taking derivatives of noise.taking derivatives of noise.

By contrast, the gradient, By contrast, the gradient, is robust and represents the is robust and represents the ‘right’ derivative.‘right’ derivative.

θ t 1:tP (Z | X )

θ t 1:t-1P (Z | Z )

Estimating the Gradient of a MapEstimating the Gradient of a Map

We have that, We have that,

1

1 +

θ t 1:t-1 θ t t θ t-1 1:t-1 t t-1 t

θ t 1:t-1 θ t t θ t 1:t-1 t t-1 t

θ t t θ t 1:t-1 t t-1 t

P Z | Z = P (Z | X )P (X | Z )P(X | X )dX

P Z | Z = P (Z | X )P (X | Z )P(X | X )dX

P (Z | X ) P (X | Z )P(X | X )dX

SimplificationSimplification

We can then show for the first term that:We can then show for the first term that:

1

1

1 1

θ t t θ t 1:t-1 t t-1 t

θ t 1:t-1 θ t t t t-1 t

θ t 1:t-1 θ t t

P (Z | X )P (X | Z )P(X | X )dX

P (X | Z ) P (Z | X )P(X | X )dX

P (X | Z ) P (Z | X )

Simplification IISimplification II

For the second term, we convert into a discrete For the second term, we convert into a discrete sum by defining ‘derivative weights’ sum by defining ‘derivative weights’

And combining them with the standard weights.And combining them with the standard weights.

(i) θ t-1 1:t-1t

θ t-1 1:t-1

P (X | Z )β = weights for

P (X | Z )

Estimating the GradientEstimating the Gradient

We can further write that:We can further write that:

1 1

( ) ( )1

11

1

( )1

1

;i itt

jt t

j

w

X

θ t 1:t-1 θ t t

θ t

θ t 1:t-1θ t t θ t 1:t-1 t t-1 t

θ t 1:t-1

(j) (j)θ t-1 t-1

P (X | Z ) P (Z | X )

P (Z | X )

P (X | Z )P (Z | X ) P (X | Z )P(X | X )dX

P (X | Z )

P ( | X )β w

Gradient (continued)Gradient (continued)

We can therefore update the gradient weights We can therefore update the gradient weights via:via:

( ) ( )1i i

ttw

(j) (j) (j)θ t t-1 t-1 t-1

j=1(i)θ tt

θ t 1:t

P (X | X )β w

ρ P (Z | X )q (X | Y )

Parameter UpdatesParameter Updates

We update We update θθ by: by:

(j)t-1

j=1t t-1 t (j)

t-1

ρ

θ = θ + γ (Newton - Raphson)w

NormalizationNormalization

The The ββ’s are normalized differently from the w’s. ’s are normalized differently from the w’s. In effect we put: In effect we put:

And then compute that:And then compute that:

θ t 1:tθ t 1:t

θ t 1:t t

ξ (X ,Y )P (X | Y ) =

ξ (X ,Y )dX

θ t 1:t tθ t 1:t

θ t 1:t θ t 1:t

θ t 1:t t θ t 1:t t

ξ (X ,Y )dXξ (X ,Y )

P (X | Y ) = - p (X | Y )

ξ (X ,Y )dX ξ (X ,Y )dX

Weight UpdatesWeight Updates

(i)(i) t(i) (i) (i)tt t t(i) (i)

t ti=1 i=1

ρρ

w β = - ww w

The Bayesian ViewpointThe Bayesian Viewpoint Retain a posterior sample of Retain a posterior sample of θθ at time t-1. at time t-1. Call this (i=1,…,I) Call this (i=1,…,I)

At time t, update this sample:At time t, update this sample:

(i)θ t 1:t-1t-1θ P (Y | Y )π(θ)

( )( )

1ii

t t

(j)t-1

j=1t (j)

t-1

ρ

θ = θ + γ (Newton - Raphson)w

Multi Robot ModelsMulti Robot Models

Write for the poses Write for the poses and scan statistics for the r robots. and scan statistics for the r robots.

At each time point the needed weights have r At each time point the needed weights have r indices:indices:

We also need to update the derivative weights – We also need to update the derivative weights – the derivative is now a matrix derivative. the derivative is now a matrix derivative.

[1],..., [ ] ; [1],..., [ ]t t t tX X r Z Z Z r t tX����������������������������

1( ,..., )1:( | )ri ittW P tX Y ����������������������������

Multi-Robot SLAMMulti-Robot SLAM

The parameter is now a matrix (with time The parameter is now a matrix (with time being the row values and robot index being the being the row values and robot index being the column. Updates depend on derivatives with column. Updates depend on derivatives with respect to each timepoint and with respect to respect to each timepoint and with respect to each robot. each robot.

Θ