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.
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