Notes on Kalman Filtering

Transcription

1 Noes on Kalman Filering Brian Borchers and Rick Aser November 7, Inroducion Daa Assimilaion is he problem of merging model predicions wih acual measuremens of a sysem o produce an opimal esimae of he curren sae of he sysem and/or predicions of he fuure sae of he sysem. For example, weaher forecasers run massive compuaional models ha predic winds, emperaure, ec. As ime progresses, i is imporan o incorporae available weaher observaions ino he mahemaical model. Since hese weaher observaions are noisy, he problem of incorporaing he observaions ino he model is inherenly saisical in naure. Daa Assimilaion is becoming a very ho opic in many areas of science, including amospheric physics, oceanography, and hydrology. In he nex few lecures, we ll inroduce Kalman filering, which is one of he simples approaches o daa assimilaion. The Kalman filer was inroduced in a 96 paper by R. E. Kalman. The Model Of The Sysem Consider a discree ime dynamical sysem governed by he equaion x k = Ax k + Bu k + w k. () Here, x k, u k, and w k are vecors and he subscrips refer o he ime seps raher han indexing elemens of he vecors. The sae of he sysem a ime k is given by he vecor x k. Deerminisic inpus o he sysem a ime k are given by u k. Random noise affecing he sysem a ime k is given by w k. We ll assume ha w k has a mulivariae normal disribuion wih mean and covariance marix Q. We ll obain a vecor of measuremens z k a ime k, where z k is given by z k = Hx k + v k. () Here v k represens random noise in he observaion z k. We ll assume ha v k is normally disribued wih mean and covariance marix R.

2 The marices A, B, H, Q, and R are all assumed o be known, alhough he Kalman filer can be exended o simulaneously esimae hese marices along wih x k. For now, our goal is o esimae x k and predic x k+, x k+,..., as accuraely as possible given z, z,..., z k. The esimae ha we will obain will come in he form of a mulivariae normal disribuion wih a specified mean ˆx k and covariance marix ˆP k. We will wan o measure he ighness of his mulivariae normal disribuion. A convenien measure of he ighness of an MVN disribuion wih covariance marix C is race(c) = C, + C, C n,n. (3) race(c) = V ar(x ) + V ar(x ) V ar(x n ). (4) Example For example, x k migh be a six elemen vecor conaining he posiion (3 coordinaes) and velociy (3 coordinaes) of an aircraf a ime k. The vecor u k migh represen conrol inpus (hrus, elevaor, rudder, ec.) o he aircraf a ime k, and w k migh represen he effecs of urbulence on he aircraf. We may be using a very simple radar o observe he aircraf, so ha we ge measuremens of he posiion, z, bu no he velociy of he aircraf a each momen in ime. These measuremens of he aircraf s posiion migh also be noisy. In many cases, he sysem ha we re ineresed in is described by a sysem of differenial equaions in coninuous ime: x = Ax + Bu. (5) We can discreize his sysem of equaions using ime seps of lengh, o ge x( + ) = x + x. (6) x( + ) = x + (Ax + Bu). (7) Leing x k = x( + ) and x k = x, we ge x k = (I + A)x k + Bu k. (8) In many pracical applicaions of Kalman filering he mahemaical model of he sysem consiss of an even more complicaed sysem of parial differenial equaions. Such sysems are commonly discreized using finie difference or finie elemen mehods. Raher han diving ino he deails of he numerical analysis used in discreizing PDE s, we will simply assume ha our problem has been cas in he form of (). The Kalman Filer We have wo sources of informaion ha can help us in esimaing he sae of he sysem a ime k. Firs, we can use he equaions ha describe he dynamics of he sysem. Subsiuing w k = ino (), we migh reasonably esimae ˆx k = Ax k + Bu k (9)

3 A second useful source of informaion is our observaion z k. We migh pick ˆx k so as o minimize z k Hx k. There s an obvious rade-off beween hese wo mehods of esimaing x k. The Kalman filer produces a weighed combinaion of hese wo esimaes ha is opimal in he sense ha i minimizes he uncerainy of he resuling esimae. We ll begin he esimaion process wih an iniial guess for he sae of he sysem a ime. Since we wan o keep rack of he uncerainy in our esimaes, we ll have o specify he uncerainy in our iniial guess. We describe his by using a mulivariae normal disribuion x N(ˆx, ˆP ). () In he predicion sep, we are given an esimae ˆx k of he sae of he sysem a ime k, wih associaed covariance marix ˆP k. We subsiue he mean value of w k = ino () o obain he esimae ˆx k = Aˆx k + Bu k. () The minus superscrip is used o disinguish his esimae from he final esimae ha we ge afer including he observaion z k. The covariance of our new esimae is ˆP k = Cov(ˆx k ). () ˆP k = Cov(Aˆx k + Bu k + w k ). (3) The Bu k erm is no random, so is covariance is zero. The covariance of w k is Q. The covariance of Aˆx k is A Cov(ˆx k )A T. Thus ˆP k = ACov(ˆx k )A T + Q. (4) ˆP k = A ˆP k A T + Q. (5) We could simply repea his process for x, x,.... If no observaions of he sysem are available, ha would be an appropriae way o esimae he sysem sae. In he updae sep, we modify he predicion esimae o include he observaion. ˆx k = ˆx k + K k(z k H ˆx k ) (6) ˆx k = (I K k H)ˆx k + K kz k. (7) Here he facor K k is called he Kalman gain. I adjuss he relaive influence of z k and ˆx k. We will soon show ha K k = ˆP k HT (H ˆP k HT + R) (8) is opimal in he sense ha i minimizes he race of ˆP k. The covariance of our updaed esimae is ˆP k = Cov(ˆx k ). (9) 3

4 ˆP k = Cov((I K k H)ˆx k + K kz k ). () ˆP k = (I K k H) ˆP k (I K kh) T + K k Cov(z k )Kk T. () Since Cov(z k ) = R, ˆP k = (I K k H) ˆP k (I K kh) T + K k RKk T. () This simplifies o ˆP k = ˆP k K kh ˆP k ˆP k HT Kk T + K k (H ˆP k HT )Kk T + K k RKk T. (3) ˆP k = ˆP k K kh ˆP k ˆP k HT K T k + K k (H ˆP k HT + R)K T k. (4) We wan o minimize he race of ˆP k. Using vecor calculus, i can be shown ha race( ˆP k ) K k = (H ˆP k )T + K k (H ˆP k HT + R). (5) Seing he derivaive equal o, (H ˆP k )T + K k (H ˆP k HT + R) =. (6) K k = (H ˆP k )T (H ˆP k HT + R). (7) K k = ˆP k HT (H ˆP k HT + R). (8) Using his opimal Kalman gain, ˆP k simplifies furher. ˆP k = ˆP k K kh ˆP k ˆP k HT K T k + K k (H ˆP k HT + R)K T k. (9) ˆP k = ˆP k ˆP K HT (H ˆP k HT + R) H ˆP k ˆP k HT ( ˆP K HT (H ˆP k HT + R) )) T + ˆP K HT (H ˆP k HT + R) (H ˆP k HT + R)( ˆP k HT (H ˆP k HT + R) ) T (3) ˆP k = ˆP k ˆP k HT (H ˆP k HT + R) H ˆP k (3) ˆP k = (I K k H) ˆP k. (3) The algorihm can be summarized as follows. For k =,,...,. Le ˆx k = Aˆx k + Bu k.. Le ˆP k = A ˆP k A T + Q. 3. Le K k = ˆP k HT (H ˆP k HT + R). 4. Le ˆx k = ˆx k + K k(z k H ˆx k ). 5. Le ˆP k = (I K k H) ˆP k. 4

5 In pracice, we may no have an observaion a every ime sep. In ha case, we can use predicions a each ime sep and compue updaes seps whenever observaions become available. Example In his example, we ll consider a sysem governed by he second order differenial equaion y +.y + y = sin (33) wih he iniial condiions y() =., y () =.5. We mus firs use a sandard rick o conver his second order ordinary differenial equaion ino a sysem of wo firs order differenial equaions. Le and The relaion beween x and x is Also, (33) becomes x = y (34) x = y. (35) x = x. (36) x = x.x + sin. (37) This sysem of wo firs order equaions can be wrien as where and x = Ax + Bu (38) [ A = B = [ u =. [ sin ], (39) ], (4) ]. (4) This sysem of differenial equaions will be discreized using (8) wih ime seps of =.. A each ime sep, he sae vecor will be randomly perurbed wih N(, Q), noise, where [ ].5. Q =. (4)..5 We will observe x once per second (every ime seps.) Thus H = [ ]. (43) 5

6 Our observaions will have a variance of.5. For he iniial condiions we will begin wih he esimae [ ] ˆx = (44) and covariance ˆP = [ ]. (45) Figure shows he rue sae of he sysem. Figure shows he esimae of he sysem sae using only predicion seps. The doed lines in his plo are one sandard-deviaion error bars. The iniial uncerainy in x is due o uncerainy in he iniial condiions. Laer, his uncerainy increases due o he effec of noise on he sae of he sysem. Figure 3 shows he Kalman filer esimaes including observaions of x once per second. Alhough he iniial uncerainy is quie high, he Kalman filer quickly learns he acual sae of he sysem and hen racks i quie closely. Each circle on he x plo represens an observaion of he sysem. Noice ha when an observaion is obained he Kalman esimae jumps o incorporae he new observaion. Also noe ha alhough we only observe x, he Kalman filer also manages o rack x. This happens because he sysem of differenial equaions connecs x and x. Figure 4 shows he rue sae of he sysem and he Kalman filer esimae on he same plo. Figure 5 shows he differences beween he sysem sae and he simple predicion. Figure 6 shows he difference beween he sysem sae and he Kalman predicion. Noice ha he Kalman filer produced much igher esimaes of boh x and x using only a few observaions of x. 6

7 x x Figure : Plo of he sysem sae x. xpred xpred Figure : Esimae of x using predicion seps only. 7

8 xpredk xpredk Figure 3: Kalman esimaes of he x and x. x xrue xpredk x Figure 4: Kalman esimae versus he rue values of x and x. 8

9 xpred x xpred x Figure 5: Difference beween he sysem sae and predicion esimae. xpredk x xpredk x Figure 6: Difference beween he sysem sae and Kalman esimae. 9

10 The Exended Kalman Filer The Exended Kalman Filer (EKF) exends he Kalman filering concep o problems wih nonlinear dynamics. Our new equaion for he ime evoluion of he sysem sae will be of he form x k = f(x k, u k, w k ) (46) where w k is a random perurbaion of he sysem. This ime, we ll assume ha w k has a mulivariae normal disribuion wih mean and covariance marix Q k. Tha is, he covariance is allowed o be ime dependen. Our new measuremen model will be z k = h(x k, v k ) (47) where v k is a mulivariae normal N(, R k ) noise vecor. The predicion sep is a sraigh forward generalizaion of wha we have previously done in he Kalman filer. ˆx k = f(ˆx k, u k, ). (48) We ll also inroduce a new noaion for he prediced observaion ẑ k = h(ˆx k, ). (49) In general, for a nonlinear funcion f, ˆx k will no have a mulivariae normal disribuion. However, we can reasonably hope ha f(x, u, w) will be approximaely linear for relaively small changes in x and w, so ha ˆx k will be a leas approximaely normally disribued. We linearize f(x, u, w) around (ˆx k, u k, ) as f(x, u, w) = f(ˆx k, u k, ) + A k (x ˆx k ) + W k (w ) (5) where A and W are marices of parial derivaives of f wih respec o x and w. Noe ha since u k is assumed o be known exacly, we don need o linearize in he u variable. The enries in A k and W k are given by A i,j,k = f i(ˆx k, u k, ) x j. (5) W i,j,k = f i(ˆx k, u k, ) w j. (5) Using his linearizaion, we end up wih an approximae covariance marix for ˆx k, ˆP k = A k ˆP k A T k + W k Q k W T k. (53) Similarly, we can linearize h(). Le H i,j,k = h i(ˆx k, ) x j. (54)

11 and Now, le V i,j,k = h i(ˆx k, ) v j. (55) ê x k = x k ˆx k (56) ê z k = z k ẑ k. (57) We don acually know x k, bu we do expec x k ˆx k o be relaively small. Thus we can use our linearizaion of f() o derive an approximaion for ê x k. By he linearizaion, ê x k = f(x k, u k, w k ) f(ˆx k, u k, ). (58) ê x k A k (x k ˆx k ) + ɛ k (59) where ɛ k accouns for he effec of he random w k. The disribuion of ɛ k is N(, W k Q k Wk T ). Similarly, By he linearizaion his is approximaely where η k has an N(, V k R k Vk T ) disribuion. Ideally, we could updae ˆx k o ge x k by ê z k = h(x k, v k ) h(ˆx k, ). (6) ê z k Hê x k + η k (6) x k = ˆx k + ê x k. (6) Of course, we don know ê x k, bu we can esimae i. Le ê xk = K k (z k ẑ k ) (63) where K k is a Kalman gain facor o be deermined. Then le ˆx k = ˆx k + ê x k. (64) By a derivaion similar o our earlier derivaion of he opimal Kalman gain for he linear Kalman filer, i can be shown ha he opimal Kalman gain for he EKF is K k = ˆP k HT k (H k ˆP k HT k + V k R k Vk T ). (65) Using his opimal Kalman gain, he covariance marix for he updaed esimae ˆx k is ˆP k = (I K k H k ) ˆP k. (66)

12 The EKF algorihm can be summarized as follows. For k =,,...,. Le ˆx k = f(ˆx k, u k, ).. Le ˆP k = A k ˆP k A T k + W k Q k W T k. 3. Le K k = ˆP k HT k (H k ˆP k HT k + V kr k V T k ). 4. Le ˆx k = ˆx k + K k(z k h(ˆx k, ))). 5. Le ˆP k = (I K k H k ) ˆP k. The Ensemble Kalman Filer A fundamenal problem wih he EKF is ha we mus compue he parial derivaives of f() so ha hey re available for compuing he covariance marix in he predicion sep. An alernaive approach involves using Mone Carlo simulaion. In he Ensemble Kalman Filer (EnKF), we generae a collecion of sae variables according o he MVN disribuion and ime k =, and hen follow he evoluion of his ensemble hrough ime. In he following, we ll assume ha our sysem sae evolves according o and ha our observaion model is x k = f(x k, u k, w k ) (67) z k = Hx k + v k. (68) As in he exended Kalman filer, we predic x k wih ˆx k = f(ˆx k, u k, ). (69) Insead of using parial derivaives of f() o esimae ˆP k, we ll use Mone Carlo simulaion. Suppose ha we re given a collecion of random sae vecors a ime k, ˆx i k, i =,,..., m. For each random sae vecor, we can generae a random N(, Q k ) vecor wk i, and hen updae he vecor wih ˆx i, k = f(ˆx i k, u k, w i k ), i =,,..., m. (7) Now, we can esimae he covariance marix ˆP k from he vecors ˆxi, k. Le C be his esimae of ˆP k. A ime k, we obain a new observaion z k, which is assumed o include MVN N(, R) noise. By adding N(, R) noise o z k, we obain an ensemble of simulaed observaions, zk i, i =,,..., m. Nex, we updae our ensemble of soluions wih ˆx i k = ˆx i, k + CH T (HCH T + R) (zk i H ˆx i, k ). (7) This is simply he Kalman filer updae, bu using he esimaed covariance marix C, and he Mone Carlo simulaed observaions zk i. Finally, we can use he ensemble of saes ˆx i k, i =,,..., m o esimae he mean sae, ˆx k, and he covariance ˆP k.