Data Fusion using Kalman Filter. Ioannis Rekleitis

Transcription

1 Daa Fusion using Kalman Filer Ioannis Rekleiis

2 Eample of a arameerized Baesian Filer: Kalman Filer Kalman filers (KF represen poserior belief b a Gaussian (normal disribuion A -d Gaussian disribuion is given b: ( 2 2 ( e 2 2 An n-d Gaussian disribuion is given b: ( (2 n e ( 2 ( CS-47 Inroducion o Roboics and Inelligen Ssems 2

3 Kalman Filer : a Baesian Filer Iniial belief Bel( is a Gaussian disribuion Wha do e do for an unknon saring posiion? Sae a ime + is a linear funcion of sae a ime : Observaions are linear in he sae: F ( acion o Error erms are zero-mean random variables hich are normall disribued hese assumpions guaranee ha he poserior belief is Gaussian he Kalman Filer is an efficien algorihm o compue he poserior Normall, an updae of his naure ould require a mari inversion (similar o a leas squares esimaor he Kalman Filer avoids his compuaionall comple operaion Bu ( observaion CS-47 Inroducion o Roboics and Inelligen Ssems 3

4 he Kalman Filer Moion model is Gaussian Sensor model is Gaussian Each belief funcion is uniquel characerized b is mean and covariance mari Compuing he poserior means compuing a ne mean and covariance from old daa using acions and sensor readings Wha are he ke limiaions? Unimodal disribuion 2 inear assumpions CS-47 Inroducion o Roboics and Inelligen Ssems 4

5 he Kalman Filer Kalman, 96 inear process and measuremen models Gaussian noise (or hie Gaussian sae esimae rior Measuremen Kalman filer poserior rocess model is A Bu q Measuremen model is z r CS-47 Inroducion o Roboics and Inelligen Ssems Images coures of Mabeck, 979 5

6 Wha e kno Wha e don kno We kno ha he conrol inpus of our process are We kno ha e ve old he ssem o do and have a model for ha he epeced oupu should be if everhing orks righ We don kno ha he noise in he ssem rul is We can onl esimae ha he noise migh be and r o pu some sor of upper bound on i When esimaing he sae of a ssem, e r o find a se of values ha comes as close o he ruh as possible here ill alas be some mismach beeen our esimae of he ssem and he rue sae of he ssem iself. We jus r o figure ou ho much mismach here is and r o ge he bes esimae possible CS-47 Inroducion o Roboics and Inelligen Ssems 6

7 Minimum Mean Square Error Reminder: he epeced value, or mean value, of a Coninuous random variable is defined as: E [ ] p( d Minimum Mean Square Error (MMSE Wha is he mean of his disribuion? ( Z his is difficul o obain eacl. Wih our approimaions, e can ge he esimae such ha E[( 2 Z ] is minimized. According o he Fundamenal heorem of Esimaion heor his esimae is: MMSE E[ Z] p( Z d CS-47 Inroducion o Roboics and Inelligen Ssems 7

8 Fundamenal heorem of Esimaion heor he minimum mean square error esimaor equals he epeced (mean value of condiioned on he observaions Z he minimum mean square error erm is quadraic: 2 E[( Z ] Is minimum can be found b aking he derivaive of he funcion.r. and seing ha value o. 2 ( E[( Z] I is ineresing o noe ha hen he use he Gaussian assumpion, Maimum A oseriori esimaors and MMSE esimaors find he same value for he parameers. his is because mean and he mode of a Gaussian disribuion are he same. CS-47 Inroducion o Roboics and Inelligen Ssems 8

9 Kalman Filer Componens (also knon as: Wa oo Man Variables inear discree ime dnamic ssem (moion model Sae Conrol inpu rocess noise F B u G Sae ransiion funcion Conrol inpu funcion Noise inpu funcion ih covariance Q Measuremen equaion (sensor model Sensor reading Sae Sensor noise ih covariance R z n Sensor funcion CS-47 Inroducion o Roboics and Inelligen Ssems Noe:Wrie hese don!!! 9

10 Compuing he MMSE Esimae of he Sae and Covariance CS-47 Inroducion o Roboics and Inelligen Ssems Given a se of measuremens: }, { i z Z i According o he Fundamenal heorem of Esimaion, he sae and covariance ill be: ] [( ] [ 2 MMSE MMSE Z E Z E We ill no use he folloing noaion: ] [ ] [ ] [ Z E Z E Z E

11 Compuing he MMSE Esimae of he Sae and Covariance Wha is he minimum mean square error esimae of he ssem sae and covariance? F B u z S Esimae of he sae variables Esimae of he sensor reading F F GQ G Covariance mari for he sae R Covariance mari for he sensors CS-47 Inroducion o Roboics and Inelligen Ssems

12 A las! he Kalman Filer CS-47 Inroducion o Roboics and Inelligen Ssems 2 ropagaion (moion model: G Q G F F B u F / / / / Updae (sensor model: S r K S K R S z z r z / / / / / / / / /

13 bu ha does ha mean in English?!? CS-47 Inroducion o Roboics and Inelligen Ssems 3 ropagaion (moion model: Updae (sensor model: - Sae esimae is updaed from ssem dnamics - Uncerain esimae GROWS - Compue epeced value of sensor reading - Compue he difference beeen epeced and rue - Compue covariance of sensor reading - Compue he Kalman Gain (ho much o correc es. - Mulipl residual imes gain o correc sae esimae - Uncerain esimae SRINKS G Q G F F B u F / / / / S r K S K R S z z r z / / / / / / / / /

14 Kalman Filer Block Diagram CS-47 Inroducion o Roboics and Inelligen Ssems 4

15 Eample : Simple D inear Ssem Given: F=G==, u= Iniial sae esimae = inear ssem: n z ropagaion: Updae: Q / / / / Unknon noise parameers S r K S K R S z r z / / / / / / / / / 5 CS-47 Inroducion o Roboics and Inelligen Ssems

16 Sae Esimae CS-47 Inroducion o Roboics and Inelligen Ssems 6

17 Sae Esimaion Error vs 3 Region of Confidence CS-47 Inroducion o Roboics and Inelligen Ssems 7

18 Sensor Residual vs 3 Region of Confidence CS-47 Inroducion o Roboics and Inelligen Ssems 8

19 Kalman Gain and Sae Covariance CS-47 Inroducion o Roboics and Inelligen Ssems 9

20 Eample 2: Simple D inear Ssem ih Erroneous Sar Given: F=G==, u=cos(/5 Iniial sae esimae = 2 inear ssem: / 5 cos( n z ropagaion: Updae: (no change Q / / / / / 5 cos( Unknon noise parameers S r K S K R S z r z / / / / / / / / / 2 CS-47 Inroducion o Roboics and Inelligen Ssems

21 Sae Esimae CS-47 Inroducion o Roboics and Inelligen Ssems 2

22 Sae Esimaion Error vs 3 Region of Confidence CS-47 Inroducion o Roboics and Inelligen Ssems 22

23 Sensor Residual vs 3 Region of Confidence CS-47 Inroducion o Roboics and Inelligen Ssems 23

24 Kalman Gain and Sae Covariance CS-47 Inroducion o Roboics and Inelligen Ssems 24

25 Some observaions he larger he error, he smaller he effec on he final sae esimae If process uncerain is larger, sensor updaes ill dominae sae esimae If sensor uncerain is larger, process propagaion ill dominae sae esimae Improper esimaes of he sae and/or sensor covariance ma resul in a rapidl diverging esimaor As a rule of humb, he residuals mus alas be bounded ihin a ±3 region of uncerain his measures he healh of he filer Man propagaion ccles can happen beeen updaes CS-47 Inroducion o Roboics and Inelligen Ssems 25

26 Using he Kalman Filer for Mobile Robos Sensor modeling he odomer esimae is no a reflecion of he robo s conrol ssem is raher reaed as a sensor Insead of direcl measuring he error in he sae vecor (such as hen doing racking, he error in he sae mus be esimaed his is referred o as he Indirec Kalman Filer Sae vecor for robo moving in 2D he sae vecor is 3: [,,q] he covariance mari is 33 roblem: Mobile robo dnamics are NO linear CS-47 Inroducion o Roboics and Inelligen Ssems 26

27 roblems ih he inear Model Assumpion Man ssems of ineres are highl non-linear, such as mobile robos In order o model such ssems, a linear process model mus be generaed ou of he non-linear ssem dnamics he Eended Kalman filer is a mehod b hich he sae propagaion equaions and he sensor models can be linearized abou he curren sae esimae inearizaion ill increase he sae error residual because i is no he bes esimae CS-47 Inroducion o Roboics and Inelligen Ssems 27

28 Approimaing Robo Moion Uncerain ih a Gaussian CS-47 Inroducion o Roboics and Inelligen Ssems 28

29 inearized Moion Model for a Robo Y G v V From a robo-cenric perspecive, he velociies look like his: From he global perspecive, he velociies look like his: V V sin cos he discree ime sae esimae (including noise looks like his: V V V V ( sin ( cos ( roblem! We don kno linear and roaional veloci errors. he sae esimae ill rapidl diverge if his is he onl source of informaion! 29 CS-47 Inroducion o Roboics and Inelligen Ssems

30 inearized Moion Model for a Robo he indirec Kalman filer derives he pose equaions from he esimaed error of he sae: In order o linearize he ssem, he folloing small-angle assumpions are made: sin cos No, e have o compue he covariance mari propagaion equaions. 3 CS-47 Inroducion o Roboics and Inelligen Ssems

31 ( Calculaion of CS-47 Inroducion o Roboics and Inelligen Ssems 3

32 Calculaion of... cos( sin( cos( cos( sin( cos( cos( cos( ] sin( sin( cos( [cos( cos( cos( cos( cos( cos( cos( cos( ( cos( v v v v v v v v v v v v v v v v v v 32 CS-47 Inroducion o Roboics and Inelligen Ssems

33 inearized Moion Model for a Robo v G W F V V sin cos cos sin From he error-sae propagaion equaion, e can obain he Sae propagaion and noise inpu funcions F and G : From hese values, e can easil compue he sandard covariance propagaion equaion: G G Q F F / / 33 CS-47 Inroducion o Roboics and Inelligen Ssems

34 Covariance Esimaion 34 CS-47 Inroducion o Roboics and Inelligen Ssems 2 2 / / ] [ here ] [ ] [ ] ( [( ] [ v E Q G Q G F F G G E F F E G F G F E E

35 f F v v f F F F F F f sin( cos( cos( sin( ( (, ( Alernaive Calculaion CS-47 Inroducion o Roboics and Inelligen Ssems 35

36 ropagaion Finall, for a mobile robo EKF propagaion sep e have: CS-47 Inroducion o Roboics and Inelligen Ssems / /, sin cos, cos sin here : sin cos v m m m Q G V V F G Q G F F V V

37 Sensor Model for a Robo ih a erfec Map Y G i =[ i, i ] z q q q n n n n z r r r r r i i i i, aan From he robo, he measuremen looks like his: he measuremen equaion is nonlinear and mus also be linearized! 37 CS-47 Inroducion o Roboics and Inelligen Ssems r r i r i r i r i r i i

38 Updae CS-47 Inroducion o Roboics and Inelligen Ssems 38 2 * * * * *( * * ( * * * * * / / K R K K I K I r K S K R S z z r

39 See Malab Eamples CS-47 Inroducion o Roboics and Inelligen Ssems 39

40 Sensor Model for a Robo ih a erfec Map Y G z n n n z From he robo, he measuremen looks like his: From a global perspecive, he measuremen looks like: n n n z cos sin sin cos he measuremen equaion is nonlinear and mus also be linearized! 4 CS-47 Inroducion o Roboics and Inelligen Ssems

41 Sensor Model for a Robo ih a erfec Map CS-47 Inroducion o Roboics and Inelligen Ssems 4 No, e have o compue he linearized sensor funcion. Once again, e make use of he indirec Kalman filer here he error in he reading mus be esimaed. In order o linearize he ssem, he folloing small-angle assumpions are made: sin cos he final epression for he error in he sensor reading is: n n n ( sin ( cos cos sin ( cos ( sin sin cos

42 Updaing he Sae Vecor CS-47 Inroducion ropagaion o Roboics and Inelligen onl Ssems ropagaion and updae 42

43 Eended Kalman Filer for SAM Sae vecor Epanded o conain enries for all landmarks posiions: Sae vecor can be gron as ne landmarks are discovered Covariance mari is also epanded R n N N N N N N R R R R RR 43 CS-47 Inroducion o Roboics and Inelligen Ssems

44 Eended Kalman Filer for SAM Kinemaic equaions for landmark propagaion V V i i i i i i V V ( sin ( cos ( 44 CS-47 Inroducion o Roboics and Inelligen Ssems

45 Eended Kalman Filer for SAM Sensor equaions for updae: Ver poerful because covariance updae records shared informaion beeen landmarks and robo posiions i n i R R 45 CS-47 Inroducion o Roboics and Inelligen Ssems

46 EKF for SAM CS-47 Inroducion o Roboics and Inelligen Ssems 46

47 Enhancemens o EKF Ieraed Eended Kalman Filer Ierae sae updae equaion unil convergence k k k k k k k k k k k k K S K r K S K R S z z r / / / / / / Sae and covariance updae 47 CS-47 Inroducion o Roboics and Inelligen Ssems

48 Enhancemens o he EKF Muliple hpohesis racking Muliple Kalman filers are used o rack he daa Muli-Gaussian approach allos for represenaion of arbirar probabili densiies Consisen hpohesis are racked hile highl inconsisen hpoheses are dropped Similar in spiri o paricle filer, bu orders of magniude feer filers are racked as compared o he paricle filer CS-47 Inroducion o Roboics and Inelligen Ssems 48