A Java Tool for Exploring State Estimation using the Kalman Filter

ISSC 2004, Bel as , June 30 - July 2 A Ja a Tool o Explo ing S a e Es ima ion using he Kalman Fil e Declan Delaney 1 and Tomas Wa d 2 1 Depa men o Compu e Science, Na ional Uni e si y o I eland, Maynoo h IRELAND E-mail: [email protected] 2 Depa men o Elec onic Enginee ing, Na ional Uni e si y o I eland, Maynoo h IRELAND E-mail: omas.wa [email protected] __________________________________________________________________________________________ Abs ac – This pape desc ibes a no el so wa e applica ion ha assis s in unde s anding he p ocess o es ima ing he s a e o a linea dynamic sys em based on noisy ou pu signal measu emen s. An in e ac i e Ja a ool, based on he Kalman Fil e , is desc ibed. This consis s o wo main pa s, a simple one-dimensional il e and a mul i-dimensional il e ool. The use can se all inpu pa ame e s h ough a single in e ace o by ollowing a se ies o guided s eps. Raw measu emen da a can be gene a ed au oma ically o inse ed manually by he use and he esul s o es ima ing he ue sys em s a e a e hen p esen ed as a se ies o g aphs in eal- ime. The applica ion is desc ibed h ough he use o wo simple examples. Such an applica ion could be used o each signal and sys ems enginee ing. Keywo ds – Kalman Fil e , Ja a applica ion, Signal and sys ems enginee ing, Compu e assis ed lea ning ________________________________________________________________________________________ I INTRODUCTION Fo mal enginee ing educa ion appea ed o he i s ime a ound he middle o he 18 h cen u y [1]. As he discipline e ol ed, a ious in en ions se he s anda ds o ‘mode n’ enginee ing. These in en ions called in o exis ence new indus ies and new s anda ds o educa ion and aining [2]. P io o Wo ld Wa II he ypical elec ical/elec onic enginee s udied e y li le undamen al science and ma hema ics, wi h he emphasis es ing on he acquisi ion o expe ience [3, 4]. By he end o Wo ld Wa II all his had changed as i was ealised ha undamen al knowledge o science led di ec ly o new applica ions in he mili a y domain, wi h consequen ial e ec s on enginee ing cu icula [5] [1]. Du ing he pas decade he Wo ld Wide Web and compu e -assis ed lea ning has again posed in e es ing challenges and possibili ies o app oaching he educa ion o elec ical and elec onic enginee s [6 -9]. Fundamen al unde s anding o key concep s in signals and sys ems is o en di icul o each success ully. In dynamic linea sys ems, he concep o es ima ing he s a e a iables om a se ies o noisy measu emen s is one such example. The mos widely accep ed algo i hm o pe o ming an es ima ion o his kind is he Kalman Fil e . This algo i hm is decep i ely simple and i s ho ough unde s anding encapsula es a numbe o enginee ing opics such as s a is ical modelling, enginee ing compu a ion, linea sys em dynamics, measu emen science and digi al signal p ocessing [10]. S udying he ope a ion o he Kalman il e he e o e leads o an app ecia ion o he in e -disciplina y na u e o elec onic enginee ing. Howe e , a deepe unde s anding o he heo y, and awa eness o p ac ical implemen a ion issues, can only be expe ienced by employing he il e in p ac ical si ua ions. A compu e -based ool ha allows expe imen s based on ‘wha i ’ scena ios would challenge use s, mo i a e hem o ask ques ions and acili a e he consolida ion o hei enginee ing knowledge. This pape desc ibes a no el Ja a implemen a ion o he Kalman Fil e , which aims o assis use s in unde s anding he e ec s o noise on he ou pu signals gene a ed by a linea dynamic sys em and on es ima ing he s a e o such a sys em. Two dis inc implemen a ions a e desc ibed - a wo-dimensional and a mul i-dimensional Kalman Fil e – and wo illus a i e examples a e p o ided. In sec ion II we b ie ly examine he o igins o he Kalman Fil e and desc ibe he heo y unde lying i s ope a ion. Sec ions III and IV desc ibe he wo- dimensional and mul i-dimensional Ja a implemen a ions espec i ely oge he wi h examples ha illus a e hei use. Inspi a ion o he ISSC 2004, Bel as , June 30 - July 2 applica ion de i ed om one o he au ho s’ indus ial expe ience in seismic explo a ion and om expe ience in eaching elec onic enginee ing s uden s. II THE KALMAN FILTER The i s me hod o o ming an op imal es ima e om noisy da a is he me hod o leas squa es, a disco e y a ibu ed o Ca l F ied ich Gauss in 1795 [10]. Du ing Wo ld Wa II No be Wiene wo ked on he p oblem o au oma ically con olling he di ec ion o an i-ai c a i e using noisy ada in o ma ion and de i ed he solu ion o he leas - mean-squa ed p edic ion e o in e ms o he au oco ela ion unc ions o he signal and noise. By combining p obabili y heo y and he idea o s a e a iables Kalman subsequen ly de i ed he Weine Fil e , which became known as he Kalman Fil e a) Wha is he Kalman Fil e ? The Kalman Fil e is named a e Rudol Kalman who i s in oduced he algo i hm in 1960. I has been employed in my iad applica ions including p ocess con ol sys ems, ehicle acking, ma ine na iga ion, geology, demog aphic es ima ion and s ock p ice p edic ion. I es ima es he ins an aneous s a e o a linea dynamic sys em pe u bed by Gaussian whi e noise by using measu emen s ha a e linea ly ela ed o he sys em s a e bu ha a e co up ed by Gaussian whi e noise. The il e ecu si ely minimizes he mean squa e es ima ion e o wi hou di ec ly obse ing he sys em s a e o knowing he na u e o he modelled sys em. We only obse e some measu emen s using an a ay o noisy senso s – see Figu e 1. The sys em s a e i sel e ol es wi h ime unde he e ec o andom pe u ba ions o con ol inpu s. The Kalman Fil e hen p o ides an op imal es ima e o he unobse ed sys em s a es and hei unce ain ies based on noisy measu emen s o he p ocess. The Kalman il e ope a es online so ha he bes es ima e o he sys em s a e and i s unce ain y can be compu ed by upda ing he p e ious es ima es wi h new measu emen s. b) Kalman Fil e Theo y An ou line desc ip ion o he Kalman Fil e algo i hm is p esen ed he e in equa ions (1) o (7). Fo a de ailed heo e ical explana ion o he Kalman Fil e e e o [10] and [11]. The Kalman Fil e a emp s o es ima e he n- dimensional s a e ec o x o a i s o de , disc e e- ime con olled p ocess wi h inpu s go e ned by a linea s ochas ic di e ence equa ion, and an m- dimensional measu emen ec o : κκκκκ wBuxx + + Φ = −−− 111 (1) κκκκ xHz + = (2) whe e k is he ime index, w k is he p ocess noise, k is he measu emen noise, Φ is he s a e ansi ion ma ix, B is he con ol ma ix, u is he con ol inpu ec o , H is he sensi i i y ma ix and z is he measu emen ec o . Equa ion (1) de ines he sys em dynamic model and equa ion (2) de ines he measu emen model. The Kalman Fil e i e a i ely applies wo s ages o compu a ions using eedback con ol: 1. ime upda e compu a ions ( he p edic ion) – equa ions (3) and (4); 2. measu emen upda e compu a ions ( he adjus ed p edic ion) – equa ions (5) o (7) 111 ˆˆ −−− −+Φ= κκκκ Buxx (3) 1111 −−−− −+ΦΦ= κκκκκ QPP T (4) ( ) 1− −− += κκκκκκκ RHPHHPK TT (5) ( ) −− −+= kkkkkk xHzKxx ˆˆˆ (6) ( ) − −= κκκκ PHKIP (7) Figu e 1: The Kalman il e es ima es he sys em s a e a iables based on a mul idimensional signal + noise as inpu Kalman Fil e Con ol inpu s Noise Sys em signal + noise Noise Measu ing de ice Es ima e o sys em s a e a iables Kalman Fil e Con ol inpu s Noise Sys em signal + noise Noise Measu ing de ice Es ima e o sys em s a e a iables ISSC 2004, Bel as , June 30 - July 2 whe e is he a p io i s a e es ima e a s ep k, κ x ˆ is he a pos e io i s a e es ima e a s ep k, P k - is he a p io i es ima e e o co a iance ma ix, P k-1 is he a pos e io i es ima e e o co a iance ma ix , K n is he Kalman Gain ma ix. Equa ion (3) is he S a e es ima ion ex apola ion, (4) is he e o co a iance ex apola ion, (5) is he Kalman Gain, (6) is he s a e es ima e obse a ional upda e and (7) is he e o co a iance upda e. These equa ions a e implemen ed in he Kalman Fil e applica ion which we will now desc ibe. III THE 2D TEACHING TOOL The Kalman Fil e eaching ool is a Ja a applica ion ha can be used o e i y he s uden s’ knowledge and allow hem explo e hei own unde s anding o he e ec s o noise on measu ing a sys em esponse. The ini ial applica ion window o e s he use h ee pa hs o ollow (see Figu e 3): 1. A wo-dimensional e sion wi h sc olling g aphics and wi h pa ame e s ha can be a ied in eal- ime; 2. A guided s ep-by-s ep e sion o inpu ing da a ha can ex end o six inpu s and six measu emen s; 3. A mul i-dimensional e sion so ha he use can inpu da a in any o de . The e is an uppe limi o six inpu s and six measu emen s. We will now explo e each o hese e sions in mo e de ail h ough he use o wo simple examples. The 2D e sion is limi ed o a single measu emen o a single a iable as a unc ion o ime. Fo example, we migh wan o op imally es ima e he empe a u e o a oom o e ime using a sensing de ice. The use se s he ele an pa ame e s h ough an in ui i e g aphical use in e ace (see Figu e 4). When all da a is en e ed he use s a s he Kalman Fil e p ocess and da a is plo ed o sc een. The applica ion can be s opped and s a ed o expe imen wi h he uning and ansien e ec s o he il e se ings. All pa ame e s can be a ied in eal- ime and he e ec s on he ou pu obse ed simul aneously. When he ou pu window ills wi h da a i sc olls o accommoda e new da a. Help bu ons a e associa ed wi h each unc ion bu on o p o ide a sho explana ion o each ea u e. As an example we will imagine we ha e a bucke o nominally 10-ohm esis o s, accu a e o 1% RMS – see du Plessis [12]. Imagine ha we selec each esis o in u n om he bucke and using an ohmme e wi h an accu acy o 0.3-ohms RMS andom e o , we andomly choose a esis o om he bucke , measu e i s alue using he ohmme e and a emp o es ima e i s ue alue. This example in ol es no dynamics and no ma ix no a ion. The p ocess ansi ion assumes ha all he esis o s ha e he same alue and ha any a ia ion is accoun ed o by andom Gaussian noise. The measu emen model says ha we a e measu ing he ac ual s a e wi h he addi ion o noise. The alues gi en a e inse ed in o he applica ion as ollows: P ocess T ansi ion 1.0 Meas./S a e ela ion 1.0 Measu emen noise a iance 0.3 P ocess noise a iance 0.1 T ue alue 10 The Kalman Fil e is hen s a ed and he plo ob ained is shown in Figu e 4. In his plo , we can see wo ansien pe iods. The p ocess noise is he same in bo h cases. The da a be ween he i s and second ansien a e calcula ed based on he pa ame e s gi en abo e. Fo he second ansien , he measu emen noise has been inc eased so ha he Kalman Fil e will place less us in he measu emen model and mo e us in he p ocess model. IV THE MULTI-DIMENSIONAL TEACHING TOOL This e sion ope a es up o a maximum o six inpu s, six s a es and six measu emen s. The ac ual measu emen s can ei he be simula ed by he p og am o inse ed manually by he use . The e a e wo ways o inpu he Kalman Fil e se -up da a, by ollowing a guided s ep-by-s ep p ocedu e o iewing he en i e Kalman Fil e se -up sc een a once. The choice is made om he ini ial sc een iew (see Figu e 3). We will b ie ly desc ibe each e sion be o e p esen ing a simple illus a i e example. The s ep-by-s ep e sion guides he use h ough a logical p ocess o en e ing he da a. Each s ep p o ides an explana ion o he da a being eques ed Figu e 3: The ini ial sc een iew o he Kalman Fil e eaching ool showing he h ee op ions : 2D, s ep-by- s ep o mul iD e sions. − κ x ˆ ISSC 2004, Bel as , June 30 - July 2 and ou lines he heo e ical p e equisi es o being able o ully unde s and he s ep a hand. An example o one o hese s eps is shown in Figu e 5. When he las s ep is eached he ull mul i- dimensional Kalman Fil e ool is launched wi h all he alues ini ialised o hose he use inse ed. The ull mul iD e sion can also be launched by choosing he hi d op ion in he ini ial sc een. Now he use can click any bu on on he sc een in any o de o inse he il e se -up da a. The sc een layou is designed o be in ui i e and o e lec he ac ual Kalman equa ions. Da a is en e ed s a ing om he le and wo king owa ds he igh , as he dimensions o he ma ices depends on he numbe o s a es, inpu s and measu emen s. When he da a is inse ed he use p esses he 'S a ' bu on, he calcula ions a e pe o med and he esul s a e g aphically p esen ed o he use . Figu es 6a and 6b show wo examples o he main sc een o en e ing da a. In each case he e is a schema ic diag am o assis he unde s anding o he pa ame e cu en ly being se . The e is a hype ex help ile, which also explains he Kalman Fil e heo y, bu i is indispensable ha a no ice be guided in he use o he mul iD e sion o he applica ion. Now we will examine a simple illus a i e example based on an objec mo ing in he X-Y plane a a cons an eloci y subjec o andom a ia ions in i s ajec o y. The mo ion is desc ibed by a ixed mo ion model. The new posi ion (X,Y) is he old posi ion plus he eloci y (∆X, ∆Y) plus noise w. A any ime we he e o e ha e:             +             −∆ −∆ − −             =             ∆ ∆ ∆ ∆ Y X Y X w w w w Y X Y X Y X Y X )1( )1( )1( )1( 1000 0100 1010 0101 )( )( )( )( (8) Figu e 4: The 2D Kalman Fil e Teaching Tool Sc een Sho . All inpu pa ame e s can be a ied in eal ime o iew he e ec s o noise on he sys em and he measu emen . In his example he applica ion has been s opped and es a ed wi h new pa ame e s. The pa ame e s can also be a ied in eal ime. Sc olling Sc een Figu e 5: S ep ou o he s ep-by-s ep da a inse ion ool. ISSC 2004, Bel as , June 30 - July 2 I we assume ha we can only obse e he posi ion o he objec (no i s eloci y), hen we ge he ollowing measu emen model equa ion:       +             ∆ ∆       =       Y X Y X Y X Y X Z Z )( )( )( )( 0010 0001 )( )( (9) Suppose we s a ou a posi ion (0,0) mo ing o he igh wi h eloci y (4,3) and we sample a andom ajec o y o leng h 50. The ini ial co a iance ma ix is assumed o be a diagonal ma ix wi h alues o 1000, indica ing ha we do no us ou ini ial es ima e. The measu ed da a we use comes om a simple model o he sys em simula ed using ano he applica ion. When all he measu emen da a is en e ed and he Kalman Fil e pa ame e s a e se up, he p ocess should be s a ed by selec ing he START bu on. This p oduces a numbe o g aphs as ou pu . The ou pu g aphs o he inpu da a used a e shown in Figu es 7a and 7b. These plo s indica e he ac ion o he Kalman Fil e . We a e es ima ing he objec ’s posi ion and eloci y as a unc ion o ime, gi en i s posi ion only. Ou inpu model desc ibes he noise associa ed wi h he measu emen s and wi h ou model o he ac ual sys em. As we collec mo e da a we combine hese unce ain ies o ob ain a maximum likelihood es ima e o he objec posi ion and eloci y. The co a iances associa ed wi h he s a e es ima es hen dec eases exponen ially as can be seen in Figu e 7a. In Figu e 7b we can see a plo o he aw measu emen s (inpu X s inpu Y) and he es ima ed s a e (S a e 1 s S a e 2). The es ima ed s a e plo is a smoo hed es ima e o he aw measu emen s plo . Figu e 6a : A iew o he main applica ion da a inpu sc een. In his case he use has selec ed o inpu he ansi ion ma ix. They ha e al eady in dica ed ha he e a e ou s a es. The schema ic assis s in unde s anding he ansi ion ma ix as pa o he sys em p ocess. Figu e 6b : Ano he iew o he main applica ion da a inpu sc een. In his case he use has selec ed o inpu he measu emen ma ix. They ha e al eady indica ed ha he e a e wo measu emen s and ou s a es. The schema ic assis s in unde s anding he measu emen model. Figu e 7a : Each s a e and i ’s co a iance ma ix a e plo ed as a unc ion o ime. Figu e 7b : Each measu emen is plo ed oge he wi h i s inno a ion. ISSC 2004, Bel as , June 30 - July 2 V CONCLUDING REMARKS This pape desc ibed he heo y and ope a ion o he Kalman Fil e . I hen de ailed an inno a i e Ja a-based Kalman Fil e ool ha ope a es in wo dimensional o mul i-dimensional mode. Such compu e -assis ed lea ning applica ions a e essen ial o allow s uden s expe imen wi h ‘wha i ’ scena ios in he domain o signals and sys ems and hus deepen hei awa eness o undamen al enginee ing concep s. Ou u u e wo k will ex end he cu en applica ion o include signal il e ing and p edic ion and will hen look a he ex ended Kalman il e o non- linea sys ems. REFERENCES [1] J. Michel, Ano he Look a Enginee ing Educa ion: F om In o ma ion o Knowledge, Aus alasian J. o Engng. Educ., Vol. 6, No. 2, 1995 [2] M. C. Du y, The Changing Na u e o Enginee ing, Enginee ing Science and Educa ion Jou nal, Vol. 5 , Issue 5, pp. 231-239, Oc obe 1996. [3] R. Kline, Wo ld Wa II: A wa e shed in Elec ical Enginee ing Educa ion, IEEE Technology and Socie y Magazine, Vol. 13, Issue 2, pp17-23, Summe 1994. [4] F. E. Te man, A B ie His o y o Elec ical Enginee ing Educa ion, P oceedings o IEEE, Vol. 64, No. 9, pp. 1399-1406, 1976 (also: P oceedings o IEEE, Vol. 86, No. 8, Augus 1998). [5] K. D. S ephan, All his and Enginee ing Too: His o y o Acc edi a ion Requi emen s o Non-Technical Cu iculum Con en in U.S. Enginee ing Educa ion 1933-2000, P oceedings o In e na ional Symposium on Technology and Socie y, pp. 155-161 S am o d, CT, USA, July 6 h - 7 h 2001. [6] E Ib ahim and R. Cock um, Enginee ing Educa ion in he E a o Global Ma ke s, P oceedings o he Twen y-Thi d Annual F on ie s o Educa ion Con e ence, pp. 291-296, Washing on DC, 1993. [7] S. Ro hbe g, F. Lamb and A. Wallace, Compu e Assis ed Lea ning in Enginee ing Deg ee P og ammes: A Su ey a he End o he 20 h Cen u y, In . J. Engng Ed. Vol. 17, No. 6, pp502-511, 2001. [8] E. A. Lee and D. G. Messe schmi , Enginee ing an Educa ion o he Fu u e, IEEE Compu e , Janua y 1998. [9] D. McG a h, The Bologna Decla a ion and Enginee ing Educa ion in Eu ope, Uni e si ies and he Bologna Decla a ion - The S a egy o Changes, P oceedings o he Con e ence held in B no, Czech Republic, 2-3 No embe 2000, Edi ed by J.Fukac, J. Kazelle, A.Mize o a, B no Uni e si y o Technology - VUTIUM P ess, B no 2001. [10] M. S. G ewal and A. P. And ews, Kalman Fil e ing Theo y and P ac ice, ISBN 013211335X, P en ice Hall, 1993. [11] G. Bishop and G. Welch: "An In oduc ion o he Kalman Fil e ", SIGGRAPH 2001 sho cou se. [12] R. M. du Plessis, Poo Man’s Explana ion o Kalman Fil e ing o How I s opped Wo ying and Lea ned o Lo e Ma ix In e sion, Au one ics Di ision o No h Ame ican Rockwell Co po a ion, June 1967.