Announcements

 Final project: 45% of the grade, 10% presentation, 35% write-up CS 287: Advanced Robotics

 Presentations: in lecture Dec 1 and 3 Fall 2009

 If you have constraints, inform me by email by Wednesday night, we will assign the others at random on Thursday Lecture 23: HMMs: Kalman filters, particle filters  PS2: due Friday 23:59pm.

 Tuesday 4-5pm 540 Cory: Hadas Kress-Gazit (Cornell) Pieter Abbeel High-level tasks to correct low-level robot control UC Berkeley EECS In this talk I will present a formal approach to creating robot controllers that ensure the robot satisfies a given high level task. I will describe a framework in which a user specifies a complex and reactive task in Structured English. This task is then automatically translated, using temporal logic and tools from the formal methods world, into a hybrid controller. This controller is guaranteed to control the robot such that its motion and actions satisfy the intended task, in a variety of different environments.

Hidden Markov Models Filtering in HMM

Joint distribution is assumed to be of the form:  Init P(x 1) [e.g., uniformly]

P(X 1 = x 1) P(Z 1 = z 1 | X 1 = x 1 ) P(X 2 = x 2 | X 1 = x 1) P(Z 2 = z 2 | X 2 = x 2 ) …  Observation update for time 0: P(X T = xT | X T-1 = x T-1) P(Z T = zT | X T = xT)

 For t = 1, 2, …

 Time update X1 X2 X3 X4 XT

 Observation update Z1 Z2 Z3 Z4 ZT

 For discrete state / observation spaces: simply replace integral by summation

Discrete-time Kalman Filter Algorithm

Estimates the state x of a discrete-time controlled process Algorithm Kalman_filter ( µt-1, Σt-1, ut, zt): that is governed by the linear stochastic difference Prediction: equation

µ t = At µt−1 + Btut T xt = At xt −1 + Btut + ε t Σt = AtΣt−1 At + Rt

Correction: with a measurement T T −1 Kt = ΣtCt (Ct ΣtCt + Qt )

µt = µ t + Kt (zt −Ct µ t ) zt = Ct xt +δt Σt = (I − KtCt )Σt

Return µt, Σt 5 6

Page 1 Nonlinear systems Extended Kalman filter (EKF)

xt = g(ut , xt−1) + noise

zt = h(xt ) + noise

EKF Linearization: First Order Taylor EKF Algorithm Series Expansion

 Prediction: Extended_Kalman_filter ( µt-1, Σt-1, ut, zt): Prediction:

∂g(ut , µt−1) g(ut , xt−1) ≈ g(ut , µt−1) + (xt−1 − µt−1) µt = g(ut , µt−1) µ t = At µt−1 + Btut ∂xt−1 T T Σt = Gt Σt−1Gt + Rt Σt = AtΣt−1 At + Rt g(ut , xt−1) ≈ g(ut , µt−1) + Gt (xt−1 − µt−1)

 Correction: Correction:

T T −1 T T −1 Kt = Σt H t (Ht Σt H t + Qt ) Kt = ΣtCt (Ct ΣtCt + Qt ) ∂h(µt ) h(xt ) ≈ h(µt ) + (xt − µt ) µt = µt + Kt (zt − h(µt )) µt = µ t + Kt (zt − Ct µ t ) ∂xt Σt = (I − Kt Ht )Σt Σt = (I − KtCt )Σt h(xt ) ≈ h(µt ) + H t (xt − µt )

∂h(µ ) ∂g(u , µ ) H t G = t t−1 Return µt, Σt t = t ∂xt ∂xt −1

Linearization via Unscented Transform UKF Sigma-Point Estimate (4)

EKF UKF

11

Page 2 UKF intuition why it can perform better UKF intuition why it can perform better

 Assume we know the distribution over X and it has a mean \bar{x}  Assume

 Y = f(X)  1. We represent our distribution over x by a set of sample points.

 2. We propagate the points directly through the function f.

 Then:

 We don’t have any errors in f !!

 The accuracy we obtain can be related to how well the first, second, third, … moments of the samples correspond to the first, second, third, … moments of the true distribution over x.

[Julier and Uhlmann, 1997]

Self-quiz Original unscented transform

st nd  When would the UKF significantly outperform the EKF?  Picks a minimal set of sample points that match 1 , 2 and 3 rd moments of a Gaussian:

n  \bar{x} = mean, Pxx = , i  i’th row, x ∈ ℜ

 \kappa : extra degree of freedom to fine-tune the higher order moments of the approximation; when x is Gaussian, n+\kappa = 3 is a suggested heuristic

[Julier and Uhlmann, 1997]

Algorithm Unscented Kalman filter(t 1, Σt 1, ut, zt): − −

Unscented Kalman filter 1. Xt 1 = t 1 t 1 + γ Σt 1 t 1 − γ Σt 1 − − − − − − 2. Xt∗ = g(t, Xt 1)    −  Dynamics update: 2n [i]X [i] 3. t = i=0 wm t∗

  2n [i] X [i] − X [i] − Can simply use unscented transform and estimate 4. Σt = i=0 wc ( t∗ t)( t∗ t)⊤ + Rt the mean and variance at the next time from the 5. X =  + γ Σ − γ Σ sample points t t t t t t     6. Zt = h(Xt)  Observation update: 2n [i]Z[i] 7. zˆt = i=0 wm t  Use sigmapoints from unscented transform to  2n [i] Z[i] − Z[i] − ⊤ 8. St = i=0 wc t zˆt t zˆt + Qt compute the covariance matrix between xt and zt.      Then can do the standard update. x,z 2n [i] X[i] − Z˜[i] − ⊤ 9. Σt = i=0 wc t t t zˆt     x,z 1 10. Kt = Σt St−

11. t = t + Kt(zt − zˆt) 12. Σt = Σt − KtStKt⊤

13. return t, Σt [Table 3.4 in Probabilistic Robotics]

Page 3 UKF Summary Particle filters motivation

 Particle filters are a way to efficiently represent  Highly efficient : Same complexity as EKF, with a constant factor slower in typical practical applications non-Gaussian distribution

 Better linearization than EKF : Accurate in first two terms of Taylor expansion (EKF only first term) + capturing more  Basic principle aspects of the higher order terms  Set of state hypotheses (“particles”)  Derivative-free : No Jacobians needed  Survival-of-the-fittest

 Still not optimal !

30

FastSLAM [ + Rao-Blackwellization + Sample-based Localization (sonar) occupancy grid mapping + scan matching based odometry]

Sample-based Mathematical Description of Dynamics update with sample representation Probability Distributions of distribution: sample from P(x t+1 | xt)

 Particle sets can be used to approximate functions

 The more particles fall into an interval, the higher the probability of that interval  If a continuous density needs to be extracted  can place a narrow Gaussian at the location of each particle  How to efficiently draw samples in an HMM?

Page 4 Observation update Importance sampling

 Interested in estimating:  Goal: go from a sample-based representation of

EX P [f(X)] = f(x)P (x)dx P(x t+1 | z1, …, zt) ∼ x Q(x) to a sample-based representation of = f(x)P (x) dx ifQ(x) = 0 ⇒ P (x) = 0 Q(x) x P(x t+1 | z1, …, zt, zt+1 ) = P (x) = f(x) Q(x)dx Q(x) C * P(x t+1 | z1, …, zt) * P(z t+1 | xt+1 ) x P (X) = EX Q[ f(X)] ∼ Q(X) m 1 P (x(i)) ≈ f(x(i)) with x(i) ∼ Q m Q(x(i)) i =1

 Hence we could sample from an alternative distribution Q and simply re-weight the samples == Importance Sampling

Sequential Importance Sampling (SIS) Particle Filter SIS particle filter major issue

(1) (2) (N)  Sample x , x , …, x from P(X ) 1 1 1 1  The resulting samples are only weighted by the (i)  Set w 1= 1 for all i=1,…,N evidence

 For t=1, 2, …  The samples themselves are never affected by the  Dynamics update: evidence  For i=1, 2, …, N (i) (i)  Sample x t from P(X t+1 | Xt = x t)  Fails to concentrate particles/computation in the high

 Observation update: probability areas of the distribution P(xt | z1, …, zt)  For i=1, 2, …, N (i) (i) (i)  w t+1 = w t* P(z t+1 | Xt+1 = x t+1 )

 At any time t, the distribution is represented by the weighted set of samples

(i) (i) { < x t, w t> ; i=1,…,N}

Resampling

 At any time t, the distribution is represented by the 1. Algorithm particle_filter ( St-1, u t-1 zt): 2. weighted set of samples St = ∅, η = 0 (i) (i) 3. For i =1Kn Generate new samples { < x t, w t> ; i=1,…,N}

4. Sample index j(i) from the discrete distribution given by wt-1  Sample N times from the set of particles 5. Sample i from using j ( i ) and xt p(xt | xt−1,ut−1) xt −1 ut−1  The probability of drawing each particle is given by its i i importance weight 6. wt = p(zt | xt ) Compute importance weight i 7.η = η + wt Update normalization factor 8. S = S ∪{< xi ,wi >} Insert  More particles/computation focused on the parts of the t t t t state space with high probability mass 9. For i =1Kn i i 10. wt = wt /η Normalize weights

Page 5 Particle Filters Sensor Information: Importance Sampling Bel (x) ← α p(z | x) Bel − (x) α p(z | x) Bel − (x) w ← = α p(z | x) Bel − (x)

Robot Motion Sensor Information: Importance Sampling − − Bel (x) ← α p(z | x) Bel (x) Bel x p x u, x Bel x x − ( ) ← ∫ ( | )' ( )' d ' α p(z | x) Bel (x) w ← = α p(z | x) Bel − (x)

Robot Motion Resampling issue − Bel (x) ← ∫ p(x | u, x )' Bel (x )' d x'  Loss of samples …

Page 6 Low Variance Resampling Low Variance Resampling

 Advantages:

 More systematic coverage of space of samples

 If all samples have same importance weight, no samples are lost

 Lower computational complexity

Regularization Mobile Robot Localization

 If no dynamics noise  all particles will start to coincide  Each particle is a potential pose of the robot

  regularization: resample from a (narrow) Gaussian around the particle  Proposal distribution is the motion model of the robot (prediction step)

 The observation model is used to compute the importance weight (correction step)

Motion Model Reminder Proximity Sensor Model Reminder

Start Laser sensor Sonar sensor

Note: sensor model is not Gaussian at all!

Page 7 Sample-based Localization (sonar)

55

56 57

58 59

Page 8 60 61

62 63

64 65

Page 9 66 67

68 69

70 71

Page 10 Summary – Particle Filters

 Particle filters are an implementation of recursive Bayesian filtering

 They represent the posterior by a set of weighted samples

 They can model non-Gaussian distributions

 Proposal to draw new samples

 Weight to account for the differences between the proposal and the target  Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter 72 77

Summary – PF Localization

 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.

78

Page 11