it d tiintroduction to ebddde mbedded sts ystems - chessstatistics background (cont.) • theorem of...
TRANSCRIPT
![Page 1: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/1.jpg)
4/22/2008
1
I t d ti t E b dd d S tIntroduction to Embedded SystemsEECS 124, UC Berkeley, Spring 2008Lecture 23: Localization and Mapping
Gabe HoffmannGabe HoffmannPh.D. Candidate, Aero/Astro EngineeringStanford University
Overview
• Statistical Models
• Localization
• Occupancy Grid Mapping
• Simultaneous Localization and Mapping (SLAM)
This lecture draws material fromS. Thrun, W. Burgard, & D. Fox, Probabilistic Robotics, 2005
![Page 2: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/2.jpg)
4/22/2008
2
Statistics Background
• Conditional Probability
• Bayes’ Rule
• Independence of two variables
Conditional Independence
Statistics Background (cont.)• Theorem of Total Probability
• Gaussian Probability Distributions
-5
0
5
-5
0
50
0.05
0.1
x1
x2
![Page 3: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/3.jpg)
4/22/2008
3
Markov Assumption
• Future and past independent given present
Motion Model
• Stochastic model of future state:
![Page 4: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/4.jpg)
4/22/2008
4
• Velocity input model
Two Wheeled Robots
• Odometry “input” model
NoiseCommand
NoiseMeasurement
Two Wheeled Robots (cont.)
• Next state
• Noise model
![Page 5: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/5.jpg)
4/22/2008
5
(Forward) Measurement Model
• Stochastic model of measurements:
Sensor Examples
• Bump Bearing Sensor Model
• GPS
• Camera
• Directional Antenna
• Ultrasonic Ranger
• LIDAR
![Page 6: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/6.jpg)
4/22/2008
6
Forward vs. Inverse Sensor Models
• Forward ModelC f
Forward Measurement Model
– Common for localization
• Inverse Model– Useful when state less complicated than InverseMeasurement Model
actual range
complicated than measurement
– Used in binary occupancy grids
Inverse Measurement Model
Which way did we go? Error Adds Up
Start
![Page 7: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/7.jpg)
4/22/2008
7
Localization• Recursive state estimation
– General: Bayes FilterHistogram (Grid Cell) Filter– Histogram (Grid Cell) Filter
– Particle Filter– Gaussian (Kalman) Filters
Bayes Filter
• Start with a prior distribution:
• Make an observation:
• Compute posterior distribution:
• Posterior distribution becomes the prior
• Iterate
![Page 8: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/8.jpg)
4/22/2008
8
• For all possible states, iteratively compute
Histogram Filter (Discrete Bayes)
• Large computational expense• Allows nonlinear non‐Gaussian modelsAllows nonlinear, non‐Gaussian models• Implementation issues
– Grid resolution (computationally intense)– Measurement independence– Volume of information– Overconfidence vs. information loss
Histogram Localization
Robot’s Position
Robot’s Position
Robot’s Position
![Page 9: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/9.jpg)
4/22/2008
9
Particle Filter (Monte Carlo)
• Permits nonlinear & non‐Gaussian models
… at a large computational expensiveParticle Set
Correction(weighting)
Resampling
Prediction
Monte Carlo Localization (MCL)
• Benefits over grid– Continuous state
t tirepresentation• Implementation
issues– “Kidnapped
robot”, deprivation
– Measurement independence
– Volume of information
– Overconfidence vs. information loss
![Page 10: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/10.jpg)
4/22/2008
10
Kalman Filter
• Linear system
• Algorithm (recursive least‐squares solution!)
• Nonlinear system
Extended Kalman Filter
• Linear approximation
Jacobians
![Page 11: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/11.jpg)
4/22/2008
11
Extended Kalman Filter (cont.)
• Approximate least squares solution
• Covariance estimate can be erroneous
• Basis for most modern localization systems
• Algorithm (very analogous to Kalman filter)
Other Localization Algorithms
• Unscented Kalman Filter (Sigma Point)– “Linearization‐free”
– Improved estimate of covariance (usually!)
• Information Filter (equivalent to KF)– Beneficial when more measurements than states
– Basis for distributed optimization– Basis for distributed optimization
Information Matrix Information State
![Page 12: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/12.jpg)
4/22/2008
12
• Two wheeled robot
Linearization Example
– Motion model Jacobian
– Control input noise
wherewhere
Linearization Example (cont.)Bearing Sensor Model
Sensor Jacobian
June 6, 2007 Hybrid Systems Lab Group Meeting 24
![Page 13: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/13.jpg)
4/22/2008
13
GPS + Inertial Navigation
• SensorsGPS (l ltit d )– GPS (lower altitude accuracy)
– Accelerometer– Rate Gyroscope– Magnetometer
• Implementation– Outlier rejection– Observability requires motion– Sensor drift
Occupancy Grid Mapping
• Estimate the map using measurements, assume pose is known.pose is known.
![Page 14: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/14.jpg)
4/22/2008
14
Binary Bayes Filter
• Estimate log odds ratio of a binary variable
• Uses inverse measurement model
• Assumes a static state
• The update for each variable is
• Log Odds form
Occupancy Grid with Log Odds
• Iteratively apply to all map points in range:
• Good numerical stability, a standard method
Inverse Sensor Model
Prior Occupancy Odds:
![Page 15: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/15.jpg)
4/22/2008
15
Related: DARPA Grand Challenge
• Estimated drivabilitydrivability (flatness)
• Used additional information– TimeS ib i– Sensor vibration
• Tuned automatically
Simultaneous Localization and Mapping
• EKF SLAMF t b d t i
Posterior Distribution SLAM
– Feature based posterior distribution estimation
• Graph SLAM– Feature based full trajectory estimation
• Fast SLAMFull SLAM
• Fast SLAM– Uses Monte Carlo methods
![Page 16: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/16.jpg)
4/22/2008
16
• Standard implementation tracks features
EKF SLAM
• Challenges– Data association
– Divergence
– Loop closure
– Matrix inversion– Matrix inversion
Graph SLAM
• Solves full SLAM using the information form
• Stores information links, linear storage costs
0
0 10 20 30 40 50 60 70 80 90 100
10
20
30
40
50
60
70
80
90
100
Information Matrix
![Page 17: It d tiIntroduction to EbdddE mbedded StS ystems - ChessStatistics Background (cont.) • Theorem of Total Probability • Gaussian Probability Distributions-5 0 5-5 0 5 0 0.05 0.1](https://reader034.vdocument.in/reader034/viewer/2022050221/5f67113d92e42212f002fa95/html5/thumbnails/17.jpg)
4/22/2008
17
Fast SLAM
• Solves full SLAM problem
• Each particle is a trajectory
• Uses EKF feature tracking or occupancy grid cell
Summary
• Motion and Measurement Models– Statistical models approximate the systempp y
• Localization– Enables estimation of current state– Histogram, Particle Filter, EKF (UKF, EIF, etc.)
• Occupancy Grid Mapping– Enables awareness of surroundings– Used for navigation g
• SLAM– Matches features to enable awareness of location– Used for localization and mapping where global reference is unreliable/unavailable