Chapter 14. (Supplementary) Bayesian Filtering for State Estimation of Dynamic Systems

Transcription

1 Chaper 4. Supplemenary Bayesian Filering for Sae Esimaion of Dynamic Sysems Neural Neworks and Learning Machines Haykin Lecure Noes on Selflearning Neural Algorihms ByoungTak Zhang School of Compuer Science and Engineering Seoul Naional Universiy Version 2075

2 Supplemenary Maerial o Ch 4 Kalman Filer Sequenial Mone Carlo.. 25 Paricle Filers. 35

3 Overview The Problem Why do we need Kalman Filers? Wha is a Kalman Filer? Concepual Overview The Theory of Kalman Filer Simple Eample 3

4 The Problem Sysem Error Sources Black Bo Eernal Conrols Sysem Measuring Devices Sysem Sae desired bu no known Observed Measuremens Esimaor Opimal Esimae of Sysem Sae Measuremen Error Sources Sysem sae canno be measured direcly Need o esimae opimally from measuremens 4

5 Wha is a Kalman Filer? Recursive daa processing algorihm Generaes opimal esimae of desired quaniies given he se of measuremens Opimal? For linear sysem and whie Gaussian errors, Kalman filer is bes esimae based on all previous measuremens For nonlinear sysem opimaliy is qualified Recursive? Doesn need o sore all previous measuremens and reprocess all daa each ime sep 5

6 Concepual Overview Simple eample o moivae he workings of he Kalman Filer Theoreical Jusificaion o come laer for now jus focus on he concep Imporan: Predicion and Correcion 6

7 Concepual Overview Los on he dimensional line Posiion y Assume Gaussian disribued measuremens y 7

8 Concepual Overview Sean Measuremen a : Mean = z and Variance = s z Opimal esimae of posiion is: ŷ = z Variance of error in esimae: s 2 = s 2 z Boa in same posiion a ime 2 Prediced posiion is z 8

9 Concepual Overview predicion ŷ measuremen z So we have he predicion ŷ 2 GPS Measuremen a 2 : Mean = z 2 and Variance = s z2 Need o correc he predicion due o measuremen o ge ŷ 2 Closer o more rused measuremen linear inerpolaion? 9

10 Concepual Overview predicion ŷ correced opimal esimae ŷ measuremen z Correced mean is he new opimal esimae of posiion New variance is smaller han eiher of he previous wo variances 0

11 Concepual Overview Lessons so far: Make predicion based on previous daa ŷ, s Take measuremen z k, s z Opimal esimae ŷ = Predicion + Kalman Gain * Measuremen Predicion Variance of esimae = Variance of predicion * Kalman Gain

12 Concepual Overview ŷ 2 Naïve Predicion ŷ A ime 3, boa moves wih velociy dy/d=u Naïve approach: Shif probabiliy o he righ o predic This would work if we knew he velociy eacly perfec model 2

13 Concepual Overview ŷ 2 Naïve Predicion ŷ Predicion ŷ Beer o assume imperfec model by adding Gaussian noise dy/d = u + w Disribuion for predicion moves and spreads ou 3

14 Concepual Overview Correced opimal esimae ŷ Measuremen z Predicion ŷ Now we ake a measuremen a 3 Need o once again correc he predicion Same as before 4

15 Concepual Overview Lessons learn from concepual overview: Iniial condiions ŷ k and s k Predicion ŷ k, s k Use iniial condiions and model eg. consan velociy o make predicion Measuremen z k Take measuremen Correcion ŷ k, s k Use measuremen o correc predicion by blending predicion and residual always a case of merging only wo Gaussians Opimal esimae wih smaller variance 5

16 Theoreical Basis Process o be esimaed: y k = Ay k + Bu k + w k Process Noise w wih covariance Q z k = Hy k + v k Kalman Filer Measuremen Noise v wih covariance R Prediced: ŷ k is esimae based on measuremens a previous imeseps ŷ k = Ay k + Bu k P k = AP k A T + Q Correced: ŷ k has addiional informaion he measuremen a ime k ŷ k = ŷ k + Kz k H ŷ k K = P kh T HP kh T + R P k = I KHP k 6

17 Blending Facor If we are sure abou measuremens: Measuremen error covariance R decreases o zero K decreases and weighs residual more heavily han predicion If we are sure abou predicion Predicion error covariance P k decreases o zero K increases and weighs predicion more heavily han residual 7

18 Theoreical Basis Predicion Time Updae Projec he sae ahead Correcion Measuremen Updae Compue he Kalman Gain K = P kh T HP kh T + R ŷ k = Ay k + Bu k 2 Projec he error covariance ahead P k = AP k A T + Q 2 Updae esimae wih measuremen z k ŷ k = ŷ k + Kz k H ŷ k 3 Updae Error Covariance P k = I KHP k 8

19 Quick Eample Consan Model Sysem Error Sources Black Bo Eernal Conrols Sysem Sysem Sae Measuring Devices Observed Measuremens Esimaor Opimal Esimae of Sysem Sae Measuremen Error Sources 9

20 Quick Eample Consan Model Predicion ŷ k = y k P k = P k Correcion K = P kp k + R ŷ k = ŷ k + Kz k H ŷ k P k = I KP k 20

21 Quick Eample Consan Model

22 Quick Eample Consan Model Convergence of Error Covariance P k 22

23 Quick Eample Consan Model 0 0. Larger value of R he measuremen error covariance indicaes poorer qualiy of measuremens 0.2 Filer slower o believe measuremens slower convergence

24 References. Kalman, R. E A New Approach o Linear Filering and Predicion Problems, Transacion of he ASMEJournal of Basic Engineering, pp March Maybeck, P. S Sochasic Models, Esimaion, and Conrol, Volume, Academic Press, Inc. 3. Welch, G and Bishop, G An inroducion o he Kalman Filer, hp:// 24

25 Sequenial Mone Carlo

26 Mone Carlo MC Approimaion E [ f ] = p f d p ò N» å f! p = N N i= i i 2, 0, s Mone Carlo approach. Simulae N random variables from p, e.g. Normal disribuion 2. Compue he average i 2! p = N0, s E f f N i p[ ] = å, N i =

27 MC wih Imporance Sampling E [ f ] = p f d p p = ò q f d q i= i i! q q w i = N : proposal disribuion p q» ò å i i wf i w: imporance weigh i Noe: q is easier o sample from han p.

28 Imporance Sampling IS E[ f ] = f p y d ò 0: 0: 0: : 0: N i= i 0: 0: : i 0: : i 0: y: wf i 0:! q y q : proposal disribuion w i =» å p y q i w i : imporance weigh

29 Imporance Sampling: Procedure i 0: i 0: 0: :. Draw N samples from proposal disribuion q..! q y 2. Compue imporance weigh w p y i i 0: : 0: = i q0: y: 3. Esimae an arbirary funcion f.: w N i i i i 0: 0: :» å!! 0: = N i= å j w0: j= E[ f y ] f w, w

30 Sequenial Imporance Sampling SIS: Recursive Esimaion Augmening he samples q y = q y q, y 0: : 0: : 0: :! q, y i = q y q, y 0: : i cf. nonsequenial IS:! q 0: y: Weigh updae w py p i i i i i µ w i i q, y

31 Sequenial Imporance Sampling: Idea Updae filering densiy using Bayesian filering Compue inegrals using imporance sampling The filering densiy : is represened using paricles and heir weighs Compue weighs using: p y i i N {, w } i= w p, y i i : = i q y:,

32 Sequenial Imporance Sampling: Procedure. Paricle generaion! q, y = p i i i 2. Weigh compuaion w = w p y Weigh normalizaion w" i i i i j= 3. Esimaion compuaion E[ f i = N w å i w j N i i : å " i= y ] = f w Noe: Sep above assumes he proposal densiy o be he prior. This does no use he informaion from observaions. Alernaively, he proposal densiy could be! q, y = p, y i i ha minizes he variance of w Douce el al., 999.

33 Resampling SIS suffers from degeneracy problems, i.e. a small number of paricles have big weighs and he res have eremely small values. Remedy: SIR inroduces a selecion resampling sep o eliminae samples wih low imporance raios weighs and muliply samples wih high imporance raios. Resampling maps he weighed random measure on o he equally weighed random measure by sampling uniformly wih replacemen from wih probabiliies { i } N : w i = {!, N } " {, w } N N 0: i= 0: 0: i= { } i N 0: i=

34 Sampling Imporance Resampling SIR = Sequenial Mone Carlo = Paricle Filer. Iniialize 0 i For i =,..., N: sample! p 0,. 2. Imporance sampling For i =,..., N: sample! q, y = p Le ", i i i 0: 0: i i i i i For i =,..., N: compue weighs w = p y Normalize he weighs: 3. Resampling i he imporance weighs w, N i i j = å j= w# w w Resample wih replacemen N paricles according o i 0: resuling in { #, N }. New paricle populaion { } { # }. Se + and go o sep 2. i N i N 0: i= 0: i= i N 0: i=

35 Paricle Filers

36 Moivaing Applicaions l Hand racking using paricle filers: hp:// l Roboics SLAM and localizaion wih a sereo camera: hp:// ure=relaed l Kalman filer resul on real aircraf: hp:// ure=relaed c 2008 SNU Bioinelligence Laboraory, hp://bi.snu.ac.kr/ 36

37 Problem Saemen l Tracking he sae of a sysem as i evolves over ime l We have: Sequenially arriving noisy or ambiguous observaions l We wan o know: Bes possible esimae of he hidden variables

38 Bayesian Filering / Tracking Problem l Unknown sae vecor 0: = 0,, l Observaion vecor z : l Find PDF p 0: z : poserior disribuion l or p z : filering disribuion l Prior informaion given: p 0 pz p prior on sae disribuion sensor model Markovian saespace model

39 Sequenial Updae l Soring all incoming measuremens is inconvenien l Recursive filering: Predic ne sae pdf from curren esimae Updae he predicion using sequenially arriving new measuremens l Opimal Bayesian soluion: recursively calculaing eac poserior densiy

40 Bayesian Updae and Predicion l Predicion l Updae ò = : : d z p p z p ò = = d z p z p z z p z z p z p z p z p : : : : :

41 Kalman Filer l Opimal soluion for lineargaussian case l Assumpions: Sae model is known linear funcion of las sae and Gaussian noise signal Sensory model is known linear funcion of sae and Gaussian noise signal Poserior densiy is Gaussian

42 Kalman Filer: Updae Equaions,,, :known marices, 0, ~ 0, ~ : : : P m N z p P m N z p P m N z p H F R N n n H z Q N v v F = = = + = + = = + = = + = + = = T T T S H P K R H H P S H P K P P H m z K m m F F P Q P m F m

43 Limiaions of Kalman Filering l Assumpions are oo srong. We ofen find: Nonlinear models NonGaussian noise or poserior Mulimodal disribuions Skewed disribuions l Eended Kalman Filer: Local linearizaion of nonlinear models Sill limied o Gaussian poserior

44 Gridbased Mehods l Opimal for discree and finie sae space l Keep and updae an esimae of poserior pdf for every single sae l No consrains on poserior discree densiy

45 Limiaions of Gridbased Mehods l Compuaionally epensive l Only for finie sae ses l Approimae gridbased filer Divide coninuous sae space ino finie number of cells Hidden Markov model filer Dimensionaliy increases compuaional coss dramaically

46 Many differen names Paricle Filers l Sequenial Mone Carlo filers l Boosrap filers l Condensaion l Ineracing paricle approimaions l Survival of he fies l

47 SampleBased PDF Represenaion l Mone Carlo characerizaion of pdf: Represen poserior densiy by a se of random i.i.d. samples paricles from he pdf p 0: z : For larger number N of paricles equivalen o funcional descripion of pdf For N approaches opimal Bayesian esimae

48 Samplebased PDF Represenaion l Regions of high densiy Many paricles Large weigh of paricles l Uneven pariioning l Discree approimaion for coninuous pdf P N N å i i 0 : z: = w δ 0: 0: i=

49 Imporance Sampling l Draw N samples 0: i from imporance sampling disribuion p 0: z : l Imporance weigh l Esimaion of arbirary funcions f : π : 0: : 0: 0: z z p w = ò å å = = = = = s a N N N i N j j i i i i N d y p f f I f I w w w w f f I 0: : 0: 0:.. 0: 0: 0: ˆ ~, ~ ˆ

50 Sequenial Imporance Sampling SIS l Augmening he samples l Weigh updae, ~ π, π π, π π π : 0: : 0: : 0: : 0: i i z z z z z z = = =, π i i i i i i i z p z p w w µ

51 Degeneracy Problem l Afer a few ieraions, all bu one paricle will have negligible weigh l Measure for degeneracy: effecive sample size N eff N = * + Var w i w *... rue weighs a ime l Small N eff indicaes severe degeneracy l Brue force soluion: Use very large N

52 Choosing Imporance Densiy l Choose p o minimize variance of weighs l Opimal soluion: l Pracical soluion Imporance densiy = prior,, π i i i i i op z p w w z p z µ Þ =, π i i i i i z p w w p z µ Þ =

53 Resampling l Eliminae paricles wih small imporance weighs l Concenrae on paricles wih large weighs l Sample N imes wih replacemen from he se of paricles i according o imporance weighs w i l Survival of he fies

54 Sampling Imporance Resample Filer: Basic Algorihm l. INIT, =0 for i=,..., N: sample 0 i ~p 0 ; :=; l 2. IMPORTANCE SAMPLING for i=,..., N: sample i ~ p i < 0: i := 0: i, i for i=,..., N: evaluae imporance weighs w i =pz i Normalize he imporance weighs l 3. SELECTION / RESAMPLING resample wih replacemen N paricles 0: i according o he imporance weighs Se :=+ and go o sep 2

55 Variaions l Auiliary Paricle Filer: Resample a ime wih onesep lookahead reevaluae wih new sensory informaion l Regularisaion: Resample from coninuous approimaion of poserior p z :

56 Visualizaion of Paricle Filer unweighed measure compue imporance weighs Þ p z : resampling move paricles predic p z :

57 Paricle Filer Demo moving Gaussian + uniform, N=00 paricles

58 Paricle Filer Demo 2 moving Gaussian + uniform, N=000 paricles

59 Paricle Filer Demo 3 moving sharp Gaussian + uniform, N=00 paricles

60 Paricle Filer Demo 4 moving sharp Gaussian + uniform, N=000 paricles

61 Paricle Filer Demo 5 miure of wo Gaussians, filer loses rack of smaller and less pronounced peaks

62 Obaining sae esimaes from paricles l Any esimae of a funcion f can be calculaed by discree PDFapproimaion l Mean: l MAPesimae: paricle wih larges weigh l Robus mean: mean wihin window around MAPesimae [ ] å = = N j j j f w N f E [ ] å = = N j j j w N E

63 Pros and Cons of Paricle Filers + Esimaion of full PDFs + NonGaussian disribuions + e.g. mulimodal + Nonlinear sae and observaion model + Parallelizable Degeneracy problem High number of paricles needed Compuaionally epensive LinearGaussian assumpion is ofen sufficien

64 Mobile Robo Localizaion l Animaion by Sebasian Thrun, Sanford l hp://robos.sa nford.edu

65 Model Esimaion l Tracking wih muliple moionmodels Discree hidden variable indicaes acive model manoever l Recovery of signal from noisy measuremens Even if signal may be absen e.g. synapic currens Miure model of several hypoheses l Neural Nework model selecion [de Freias] Esimae parameers and archiecure of RBF nework from inpuoupu pairs Online classificaion imevarying classes : de Freias, e.al.: Sequenial Mone Carlo Mehods for Neural Neworks. in: Douce, e.al.: Sequenial Mone Carlo Mehods in Pracice, Springer Verlag, 200

66 Oher Applicaions l Visual Tracking e.g. human moion body pars l Predicion of financial ime series e.g. mapping gold price à sock price l Qualiy conrol in semiconducor indusry l Miliary applicaions Targe recogniion from single or muliple images Guidance of missiles