Bayesian Filtering Dieter Fox CSE490 DF Robotics Capstone

Bayesian Filtering Dieter Fox CSE-490 DF Robotics Capstone

Probabilistic Robotics Key idea: Explicit representation of uncertainty (using the calculus of probability theory) • Perception = state estimation • Control = utility optimization 1/20/2022 CSE-490 DF Robotics Capstone 2

Bayes Filters: Framework • Given: • Stream of observations z and action data u: • Sensor model P(z|x). • Action model P(x|u, x’). • Prior probability of the system state P(x). • Wanted: • Estimate of the state X of a dynamical system. • The posterior of the state is also called Belief: 1/20/2022 CSE-490 DF Robotics Capstone 3

Markov Assumption Underlying Assumptions • Static world • Independent noise • Perfect model, no approximation errors 1/20/2022 CSE-490 DF Robotics Capstone 4

Bayes Filters z = observation u = action x = state Bayes Markov Total prob. Markov 1/20/2022 CSE-490 DF Robotics Capstone 5

Bayes Filters are Familiar! • Kalman filters • Particle filters • Hidden Markov models • Dynamic Bayesian networks • Partially Observable Markov Decision Processes (POMDPs) 1/20/2022 CSE-490 DF Robotics Capstone 6

Localization “Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities. ” [Cox ’ 91] • Given • Map of the environment. • Sequence of sensor measurements. • Wanted • Estimate of the robot’s position. • Problem classes • Position tracking • Global localization • Kidnapped robot problem (recovery) 1/20/2022 CSE-490 DF Robotics Capstone 7

Bayes Filters for Robot Localization 1/20/2022 CSE-490 DF Robotics Capstone 8

Probabilistic Kinematics • Odometry information is inherently noisy. p(x|u, x’) x’ u 1/20/2022 CSE-490 DF Robotics Capstone 9

Proximity Measurement • Measurement can be caused by … • • a known obstacle. cross-talk. an unexpected obstacle (people, furniture, …). missing all obstacles (total reflection, glass, …). • Noise is due to uncertainty … • • 1/20/2022 in measuring distance to known obstacle. in position of known obstacles. in position of additional obstacles. whether obstacle is missed. CSE-490 DF Robotics Capstone 10

Mixture Density How can we determine the model parameters? 1/20/2022 CSE-490 DF Robotics Capstone 11

Raw Sensor Data Measured distances for expected distance of 300 cm. Sonar 1/20/2022 Laser CSE-490 DF Robotics Capstone 12

Approximation Results Laser Sonar 300 cm 1/20/2022 400 cm CSE-490 DF Robotics Capstone 13

Representations for Bayesian Robot Localization Kalman filters (late-80 s? ) Discrete approaches (’ 95) • Topological representation (’ 95) • uncertainty handling (POMDPs) • occas. global localization, recovery • Grid-based, metric representation (’ 96) • global localization, recovery AI Particle filters (’ 99) • sample-based representation • global localization, recovery 1/20/2022 • Gaussians • approximately linear models • position tracking Robotics Multi-hypothesis (’ 00) • multiple Kalman filters • global localization, recovery CSE-490 DF Robotics Capstone 14

Discrete Grid Filters 1/20/2022 CSE-490 DF Robotics Capstone 15

Piecewise Constant Representation 1/20/2022 CSE-490 DF Robotics Capstone 16

Grid-based Localization 1/20/2022 CSE-490 DF Robotics Capstone 17

Sonars and Occupancy Grid Map 1/20/2022 CSE-490 DF Robotics Capstone 18

Tree-based Representation Idea: Represent density using a variant of Octrees 1/20/2022 CSE-490 DF Robotics Capstone 19

Tree-based Representations • Efficient in space and time • Multi-resolution 1/20/2022 CSE-490 DF Robotics Capstone 20

Particle Filters 1/20/2022 CSE-490 DF Robotics Capstone 21

Particle Filters § § Represent belief by random samples § Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter, Particle filter § § § Filtering: [Rubin, 88], [Gordon et al. , 93], [Kitagawa 96] Estimation of non-Gaussian, nonlinear processes Computer vision: [Isard and Blake 96, 98] Dynamic Bayesian Networks: [Kanazawa et al. , 95]d 1/20/2022 CSE-490 DF Robotics Capstone 22

Importance Sampling Weight samples: w = f / g 1/20/2022 CSE-490 DF Robotics Capstone 23

Particle Filter Algorithm draw xit-1 from Bel(xt-1) draw xit from p(xt | xit-1, ut-1) Importance factor for xit: 1/20/2022 CSE-490 DF Robotics Capstone 24

Particle Filters 1/20/2022 CSE-490 DF Robotics Capstone 25

Sensor Information: Importance Sampling 1/20/2022 CSE-490 DF Robotics Capstone 26

Robot Motion 1/20/2022 CSE-490 DF Robotics Capstone 27

Sensor Information: Importance Sampling 1/20/2022 CSE-490 DF Robotics Capstone 28

Robot Motion 1/20/2022 CSE-490 DF Robotics Capstone 29

Sample-based Localization (sonar) 1/20/2022 CSE-490 DF Robotics Capstone 30

Using Ceiling Maps for Localization 1/20/2022 CSE-490 DF Robotics Capstone [Dellaert et al. 31 99]

Vision-based Localization P(z|x) z h(x) 1/20/2022 CSE-490 DF Robotics Capstone 32

Under a Light Measurement z: 1/20/2022 P(z|x): CSE-490 DF Robotics Capstone 33

Next to a Light Measurement z: 1/20/2022 P(z|x): CSE-490 DF Robotics Capstone 34

Elsewhere Measurement z: 1/20/2022 P(z|x): CSE-490 DF Robotics Capstone 35

Global Localization Using Vision 1/20/2022 CSE-490 DF Robotics Capstone 36

Localization for AIBO robots 1/20/2022 CSE-490 DF Robotics Capstone 37

Adaptive Sampling 1/20/2022 CSE-490 DF Robotics Capstone 38

KLD-sampling • Idea: • Assume we know the true belief. • Represent this belief as a multinomial distribution. • Determine number of samples such that we can guarantee that, with probability (1 - d), the KL-distance between the true posterior and the sample-based approximation is less than e. • Observation: • For fixed d and e, number of samples only depends on number k of bins with support: 1/20/2022 CSE-490 DF Robotics Capstone 39

Example Run Sonar 1/20/2022 CSE-490 DF Robotics Capstone 40

Example Run Laser 1/20/2022 CSE-490 DF Robotics Capstone 41

Kalman Filters 1/20/2022 CSE-490 DF Robotics Capstone 42

Bayes Filter Reminder • Prediction • Correction 1/20/2022 CSE-490 DF Robotics Capstone 43

Gaussians m Univariate -s s m Multivariate 1/20/2022 CSE-490 DF Robotics Capstone 44

Gaussians and Linear Functions 1/20/2022 CSE-490 DF Robotics Capstone 45

Kalman Filter Updates in 1 D 1/20/2022 CSE-490 DF Robotics Capstone 46

Kalman Filter Algorithm 1. Algorithm Kalman_filter( mt-1, St-1, ut, zt): 2. 3. 4. Prediction: 5. 6. 7. 8. Correction: 9. Return mt, St 1/20/2022 CSE-490 DF Robotics Capstone 47

Nonlinear Dynamic Systems • Most realistic robotic problems involve nonlinear functions 1/20/2022 CSE-490 DF Robotics Capstone 48

Linearity Assumption Revisited 1/20/2022 CSE-490 DF Robotics Capstone 49

Non-linear Function 1/20/2022 CSE-490 DF Robotics Capstone 50

EKF Linearization (1) 1/20/2022 CSE-490 DF Robotics Capstone 51

EKF Linearization (2) 1/20/2022 CSE-490 DF Robotics Capstone 52

EKF Linearization (3) 1/20/2022 CSE-490 DF Robotics Capstone 53

Particle Filter Projection 1/20/2022 CSE-490 DF Robotics Capstone 54

Density Extraction 1/20/2022 CSE-490 DF Robotics Capstone 55

Sampling Variance 1/20/2022 CSE-490 DF Robotics Capstone 56

EKF Algorithm 1. Extended_Kalman_filter( mt-1, St-1, ut, zt): 2. 3. 4. Prediction: 5. 6. 7. 8. Correction: 9. Return mt, St 1/20/2022 CSE-490 DF Robotics Capstone 57

Landmark-based Localization 1/20/2022 CSE-490 DF Robotics Capstone 58

EKF Prediction Step 1/20/2022 CSE-490 DF Robotics Capstone 59

EKF Observation Prediction Step 1/20/2022 CSE-490 DF Robotics Capstone 60

EKF Correction Step 1/20/2022 CSE-490 DF Robotics Capstone 61

Estimation Sequence (1) 1/20/2022 CSE-490 DF Robotics Capstone 62

Estimation Sequence (2) 1/20/2022 CSE-490 DF Robotics Capstone 63

Comparison to Ground. Truth 1/20/2022 CSE-490 DF Robotics Capstone 64

EKF Summary • Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k 2. 376 + n 2) • Not optimal! • Can diverge if nonlinearities are large! • Works surprisingly well even when all assumptions are violated! 1/20/2022 CSE-490 DF Robotics Capstone 65

Linearization via Unscented Transform EKF 1/20/2022 UKF CSE-490 DF Robotics Capstone 66

UKF Sigma-Point Estimate (2) EKF 1/20/2022 UKF CSE-490 DF Robotics Capstone 67

UKF Sigma-Point Estimate (3) EKF 1/20/2022 UKF CSE-490 DF Robotics Capstone 68

Unscented Transform Sigma points Weights Pass sigma points through nonlinear function Recover mean and covariance 1/20/2022 CSE-490 DF Robotics Capstone 69

UKF Prediction Step 1/20/2022 CSE-490 DF Robotics Capstone 70

UKF Observation Prediction Step 1/20/2022 CSE-490 DF Robotics Capstone 71

UKF Correction Step 1/20/2022 CSE-490 DF Robotics Capstone 72

EKF Correction Step 1/20/2022 CSE-490 DF Robotics Capstone 73

Estimation Sequence EKF 1/20/2022 PF CSE-490 DF Robotics Capstone UKF 74

Estimation Sequence EKF 1/20/2022 UKF CSE-490 DF Robotics Capstone 75

Prediction Quality EKF 1/20/2022 UKF CSE-490 DF Robotics Capstone 76

UKF Summary • Highly efficient: Same complexity as EKF, with a constant factor slower in typical practical applications • Better linearization than EKF: Accurate in first two terms of Taylor expansion (EKF only first term) • Derivative-free: No Jacobians needed • Still not optimal! 1/20/2022 CSE-490 DF Robotics Capstone 77

SLAM: Simultaneous Localization and Mapping CSE-490 DF Robotics Capstone

Mapping with Raw Odometry 1/20/2022 CSE-490 DF Robotics Capstone 79

SLAM: Simultaneous Localization and Mapping • Full SLAM: • Online SLAM: Integrations typically done at a time 1/20/2022 CSE-490 DF Robotics Capstone 80

SLAM: Mapping with Kalman Filters • Map with N landmarks: (2 N+3)-dimensional Gaussian • Can handle hundreds of dimensions 1/20/2022 CSE-490 DF Robotics Capstone 81

SLAM: Mapping with Kalman Filters 1/20/2022 CSE-490 DF Robotics Capstone 82

SLAM: Mapping with Kalman Filters 1/20/2022 CSE-490 DF Robotics Capstone 83

SLAM: Mapping with Kalman Filters Map 1/20/2022 Correlation matrix CSE-490 DF Robotics Capstone 84

Graph-SLAM • Full SLAM technique • Generates probabilistic links • Computes map only occasionally • Based on Information Filter form 1/20/2022 CSE-490 DF Robotics Capstone 85

Graph-SLAM Idea 1/20/2022 CSE-490 DF Robotics Capstone 86

Robot Poses and Scans [Lu and Milios 1997] • Successive robot • • poses connected by odometry Sensor readings yield constraints between poses Constraints represented by Gaussians • Globally optimal estimate 1/20/2022 CSE-490 DF Robotics Capstone 87

Loop Closure • Use scan patches to detect loop closure • Add new position constraints • Deform the network based on covariances of matches 1/20/2022 Before loop closure After loop closure CSE-490 DF Robotics Capstone 88

Efficient Map Recovery • Minimize constraint function JGraph. SLAM using standard optimization techniques (gradient descent, Levenberg Marquardt, conjugate gradient) 1/20/2022 CSE-490 DF Robotics Capstone 89

Mapping the Allen Center 1/20/2022 CSE-490 DF Robotics Capstone 90

Rao-Blackwellised Particle Filters 1/20/2022 CSE-490 DF Robotics Capstone 91

Rao-Blackwellized Mapping Compute a posterior over the map and possible trajectories of the robot : map and trajectory measurements map 1/20/2022 robot motion CSE-490 DF Robotics Capstone trajectory 92

Fast. SLAM Robot Pose 2 x 2 Kalman Filters x, y, Landmark 1 Landmark 2 … Landmark N Particle #2 x, y, Landmark 1 Landmark 2 … Landmark N Particle #3 x, y, Landmark 1 Landmark 2 … Landmark N … Particle #1 Particle M 1/20/2022 CSE-490 DF Robotics Capstone 93 [Begin courtesy of Mike Montemerlo]

Fast. SLAM – Simulation • Up to 100, 000 landmarks • 100 particles • 103 times fewer parameters than EKF SLAM 1/20/2022 Blue line = true robot path Red line = estimated robot path CSE-490 DF Robotics Capstone Black dashed line = odometry 94

Victoria Park Results • 4 km traverse • 100 particles • Uses negative evidence to remove spurious landmarks Blue path = odometry Red path = estimated path 1/20/2022 CSE-490 DF Robotics Capstone 95 [End courtesy of Mike Montemerlo]

Motion Model for Scan Matching Raw Odometry Scan Matching 1/20/2022 CSE-490 DF Robotics Capstone 96

Loop Closure 1/20/2022 CSE-490 DF Robotics Capstone Map: Intel Research Lab Seattle Rao-Blackwellized Mapping with Scan-Matching 97

Loop Closure 1/20/2022 CSE-490 DF Robotics Capstone Map: Intel Research Lab Seattle Rao-Blackwellized Mapping with Scan-Matching 98

1/20/2022 CSE-490 DF Robotics Capstone Map: Intel Research Lab Seattle Rao-Blackwellized Mapping with Scan-Matching 99

Example (Intel Lab) § 15 particles § four times faster than real-time P 4, 2. 8 GHz § 5 cm resolution during scan matching § 1 cm resolution in final map 1/20/2022 CSE-490 DF Robotics Capstone 100 joint work with Giorgio Grisetti

Outdoor Campus Map § 30 particles § 250 x 250 m 2 § 1. 75 1. 088 km miles (odometry) § 20 cm resolution during scan matching § 30 cm resolution in final map 1/20/2022 CSE-490 DF Robotics Capstone 101 joint work with Giorgio Grisetti

DP-SLAM [Eliazar & Parr] scale: 3 cm CSE-490 DF Robotics Capstone Runs at real-time speed on 2. 4 GHz Pentium 4 at 10 cm/s 1/20/2022 102

Consistency 1/20/2022 CSE-490 DF Robotics Capstone 103

Results obtained with DP-SLAM 2. 0 (offline) 1/20/2022 CSE-490 DF Robotics Capstone Eliazar & Parr, 104 04

Close up 1/20/2022 CSE-490 DF Robotics Capstone 105 End courtesy of Eliazar & Parr

Fast-SLAM Summary • Full and online version of SLAM • Factorizes posterior into robot trajectories (particles) and map (EKFs). • Landmark locations are independent! • More efficient proposal distribution through Kalman filter prediction • Data association per particle 1/20/2022 CSE-490 DF Robotics Capstone 106

Ball Tracking in Robo. Cup § Extremely noisy (nonlinear) motion of observer § Inaccurate sensing, limited processing power § Interactions between target and environment Goal: Unified framework for modeling the ball and its interactions. § Interactions between robot(s) and target 1/20/2022 CSE-490 DF Robotics Capstone 107

Tracking Techniques § Kalman Filter § Highly efficient, robust (even for nonlinear) § Uni-modal, limited handling of nonlinearities § Particle Filter § Less efficient, highly robust § Multi-modal, nonlinear, non-Gaussian § Rao-Blackwellised Particle Filter, MHT § Combines PF with KF § Multi-modal, highly efficient 1/20/2022 CSE-490 DF Robotics Capstone 108

l zk-1 z lk Landmark detection r r r Map and robot location k-2 k-1 u k-2 1/20/2022 k u k-1 Robot control mk-2 mk-1 mk Ball motion mode bk-2 bk-1 bk Ball location and velocity b z k-2 b zk-1 z bk Ball tracking l z k-2 Robot localization Dynamic Bayes Network for Ball Tracking Ball observation CSE-490 DF Robotics Capstone 109

l zk-1 z lk Landmark detection r r r Map and robot location k-2 k-1 u k-2 1/20/2022 k u k-1 Robot control mk-2 mk-1 mk Ball motion mode bk-2 bk-1 bk Ball location and velocity b z k-2 b zk-1 z bk Ball tracking l z k-2 Robot localization Robot Location Ball observation CSE-490 DF Robotics Capstone 110

l zk-1 z lk Landmark detection r r r Map and robot location k-2 k-1 u k-2 1/20/2022 k u k-1 Robot control mk-2 mk-1 mk Ball motion mode bk-2 bk-1 bk Ball location and velocity b z k-2 b zk-1 z bk Ball tracking l z k-2 Robot localization Robot and Ball Location (and velocity) Ball observation CSE-490 DF Robotics Capstone 111

Ball-Environment Interactions None Bounced 1/20/2022 Grabbed Deflected CSE-490 DF Robotics Capstone Kicked 112

Ball-Environment Interactions (0. 8) Robot loses grab (0. 2) (residual prob. ) Bounced 1/20/2022 ) (1. 0 ) (0. 1 Within grab range and robot grabs (prob. from model ) (1. 0) Deflected CSE-490 DF Robotics Capstone Grabbed Kick fails (0. 1) Robot kicks ball (0. 9) Co llis i o on on bjec with ma ts p( 1. 0 ) (1. 0) None Kicked 113

l zk-1 z lk Landmark detection r r r Map and robot location k-2 k-1 u k-2 1/20/2022 k u k-1 Robot control mk-2 mk-1 mk Ball motion mode bk-2 bk-1 bk Ball location and velocity b z k-2 b zk-1 z bk Ball tracking l z k-2 Robot localization Integrating Discrete Ball Motion Mode Ball observation CSE-490 DF Robotics Capstone 114

l zk-1 z lk Landmark detection r r r Map and robot location k-2 k-1 u k-2 1/20/2022 k u k-1 Robot control mk-2 mk-1 mk Ball motion mode bk-2 bk-1 bk Ball location and velocity b z k-2 b zk-1 z bk Ball tracking l z k-2 Robot localization Grab Example (1) Ball observation CSE-490 DF Robotics Capstone 115

l zk-1 z lk Landmark detection r r r Map and robot location k-2 k-1 u k-2 1/20/2022 k u k-1 Robot control mk-2 mk-1 mk Ball motion mode bk-2 bk-1 bk Ball location and velocity b z k-2 b zk-1 z bk Ball tracking l z k-2 Robot localization Grab Example (2) Ball observation CSE-490 DF Robotics Capstone 116

l zk-1 z lk Landmark detection r r r Map and robot location k-2 k-1 u k-2 1/20/2022 k u k-1 Robot control mk-2 mk-1 mk Ball motion mode bk-2 bk-1 bk Ball location and velocity b z k-2 b zk-1 z bk Ball tracking l z k-2 Robot localization Inference: Posterior Estimation Ball observation CSE-490 DF Robotics Capstone 117

Rao-Blackwellised PF for Inference § § Represent posterior by random samples Each sample contains robot location, ball mode, ball Kalman filter § Generate individual components of a particle stepwise using the factorization 1/20/2022 CSE-490 DF Robotics Capstone 118

§ 1/20/2022 k-1 r k Map and robot location mk-1 mk Ball motion mode bk-1 bk Ball location and velocity Ball tracking r Robot localization Rao-Blackwellised Particle Filter for Inference Draw a sample from the previous sample set: CSE-490 DF Robotics Capstone 119

k-1 Landmark detection r Map and robot location k u k-1 1/20/2022 Robot control mk-1 mk Ball motion mode bk-1 bk Ball location and velocity CSE-490 DF Robotics Capstone Ball tracking r z lk Robot localization Generate Robot Location 120

k-1 Landmark detection r Map and robot location k u k-1 1/20/2022 Robot control mk-1 mk Ball motion mode bk-1 bk Ball location and velocity CSE-490 DF Robotics Capstone Ball tracking r z lk Robot localization Generate Ball Motion Model 121

k-1 Landmark detection r Map and robot location k u k-1 Robot control mk-1 mk Ball motion mode bk-1 bk Ball location and velocity Ball tracking r z lk Robot localization Update Ball Location and Velocity z bk 1/20/2022 CSE-490 DF Robotics Capstone 122

Importance Resampling § Weight sample by if observation is landmark detection and by if observation is ball detection. § Resample 1/20/2022 CSE-490 DF Robotics Capstone 123

Ball-Environment Interaction 1/20/2022 CSE-490 DF Robotics Capstone 124

Ball-Environment Interaction 1/20/2022 CSE-490 DF Robotics Capstone 125

Tracking and Finding the Ball § Cluster ball samples by discretizing § 1/20/2022 pan / tilt angles Uses negative information CSE-490 DF Robotics Capstone 126

Experiment: Real Robot § Robot kicks ball 100 times, tries to find it § afterwards Finds ball 0. 5 in 1. 5 seconds on average With Map Without Map Percentage of ball lost 0. 45 0. 4 0. 35 0. 3 0. 25 0. 2 0. 15 0. 1 0. 05 0 0 1/20/2022 5 10 15 20 25 30 35 40 45 50 Number of ball samples CSE-490 DF Robotics Capstone 127

Simulation Runs * 1/20/2022 CSE-490 DF Robotics Capstone Reference Observations 128

Comparison to KF* (optimized for straight motion) * 1/20/2022 CSE-490 DF Robotics Capstone RBPF KF* Reference Observations 129

Comparison to KF’ (inflated prediction noise) * 1/20/2022 CSE-490 DF Robotics Capstone RBPF KF’ Reference Observations 130

Orientation Errors Orientation Error [degrees] 180 RBPF KF* KF' 160 140 120 100 80 60 40 20 0 1/20/2022 2 3 4 5 6 7 Time [sec] 8 9 CSE-490 DF Robotics Capstone 10 11 131

Conclusions § Bayesian filters are the most successful technique in robotics (vision? ) § Many instances (Kalman, particle, grid, MHT, RBPF, …) § Special case of dynamic Bayesian networks § Recently: hierarchical models 1/20/2022 CSE-490 DF Robotics Capstone 132