EKF, UKF Pieter Abbeel UC Berkeley EECS - PowerPoint PPT Presentation

EKF, ¡UKF ¡ ¡ ¡ Pieter ¡Abbeel ¡ UC ¡Berkeley ¡EECS ¡ ¡ ¡ ¡ ¡ Many ¡slides ¡adapted ¡from ¡Thrun, ¡Burgard ¡and ¡Fox, ¡ProbabilisAc ¡RoboAcs ¡ ¡

Kalman ¡Filter ¡ Kalman ¡Filter ¡= ¡special ¡case ¡of ¡a ¡Bayes’ ¡filter ¡with ¡dynamics ¡and ¡sensory ¡models ¡linear ¡Gaussians: ¡ n 2 -1

Kalman ¡Filtering ¡Algorithm ¡ n At time 0: n For t = 1, 2, … n Dynamics update: n Measurement update:

Nonlinear ¡Dynamical ¡Systems ¡ n Most ¡realisAc ¡roboAc ¡problems ¡involve ¡nonlinear ¡funcAons: ¡ n Versus ¡linear ¡seOng: ¡

Linearity ¡AssumpAon ¡Revisited ¡ y ¡ y p(y) x ¡ p(x) ¡ x ¡

Linearity ¡AssumpAon ¡Revisited ¡ y ¡ y p(y) x ¡ p(x) ¡ x ¡

Non-­‑linear ¡FuncAon ¡ y ¡ y ¡ p(y) ¡ x ¡ p(x) ¡ “Gaussian ¡of ¡p(y)” ¡has ¡ mean ¡and ¡variance ¡of ¡y ¡ under ¡p(y) ¡ ¡ x ¡

EKF ¡LinearizaAon ¡(1) ¡

EKF ¡LinearizaAon ¡(2) ¡ ¡ p(x) ¡has ¡high ¡variance ¡relaAve ¡to ¡region ¡in ¡which ¡linearizaAon ¡is ¡accurate. ¡ ¡

EKF ¡LinearizaAon ¡(3) ¡ p(x) ¡has ¡small ¡variance ¡relaAve ¡to ¡region ¡in ¡ which ¡linearizaAon ¡is ¡accurate. ¡ ¡

EKF ¡LinearizaAon: ¡First ¡Order ¡Taylor ¡Series ¡Expansion ¡ n Dynamics ¡model: ¡for ¡ x t ¡“close ¡to” ¡μ t ¡ we ¡have: ¡ n Measurement ¡model: ¡ for ¡ x t ¡“close ¡to” ¡μ t ¡we ¡have: ¡

EKF ¡LinearizaAon: ¡Numerical ¡ ¡ Numerically ¡compute ¡ F t ¡column ¡by ¡column: ¡ n n Here ¡ e i ¡is ¡the ¡basis ¡vector ¡with ¡all ¡entries ¡equal ¡to ¡zero, ¡except ¡for ¡the ¡ i’th ¡entry, ¡which ¡equals ¡1. ¡ n If ¡wanAng ¡to ¡approximate ¡ F t ¡as ¡closely ¡as ¡possible ¡then ¡ε ¡is ¡chosen ¡to ¡ be ¡a ¡small ¡number, ¡but ¡not ¡too ¡small ¡to ¡avoid ¡numerical ¡issues ¡

Ordinary ¡Least ¡Squares ¡ Given: ¡samples ¡{(x (1) , ¡y (1) ), ¡(x (2) , ¡y (2) ), ¡…, ¡(x (m) , ¡y (m) )} ¡ n Problem: ¡find ¡funcAon ¡of ¡the ¡form ¡f(x) ¡= ¡ a 0 + ¡ a 1 ¡x ¡that ¡fits ¡the ¡samples ¡as ¡ n well ¡as ¡possible ¡in ¡the ¡following ¡sense: ¡

Ordinary ¡Least ¡Squares ¡ Recall ¡our ¡objecAve: ¡ n Let’s ¡write ¡this ¡in ¡vector ¡notaAon: ¡ n n ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ à ¡ ¡ Set ¡gradient ¡equal ¡to ¡zero ¡to ¡find ¡extremum: ¡ n (See the Matrix Cookbook for matrix identities, including derivatives.)

Ordinary ¡Least ¡Squares ¡ n For ¡our ¡example ¡problem ¡we ¡obtain ¡a ¡= ¡[4.75; ¡2.00] ¡ a 0 + a 1 x

Ordinary ¡Least ¡Squares ¡ 26 More ¡generally: ¡ ¡ ¡ 24 n 22 20 30 0 10 20 30 40 20 10 0 In ¡vector ¡notaAon: ¡ n ¡ ¡ ¡ ¡ ¡ ¡ ¡ à ¡ ¡ n Set ¡gradient ¡equal ¡to ¡zero ¡to ¡find ¡extremum ¡(exact ¡same ¡derivaAon ¡as ¡two ¡ n slides ¡back): ¡

Vector ¡Valued ¡Ordinary ¡Least ¡Squares ¡Problems ¡ So ¡far ¡have ¡considered ¡approximaAng ¡a ¡scalar ¡valued ¡funcAon ¡from ¡samples ¡ n {(x (1) , ¡y (1) ), ¡(x (2) , ¡y (2) ), ¡…, ¡(x (m) , ¡y (m) )} ¡with ¡ A ¡vector ¡valued ¡funcAon ¡is ¡just ¡many ¡scalar ¡valued ¡funcAons ¡and ¡we ¡can ¡ n approximate ¡it ¡the ¡same ¡way ¡by ¡solving ¡an ¡OLS ¡problem ¡mulAple ¡Ames. ¡ ¡ Concretely, ¡let ¡ ¡ ¡ ¡ ¡then ¡we ¡have: ¡ In ¡our ¡vector ¡notaAon: ¡ ¡ n This ¡can ¡be ¡solved ¡by ¡solving ¡a ¡separate ¡ordinary ¡least ¡squares ¡problem ¡to ¡find ¡ n each ¡row ¡of ¡ ¡

Vector ¡Valued ¡Ordinary ¡Least ¡Squares ¡Problems ¡ Solving ¡the ¡OLS ¡problem ¡for ¡each ¡row ¡gives ¡us: ¡ n Each ¡OLS ¡problem ¡has ¡the ¡same ¡structure. ¡ ¡We ¡have ¡ n

Vector ¡Valued ¡Ordinary ¡Least ¡Squares ¡and ¡EKF ¡LinearizaAon ¡ Approximate ¡ x t+1 ¡= ¡ f t ( x t , ¡ u t ) ¡ ¡ n ¡ ¡ ¡with ¡affine ¡funcAon ¡ a 0 ¡+ ¡ F t ¡ x t ¡ ¡ ¡ ¡ ¡by ¡running ¡least ¡squares ¡on ¡samples ¡from ¡the ¡funcAon: ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡{( ¡ x t(1) , ¡y (1) = f t ( x t(1) , u t ), ¡( ¡ x t(2) , ¡y (2) = f t ( x t(2) , u t ), ¡…, ¡( ¡ x t(m) , ¡y (m) = f t ( x t(m) , u t )} ¡ Similarly ¡for ¡ ¡ z t+1 ¡= ¡ h t ( x t ) ¡ ¡ n ¡

OLS ¡and ¡EKF ¡LinearizaAon: ¡Sample ¡Point ¡SelecAon ¡ OLS ¡vs. ¡tradiAonal ¡(tangent) ¡linearizaAon: ¡ n OLS traditional (tangent)

OLS ¡LinearizaAon: ¡choosing ¡samples ¡points ¡ n Perhaps ¡most ¡natural ¡choice: ¡ ¡ n ¡ ¡ ¡ n ¡reasonable ¡way ¡to ¡cover ¡region ¡with ¡high ¡probability ¡mass ¡

AnalyAcal ¡vs. ¡Numerical ¡LinearizaAon ¡ Numerical ¡(based ¡on ¡least ¡squares ¡or ¡finite ¡differences) ¡could ¡give ¡a ¡more ¡ n accurate ¡“regional” ¡approximaAon. ¡ ¡Size ¡of ¡region ¡determined ¡by ¡ evaluaAon ¡points. ¡ ComputaAonal ¡efficiency: ¡ n n AnalyAcal ¡derivaAves ¡can ¡be ¡cheaper ¡or ¡more ¡expensive ¡than ¡funcAon ¡ evaluaAons ¡ Development ¡hint: ¡ n n Numerical ¡derivaAves ¡tend ¡to ¡be ¡easier ¡to ¡implement ¡ n If ¡deciding ¡to ¡use ¡analyAcal ¡derivaAves, ¡implemenAng ¡finite ¡difference ¡ derivaAve ¡and ¡comparing ¡with ¡analyAcal ¡results ¡can ¡help ¡debugging ¡the ¡ analyAcal ¡derivaAves ¡

EKF ¡Algorithm ¡ At time 0: n For t = 1, 2, … n n Dynamics update: n Measurement update:

EKF ¡Summary ¡ n Highly ¡efficient: ¡Polynomial ¡in ¡measurement ¡ dimensionality ¡ k ¡and ¡state ¡dimensionality ¡ n : ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ O(k 2.376 ¡+ ¡n 2 ) ¡ ¡ n Not ¡opAmal! ¡ n Can ¡diverge ¡if ¡nonlineariAes ¡are ¡large! ¡ n Works ¡surprisingly ¡well ¡even ¡when ¡all ¡assumpAons ¡are ¡ violated! ¡

LinearizaAon ¡via ¡Unscented ¡Transform ¡ EKF UKF

UKF ¡Sigma-­‑Point ¡EsAmate ¡(2) ¡ EKF UKF

UKF ¡Sigma-­‑Point ¡EsAmate ¡(3) ¡ EKF UKF

UKF ¡Sigma-­‑Point ¡EsAmate ¡(4) ¡

[Julier and Uhlmann, 1997] UKF ¡intuiAon ¡why ¡it ¡can ¡perform ¡beter ¡ Assume ¡we ¡know ¡the ¡distribuAon ¡over ¡X ¡and ¡it ¡has ¡a ¡mean ¡\bar{x} ¡ n Y ¡= ¡f(X) ¡ n EKF ¡approximates ¡f ¡by ¡first ¡order ¡and ¡ignores ¡higher-­‑order ¡terms ¡ n UKF ¡uses ¡f ¡exactly, ¡but ¡approximates ¡p(x). ¡ ¡ n

Original ¡Unscented ¡Transform ¡ Picks ¡a ¡minimal ¡set ¡of ¡sample ¡points ¡that ¡match ¡1 st , ¡2 nd ¡and ¡3 rd ¡moments ¡of ¡a ¡Gaussian: ¡ n \bar{x} ¡= ¡mean, ¡ P xx ¡= ¡covariance, ¡i ¡ à ¡i’th ¡column, ¡x ¡in ¡R n n κ ¡: ¡extra ¡degree ¡of ¡freedom ¡to ¡fine-­‑tune ¡the ¡higher ¡order ¡moments ¡of ¡the ¡approximaAon; ¡when ¡x ¡is ¡ n Gaussian, ¡n+κ ¡= ¡3 ¡is ¡a ¡suggested ¡heurisAc ¡ L ¡= ¡\sqrt{P_{xx}} ¡can ¡be ¡chosen ¡to ¡be ¡any ¡matrix ¡saAsfying: ¡ n L ¡ L T ¡= ¡ P xx n [Julier and Uhlmann, 1997]

A ¡crude ¡preliminary ¡invesAgaAon ¡of ¡whether ¡we ¡can ¡get ¡EKF ¡to ¡match ¡ UKF ¡by ¡parAcular ¡choice ¡of ¡points ¡used ¡in ¡the ¡least ¡squares ¡fiOng ¡ Beyond scope of course, just including for completeness.

Self-­‑quiz ¡ ¡ n When ¡would ¡the ¡UKF ¡significantly ¡outperform ¡the ¡EKF? ¡ y x AnalyAcal ¡derivaAves, ¡finite-­‑difference ¡derivaAves, ¡and ¡least ¡squares ¡will ¡all ¡end ¡up ¡with ¡a ¡ n horizontal ¡linearizaAon ¡ à ¡they’d ¡predict ¡zero ¡variance ¡in ¡Y ¡= ¡f(X) ¡