It d tiIntroduction to EbdddE mbedded StS ystems · Statistics Background (cont.) • Theorem of...
Transcript of It d tiIntroduction to EbdddE mbedded StS ystems · Statistics Background (cont.) • Theorem of...
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
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
4/22/2008
3
Markov Assumption
• Future and past independent given present
Motion Model
• Stochastic model of future state:
4/22/2008
4
• Velocity input model
Two Wheeled Robots
• Odometry “input” model
NoiseCommand
NoiseMeasurement
Two Wheeled Robots (cont.)
• Next state
• Noise model
4/22/2008
5
(Forward) Measurement Model
• Stochastic model of measurements:
Sensor Examples
• Bump Bearing Sensor Model
• GPS
• Camera
• Directional Antenna
• Ultrasonic Ranger
• LIDAR
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
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
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
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
4/22/2008
10
Kalman Filter
• Linear system
• Algorithm (recursive least‐squares solution!)
• Nonlinear system
Extended Kalman Filter
• Linear approximation
Jacobians
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
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
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.
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:
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
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
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