Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot...
Transcript of Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot...
![Page 1: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/1.jpg)
Principles of Safe Autonomy: Lecture 12-13:
Filtering and Robot LocalizationSayan Mitra
March 9, 2020
Reference: Probabilistic Robotics by Sebastian Thrun, Wolfram Burgard, and Dieter FoxSlides: From the book’s website
![Page 2: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/2.jpg)
Announcements
• No final exam• Unless Class Project has to be significantly downgraded because of coronavirus
and University closure
• New date for Midterm 2: Wed April 15th
• MP4 + HW3 will be release this week• Classes may go online after spring break• Install zoom application• Stay healthy and stay tuned
![Page 3: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/3.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 4: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/4.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 5: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/5.jpg)
Outline
• Introduction: Localization problem, taxonomy• Discrete Bayes Filter• Histogram filter• Grid localization
• Particle filter• Monte Carlo localization
• Conclusions
![Page 6: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/6.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 7: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/7.jpg)
Setup
• System evolution: 𝑥"#$ = 𝑓 𝑥", 𝑢"• 𝑥": 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
𝑚This is not exactly the measurement model of MP4
𝑥"
𝑧"[1]
𝑧"[2]
𝑧"[3]
![Page 8: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/8.jpg)
Localization as coordinate transformation
m
zt-1 zt zt+1
ut-1 ut ut+1
xt-1 xt xt+1Shaded 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 9: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/9.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 EnvironmentsSingle vs Multi-robot localizationPassive 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 10: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/10.jpg)
Ambiguity in global localization arising from locally symmetric environment
![Page 11: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/11.jpg)
Discrete Bayes Filter Algorithm
• System evolution: 𝑥"#$ = 𝑓 𝑥", 𝑢"• 𝑥": state of the system at time t• 𝑢": control input at time t
• Measurement: 𝑧" = 𝑔(𝑥",𝑚)• 𝑧":measurement of state 𝑥" at time 𝑡• 𝑚: unknown underlying map
![Page 12: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/12.jpg)
Setup, notations
• Discrete time model• 𝑥"5:"6 = 𝑥"5, 𝑥"5#$, 𝑥"5#7, … , 𝑥"6 sequence of robot states 𝑡$to 𝑡7• Robot takes one measurement at a time• 𝑧"5:"6 = 𝑧"5, … , 𝑧"6 sequence of all measurements from 𝑡$to 𝑡7
• Control also exercised at discrete steps• 𝑢"5:"6 = 𝑢"5, 𝑢"5#$, 𝑢"5#7, … , 𝑢"6 sequence control inputs
![Page 13: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/13.jpg)
State evolution and measurement models
Evolution of state and measurements governed by probabilistic laws𝑝 𝑥" 𝑥::";$, 𝑧$:";$, 𝑢$:") describes motion/state evolution model• If state is complete, sufficient summary of the history then• 𝑝 𝑥" 𝑥::";$, 𝑧::";$, 𝑢::";$) = 𝑝 𝑥" 𝑥";$, 𝑢") state transition prob. • 𝑝 𝑥′ 𝑥, 𝑢) if transition probabilities are time invariant
zt-1zt
zt+
1
ut-1 utut+
1
xt-1 xtxt+
1
![Page 14: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/14.jpg)
Measurement model
Measurement process 𝑝 𝑧" 𝑥::", 𝑧$:";$, 𝑢::";$)• Again, if state is complete• 𝑝 𝑧" 𝑥::", 𝑧$:";$, 𝑢$:") = 𝑝 𝑧" 𝑥")• 𝑝 𝑧" 𝑥"): measurement probability• 𝑝 𝑧 𝑥): time invariant measurement probability
zt-1zt
zt+
1
ut-1 utut+
1
xt-1 xtxt+
1
![Page 15: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/15.jpg)
Beliefs
Belief: Robot’s knowledge about the state of the environmentTrue state is unknowable / measurable typically, so, robot must infer state from dataand we have to distinguish this inferred/estimated state from the actual state 𝑥"
𝑏𝑒𝑙(𝑥") = 𝑝(𝑥"|𝑧$:", 𝑢$:")
Posterior distribution over state at time t given all past measurements and control
Prediction: 𝑏𝑒𝑙(𝑥") = 𝑝(𝑥"|𝑧$:";$, 𝑢$:")
Calculating 𝑏𝑒𝑙(𝑥") from 𝑏𝑒𝑙(𝑥") is called correction or measurement update
![Page 16: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/16.jpg)
Recursive Bayes Filter
Algorithm Bayes_filter(𝑏𝑒𝑙 𝑥";$ , 𝑢", 𝑧")for all 𝑥" do:
𝑏𝑒𝑙 𝑥" = ∫ 𝑝(𝑥"|𝑢",𝑥";$)𝑏𝑒𝑙(𝑥";$)𝑑𝑥";$𝑏𝑒𝑙 𝑥" = 𝜂 𝑝 𝑧" 𝑥" 𝑏𝑒𝑙(𝑥")
end forreturn 𝑏𝑒𝑙(𝑥")
𝑏𝑒𝑙 𝑥";$
𝑥"𝑝′
1𝑝$
2𝑝7
3𝑝D
𝑝 𝑥"|𝑢", 1
𝑝 𝑥"|𝑢", 2
𝑝 𝑥"|𝑢", 3
𝑏𝑒𝑙 𝑥";$
𝑏𝑒𝑙(𝑥")
𝑝 𝑧" 𝑥"
![Page 17: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/17.jpg)
Histogram Filter or Discrete Bayes Filter
Finitely many states 𝑥E, 𝑥F, 𝑒𝑡𝑐. Random state vector 𝑋"𝑝F,": belief at time t for state 𝑥F; discrete probability distribution
Algorithm Discrete_Bayes_filter( 𝑝F,";$ , 𝑢", 𝑧"):for all 𝑘 do:
�̅�F," = ∑E 𝑝(𝑋" = 𝑥F|𝑢",𝑋";$ = 𝑥E)𝑝E,";$𝑝F," = 𝜂 𝑝 𝑧" 𝑋" = 𝑥F)�̅�F,"
end forreturn {𝑝F,"}
𝑏𝑒𝑙 𝑥";$
𝑥F𝑝′
1𝑝$,";$
2𝑝7,";$
3𝑝D,";$
𝑝 𝑥F|𝑢", 1
𝑝 𝑥"|𝑢", 2
𝑝 𝑥"|𝑢", 3
𝑏𝑒𝑙 𝑥";$
𝑏𝑒𝑙(𝑥")
𝑝 𝑧" 𝑥"
![Page 18: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/18.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 19: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/19.jpg)
Grid localization
Algorithm Grid_localization ( 𝑝F,";$ , 𝑢", 𝑧",𝑚)for all 𝑘 do:
�̅�F," = ∑E 𝑝E,";$𝒎𝒐𝒕𝒊𝒐𝒏_𝒎𝒐𝒅𝒆𝒍(𝑚𝑒𝑎𝑛 𝑥F , 𝑢",𝑚𝑒𝑎𝑛 𝑥E )𝑝F," = 𝜂 �̅�F,"𝒎𝒆𝒂𝒔𝒖𝒓𝒆𝒎𝒆𝒏𝒕_𝒎𝒐𝒅𝒆𝒍(𝑧",𝑚𝑒𝑎𝑛 𝑥F ,𝑚)
end forreturn 𝑏𝑒𝑙(𝑥")
![Page 20: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/20.jpg)
20
Piecewise Constant Representation
),,( >=< qyxxBel tFixing an input ut we can compute the new belief
![Page 21: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/21.jpg)
Start
Motion Model without measurements
𝑚𝑒𝑎𝑛 𝑥F
![Page 22: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/22.jpg)
Proximity Sensor Model
Laser sensor Sonar sensor
𝑥F
𝑝 𝑧" 𝑋" = 𝑥F)
𝑚
![Page 23: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/23.jpg)
23
Grid localization, 𝑏𝑒𝑙 𝑥" represented by a histogram over grid 𝑝(𝑧|𝑥)
𝑝(𝑧|𝑥)
![Page 24: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/24.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 25: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/25.jpg)
25
Grid-based Localization
![Page 26: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/26.jpg)
29
Sonars and Occupancy Grid Map
![Page 27: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/27.jpg)
Monte Carlo Localization
• Represents beliefs by particles
![Page 28: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/28.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 29: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/29.jpg)
Particle filtering algorithm 𝑋" = 𝑥"
[$], 𝑥"[7], … 𝑥"
[^] particles
Algorithm Particle_filter(𝑋";$, 𝑢", 𝑧"):_𝑋";$ = 𝑋" = ∅for all 𝑚 in [M] do:
sample 𝑥"[a]~𝑝 𝑥" 𝑢", 𝑥";$
[a])
𝑤"[a] = 𝑝 𝑧" 𝑥"
a
_𝑋" = _𝑋" + ⟨ 𝑥"a , 𝑤"
[a]⟩end forfor all 𝑚 in [M] do:
draw 𝑖 𝑤𝑖𝑡ℎ 𝑝𝑟𝑜𝑏𝑎𝑏𝑖𝑙𝑖𝑡𝑦 ∝ 𝑤"[E]
add 𝑥"[E] 𝑡𝑜 𝑋"
end for
return 𝑋"
ideally, 𝑥"[a] is selected with probability prop. to
𝑝 𝑥" 𝑧$:", 𝑢$:")_𝑋";$ is the temporary particle set
// sampling from state transition dist.
// calculates importance factor 𝑤" or weight
// resampling or importance sampling; these are distributed according to 𝜂 𝑝 𝑧" 𝑥"
[a] 𝑏𝑒𝑙 𝑥"// survival of fittest: moves/adds particles to parts of the state space with higher probability
![Page 30: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/30.jpg)
Weight samples: w = f / g
Importance Sampling
suppose we want to compute 𝐸n 𝐼 𝑥 ∈ 𝐴 but we can only sample from density 𝑔
𝐸n 𝐼 𝑥 ∈ 𝐴
= ∫ 𝑓 𝑥 𝐼 𝑥 ∈ 𝐴 𝑑𝑥
= ∫ n rs r
𝑔 𝑥 𝐼 𝑥 ∈ 𝐴 𝑑𝑥, provided 𝑔 𝑥 > 0
= ∫ 𝑤 𝑥 𝑔 𝑥 𝐼 𝑥 ∈ 𝐴 𝑑𝑥= 𝐸s 𝑤(𝑥)𝐼 𝑥 ∈ 𝐴
We need 𝑓 𝑥 > 0 ⇒ 𝑔 𝑥 > 0
![Page 31: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/31.jpg)
Monte Carlo Localization (MCL)𝑋" = 𝑥"
[$], 𝑥"[7], … 𝑥"
[^] particles
Algorithm MCL(𝑋";$, 𝑢", 𝑧",m):_𝑋";$ = 𝑋" = ∅for all 𝑚 in [M] do:
𝑥"[a] = 𝒔𝒂𝒎𝒑𝒍𝒆_𝒎𝒐𝒕𝒊𝒐𝒏_𝒎𝒐𝒅𝒆𝒍(𝑢" 𝑥";$
[a])
𝑤"[a] = 𝒎𝒆𝒂𝒔𝒖𝒓𝒆𝒎𝒆𝒏𝒕_𝒎𝒐𝒅𝒆𝒍(𝑧", 𝑥"
a ,a)_𝑋" = _𝑋" + ⟨ 𝑥"
a , 𝑤"[a]⟩
end for
for all 𝑚 in [M] do:
draw 𝑖 𝑤𝑖𝑡ℎ 𝑝𝑟𝑜𝑏𝑎𝑏𝑖𝑙𝑖𝑡𝑦 ∝ 𝑤"[E]
add 𝑥"[E] 𝑡𝑜 𝑋"
end for
return 𝑋"
Plug in motion and measurement models in the particle filter
![Page 32: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/32.jpg)
Particle Filters
![Page 33: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/33.jpg)
)|()(
)()|()()|()(
xzpxBel
xBelxzpw
xBelxzpxBel
aaa
=¬
¬
-
-
-
Sensor Information: Importance Sampling
![Page 34: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/34.jpg)
The picture can't be displayed.ò¬- 'd)'()'|()( , xxBelxuxpxBel
Robot Motion
![Page 35: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/35.jpg)
)|()(
)()|()()|()(
xzpxBel
xBelxzpw
xBelxzpxBel
aaa
=¬
¬
-
-
-
Sensor Information: Importance Sampling
![Page 36: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/36.jpg)
Robot Motion
ò¬- 'd)'()'|()( , xxBelxuxpxBel
![Page 37: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/37.jpg)
42
![Page 38: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/38.jpg)
43
![Page 39: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/39.jpg)
44
![Page 40: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/40.jpg)
45
![Page 41: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/41.jpg)
46
![Page 42: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/42.jpg)
47
![Page 43: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/43.jpg)
48
![Page 44: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/44.jpg)
49
![Page 45: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/45.jpg)
50
![Page 46: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/46.jpg)
51
![Page 47: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/47.jpg)
52
![Page 48: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/48.jpg)
53
![Page 49: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/49.jpg)
54
![Page 50: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/50.jpg)
55
![Page 51: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/51.jpg)
56
![Page 52: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/52.jpg)
57
![Page 53: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/53.jpg)
58
![Page 54: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/54.jpg)
59
![Page 55: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/55.jpg)
60
Sample-based Localization (sonar)
![Page 56: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/56.jpg)
61
Initial Distribution
![Page 57: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/57.jpg)
62
After Incorporating Ten Ultrasound Scans
![Page 58: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/58.jpg)
63
After Incorporating 65 Ultrasound Scans
![Page 59: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/59.jpg)
64
Estimated Path
![Page 60: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/60.jpg)
Using Ceiling Maps for Localization
![Page 61: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/61.jpg)
Vision-based Localization
P(z|x)
h(x)z
![Page 62: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/62.jpg)
Under a LightMeasurement z: P(z|x):
![Page 63: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/63.jpg)
Next to a LightMeasurement z: P(z|x):
![Page 64: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/64.jpg)
ElsewhereMeasurement z: P(z|x):
![Page 65: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/65.jpg)
Global Localization Using Vision
![Page 66: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/66.jpg)
71
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 67: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/67.jpg)
72
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 𝑝(𝑧"|𝑧$:";$, 𝑢$:", 𝑚)
• For particle filters: 𝑝(𝑧"|𝑧$:";$, 𝑢$:", 𝑚) ≈$^∑𝑤"
[a]
• 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 68: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/68.jpg)
73
Random SamplesVision-Based Localization936 Images, 4MB, .6secs/imageTrajectory of the robot:
![Page 69: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/69.jpg)
74
Kidnapping the Robot
![Page 70: Principles of Safe Autonomy: Lecture 12-13: Filtering and Robot …publish.illinois.edu/safe-autonomy/files/2020/03/2020... · 2020. 3. 11. · Filtering and Robot Localization Sayan](https://reader035.fdocuments.us/reader035/viewer/2022063009/5fc0f7699331c24c8d31a187/html5/thumbnails/70.jpg)
80
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.