UT CS 344R - Lecture 11: Kalman Filters

Unformatted text preview:

Lecture 11: Kalman FiltersUp To Higher DimensionsExpectationsVariance and CovarianceBiased and Unbiased EstimatorsCovariance MatrixIndependent VariationDependent VariationDiscrete Kalman FilterEstimates and ErrorsTime Update (Predictor)Measurement Update (Corrector)The Kalman GainExtended Kalman FilterThe Jacobian MatrixLinearize the Non-LinearEKF Update Equations“Catch The Ball” AssignmentTTDLecture 11:Kalman FiltersCS 344R: RoboticsBenjamin KuipersUp To Higher Dimensions•Our previous Kalman Filter discussion was of 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 possible values. 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,L x 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 an unbiased 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 population variance. An unbiased estimator is:•But: “If the difference between N and N1 ever matters to you, then you are probably up 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 are Gaussian random variables (N=100)•Generated with x=1 y=3•Covariance matrix:€ Cxy=0.90 0.440.44 8.82 ⎡ ⎣ ⎢ ⎤ ⎦ ⎥Dependent Variation•c and d are random variables.•Generated with c=x+y d=x-y•Covariance matrix:€ Ccd=10.62 −7.93−7.93 8.84 ⎡ ⎣ ⎢ ⎤ ⎦ ⎥Discrete Kalman Filter•Estimate the state of a linear stochastic difference equation–process noise w is drawn from N(0,Q), with covariance matrix Q.•with a measurement –measurement noise v is drawn from N(0,R), with covariance 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 simplified versions 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 and measurement equations are non-linear:–process noise w is drawn from N(0,Q), with covariance 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 are almost 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 HPHT in the update


View Full Document

UT CS 344R - Lecture 11: Kalman Filters

Download Lecture 11: Kalman Filters
Our administrator received your request to download this document. We will send you the file to your email shortly.
Loading Unlocking...
Login

Join to view Lecture 11: Kalman Filters and access 3M+ class-specific study document.

or
We will never post anything without your permission.
Don't have an account?
Sign Up

Join to view Lecture 11: Kalman Filters 2 2 and access 3M+ class-specific study document.

or

By creating an account you agree to our Privacy Policy and Terms Of Use

Already a member?