Kalman Filter: an instance of Bayes Filter. Kalman Filter: an instance of Bayes Filter. Kalman Filter. Linear dynamics with Gaussian noise

Transcription

1 COM47 Inoducion o Roboics and Inelligen ysems he alman File alman File: an insance of Bayes File alman File: an insance of Bayes File Linea dynamics wih Gaussian noise alman File Linea dynamics wih Gaussian noise Linea obsevaions wih Gaussian noise Linea obsevaions wih Gaussian noise Iniial belief is Gaussian Iniial belief is Gaussian

2 alman File: assumpions wo assumpions inheied fom Bayes File Linea dynamics and obsevaion models Iniial belief is Gaussian Noise vaiables and iniial sae ae joinly Gaussian and independen Noise vaiables ae independen and idenically disibued Noise vaiables ae independen and idenically disibued alman File: why hose assumpions? wo assumpions inheied fom Bayes File Linea dynamics and obsevaion models Iniial belief is Gaussian Noise vaiables and iniial sae Wihou lineaiy hee is no closed-fom soluion fo he poseio belief in he Bayes File. Recall ha if X is Gaussian hen YAXb is also Gaussian. his is no ue in geneal if Yh(X). Also, we will see lae ha applying Bayes ule o a Gaussian pio and a Gaussian measuemen likelihood esuls in a Gaussian poseio. ae joinly Gaussian and independen Noise vaiables ae independen and idenically disibued Noise vaiables ae independen and idenically disibued alman File: why hose assumpions? wo assumpions inheied fom Bayes File Linea dynamics and obsevaion models Iniial belief is Gaussian Noise vaiables and iniial sae his esuls in he belief emaining Gaussian afe each popagaion and updae sep. his means ha we only have o woy abou how he mean and he covaiance of he belief evolve ecusively wih each pedicion sep and updae sep! COOL! ae joinly Gaussian and independen Noise vaiables ae independen and idenically disibued Noise vaiables ae independen and idenically disibued alman File: why so many assumpions? wo assumpions inheied fom Bayes File Linea dynamics and obsevaion models Iniial belief is Gaussian Noise vaiables and iniial sae ae joinly Gaussian and independen Noise vaiables ae independen and idenically disibued Noise vaiables ae independen and idenically disibued his makes he ecusive updaes of he mean and covaiance much simple and allows fo a ROOF OF OIMALIY.

3 Assumpions guaanee ha if he pio belief befoe he pedicion sep is Gaussian alman File: an insance of Bayes File alman File: an insance of Bayes File Belief afe pedicion sep (o simplify noaion) hen he pio belief afe he pedicion sep will be Gaussian o, unde he alman File assumpions we ge and he poseio belief (afe he updae sep) will be Gaussian. Noaion: esimae a ime given hisoy of obsevaions and conols up o ime - alman File: an insance of Bayes File alman File wih D sae Le s sa wih he updae sep ecusion. ee s an eample: uppose you measuemen model is wih uppose you belief afe he pedicion sep is o, unde he alman File assumpions we ge wo main quesions:. ow o ge pedicion mean and covaiance fom pio mean and covaiance? uppose you fis noisy measuemen is 2. ow o ge poseio mean and covaiance fom pedicion mean and covaiance? hese quesions wee answeed in he 960s. he esuling algoihm was used in he Apollo missions o he moon, and in almos evey sysem in which hee is a noisy senso involved! COOL! Whee I hink I wen... Wha is saw suggess... Q: Wha is he mean and covaiance of?

4 alman File wih D sae: he updae sep Fom Bayes File we ge so fo his eample alman File wih D sae: he updae sep Fom Bayes File we ge so edicion esidualeo beween acual obsevaion and epeced obsevaion. You epeced he measued mean o be 0, accoding o you pedicion pio, bu you acually obseved 5. he smalle his pedicion eo is he bee you esimae will be, o he bee i will agee wih he measuemens. alman File wih D sae: he updae sep & alman Gain alman File wih D sae: he updae sep Fom Bayes File we ge so Fom Bayes File we ge so alman Gain: specifies how much effec will he measuemen have in he poseio, compaed o he pedicion pio. Which one do you us moe, you pio o you measuemen? he measuemen is moe confiden (lowe vaiance) han he pio, so he poseio mean is going o be close o 5 han o 0.

5 alman File wih D sae: he updae sep alman File wih D sae: he updae sep Fom Bayes File we ge so Fom Bayes File we ge so Noe we can wie his as he weighed aveage of A and B No mae wha happens, he vaiance of he poseio is going o be educed. I.e. new measuemen inceases confidence no mae how noisy i is. In fac you can wie his as so and I.e. he poseio is moe confiden han boh he pio and he measuemen. alman File wih D sae: he updae sep alman File wih D sae: he updae sep Fom Bayes File we ge so Anohe eample: In his eample:

6 alman File wih D sae: he popagaionpedicion sep alman File wih D sae: he updae sep Oc 26 end ake home message: unceainy inceases afe he pedicion sep, because we ae speculaing abou he fuue. ake-home message: new obsevaions, no mae how noisy, always educe unceainy in he poseio. he mean of he poseio, on he ohe hand, only changes when hee is a noneo pedicion esidual. imple vesion: ea daa is always useful, no mae how uneliable he souce. Recap: Bayesian File Deivaion of he Bayesian File Robo and envionmenal sae esimaion is a fundamenal poblem (no jus in oboics)! Nealy all algoihms ha eis fo spaial easoning make use of his appoach If you e woking in oboics, you ll see i ove and ove! "Fileing" is a name fo combining daa. Efficien sae esimaos Recusively compue he obo s cuen sae based on he pevious sae of he obo Wha is he obo s sae? Esimaion of he obo s sae given he daa: Bel ( ) p( Z ) he obo s daa, Z, is epanded ino wo ypes: obsevaions (i o o i )and acions (denoed ui o a i) Bel( ) p( o, a, o, a2,..., o0) Invoking he Bayesian heoem Bel( ) p( o, a,..., o0) p( a p( o a,..., o ) 0,..., o ) 0

7 Deivaion of he Bayesian File Denominao is consan elaive o η p o a,..., ) ( a0 Bel( ) ηp( o, a,..., o0) p( a,..., o0 ) Fis-ode Makov assumpion shoens fis em: Bel( ) ηp( o ) p( a,..., o0) Bayesian File : Requiemens fo Implemenaion Updae equaions Moion model enso model Repesenaion fo he belief funcion Iniial belief sae Bel Epanding he las em (heoem of oal pobabiliy): ( ) ηp( o ) p(, a,..., o0 ) p( a,..., o0) d Repesenaion of he Belief Funcion Reminde alman File assumpions ample-based epesenaions e.g. aicle files aameic epesenaions wo assumpions inheied fom Bayes File Linea dynamics and obsevaion models Iniial belief is Gaussian Noise vaiables and iniial sae ae joinly Gaussian and independen Noise vaiables ae independen and idenically disibued Noise vaiables ae independen and idenically disibued ( n, y ),(, y ),(, y ),...( n, y ) y m b

8 alman File is a aameeied Bayesian File alman files (F) epesen poseio belief by a paameic fo: Gaussian (nomal) disibuion alman File : a Bayesian File Iniial belief Bel( 0 ) is a Gaussian disibuion Wha do we do fo an unknown saing posiion? ae a ime is a linea funcion of sae a ime : F Bu ε ( acion) A -d Gaussian disibuion is given by: 2 ( µ ) 2 2σ ( ) e σ 2π An n-d Gaussian disibuion is given by: ( ) e n (2π ) Σ ( µ ) Σ ( µ ) 2 Obsevaions ae linea in he sae: Eo ems ae eo-mean andom vaiables which ae nomally disibued hese assumpions guaanee ha he poseio belief is Gaussian he alman File is an efficien algoihm o compue he poseio Nomally, an updae of his naue would equie a mai invesion (simila o a leas squaes esimao) he alman File avoids his compuaionally comple opeaion ε (obsevaion) Linea sysem in sae-space fom: () A Bu C now inpu veco u and oupu veco, as well as he maices A, B, and C pefecly. Conolles can be designed wih full sae knowledge: A Bu C u - Obseves can be designed o esimae he full sae fom he oupu: A Bu ( C ) he conolle and obseve can be designed independenly! Innovaion em. Why no jus use C - o ge an esimae of? Because i migh no be inveible.

9 he alman File Moion model is Gaussian enso model is Gaussian Each belief funcion is uniquely chaaceied by is mean µ and covaiance mai Σ Compuing he poseio means compuing a new mean µ and covaiance Σ fom old daa using acions and senso eadings Wha ae he key limiaions? Wha we know Wha we don know We know wha he conol inpus of ou pocess ae We know wha we ve old he sysem o do and have a model fo wha he epeced oupu should be if eveyhing woks igh We don know wha he noise in he sysem uly is We can only esimae wha he noise migh be and y o pu some so of uppe bound on i When esimaing he sae of a sysem, we y o find a se of values ha comes as close o he uh as possible hee will always be some mismach beween ou esimae of he sysem and he ue sae of he sysem iself. We jus y o figue ou how much mismach hee is and y o ge he bes esimae possible ) Unimodal disibuion 2) Linea assumpions Minimum Mean quae Eo Reminde: he epeced value, o mean value, of a Coninuous andom vaiable is defined as: E [ ] p( ) d Minimum Mean quae Eo Wha is he mean of his disibuion? ( Z) his is difficul o obain eacly. Wih ou appoimaions, we can ge he esimae 2 such ha E[( ) ] is minimied. Accoding o he Fundamenal heoem of Esimaion heoy his esimae is: MME E[ Z] p( Z) d Z Fundamenal heoem of Esimaion heoy -> alman file opimaliy he minimum mean squae eo esimao equals he epeced (mean) value of condiioned on he obsevaions Z he minimum mean squae eo em is quadaic: E[( ) Is minimum can be found by aking he deivaive of he funcion w.. and seing ha value o 0. 2 ( E[( ) Z]) 0 When hey use he Gaussian assumpion, Maimum A oseioi esimaos and MME esimaos find he same value fo he paamees. his I can be eplained (ouside he scope of he couse) because mean and he mode of a Gaussian disibuion ae he same. 2 Z ]

10 alman File Componens (also known as: Way oo Many Vaiables ) Linea discee ime dynamic sysem (moion model) w G B u F Measuemen equaion (senso model) n ae ansiion funcion Conol inpu funcion Noise inpu funcion wih covaiance Q ae Conol inpu ocess noise ae enso eading enso noise wih covaiance R enso funcion Noe:Wie hese down & emembe hem!!! Compuing he MME Esimae of he ae and Covaiance Given a se of measuemens: }, { i Z i Accoding o he Fundamenal heoem of Esimaion, he sae and covaiance will be: ] ) [( ] [ 2 MME MME Z E Z E We will now use he following noaion: ] [ ] [ ] [ Z E Z E Z E Compuing he MME Esimae of he ae and Covaiance Wha is he minimum mean squae eo esimae of he sysem sae and covaiance? u B F Esimae of he sae vaiables Esimae of he senso eading G G Q F F Covaiance mai fo he sae R Covaiance mai fo he sensos A las! he alman File opagaion (moion model): G Q G F F B u F Updae (senso model): R

11 bu wha does ha mean in English?!? opagaion (moion model): Updae (senso model): - ae esimae is updaed fom sysem dynamics - Unceainy esimae GROW - Compue epeced value of senso eading - Compue he diffeence beween epeced and ue - Compue covaiance of senso eading - Compue he alman Gain (how much o coec es.) - Muliply esidual imes gain o coec sae esimae - Unceainy esimae RIN G Q G F F B u F R alman File Block Diagam Eample : imple D Linea ysem Given: FG, u0 Iniial sae esimae 0 Linea sysem: n w opagaion: Updae: Q Unknown noise paamees R ae Esimae

12 ae Esimaion Eo vs 3σ Region of Confidence enso Residual vs 3σ Region of Confidence alman Gain and ae Covaiance Eample 2: imple D Linea ysem wih Eoneous a Given: FG, ucos(5) Iniial sae esimae 20 Linea sysem: ) 5 cos( n w opagaion: Updae: (no change) Q ) 5 cos( Unknown noise paamees R

13 ae Esimae ae Esimaion Eo vs 3σ Region of Confidence enso Residual vs 3σ Region of Confidence alman Gain and ae Covaiance

14 ome obsevaions he lage he eo, he smalle he effec on he final sae esimae If pocess unceainy is lage, senso updaes will dominae sae esimae If senso unceainy is lage, pocess popagaion will dominae sae esimae Impope esimaes of he sae ando senso covaiance may esul in a apidly diveging esimao As a ule of humb, he esiduals mus always be bounded wihin a ±3σ egion of unceainy his measues he healh of he file Many popagaion cycles can happen beween updaes Using he alman File fo Mobile Robos enso modeling he odomey esimae is no a eflecion of he obo s conol sysem is ahe eaed as a senso Insead of diecly measuing he eo in he sae veco (such as when doing acking), he eo in he sae mus be esimaed his is efeed o as he Indiec alman File ae veco fo obo moving in 2D he sae veco is 3: [,y,θ] he covaiance mai is 33 oblem: Mobile obo dynamics ae NO linea oblems wih he Linea Model Assumpion Many sysems of inees ae highly non-linea, such as mobile obos In ode o model such sysems, a linea pocess model mus be geneaed ou of he non-linea sysem dynamics he Eended alman file is a mehod by which he sae popagaion equaions and he senso models can be lineaied abou he cuen sae esimae Lineaiaion will incease he sae eo esidual because i is no he bes esimae Dynamics alman File in N dimensions Received measuemen Ini edicion ep Updae ep bu epeced o eceive edicion esidual is a Gaussian andom vaiable whee he covaiance of he esidual is alman Gain (opimal coecion faco): Measuemens

15 Dynamics alman File in N dimensions Measuemens Dynamics alman File in N dimensions Measuemens Ini Ini edicion ep edicion ep Updae ep Updae ep Received measuemen bu epeced o eceive Received measuemen bu epeced o eceive edicion esidual is a Gaussian andom vaiable whee he covaiance of he esidual is alman Gain (opimal coecion faco): oenially epensive and eo-pone opeaion: mai invesion O( ^2.4) edicion esidual is a Gaussian andom vaiable whee he covaiance of he esidual is alman Gain (opimal coecion faco): Numeical eos may make he covaiance non symmeic a some poin. In pacice, we eihe foce symmey, o we decompose he covaianc duing he updae. ee Facoiaion mehods fo discee sequenial esimaion by Geald Bieman fo moe info. Appendi Claim: whee oof: Define whee