Kalman Filter. Wim van Drongelen, Introduction

Transcription

1 alman Filter Wim an Drongelen alman Filter Wim an Drongelen, 03. Introduction Getting to undertand a ytem can be quite a challenge. One approach i to create a model, an abtraction of the ytem. The idea i that thi abtraction capture the ytem important feature. On the other hand one might attempt to learn the ytem behaior by meauring it pontaneou output or it input-output relationhip. A peimit would ay that all model are wrong ince they only repreent part of the real thing, and all meaurement are wrong too becaue they are alo incomplete and noiy a well. Unfortunately, the peimit i correct with thi aement. The alman approach repreent a more optimitic iew; thi technique i to optimally combine the not o perfect information obtained ia both modeling and meaurement in order to gain the bet poible nowledge about a ytem tate. The baic idea i that we perform multiple (probably regular meaurement of a ytem output and that we ue thee meaurement and our nowledge of the ytem to recontruct the tate of the ytem. Practically, the alman filter proce conit of two tep: ( We mae an a priori etimate of the tate of the ytem before a meaurement i made. ( Subequently, after we hae made the meaurement, we compute a new a poteriori etimate by fuing the a priori prediction with thi latet meaurement. In our eample we will aume triial dynamic: i.e., our prediction of the net tate i the current tate. Of coure, when your ytem dynamic i more comple, the prediction tep mut reflect thi, but it wouldn t change tep, i.e. the procedure of the aimilation/fuion of a prediction with a meaurement. Page

2 alman Filter Wim an Drongelen For more in depth treatment of the alman filter, ee the tet of Brown and Hwang (997, Maybec (98, and epecially for neurocience application Schiff (0. The webite from Welch and Bihop lited in the reference are alo ery ueful.. Introductory Terminology Thi ection briefly ummarize of the mot important terminology required to follow the deelopment of the alman filter procedure in Section 3... Normal or Gauian Ditribution The noie component in the alman filter approach decribed in Section 3 are Gauian white noie term with zero mean. Recall that the PDF of the normal or Gauian ditribution i ( f ( e ( mean (zero in our cae, and tandard deiation Note: There are alman filter erion that deal with different type of noie procee... Minimum Mean-Square Error (MMSE An optimal fit of a erie of prediction with a erie of oberation can be achieed in eeral way. Uing the MMSE approach, one compute the difference between an etimate ŷ and a yˆ y target alue y. The quare of thi difference, gie an impreion how well the two fit. In cae of a erie of N etimate and target alue, one get a quantification of the error E by determining the um of the quare of their difference E ˆ N i y i y i, and a common technique i to minimize thi epreion with repect to the parameter of interet. For eample if our model i yˆ a, then a i the parameter we need to find. The minimum of E with repect to i i parameter a can be found by differentiating the epreion for the um of quare with repect to Page

3 alman Filter Wim an Drongelen thi parameter and etting thi deriatie equal to zero, i.e. E 0. In Section 3, the alman a filter i deeloped by uing the MMSE technique. For another eample ee Section 5.. in an Drongelen ( Recurie Algorithm Aume we meaure the reting potential of a neuron baed on a erie of meaurement: z, z,, z n. It eem reaonable to ue the etimated mean m of the n meaurement a the etimate of the reting potential: m n z z n... z n n. ( Thi equation in t an optimal bai for the deelopment of an algorithm. For each new meaurement we need more memory: i.e. we need n memory location for meaurement z - z n. In addition we need to complete all meaurement before we hae the etimate. With a recurie approach (a i ued in the alman filter algorithm one etimate the reting potential after each meaurement uing update for each meaurement t meaurement: m z (3 nd meaurement: m m z (4 3 rd meaurement: m 3 m z3 (5 3 3 Etc. Now we update the etimate at each meaurement and we only need memory for the preiou etimate and the new meaurement to mae a new etimate..4. Data Aimilation Suppoe we hae two meaurement y and y and we would lie to combine/fue thee oberation into a ingle etimate y. Further, the uncertainty of the two meaurement i gien by their ariance (=tandard deiation quared, and repectiely. If we follow the fuion principle ued in the alman approach, which i baed on MMSE, we will get Page 3

4 alman Filter Wim an Drongelen y (6 y y for the new etimate, and it ariance can be obtained from (7 By following thi procedure, we can obtain an improed etimate baed on two eparate one (Fig.. Fig. An eample of the fuion procedure i depicted in Fig.. Here we hae two oberation (blac and blue aociated with two ditribution with mean alue of 5 and 0 and tandard deiation and 3. Uing Eq (6 and (7 we find that the bet etimate i reflected by a ditribution with a mean of 5.5 and a tandard deiation of (red. From thi eample it i obiou that the meaurement with le uncertainty (blac cure ha more weight in the etimate while the one with more uncertainty (blue cure only contribute a little..5. State Model One way of characterizing the dynamic of a ytem i to decribe how it eole through a number of tate. If we characterize tate a a ector of ariable in a tate pace, we can decribe Page 4

5 alman Filter Wim an Drongelen the ytem dynamic by it path through tate pace. A imple eample i a moing object. If we tae an eample where there i no acceleration, it tate can be decribed by the coordinate of it poition in three dimenion: i.e., the 3D pace i it tate pace. The dynamic can be een by plotting poition. time, which alo gie it elocity d dt (i.e. the time deriatie of it poition. Let tae another eample, a econd order ordinary differential equation (ODE b c 0 (8 Now we define, and rewrite the ODE a (9a b c 0 (9b In thi way we rewrote the ingle nd order ODE in (8 a two t order ODE: (9a, b. Now we can define a ytem tate ector (0 Note that, unlie the preiou eample, thi ector i not the poition of a phyical object but a ector that define the tate of the ytem. The equation in (9a, b can now be compactly written a d 0 A, with A ( d t c b Here, the dynamic i epreed a an operation of matri A on tate ector. Thi notation can be ued a an alternatie to any ODE. Page 5

6 alman Filter Wim an Drongelen.6. Bayeian Analyi In the alman filter approach we update the probability ditribution function (i.e. the mean and ariance of the tate of a ytem baed on meaurement z, i.e. z p. Conditional probabilitie and ditribution are the topic of Bayeian tatitic and therefore the alman approach i a form of Bayeian analyi. Page 6

7 alman Filter Wim an Drongelen 3. Deriation of a alman Filter for a Simple Cae In thi ection, we derie a imple erion of a alman filter application following the procedure of Section 5.5 in Brown and Hwang (997. They how a deriation for a ector oberation, here I how the equialent implification for a calar. NOTES: - To facilitate comparion, the equation in thi ection are numbered a the (much more complicated equialent in the original tet (Brown and Hwang, All equation with a border are ued in the eample algorithm of the Matlab eample in Appendi and are alo hown in the flowchart in Fig.. Fig. Flowchart of the alman filter procedure deried in thi ection and implemented in the Matlab cript alman_vm (Appendi. In the Matlab cript, tep - are indicated. Step : Eq (5.5.7, Eq (5.5.8, Eq (5.5.; Step : Eq (5.5.3 and ( The equation in the diagram are deried in the tet. Page 7

8 alman Filter Wim an Drongelen We tart with a imple proce, for eample a neuron reting potential: i.e. it membrane potential in the abence of oert timulation. We aume that the update rule for the tate of the proce i w (5.5. The proce meaurement i z and i defined (a imple a poible a z (5.5. The Gauian white noie ariable w and model the proce and meaurement noie repectiely, and (for the ae of thi eample they are aumed tationary and ergodic. The mean for both i zero and the aociated ariance alue are w and. The coariance of w and i zero becaue they are independent. Now aume we hae an a priori etimate. In mot tet author include a hat : i.e. indicate one i dealing with an etimate. To implify notation, hat to denote etimate are omitted here. The a priori error we mae with thi etimate i aociated with thi error i gien by and the ariance E (5.5.7 ˆ to Now we perform a meaurement z and aimilate thi meaurement with our a priori etimate to obtain an a poteriori etimate, i.e. z (5.5.8 The factor in Equation (5.5.8 i the blending factor that determine to what etend the meaurement and a priori etimate affect the new a poteriori etimate. Page 8

9 alman Filter Wim an Drongelen Page 9 The a poteriori error we mae with thi etimate i and the ariance aociated with thi error i gien by E (5.5.9 Subtitution of Eq (5.5. in Eq (5.5.8 gie. If we plug thi reult in Eq (5.5.9, we get: E (5.5.0 We can epand thi epreion and tae the Epectation. Recall that i the a priori etimation error and uncorrelated with meaurement noie. The outcome of thi i (in cae thi in t obiou, detail of thee tep are ummarized in Appendi : Thi i uually written a: (, (5.5. or ( To optimize the a poteriori etimate uing optimal blending in Eq (5.5.8, we mut minimize the ariance (= quare of the etimated error with repect to, i.e. we differentiate the epreion for with repect to and et the reult equal to zero:

10 alman Filter Wim an Drongelen Page 0 0 ( Thi generate the epreion for an optimal, (. (5.5.7 NOTE: At thi point it i intereting to ealuate the reult. In the update Equation (5.5.8 you can ee what thi optimized reult mean: z (. If the a priori etimate i unreliable, indicated by it large ariance, we will ignore the etimate and we beliee more of the meaurement, i.e. z. On the other hand, if the meaurement i completely unreliable,, we ignore the meaurement and beliee the a priori etimate:. Now we proceed and ue the reult in (5.5.7 to obtain an epreion for : Subtitution of thi reult in (5.5. gie an epreion to find the a poteriori error baed on an optimized blending factor : 3 ( With a little bit of algebra we get,

11 alman Filter Wim an Drongelen which we implify to: (5.5. ( Here it hould be noted that depending on how one implifie Eq (5.5., one might get different epreion. Some wor better than other, depending on the ituation. We ue Eq (5.5. here becaue of it implicity. Latly we need to ue the information to mae a projection toward the net time tep occurring at t + (Fig., Step. Thi procedure depend on the model for the proce we are meauring from! Since we employ a imple model in thi eample, Eq (5.5., our a priori etimate of i imply the a poteriori etimate of the preiou time tep: (5.5.3 Here we ignore the noie (w term in Equation (5.5. ince the Epectation of w i zero. In many application thi tep can become more complicated or een an etremely comple procedure depending on the dynamic between t and t +. The a priori etimate of the ariance at t + i baed on the error of the prediction in Eq (5.5.3, e. If we ubtitute Equation (5.5. and (5.5.3 in thi epreion, we get e w. Since the a poteriori error at t i e, we can plug thi into the aboe epreion and we get e e w Page

12 alman Filter Wim an Drongelen Uing thi epreion for the a priori error, we ee that the aociated a priori ariance at t + depend on the a poteriori error at t and the ariance of the noie proce w: E e w. Becaue w and the error in the etimate are uncorrelated, we finally hae (5.5.5 w Note: If we integrate a noie term w (drawn from a zero mean normal ditribution with ariance t oer time, i.e. w dt, we get t t. So, in cae of a large interal between meaurement at t ( t and t, the uncertainty grow with a factor t and, conequently, ( t will become ery large, and the prediction ha almot no alue in the fuion of prediction and meaurement. Page

13 alman Filter Wim an Drongelen 4. Matlab Eample The eample Matlab cript alman_vm.m (lited in Appendi ue the aboe deriation to demontrate how thi could be applied to a recording of a membrane potential of a nontimulated neuron. In thi eample we aume that the reting potential Vm = -70 mv and that the tandard deiation of the proce noie and the meaurement noie are mv and mv repectiely. The program follow the flowchart in Fig. and a typical reult i depicted in Fig. 3. Due to the random apect of the procedure, your reult will ary lightly from the eample hown here. Fig.3 Eample of the alman filter application created with the attached Matlab cript alman_vm. The true alue are green, the meaurement are the red dot and the alman filter output i the blue line. Although the reult i not perfect, it can be een that the alman filter output i much cloer to the real alue than the meaurement. Page 3

14 alman Filter Wim an Drongelen 5. alman Filter: Notation (See Brown and Hwang (997, Section 5.5 ( (5.5.7 z (5.5.8 (5.5. ( (5.5.3 (5.5.5 w 6. alman Filter to Etimate Model Parameter (See Brown and Hwang (997, p. 5: Augmented Vector. The ector-matri erion of the alman filter can etimate a erie of ariable but can alo be ued to etimate model parameter. Mot of the time, the parameter of the model i a contant or change ery lowly o that it can be conidered a contant. In thi cae, one treat the parameter to etimate a a contant with noie, jut a we did with in Equation (5.5.. The noie component allow the algorithm of the alman filter to find the bet fit of the parameter. In addition, ince we don t meaure the parameter (mot liely becaue we cannot meaure it, we treat it a a hidden ariable, i.e. it i part of the model of the tate, but it doen t how up in the meaurement z. Note that and z are ector here, and that contain the ariable a well a the parameter we want to etimate, i.e. i augmented with the parameter. Page 4

15 alman Filter Wim an Drongelen Appendi : Matlab Script that Perform alman Filtering % alman_vm % % Eample uing a alman filter % to etimate the membrane potential % of a neuron % (Algorithm in't optimized % % WD 03 clear cloe all % Parameter N=000; % number of meaurement Vm=-70; % the membrane potential (Vm alue w=.^; % ariance proce noie =^; % meaurement noie % Initial Etimate _apriori(=vm+90; % firt mi-etimate of Vm % Thi i the a priori meaurement _apriori(=.; % a priori etimate of the error; here ariance =. % create a imulated meaurement ector % for i=:n; tru(i=vm+randn*w; z(i=tru(i+randn*; end; % Perform the alman filter % % Note: Malab indice tart at, and not % at 0 a in mot tetboo deriation % Equation are numbered a in the Handout and they % follow the numbering in Brown and Hwang (997 % Introduction to Random Signal and Applied alman % Filtering, Wiley & Son, New Yor for i=:n; % tep : Blending of Prediction and Meaurement % Compute blending factor (i (i=_apriori(i/(_apriori(i+; % Eq (5.5.7 % Update Etimate uing the meaurement % thi i the a poteriori etimate _apoteriori(i=_apriori(i+(i*(z(i-_apriori(i; % Eq (5.5.8 % Update the error etimate [Note that there % are eeral ariant for thi procedure; % here we ue the implet epreion _apoteriori(i=_apriori(i*(-(i; % Eq (5.5. % tep : Project Etimate for the net Step _apriori(i+=_apoteriori(i; % Eq (5.5.3 _apriori(i+=_apoteriori(i+w; % Eq (5.5.5 end; % plot the reult figure;hold; plot(z,'r.'; plot(tru,'g'; plot(_apoteriori ai([ N -00 0]; label ('Time (AU' ylabel ('Membrane Potential (mv' title ('alman Filter Application: true alue (green; meaurement (red.; alman Filter Output (blue' Page 5

16 alman Filter Wim an Drongelen Page 6 Appendi : Detail of the Step between Equation (5.5.0 and (5.5. Thi Appendi gie the detail of the tep between equation (5.5.0 and (5.5.. For conenience, we repeat the equation. E (5.5.0 We can epand thi epreion and tae the Epectation. If we epand the quared function, we get three term further ealuated in (-(3 below. ( The firt term can be rewritten uing Equation (5.5.7, i.e.: E ( The econd term E E The nd term in between the curly bracet in the aboe epreion, will anih when taing the Epectation becaue i the a priori etimation error which i uncorrelated with meaurement noie. Thu, uing equation (5.5.7 again, we can implify E (3 The third term i E. Firt we focu on the epreion in between the curly bracet firt and we get

17 alman Filter Wim an Drongelen Page 7 ( ( When we tae Epectation, the firt term of the implified epreion will anih (why?, o we get the following reult: E If we now collect all term from (-(3, we can rewrite Eq (5.5.0 a Thi i uually written a: (, (5.5. giing u the reult in equation (5.5.

18 alman Filter Wim an Drongelen Reference Brown R.G., Hwang P.Y.C. (997 Introduction to Random Signal and Applied alman Filtering. John Wiley & Son, New Yor, NY. Maybec PS (98 Stochatic Model, Etimation, and Control, Vol. 4, Academic Pre, New Yor. Schiff S.J. (0 Neural Control Engineering: The Emerging Interection between Control Theory and Neurocience. MIT Pre, Cambridge, MA. Drongelen W an (007 Signal Proceing for Neurocientit: An Introduction to the Analyi of Phyiological Signal, Academic Pre, Eleier, Amterdam Welch G, Bihop G An Introduction to the alman Filter, Unierity of North Carolina or a more complete tet at Page 8

19 alman Filter Wim an Drongelen HOMEWOR In thi quetion, the Equation are numbered a in the Handout alman Filter. Gie the epreion for etimate m n at the n th meaurement, uing a recurie approach a in Eq (3-(5 In thi quetion, the Equation are numbered a in the Handout alman Filter. Why doe (in the alman data fuion adding a meaurement alway reult in a decreae of the uncertainty (=ariance of the etimate? Would thi till be true if or wa infinite (i.e. total uncertainty? Hint: ue Eq (7. 3 In thi quetion, the Equation are numbered a in the Handout alman Filter. Write the following 3 rd order ODE in the form of a tate ector and matri operator b c d 0. Now repeat thi for b c d e. Gie the detail of the procedure, i.e. define the tate and matri A (ee Equation (8 (. 4 We hae a erie of meaurement y n and ytem tate n. We want to lin both uing a imple linear model: y n = a n. Ue the MMSE method to find an optimal etimate for a. Comment on how thi reult relate to linear regreion. 5 In thi quetion, unle tated otherwie, the Equation are numbered a in the Handout alman Filter. a. Show that Eq (6 and (7 are the ame a the alman approach to fuion of data. y y Hint : Combine Eq (5.5.8 and (5.5.7; add reult and rearrange the term a in Eq (6. Hint : Combine Eq (5.5. and (5.5.7; rearrange Eq (7 a and compare the reult. to the Page 9

20 alman Filter Wim an Drongelen b. Dicu the difference of combining ariance (noie between equation (7 in thi handout and Equation 3.5 in Van Drongelen (007. Page 0