Page 1CS 287: Advanced RoboticsFall 2009Lecture 22: HMMs, Kalman filtersPieter AbbeelUC Berkeley EECS Current and next set of lectures The state is not observed Instead, we get some sensory information about the state Challenge: compute a probability distribution over the state which accounts for the sensory information (“evidence”) which we have observed.OverviewPage 2Hidden Markov Models Underlying Markov model over states Xt Assumption 1: Xtindependent of X1, …, Xt-2given Xt-1 For each state Xtthere is a random variable Ztwhich is a sensory measurement of Xt Assumption 2: Ztis assumed conditionally independent of the other variables given Xt This gives the following graphical (Bayes net) representation:XTX2Z1X1X3X4Z2Z3Z4ZT Init P(x1) [e.g., uniformly] Observation update for time 0: For t = 1, 2, … Time update Observation update For discrete state / observation spaces: simply replace integral by summationFiltering in HMMPage 3With control inputs Control inputs known: They can be simply seen as selecting a particular dynamics function Control inputs unknown: Assume a distribution over them Above drawing assumes open-loop controls. This is rarely the case in practice. [Markov assumption is rarely the case either. Both assumptions seem to have given quite satisfactory results.]X5X2Z1X1X3X4Z2Z3Z4E5U2U1U3U46Discrete-time Kalman FilterttttttuBxAxε++=−1ttttxCzδ+=Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equationwith a measurementPage 47Kalman Filter Algorithm Algorithm Kalman_filter( µt-1, Σt-1, ut, zt):Prediction:Correction:Return µt, ΣttttttuBA +=−1µµtTttttRAA +Σ=Σ−11)(−+ΣΣ=tTtttTtttQCCCK)(ttttttCzKµµµ−+=ttttCKI Σ−=Σ )( From an analytical point of view == Kalman filter Difference: keep track of the inverse covariance rather than the covariance matrix [matter of some linear algebra manipulations to get into this form] Why interesting? Inverse covariance matrix = 0 is easier to work with than covariance matrix = infinity (case of complete uncertainty) Inverse covariance matrix is often sparser than the covariance matrix --- for the “insiders”: inverse covariance matrix entry (i,j) = 0 if xiis conditionally independent of xjgiven some set {xk, xl, …} Downside: when extended to non-linear setting, need to solve a linear system to find the mean (around which one can then linearize) See Probabilistic Robotics pp. 78-79 for more in-depth pros/cons Intermezzo: information filterPage 5 A = [ 0.99 0.0074; -0.0136 0.99]; C = [ 1 1 ; -1 +1]; x(:,1) = [-3;2]; Sigma_w = diag([.3 .7]); Sigma_v = [2 .05; .05 1.5]; w = randn(2,T); w = sqrtm(Sigma_w)*w; v = randn(2,T); v = sqrtm(Sigma_v)*v; for t=1:T-1x(:,t+1) = A * x(:,t) + w(:,t);y(:,t) = C*x(:,t) + v(:,t);end % now recover the state from the measurements  P_0 = diag([100 100]); x0 =[0; 0]; % run Kalman filter and smoother here % + plotMatlab code data generation exampleKalman filter/smoother examplePage 6 If system is observable (=dual of controllable!) then Kalman filter will converge to the true state. System is observable iffO = [C ; CA ; CA2; … ; CAn-1] is full column rank (1)Intuition: if no noise, we observe y0, y1, … and we have that the unknown initial state x0satisfies:y0= C x0y1 = CA x0...yK= CAKx0This system of equations has a unique solution x0iff the matrix [C; CA; … CAK] has full column rank. B/c any power of a matrix higher than n can be written in terms of lower powers of the same matrix, condition (1) is sufficient to check (i.e., the column rank will not grow anymore after having reached K=n-1).Kalman filter property The previous slide assumed zero control inputs at all times. Does the same result apply with non-zero control inputs? What changes in the derivation?Simple self-quizPage 713Kalman Filter Summary Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k2.376+ n2) Optimal for linear Gaussian systems! Most robotics systems are nonlinear! Extended Kalman filter (EKF) Unscented Kalman filter (UKF)[And also: particle filter (next lecture)] I provided a game log of me playing for your convenience. However, to ensure you fully understand, I suggest you follow the following procedure: Code up a simple heuristic policy to collect samples from the state space for question 1. Then use these samples as your state samples for ALP and for approximate value iteration. Play the game yourself for question 2 and have it learn to clone your playing style. You don’t *need* to follow the above procedure, but I strongly suggest to in case you have any doubt about how the algorithms in Q1 and Q2 operate, b/c following the above procedure will force you more blatantly to see the differences (and can only increase you ability to hand in a good PS2).Announcement: PS2Page 8 PS1: will get back to you over the weekend, likely Saturday Milestone: ditto PS2: don’t underestimate it! Office hours: canceled today. Feel free to set appointment over email. Also away on Friday actually. Happy to come in on Sat or Sun afternoon by appointment. Tuesday 4-5pm 540 Cory: Hadas Kress-Gazit (Cornell) High-level tasks to correct low-level robot controlIn 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.AnnouncementsNonlinear Dynamic Systems Most realistic robotic problems involve nonlinear functionsnoisexugxttt+=−),(1noisexhztt+=)(Page 9Linearity Assumption RevisitedNon-linear FunctionThroughout: “Gaussian of P(y)” is the Gaussian which minimizes the KL-divergence with P(y). It turns out that this means the Gaussian with the same mean and covariance as P(y).Page 10EKF Linearization (1)EKF Linearization (2)Page 11EKF Linearization (3) Prediction: Correction:EKF Linearization: First Order Taylor Series