Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc...
-
Upload
sharleen-nicholson -
Category
Documents
-
view
223 -
download
0
Transcript of Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc...
![Page 1: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/1.jpg)
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)
![Page 2: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/2.jpg)
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
![Page 3: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/3.jpg)
SLAM ApplicationsSLAM Applications
Images – Probabilistic RoboticsImages – Probabilistic Robotics
Indoors
Space
Undersea
Underground
![Page 4: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/4.jpg)
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)
![Page 5: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/5.jpg)
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.
![Page 6: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/6.jpg)
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
![Page 7: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/7.jpg)
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 )
![Page 8: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/8.jpg)
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
![Page 9: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/9.jpg)
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
![Page 10: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/10.jpg)
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.
![Page 11: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/11.jpg)
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 )
![Page 12: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/12.jpg)
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
![Page 13: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/13.jpg)
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 )
![Page 14: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/14.jpg)
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
![Page 15: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/15.jpg)
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.
![Page 16: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/16.jpg)
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
![Page 17: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/17.jpg)
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 )
![Page 18: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/18.jpg)
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
![Page 19: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/19.jpg)
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
![Page 20: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/20.jpg)
Particle Filter AdvantagesParticle Filter Advantages
Can represent multi-modal distributionsCan represent multi-modal distributionsP
roba
bilit
y
Position
![Page 21: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/21.jpg)
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.
![Page 22: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/22.jpg)
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 )
![Page 23: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/23.jpg)
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 )
![Page 24: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/24.jpg)
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
![Page 25: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/25.jpg)
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
![Page 26: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/26.jpg)
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 )
![Page 27: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/27.jpg)
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 )
![Page 28: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/28.jpg)
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
![Page 29: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/29.jpg)
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 )
![Page 30: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/30.jpg)
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 )
![Page 31: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/31.jpg)
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
![Page 32: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/32.jpg)
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 )
![Page 33: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/33.jpg)
Parameter UpdatesParameter Updates
We update We update θθ by: by:
(j)t-1
j=1t t-1 t (j)
t-1
ρ
θ = θ + γ (Newton - Raphson)w
![Page 34: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/34.jpg)
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
![Page 35: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/35.jpg)
Weight UpdatesWeight Updates
(i)(i) t(i) (i) (i)tt t t(i) (i)
t ti=1 i=1
ρρ
w β = - ww w
![Page 36: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/36.jpg)
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
![Page 37: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/37.jpg)
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 ����������������������������
![Page 38: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc Sobel (Includes references to Brian Clipp Comp 790-072 Robotics)](https://reader033.fdocuments.us/reader033/viewer/2022052701/56649cf85503460f949c8fba/html5/thumbnails/38.jpg)
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.
Θ