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 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 = covariance, 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. Z¯t = h(X¯t) 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 [particle filter + 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.