Lecture 11:Kalman FiltersCS 344R: RoboticsBenjamin KuipersUp To Higher Dimensions• Our previous Kalman Filter discussion wasof a simple one-dimensional model.• Now we go up to higher dimensions:– State vector:– Sense vector:– Motor vector:• First, a little statistics.! x " #n! z " #m! u " #lExpectations• Let x be a random variable.• The expected value E[x] is the mean:– The probability-weighted mean of all possiblevalues. The sample mean approaches it.• Expected value of a vector x is by component.! E[x] = x p(x) dx"# x =1Nxi1N$ ! E[x] = x = [x 1,Lx n]TVariance and Covariance• The variance is E[ (x-E[x])2 ]• Covariance matrix is E[ (x-E[x])(x-E[x])T ]– Divide by N−1 to make the sample variance anunbiased estimator for the population variance.! "2= E[(x # x )2] =1N(xi# x )21N$! Cij=1N(xik" x i)(xjk" x j)k =1N#Biased and Unbiased Estimators• Strictly speaking, the sample variance is a biased estimate of the populationvariance. An unbiased estimator is:• But: “If the difference between N and N−1ever matters to you, then you are probablyup to no good anyway …” [Press, et al]! "2= E[(x # x )2] =1N(xi# x )21N$! s2=1N "1(xi" x )21N#Covariance Matrix• Along the diagonal, Cii are variances.• Off-diagonal Cij are essentially correlations. ! C1,1="12C1,2C1,NC2,1C2,2="22O MCN ,1L CN ,N="N2# $ % % % % & ' ( ( ( (Independent Variation• x and y areGaussian randomvariables (N=100)• Generated withσx=1 σy=3• Covariance matrix:! Cxy=0.90 0.440.44 8.82" # $ % & 'Dependent Variation• c and d are randomvariables.• Generated withc=x+y d=x-y• Covariance matrix:! Ccd=10.62 "7.93"7.93 8.84# $ % & ' (Discrete Kalman Filter• Estimate the state of a linearstochastic difference equation– process noise w is drawn from N(0,Q), withcovariance matrix Q.• with a measurement– measurement noise v is drawn from N(0,R), withcovariance matrix R.• A, Q are nxn. B is nxl. R is mxm. H is mxn.! xk= Axk "1+ Buk+ wk "1! x " #n! z " #m! zk= Hxk+ vkEstimates and Errors• is the estimated state at time-step k.• after prediction, before observation.• Errors:• Error covariance matrices:• Kalman Filter’s task is to update! ˆ x k" #n! ˆ x k"# $n! ek"= xk"ˆ x k"ek= xk"ˆ x k! Pk"= E[ek"ek"T]Pk= E[ekekT]! ˆ x kPkTime Update (Predictor)• Update expected value of x• Update error covariance matrix P• Previous statements were simplifiedversions of the same idea:! ˆ x k"= Aˆ x k "1+ Buk! Pk"= APk "1AT+ Q! ˆ x (t3") =ˆ x (t2) + u[t3" t2]! "2(t3#) ="2(t2) +"$2[t3# t2]Measurement Update (Corrector)• Update expected value– innovation is• Update error covariance matrix• Compare with previous form! ˆ x k=ˆ x k"+ Kk(zk" Hˆ x k")! zk" Hˆ x k"! Pk= (I" KkH)Pk"! ˆ x (t3) =ˆ x (t3") + K(t3)(z3"ˆ x (t3"))! "2(t3) = (1# K(t3))"2(t3#)The Kalman Gain• The optimal Kalman gain Kk is• Compare with previous form! Kk= Pk"HT(HPk"HT+ R)"1! =Pk"HTHPk"HT+ R! K(t3) ="2(t3#)"2(t3#) +"32Extended Kalman Filter• Suppose the state-evolution andmeasurement equations are non-linear:– process noise w is drawn from N(0,Q), withcovariance matrix Q.– measurement noise v is drawn from N(0,R),with covariance matrix R.! xk= f (xk "1,uk) + wk "1! zk= h(xk) + vkThe Jacobian Matrix• For a scalar function y=f(x),• For a vector function y=f(x),! "y =# f (x)"x ! "y = J"x ="y1M"yn# $ % % % & ' ( ( ( =)f1)x1(x) L)f1)xn(x)M M)fn)x1(x) L)fn)xn(x)# $ % % % % % % & ' ( ( ( ( ( ( *"x1M"xn# $ % % % & ' ( ( (Linearize the Non-Linear• Let A be the Jacobian of f with respect to x.• Let H be the Jacobian of h with respect to x.• Then the Kalman Filter equations arealmost the same as before!! Aij="fi"xj(xk#1,uk)! Hij="hi"xj(xk)EKF Update Equations• Predictor step:• Kalman gain:• Corrector step:! ˆ x k"= f (ˆ x k "1,uk)! Pk"= APk "1AT+ Q! Kk= Pk"HT(HPk"HT+ R)"1! ˆ x k=ˆ x k"+ Kk(zk" h (ˆ x k"))! Pk= (I" KkH)Pk"“Catch The Ball” Assignment• State evolution is linear (almost).– What is A?– B=0.• Sensor equation is non-linear.– What is y=h(x)?– What is the Jacobian H(x) of h with respect to x?• Errors are treated as additive. Is this OK?– What are the covariance matrices Q and R?TTD• Intuitive explanations for APAT and HPHTin the update