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

Augmened Realiy II Kalman Filers Gudrun Klinker May 25, 2004

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

Lieraure G. Welch and G. Bishop, An Inroducion o he Kalman Filer, SIGGRPAPH 2001 Course 8. hp://www.cs.unc.edu/~welch/kalman A. Gelb edior, Applied Opimal Esimaion

Moivaion s Sensor measuremens

Moivaion s Sensor measuremens complex moion or noisy daa?

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

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

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

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

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

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

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.

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!

Discree Kalman Filer Modeled Process

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

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

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

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

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

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

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

Discree Kalman Filer Compuing Model Parameers

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

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

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

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

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 [ ]

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

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

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

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

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

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

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

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

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

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

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!

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

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

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

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