Augmented Reality II - Kalman Filters - Gudrun Klinker May 25, 2004

Transcription

1 Augmened Realiy II Kalman Filers Gudrun Klinker May 25, 2004

2 Ouline Moivaion Discree Kalman Filer Modeled Process Compuing Model Parameers Algorihm Exended Kalman Filer Kalman Filer for Sensor Fusion

3 Lieraure G. Welch and G. Bishop, An Inroducion o he Kalman Filer, SIGGRPAPH 2001 Course 8. hp:// A. Gelb edior, Applied Opimal Esimaion

4 Moivaion s Sensor measuremens

5 Moivaion s Sensor measuremens complex moion or noisy daa?

6 Moivaion s Sensor measuremens complex moion or noisy daa? Moion model

7 Moivaion s Sensor measuremens complex moion or noisy daa? Moion model e.g., consan speed: s = v

8 Moivaion s Sensor measuremens complex moion or noisy daa? Moion model e.g., consan speed: Moion predicion s = v

9 Moivaion s Sensor measuremens complex moion or noisy daa? Moion model e.g., consan speed: Moion predicion s + D = s + v D s = v

10 Moivaion s Sensor measuremens complex moion or noisy daa? Moion model e.g., consan speed: Moion predicion s + D = s + v D New measuremen s = v

11 Moivaion s Sensor measuremens complex moion or noisy daa? Moion model e.g., consan speed: Moion predicion s + D = s + v D New measuremen s = v updae model o some exen

12 Rudolf Emil Kalman Born in Budapes, 1930 B.S., M.S from MIT Ph.D. from Columbia U. Professor a Sanford U. and U. Florida Many awards Seminal paper: A new Approach o Linear Filering and Predicion Problems, Transacions ASME, 1960.

13 Kalman Filer Opimal daa processing algorihm Major use: filer ou noise of measuremen daa bu can also be applied o oher fields, e.g. Sensor Fusion Resul: Compues an opimal esimaion of he sae of an observed sysem based on measuremens Ieraive Opimal: incorporaes all informaion i.e. measuremen daa ha can be provided o i Does no need o keep all previous measuremen daa in sorage!

14 Discree Kalman Filer Modeled Process

15 Discree Kalman Filer Modeled Process Sysem sae vecor x k a ime sep k 1 x n vecor process noise w k wih pw ~ N0,Q and n x n covariance marix Q ofen assumed o be consan

16 Discree Kalman Filer Modeled Process Sysem sae vecor x k a ime sep k 1 x n vecor process noise w k wih pw ~ N0,Q and n x n covariance marix Q ofen assumed o be consan Sae ransiion marix A n x n marix ofen assumed o be consan

17 Discree Kalman Filer Modeled Process Sysem sae vecor x k a ime sep k 1 x n vecor process noise w k wih pw ~ N0,Q and n x n covariance marix Q ofen assumed o be consan Sae ransiion marix A n x n marix ofen assumed o be consan Conrol inpu vecor u k opional 1 x l vecor n x l marix B

18 Discree Kalman Filer Modeled Process Sysem sae vecor x k a ime sep k 1 x n vecor process noise w k wih pw ~ N0,Q and n x n covariance marix Q ofen assumed o be consan Sae ransiion marix A n x n marix ofen assumed o be consan Conrol inpu vecor u k opional 1 x l vecor n x l marix B Incremenal sae change: x k = Ax k1 + Bu k + w k1

19 Discree Kalman Filer Modeled Process Measuremen vecor z k 1 x m vecor measuremen noise v k wih pv ~ N0,R and m x m covariance marix R ofen assumed o be consan

20 Discree Kalman Filer Modeled Process Measuremen vecor z k 1 x m vecor measuremen noise v k wih pv ~ N0,R and m x m covariance marix R ofen assumed o be consan Measuremen predicion marix H m x n marix ofen assumed o be consan

21 Discree Kalman Filer Modeled Process Measuremen vecor z k 1 x m vecor measuremen noise v k wih pv ~ N0,R and m x m covariance marix R ofen assumed o be consan Measuremen predicion marix H m x n marix ofen assumed o be consan Measuremen predicion: z k = Hx k + v k

22 Discree Kalman Filer Compuing Model Parameers

23 Discree Kalman Filer Compuing Model Parameers Procedure: ˆ x k1 use bes esimae of sae x k1 a ime sep k1

24 Discree Kalman Filer Compuing Model Parameers Procedure: use bes esimae of sae x k1 a ime sep k1 ˆ x k ˆ x k1 predic sae a ime sep k a priori sae esimae

25 Discree Kalman Filer Compuing Model Parameers Procedure: use bes esimae of sae x k1 a ime sep k1 predic sae a ime sep k a priori sae esimae obain real measuremen z k ˆ x k ˆ x k1

26 Discree Kalman Filer Compuing Model Parameers Procedure: ˆ x k1 use bes esimae of sae x k1 a ime sep k1 ˆ x k predic sae a ime sep k a priori sae esimae obain real measuremen z k compue sae updae x ˆ k a ime sep k a poseriori sae esimae using gain marix K ˆ x k = ˆ x k + Kz k Hˆ x k

27 Discree Kalman Filer Compuing Model Parameers A priori sae esimae A priori esimae error A priori esimae covariance A poseriori sae esimae A poseriori esimae error A poseriori esimae covariance x ˆ k e k = x k x ˆ k [ ] P k = E e k e k T ˆ x k e k = x k ˆ x k P k = E e k e k T [ ]

28 Discree Kalman Filer s A poseriori esimae ˆ x k ˆ x k Compuing Model Parameers

29 Discree Kalman Filer s A poseriori esimae is ˆ x k z k linear combinaion of difference beween measuremen z k ˆ x k Compuing Model Parameers

30 Discree Kalman Filer s A poseriori esimae is ˆ x k z k Hˆ x k linear combinaion of difference beween measuremen z k and measuremen predicion ˆ x k Hˆ x k Compuing Model Parameers

31 Discree Kalman Filer s A poseriori esimae is ˆ x k z k Hˆ x k linear combinaion of difference beween measuremen z k and measuremen predicion and a priori sae ˆ x k ˆ x k Hˆ x k x ˆ k x ˆ k = x ˆ k + Kz k Hˆ x k Compuing Model Parameers

32 Discree Kalman Filer s A poseriori esimae is ˆ x k z k Hˆ x k x ˆ k linear combinaion of difference beween measuremen z k and measuremen predicion and a priori sae x ˆ k = x ˆ k + Kz k Hˆ x k ˆ x k ˆ x k Hˆ x k residual, innovaion Kalman gain, blending facor Compuing Model Parameers

33 Discree Kalman Filer Compuing Model Parameers Kalman gain P K = k H T x ˆ HP k H T k = x ˆ k + Kz k Hˆ x k + R n x m marix minimizes he a poseriori error covariance T equaion P k = E e k e k [ ] residual, when R small a priori esimae, when P k small

34 Discree Kalman Filer Algorihm z k1 z k z k+1 Measuremens observed Measuremen Model measuremen equaion Saes of he sysem canno be observed x k1 x k x k+1 Process Model sae ransiion equaion

35 Discree Kalman Filer Algorihm Time Updae Predic Measuremen Updae Correc z k1 z k z k+1 Measuremens observed Measuremen Model measuremen equaion Saes of he sysem canno be observed x k1 x k x k+1 Process Model sae ransiion equaion

36 Discree Kalman Filer Algorihm Time Updae Predic Measuremen Updae Correc Time updae: predic Measuremen updae: correc x ˆ k = Aˆ x k1 + Bu k P k = AP k1 A T + Q K k = P k H T HP k H T + R 1 x ˆ k = x ˆ k + K k z k Hˆ x k P k = I K k HP k

37 Exended Kalman Filer EKF Nonlinear process model Nonlinear measuremen model Linearize esimaion around he curren esimae using parial derivaives of he process and measuremen funcions Fundamenal flaw: disribuions densiies of random variables are no longer normal EKF ad hoc sae esimaor ha approximaes opimaliy of Bayes rule by linearizaions

38 How o use a Kalman Filer Find a sae represenaion Find a process model Find a measuremen model There are many ways o apply a Kalman Filer, i.e. i depends on he chosen models!

39 Kalman Filer for Sensor Fusion SCAAT Welch and Bishop Sae x: pose and derivaives Process model: Sae ransiion via A: Sysem noise:,,,,,,,,,,, g b a g b a & & & & & & r z y x z y x x = w x A x d d d r r r + = ; ; y y y y y d d d = + = & & & 0, Q N w i i Æ r

40 Kalman Filer for Sensor Fusion SCAAT Welch and Bischop Individual sensor model for sensor i Measuremen funcion h i wih Jacobian H i Measuremen noise,, v c b x h z i i i r r r r r + = ] [,, ] [ ], [,, ˆ k c b x h l x l k c b x H i i r r r r = 0, R N v i i Æ r

41 Kalman Filer for Sensor Fusion SCAAT Welch and Bishop Single consrain a a ime Asynchronous algorihm Each ime a new measuremen z becomes available, a new esimae x is compued Sensor 2 Sensor 3 Kalman Fusion Filer Sensor 1

42 Algorihm: ˆ Q A P A P x A x T d d d d d d + = = [ ] [ ] ˆ ˆ ˆ P KH I P z z K x x i = + = 1. Predic 2. Correc,, ˆ,, ˆ ˆ c b x H H c b x h z R H HP H P K i i i T T = = + = Kalman Gain Corresponding Jacobian Prediced measuremen i Kalman Filer for Sensor Fusion SCAAT Welch and Bishop

43