Principles of Safe Autonomy: Lecture 13: State Estimation ...
Transcript of Principles of Safe Autonomy: Lecture 13: State Estimation ...
![Page 1: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/1.jpg)
Principles of Safe Autonomy: Lecture 13:
State Estimation, Filtering and Robot Localization
Sayan Mitra
Reference: Probabilistic Robotics by Sebastian Thrun, Wolfram Burgard, and Dieter Fox
Slides: From the book’s website
![Page 2: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/2.jpg)
Roomba mapping
iRobot Roomba uses VSLAM algorithm to create maps for cleaning areas
![Page 3: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/3.jpg)
Announcements
![Page 4: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/4.jpg)
Autonomy pipeline
Control
Dynamical models of engine, powertrain, steering, tires, etc.
Decisions and planning
Programs and multi-agent models of
pedestrians, cars, etc.
Perception
Programs for object detection, lane tracking, scene
understanding, etc.
Sensing
Physics-based models of camera,
LIDAR, RADAR, GPS, etc.
GEM platform
![Page 5: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/5.jpg)
Control
Dynamical models of engine, powertrain, steering, tires, etc.
Decisions and planning
Programs and multi-agent models of
pedestrians, cars, etc.
Perception
Programs for object detection, lane tracking, scene
understanding, etc.
Sensing
Physics-based models of camera,
LIDAR, RADAR, GPS, etc.
![Page 6: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/6.jpg)
Outline of filtering and state estimation module• Introduction: Localization problem, taxonomy
• Probabilistic models
• Discrete Bayes Filter• Review of Bayes rule and conditional probability
• Histogram filter• Grid localization
• Particle filter (next time)• Monte Carlo localization
![Page 7: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/7.jpg)
Localization problem (MP4)
• Determine the pose of the robot relative to the given map of the environment
• Pose: position, velocity, attitude, angles
• Also known as position or state estimation problem
• First: why localize?
• How does your robot know its position in ECEB?
• “Localization is the biggest hack in autonomous cars” --- people drive without localization
![Page 8: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/8.jpg)
Setup: State evolution and measurement models
• Deterministic model:System evolution: 𝑥𝑡+1 = 𝑓 𝑥𝑡 , 𝑢𝑡• 𝑥𝑡: unknown state of the system at time t
• 𝑢𝑡: known control input at time t
• 𝑓: known dynamic function, possibly stochastic
Measurement: 𝑧𝑡 = 𝑔(𝑥𝑡 , 𝑚)
• 𝑧𝑡: known measurement of state 𝑥𝑡 at time 𝑡
• 𝑚: unknown underlying map
• 𝑔: known measurement function
• We will work with probabilistic models going forward 𝑚
This is not exactly the measurement model of MP4
𝑥𝑡
𝑧𝑡[1]
𝑧𝑡[2]
𝑧𝑡[3]
![Page 9: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/9.jpg)
Localization as coordinate transformation
m
zt-1zt zt+1
ut-1 ut ut+1
xt-1 xt xt+1
Shaded known: map (m), control inputs (u), measurements(z). White nodes to be determined (x)
maps (m) are described in global coordinates. Localization = establish coord transf. between m and robot’s local coordinates
Transformation used for objects of interest (obstacles, pedestrians) for decision, planning and control
![Page 10: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/10.jpg)
Localization taxonomyGlobal vs Local
• Local: assumes initial pose is known, has to only account for the uncertainty coming from robot motion (position tracking problem)
• Global: initial pose unknown; harder and subsumes position tracking
• Kidnapped robot problem: during operation the robot can get teleported to a new unknown location (models failures)
Static vs Dynamic Environments
Single vs Multi-robot localization
Passive vs Active Approaches
• Passive: localization module only observes and is controlled by other means; motion not designed to help localization (Filtering problem)
• Active: controls robot to improve localization
![Page 11: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/11.jpg)
Ambiguity in global localization arising from locally symmetric environment
![Page 12: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/12.jpg)
Discrete Bayes Filter Algorithm
• System evolution: 𝑥𝑡+1 = 𝑓 𝑥𝑡 , 𝑢𝑡• 𝑥𝑡: state of the system at time t
• 𝑢𝑡: control input at time t
• Measurement: 𝑧𝑡 = 𝑔(𝑥𝑡 , 𝑚)• 𝑧𝑡: measurement of state 𝑥𝑡 at time 𝑡
• 𝑚: unknown underlying map
![Page 13: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/13.jpg)
Setup, notations
• Discrete time model
• 𝑥𝑡1:𝑡2 = 𝑥𝑡1 , 𝑥𝑡1+1, 𝑥𝑡1+2, … , 𝑥𝑡2 sequence of robot states 𝑡1to 𝑡2
• Robot takes one measurement at a time• 𝑧𝑡1:𝑡2 = 𝑧𝑡1 , … , 𝑧𝑡2 sequence of all measurements from 𝑡1to 𝑡2
• Control also exercised at discrete steps• 𝑢𝑡1:𝑡2 = 𝑢𝑡1 , 𝑢𝑡1+1, 𝑢𝑡1+2, … , 𝑢𝑡2 sequence control inputs
![Page 14: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/14.jpg)
Review of conditional probabilities
Random variable 𝑋 takes values 𝑥1, 𝑥2, . .
Example: Result of a dice roll (𝑋) and 𝑥𝑖 = 1,… , 6
𝑃 𝑋 = 𝑥 is written as 𝑃 𝑥
Conditional probability: 𝑃 𝑥 𝑦 =𝑃 𝑥,𝑦
𝑃(𝑦)provided 𝑃 𝑦 > 0
𝑃 𝑥, 𝑦 = 𝑃 𝑥 𝑦 𝑃(𝑦)
= 𝑃 𝑦 𝑥 𝑃(𝑥)
Substituting in the definition of Conditional Prob. we get Bayes Rule
𝑃 𝑥 𝑦 =𝑃 𝑦 𝑥 𝑃 𝑥
𝑃(𝑦), provided 𝑃 𝑦 > 0
![Page 15: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/15.jpg)
Using measurements to update state estimates
𝑃 𝑥 𝑦 =𝑃 𝑦 𝑥 𝑃 𝑥
𝑃(𝑦), provided 𝑃 𝑦 > 0---- Equation (*)
𝑋 : Robot position, 𝑌 : measurement,
𝑃 𝑥 : Prior distribution (before measurement)
𝑃 𝑥 𝑦 : Posterior distribution (after measurement)
𝑃 𝑦 𝑥 : Measurement model / inverse conditional / generative model
𝑃(𝑦): does not depend on x; normalization constant
![Page 16: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/16.jpg)
State evolution and measurement: probabilistic modelsEvolution of state and measurements governed by probabilistic laws
𝑝 𝑥𝑡 𝑥0:𝑡−1, 𝑧1:𝑡−1, 𝑢1:𝑡) describes motion/state evolution model
• If state is complete, sufficient summary of the history then• 𝑝 𝑥𝑡 𝑥0:𝑡−1, 𝑧0:𝑡−1, 𝑢0:𝑡−1) = 𝑝 𝑥𝑡 𝑥𝑡−1, 𝑢𝑡) state transition prob.
• 𝑝 𝑥′ 𝑥, 𝑢) if transition probabilities are time invariant
zt-1zt
zt+
1
ut-
1ut
ut+
1
xt-1 xt
xt+
1
𝑥𝑡+1 = 𝑓 𝑥𝑡, 𝑢𝑡
![Page 17: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/17.jpg)
Measurement model
Measurement process 𝑝 𝑧𝑡 𝑥0:𝑡 , 𝑧1:𝑡−1, 𝑢0:𝑡−1)• Again, if state is complete
• 𝑝 𝑧𝑡 𝑥0:𝑡 , 𝑧1:𝑡−1, 𝑢1:𝑡) = 𝑝 𝑧𝑡 𝑥𝑡)
• 𝑝 𝑧𝑡 𝑥𝑡): measurement probability
• 𝑝 𝑧 𝑥): time invariant measurement probability
zt-1zt
zt+
1
ut-
1ut
ut+
1
xt-1 xt
xt+
1
𝑧𝑡 = 𝑔 𝑥𝑡
![Page 18: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/18.jpg)
BeliefsBelief: Robot’s knowledge about the state of the environment
True state is unknowable / measurable typically, so, robot must infer state from data
and we have to distinguish this inferred/estimated state from the actual state 𝑥𝑡𝑏𝑒𝑙(𝑥𝑡) = 𝑝(𝑥𝑡|𝑧1:𝑡, 𝑢1:𝑡)
Posterior distribution over state at time t given all past measurements and control.
This will be calculated in two steps:
1. Prediction: 𝑏𝑒𝑙(𝑥𝑡) = 𝑝 𝑥𝑡 𝑧1:𝑡−1, 𝑢1:𝑡
2. Correction: Calculating 𝑏𝑒𝑙(𝑥𝑡) from 𝑏𝑒𝑙(𝑥𝑡) a.k.a measurement update (will
use Equation (*) from earlier)
![Page 19: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/19.jpg)
Recursive Bayes Filter
Algorithm Bayes_filter(𝑏𝑒𝑙 𝑥𝑡−1 , 𝑢𝑡 , 𝑧𝑡)
for all 𝑥𝑡 do:
𝑏𝑒𝑙 𝑥𝑡 = ∫ 𝑝(𝑥𝑡|𝑢𝑡,𝑥𝑡−1)𝑏𝑒𝑙(𝑥𝑡−1)𝑑𝑥𝑡−1𝑏𝑒𝑙 𝑥𝑡 = 𝜂 𝑝 𝑧𝑡 𝑥𝑡 𝑏𝑒𝑙(𝑥𝑡)
end for
return 𝑏𝑒𝑙(𝑥𝑡)
𝑏𝑒𝑙 𝑥𝑡−1
𝑥𝑡𝑝′
1𝑝1
2𝑝2
3𝑝3
𝑝 𝑥𝑡|𝑢𝑡 , 1
𝑝 𝑥𝑡|𝑢𝑡 , 2
𝑝 𝑥𝑡|𝑢𝑡 , 3
𝑏𝑒𝑙 𝑥𝑡−1
𝑏𝑒𝑙(𝑥𝑡)
𝑝 𝑧𝑡 𝑥𝑡
![Page 20: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/20.jpg)
Histogram Filter or Discrete Bayes Filter
Finitely many states 𝑥𝑖 , 𝑥𝑘 , 𝑒𝑡𝑐. Random state vector 𝑋𝑡
𝑝𝑘,𝑡: belief at time t for state 𝑥𝑘; discrete probability distribution
Algorithm Discrete_Bayes_filter( 𝑝𝑘,𝑡−1 , 𝑢𝑡, 𝑧𝑡):
for all 𝑘 do:
ҧ𝑝𝑘,𝑡 = σ𝑖 𝑝(𝑋𝑡 = 𝑥𝑘|𝑢𝑡,𝑋𝑡−1 = 𝑥𝑖)𝑝𝑖,𝑡−1
𝑝𝑘,𝑡 = 𝜂 𝑝 𝑧𝑡 𝑋𝑡 = 𝑥𝑘) ҧ𝑝𝑘,𝑡
end for
return {𝑝𝑘,𝑡}
𝑏𝑒𝑙 𝑥𝑡−1
𝑥𝑘𝑝′
1𝑝1,𝑡−1
2𝑝2,𝑡−1
3𝑝3,𝑡−1
𝑝 𝑥𝑘|𝑢𝑡, 1
𝑝 𝑥𝑡|𝑢𝑡 , 2
𝑝 𝑥𝑡|𝑢𝑡 , 3
𝑏𝑒𝑙 𝑥𝑡−1
𝑏𝑒𝑙(𝑥𝑡)
𝑝 𝑧𝑡 𝑥𝑡
![Page 21: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/21.jpg)
Grid Localization
• Solves global localization in some cases kidnapped robot problem
• Can process raw sensor data• No need for feature extraction
• Non-parametric• In particular, not bound to unimodal distributions (unlike Extended Kalman
Filter)
![Page 22: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/22.jpg)
Grid localization
Algorithm Grid_localization ( 𝑝𝑘,𝑡−1 , 𝑢𝑡 , 𝑧𝑡, 𝑚)
for all 𝑘 do:
ҧ𝑝𝑘,𝑡 = σ𝑖 𝑝𝑖,𝑡−1𝒎𝒐𝒕𝒊𝒐𝒏_𝒎𝒐𝒅𝒆𝒍(𝑚𝑒𝑎𝑛 𝑥𝑘 , 𝑢𝑡, 𝑚𝑒𝑎𝑛 𝑥𝑖 )
𝑝𝑘,𝑡 = 𝜂 ҧ𝑝𝑘,𝑡𝒎𝒆𝒂𝒔𝒖𝒓𝒆𝒎𝒆𝒏𝒕_𝒎𝒐𝒅𝒆𝒍(𝑧𝑡 , 𝑚𝑒𝑎𝑛 𝑥𝑘 , 𝑚)
end for
return 𝑏𝑒𝑙(𝑥𝑡)
![Page 23: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/23.jpg)
23
Piecewise Constant Representation
),,( = yxxBel tFixing an input ut we can compute the new belief
![Page 24: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/24.jpg)
Start
Motion Model without measurements
𝑚𝑒𝑎𝑛 𝑥𝑘
![Page 25: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/25.jpg)
Proximity Sensor Model
Laser sensor Sonar sensor
𝑥𝑘
𝑝 𝑧𝑡 𝑋𝑡 = 𝑥𝑘)
𝑚
![Page 26: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/26.jpg)
26
Grid localization, 𝑏𝑒𝑙 𝑥𝑡 represented by a histogram over grid 𝑝(𝑧|𝑥)
𝑝(𝑧|𝑥)
![Page 27: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/27.jpg)
Summary
• Key variable: Grid resolution
• Two approaches
• Topological: break-up pose space into regions of significance (landmarks)
• Metric: fine-grained uniform partitioning; more accurate at the expense of higher computation costs
• Important to compensate for coarseness of resolution
• Evaluating measurement/motion based on the center of the region may not be enough. If motion is updated every 1s, robot moves at 10 cm/s, and the grid resolution is 1m, then naïve implementation will not have any state transition!
• Computation
• Motion model update for a 3D grid required a 6D operation, measurement update 3D
• With fine-grained models, the algorithm cannot be run in real-time
• Some calculations can be cached (ray-casting results)
![Page 28: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/28.jpg)
28
Grid-based Localization
![Page 29: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/29.jpg)
32
Sonars and Occupancy Grid Map
![Page 30: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/30.jpg)
Monte Carlo Localization
• Represents beliefs by particles
![Page 31: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/31.jpg)
• Represent belief by finite number of parameters (just like histogram filter)
• But, they differ in how the parameters (particles) are generated and populate
the state space
• Key idea: represent belief 𝑏𝑒𝑙 𝑥𝑡 by a random set of state samples
• Advantages
• The representation is approximate and nonparametric and therefore can
represent a broader set of distributions than e.g., Gaussian
• Can handle nonlinear tranformations
• Related ideas: Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap
filter, Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96], Dynamic Bayesian
Networks: [Kanazawa et al., 95]d
Particle Filters
![Page 32: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/32.jpg)
Particle filtering algorithm
𝑋𝑡 = 𝑥𝑡[1], 𝑥𝑡
[2], … 𝑥𝑡
[𝑀]particles
Algorithm Particle_filter(𝑋𝑡−1, 𝑢𝑡 , 𝑧𝑡):ത𝑋𝑡−1 = 𝑋𝑡 = ∅
for all 𝑚 in [M] do:
sample 𝑥𝑡[𝑚]
~𝑝 𝑥𝑡 𝑢𝑡 , 𝑥𝑡−1[𝑚]
)
𝑤𝑡[𝑚]
= 𝑝 𝑧𝑡 𝑥𝑡𝑚
ത𝑋𝑡 = ത𝑋𝑡 + ⟨ 𝑥𝑡𝑚, 𝑤𝑡
[𝑚]⟩
end for
for all 𝑚 in [M] do:
draw 𝑖 𝑤𝑖𝑡ℎ 𝑝𝑟𝑜𝑏𝑎𝑏𝑖𝑙𝑖𝑡𝑦 ∝ 𝑤𝑡[𝑖]
add 𝑥𝑡[𝑖]𝑡𝑜 𝑋𝑡
end for
return 𝑋𝑡
ideally, 𝑥𝑡[𝑚]
is selected with probability prop. to 𝑝 𝑥𝑡 𝑧1:𝑡, 𝑢1:𝑡)
ത𝑋𝑡−1 is the temporary particle set
// sampling from state transition dist.
// calculates importance factor 𝑤𝑡 or weight
// resampling or importance sampling; these are
distributed according to 𝜂 𝑝 𝑧𝑡 𝑥𝑡[𝑚]
𝑏𝑒𝑙 𝑥𝑡
// survival of fittest: moves/adds particles to parts of the state space with higher probability
![Page 33: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/33.jpg)
Weight samples: w = f / g
Importance Sampling
suppose we want to compute 𝐸𝑓 𝐼 𝑥 ∈ 𝐴 but we can only sample from density 𝑔
𝐸𝑓 𝐼 𝑥 ∈ 𝐴
= ∫ 𝑓 𝑥 𝐼 𝑥 ∈ 𝐴 𝑑𝑥
= ∫𝑓 𝑥
𝑔 𝑥𝑔 𝑥 𝐼 𝑥 ∈ 𝐴 𝑑𝑥, provided 𝑔 𝑥 > 0
= ∫ 𝑤 𝑥 𝑔 𝑥 𝐼 𝑥 ∈ 𝐴 𝑑𝑥
= 𝐸𝑔 𝑤(𝑥)𝐼 𝑥 ∈ 𝐴
We need 𝑓 𝑥 > 0 ⇒ 𝑔 𝑥 > 0
![Page 34: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/34.jpg)
Monte Carlo Localization (MCL)𝑋𝑡 = 𝑥𝑡
[1], 𝑥𝑡
[2], … 𝑥𝑡
[𝑀]particles
Algorithm MCL(𝑋𝑡−1, 𝑢𝑡 , 𝑧𝑡,m):ത𝑋𝑡−1 = 𝑋𝑡 = ∅
for all 𝑚 in [M] do:
𝑥𝑡[𝑚]
= 𝒔𝒂𝒎𝒑𝒍𝒆_𝒎𝒐𝒕𝒊𝒐𝒏_𝒎𝒐𝒅𝒆𝒍(𝑢𝑡 𝑥𝑡−1[𝑚]
)
𝑤𝑡[𝑚]
= 𝒎𝒆𝒂𝒔𝒖𝒓𝒆𝒎𝒆𝒏𝒕_𝒎𝒐𝒅𝒆𝒍(𝑧𝑡 , 𝑥𝑡𝑚 ,𝑚
)
ത𝑋𝑡 = ത𝑋𝑡 + ⟨ 𝑥𝑡𝑚, 𝑤𝑡
[𝑚]⟩
end for
for all 𝑚 in [M] do:
draw 𝑖 𝑤𝑖𝑡ℎ 𝑝𝑟𝑜𝑏𝑎𝑏𝑖𝑙𝑖𝑡𝑦 ∝ 𝑤𝑡[𝑖]
add 𝑥𝑡[𝑖]𝑡𝑜 𝑋𝑡
end for
return 𝑋𝑡
Plug in motion and measurement models in the particle filter
![Page 35: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/35.jpg)
Particle Filters
![Page 36: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/36.jpg)
)|()(
)()|(
)()|()(
xzpxBel
xBelxzpw
xBelxzpxBel
=
−
−
−
Sensor Information: Importance Sampling
![Page 37: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/37.jpg)
− 'd)'()'|()( , xxBelxuxpxBel
Robot Motion
![Page 38: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/38.jpg)
)|()(
)()|(
)()|()(
xzpxBel
xBelxzpw
xBelxzpxBel
=
−
−
−
Sensor Information: Importance Sampling
![Page 39: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/39.jpg)
Robot Motion
− 'd)'()'|()( , xxBelxuxpxBel
![Page 40: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/40.jpg)
45
![Page 41: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/41.jpg)
46
![Page 42: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/42.jpg)
47
![Page 43: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/43.jpg)
48
![Page 44: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/44.jpg)
49
![Page 45: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/45.jpg)
50
![Page 46: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/46.jpg)
51
![Page 47: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/47.jpg)
52
![Page 48: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/48.jpg)
53
![Page 49: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/49.jpg)
54
![Page 50: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/50.jpg)
55
![Page 51: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/51.jpg)
56
![Page 52: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/52.jpg)
57
![Page 53: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/53.jpg)
58
![Page 54: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/54.jpg)
59
![Page 55: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/55.jpg)
60
![Page 56: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/56.jpg)
61
![Page 57: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/57.jpg)
62
![Page 58: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/58.jpg)
63
Sample-based Localization (sonar)
![Page 59: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/59.jpg)
64
Initial Distribution
![Page 60: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/60.jpg)
65
After Incorporating Ten Ultrasound Scans
![Page 61: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/61.jpg)
66
After Incorporating 65 Ultrasound Scans
![Page 62: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/62.jpg)
67
Estimated Path
![Page 63: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/63.jpg)
Using Ceiling Maps for Localization
![Page 64: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/64.jpg)
Vision-based Localization
P(z|x)
h(x)
z
![Page 65: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/65.jpg)
Under a LightMeasurement z: P(z|x):
![Page 66: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/66.jpg)
Next to a LightMeasurement z: P(z|x):
![Page 67: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/67.jpg)
ElsewhereMeasurement z: P(z|x):
![Page 68: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/68.jpg)
Global Localization Using Vision
![Page 69: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/69.jpg)
74
Limitations
• The approach described so far is able to • track the pose of a mobile robot and to
• globally localize the robot.
• Can we deal with localization errors (i.e., the kidnapped robot problem)?
• How to handle localization errors/failures? • Particularly serious when the number of particles is small
![Page 70: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/70.jpg)
75
Approaches• Randomly insert samples
• Why?
• The robot can be teleported at any point in time
• How many particles to add? With what distribution? • Add particles according to localization performance
• Monitor the probability of sensor measurements 𝑝(𝑧𝑡|𝑧1:𝑡−1, 𝑢1:𝑡 , 𝑚)
• For particle filters: 𝑝(𝑧𝑡|𝑧1:𝑡−1, 𝑢1:𝑡 , 𝑚) ≈1
𝑀σ𝑤𝑡
[𝑚]
• Insert random samples proportional to the average likelihood of the particles (the robot has been teleported with higher probability when the likelihood of its observations drops).
![Page 71: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/71.jpg)
76
Random SamplesVision-Based Localization936 Images, 4MB, .6secs/image
Trajectory of the robot:
![Page 72: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/72.jpg)
77
Kidnapping the Robot
![Page 73: Principles of Safe Autonomy: Lecture 13: State Estimation ...](https://reader031.fdocuments.us/reader031/viewer/2022012209/61def3bf3f7376700f66a37e/html5/thumbnails/73.jpg)
83
Summary• Particle filters are an implementation of recursive
Bayesian filtering
• They represent the posterior by a set of weighted samples.
• In the context of localization, the particles are propagated according to the motion model.
• They are then weighted according to the likelihood of the observations.
• In a re-sampling step, new particles are drawn with a probability proportional to the likelihood of the observation.