Kalmani filter

Kalmani filter on Rudolf Emil Kalmani loodud algoritm, mida kasutatakse kosmonautikas, merenavigatsioonis, tuumaelektrijaamades, demograafilises modelleerimises ja mujal. Inseneriteadustes on palju valdkondi, kus filtreerimine on eluliselt tähtis. Raadiosignaalid on tihti suure müratasemega (mingid segavad lained). Väga hea filtreerimise algoritm saab elimineerida selle müra, et saada kätte olulist ja vajalikku informatsiooni. Ka elektrienergia juures on tegemist filtreerimise protseduuriga. Paljudes maades nõutakse koduses majapidamises kasutatava elektrienergia voltide filtreerimist, et tagada kodus kasutatavate seadmete pikaealisus (näiteks arvutid, telerid, video- ja DVD-mängijad, jne). Rudolf Emil Kalman arendas filtri välja aastatel 1960–1961, selle juured ulatuvad Gaussi teooriateni 1795. aastast. Kalmani filter on lineaarne, ennustav ja korduv algoritm. Ta on võimeline optimaalselt ennustama reaal-ajas süsteemi seisundit. Kui eksisteerivad mittelineaarsused, siis tuleb mudel eelnevalt lineariseerida. Korduvus Kalmani filtris tähendab seda, et see ei nõua eelnevate andmete salvestamist ja ümber arvutamist niipea, kui uus mõõtmine on tehtud. Kalmani filtri töös kasutatakse (Maybeck, 1979): (1) teadmisi süsteemi ja mõõtmiste dünaamikast; (2) statistilist iseloomustust süsteemi müra, mõõtmiste vigade ning dünaamilistes mudelites oleva ebakindluse kohta; (3) informatsiooni algsete tingimuste kohta. Kalmani filter töötab tingimustes, kus süsteemi oleks võimalik kirjeldada lineaarse mudeli abil ning süsteemi ja mõõtmiste müra on iseloomustatav valge gaussi mürana. Valge müra tähendab, et müra väärtus ei ole ajaga korrelatsioonis ning tal on võrdne tugevus kõikidel sagedustel. Gaussi müra on seotud müra amplituudiga. Eeldatakse, et igal ajahetkel Gaussi müra amplituudi tõenäosusjaotus moodustab kella-kujulise kõvera iga üksiku punkti jaoks. Kalmani filter lihtsalt seletatuna on rakenduslik matemaatika. See on lineaarne süsteem, millele vastav funktsioon on: f ( a + b ) = f ( a ) + f ( b ) {\displaystyle f(a+b)=f(a)+f(b)} . Kalmani filtrisse sisestatakse mürased andmed, filtrist väljuvad loodetavalt vähem mürased andmed. Kuid viivitus on hind selle filtreerimise juures. Puhas Kalmani filter ei kohanda andmeid. Kalmani filter on formuleeritud järgnevalt: Oletame, et protsessi müra w k {\displaystyle w_{k}} on valge gaussi müra koodispersiooni maatriksiga Q {\displaystyle Q} . Edasi oletame et mõõtmiste müra on valge gaussi müra koodispersiooni maatriksiga R {\displaystyle R} ja see ei ole korreleerunud protsessi müraga. Me tahaksime formuleerida määramise algoritmi, mille puhul järgmised statistilised tingimused oleks täidetud (Simon, 2004): Määrang on võrdne õige seisundi väärtusega. Algoritm annab väikseima võimaliku määramise vea. Kalmani filtri iseloomustamiseks on erinevad autorid kasutanud mitmeid erinevaid tähistusi, siin on kasutatud Welch ja Bishop poolt pakutud valemeid ja Kalmani filtri tuletuskäiku. Kalmani filter opereerib põhimõttel: (1) ennustab uue seisundi ja tema ebakindluse ja (2) korrigeerib seda uue mõõtmisega. Kalmani filter kasutab protsessi määramiseks tagasiside kontrolli mehhanismi: filter määrab protsessi seisundi mingil ajahetkel ning siis saab tagasiside (mürastest) mõõtmistest. Kalmani filtri valem koosneb kahest grupist: aja uuenduse valemid ja mõõtmiste uuenduse valemid. Aja uuenduse valemid vastutavad praeguse seisundi projekteerimisest tulevikku (ajas) ja vea koodispersioon määrab aprioorse määrangu järgnevaks aja sammuks. Nende kohta võiks ka öelda, et nad on ennustavad valemid. Mõõtmise uuenduse valemid vastutavad tagasiside eest – kasutavad uut mõõtmist apriori määrangus, et saada täpsustatud aposterioorne määrang. Mõõtmiste uuendamise valemeid võib nimetada ka korrigeerivateks valemiteks. Kalmani filtri aja uuenduse ja mõõtmise uuenduse valemid on antud järgnevalt: aja uuendus: x ^ k − = A x ^ k − 1 + B u k − 1 {\displaystyle {\hat {x}}_{k}^{-}=A{\hat {x}}_{k-1}+Bu_{k-1}} (1) P k − = A P k − 1 A T + Q {\displaystyle P_{k}^{-}=AP_{k-1}A^{T}+Q} (2) mõõtmiste uuendus: K k = P k − H T ( H P k − H T + R ) − 1 {\displaystyle K_{k}=P_{k}^{-}H^{T}(HP_{k}^{-}H^{T}+R)^{-1}} (3) x ^ k = x ^ k − + K k ( z k − H x ^ k − ) {\displaystyle {\hat {x}}_{k}={\hat {x}}_{k}^{-}+K_{k}(z_{k}-H{\hat {x}}_{k}^{-})} (4) P k = ( I − K k H ) P k − {\displaystyle P_{k}=(I-K_{k}H)P_{k}^{-}} (5) Valemites on kasutatud järgmisi tähistusi: A {\displaystyle A} - Protsessi dünaamika maatriks B {\displaystyle B} - Protsessi mõõtmistulemuste vektor P {\displaystyle P} - Vea koodispersioon Q {\displaystyle Q} - Protsessi müra koodispersioon R {\displaystyle R} - Mõõtmise müra koodispersioon K k {\displaystyle K_{k}} - Kalmani juurdekasv Esimene samm mõõtmiste uuenduses on Kalmani juurdekasvu K k {\displaystyle K_{k}} arvutamine. Järgmine samm on tegelikult mõõta protsessi, et leida z k {\displaystyle z_{k}} , seejärel tekitada aposterioorne määrang, mis sisaldab mõõtmist (4). Viimane samm on määrata aposterioorselt vea koodispersioon valemiga (5). Peale igat aja ja mõõtmise uuenduse paari protsessid korratakse viimase aposterioorse määrangutega, mida kasutatakse uue aprioorse määrangu ennustamiseks. See korduv loomus võimaldab muuta praktilised lahendused teostatavaks paremini, kui teised sarnased meetodid. Tegelik filtri teostus, mõõtmise müra koodispersioon R {\displaystyle R} on tavaliselt eelnevalt mõõdetud. Protsessi müra koodispersiooni Q {\displaystyle Q} määramine on raskem, kuna ei ole võimalik otse mõõta protsessi, mida me määrame. Kuid võib kasutada teist Kalmani filtrit nende parameetrite ( R {\displaystyle R} ja Q {\displaystyle Q} ) määramiseks. Seda tehakse tavaliselt sõltumatult (nn off-line) ja sellele viidatakse kui süsteemi identifitseerimisele. Kuid oletagem, et me liigume. Siis kõik müra ei ole mõõtmiste viga, osa sellest on ka liikumine. Kalmani filter võimaldab kaasata liikumiste mudeli, määramaks kiirust ja asukohta. Protsessi mudel kirjeldab seisundi muutust ajas järgmiselt: seisund on kaherealine vektor [asukoht, kiirus] a s u k o h t n + 1 = a s u k o h t n + k i i r u s n ∗ a e g {\displaystyle asukoht_{n+1}=asukoht_{n}+kiirus_{n}*aeg} k i i r u s n + 1 = k i i r u s n {\displaystyle kiirus_{n+1}=kiirus_{n}} Kalmani filtri parameetrite optimeerimine toimub järgmise algoritmi kohaselt (http://www.cs.unc.edu/~welch/kalman/): Joonis 1. Kalmani filtri parameetrite optimeerimise protsess. Kalmani filter eeldab lineaarsust, kus on lubatud ainult tehted maatriksitega. Mõõtmine on seisukorra lineaarne funktsioon, kusjuures järgmine seisund on eelmise lineaarne funktsioon. Lineaarne Kalmani filter ei ole võimeline määrama juurdekasvu, ta ei suuda käsitleda pöördeid ega projektsiooni. Et sellest üle saada, kasutatakse Laiendatud Kalmani filtrit. Laiendatud Kalmani filtri kasutamine aitab vähendada lähendamise vigu, mitte-lineaarne süsteem lineariseeritakse rakendades selleks parimat seisundi vektori määrangut, mida kasutatakse igas lineariseerimise sammus. Olgu protsess on juhitud mitte-lineaarsete võrrandite poolt: x k = f ( x k − 1 , u k − 1 , w k − 1 ) {\displaystyle x_{k}=f(x_{k-1},u_{k-1},w_{k-1})} (6) mõõtmisega z k = h ( x k , v k ) {\displaystyle z_{k}=h(x_{k},v_{k})} , (7) kus juhuslikud muutujad w k {\displaystyle w_{k}} ja v k {\displaystyle v_{k}} iseloomustavad protsessi ja mõõtmiste müra. Funktsioon f {\displaystyle f} seob seisundi eelmisest ajaetapist k − 1 {\displaystyle k-1} praegusse ajaetappi k {\displaystyle k} , mis sisaldab parameetreid ükskõik millise liikuva funktsiooni u k − 1 {\displaystyle u_{k-1}} ja keskmise müra w k {\displaystyle w_{k}} jaoks. Mittelineaarne funktsioon h {\displaystyle h} mõõtmiste võrrandis seob seisundi x k {\displaystyle x_{k}} mõõtmistega z k {\displaystyle z_{k}} . Praktikas võiks määrata ligikaudselt seisundi ja mõõtmise vektori, teadmata konkreetseid väärtusi müra w k {\displaystyle w_{k}} ja v k {\displaystyle v_{k}} jaoks: x ¯ k = f ( x ^ k − 1 , u k − 1 , 0 ) {\displaystyle {\bar {x}}_{k}=f({\hat {x}}_{k-1},u_{k-1},0)} (8) ja z ¯ k = h ( x ¯ k , 0 ) {\displaystyle {\bar {z}}_{k}=h({\bar {x}}_{k},0)} , (9) kus x ^ k {\displaystyle {\hat {x}}_{k}} on aposterioorse seisundi määrang (eelmisest ajaetapist k {\displaystyle k} ). Protsessi mittelineaarse erinevuse ja mõõtmise suhte määramiseks võiks alustada võrranditest, mis lineariseerivad valemid (8) ja (9): x k ≈ x ¯ k + A ( x k − 1 − x ^ k − 1 ) + W w k − 1 {\displaystyle x_{k}\approx {\bar {x}}_{k}+A(x_{k-1}-{\hat {x}}_{k-1})+Ww_{k-1}} (10) ja z k ≈ z ¯ k + H ( x k − x ¯ k ) + V v k {\displaystyle z_{k}\approx {\bar {z}}_{k}+H(x_{k}-{\bar {x}}_{k})+Vv_{k}} , (11) kus x k {\displaystyle x_{k}} ja z k {\displaystyle z_{k}} on tegelikud seisundi ja mõõtmiste vektorid; x ¯ k {\displaystyle {\bar {x}}_{k}} ja z ¯ k {\displaystyle {\bar {z}}_{k}} on ligikaudsed seisundi ja mõõtmise vektorid valemitest (8) ja (9); x ^ k {\displaystyle {\hat {x}}_{k}} on aposterioorne määrang viimasel aja etapil k {\displaystyle k} ; Juhuslikud muutujad w k {\displaystyle w_{k}} ja v k {\displaystyle v_{k}} tähistavad protsessi ja mõõtmiste müra; A {\displaystyle A} on Jakobi maatriks osatuletistest funktsioonist f   x {\displaystyle f\ x} suhtes: A i , j = ∂ f i ∂ x f ( x ^ k − 1 , u k − 1 , 0 ) {\displaystyle A_{i,j}={\frac {\partial f_{i}}{\partial x_{f}}}({\hat {x}}_{k-1},u_{k-1},0)} ; (12) W {\displaystyle W} on Jakobi maatriks osatuletistest funktsioonist f   w {\displaystyle f\ w} suhtes: W i , j = ∂ f i ∂ w f ( x ^ k − 1 , u k − 1 , 0 ) {\displaystyle W_{i,j}={\frac {\partial f_{i}}{\partial w_{f}}}({\hat {x}}_{k-1},u_{k-1},0)} ; (13) H {\displaystyle H} on Jakobi maatriks osatuletistest funktsioonist h   x {\displaystyle h\ x} suhtes: H i , j = ∂ h i ∂ x f ( x ¯ k , 0 ) {\displaystyle H_{i,j}={\frac {\partial h_{i}}{\partial x_{f}}}({\bar {x}}_{k},0)} ; (14) V {\displaystyle V} on Jakobi maatriks osatuletistest funktsioonist h   v {\displaystyle h\ v} suhtes: V i , j = ∂ h i ∂ v f ( x ¯ k , 0 ) {\displaystyle V_{i,j}={\frac {\partial h_{i}}{\partial v_{f}}}({\bar {x}}_{k},0)} . (15) Ennustatava vea jaoks võib kirjutada: e x k − = x k − x ¯ k {\displaystyle e_{xk}^{-}=x_{k}-{\bar {x}}_{k}} , (16) mõõtmiste hälve: e z k − = z k − z ¯ k {\displaystyle e_{zk}^{-}=z_{k}-{\bar {z}}_{k}} . (17) Kasutades (31) ja (32) võib kirjutada valemid vea protsessi jaoks järgnevalt: e x k − ≈ A ( x k − 1 − x ^ k − 1 ) + ε k {\displaystyle e_{xk}^{-}\approx A(x_{k-1}-{\hat {x}}_{k-1})+\varepsilon _{k}} (18) ja e z k − ≈ H − e x k + η k {\displaystyle e_{zk}^{-}\approx H^{-}e_{xk}+\eta _{k}} , (19) kus ε k {\displaystyle \varepsilon _{k}} ja η k {\displaystyle \eta _{k}} tähistavad juhuslikke muutujad, mille keskmine on null ja koodispersiooni maatriksid on vastavalt W Q W T {\displaystyle WQW^{T}} ja V R V T {\displaystyle VRV^{T}} , kus Q {\displaystyle Q} ja R {\displaystyle R} on eelnevalt defineeritud. Nüüd saab kasutada tegelikku mõõtmise hälvet e ¯ z k {\displaystyle {\bar {e}}_{zk}} valemis (17) ja teist (oletuslikku) Kalmani filtrit et määrata ennustamise viga e x k − {\displaystyle e_{xk}^{-}} . Seda määrangut e ^ k {\displaystyle {\hat {e}}_{k}} võiks kasutada selleks, et saada aposterioorset seisundi määrangut originaalsest mitte-lineaarsest protsessist: x ^ k = x ¯ k + e ^ k {\displaystyle {\hat {x}}_{k}={\bar {x}}_{k}+{\hat {e}}_{k}} . (20) Juhuslikud muutujad valemites (18) ja (19) omavad ligikaudselt järgnevat tõenäosusjaotust: p ( e ¯ x k ) ≈ N ( 0 , E [ e ¯ x k e ¯ x k T ] ) {\displaystyle p({\bar {e}}_{xk})\approx N(0,E[{\bar {e}}_{xk}{\bar {e}}_{xk}^{T}])} , (21) p ( ε k ) ≈ N ( 0 , W Q k W T ) {\displaystyle p(\varepsilon _{k})\approx N(0,WQ_{k}W^{T})} , (22) p ( η k ) ≈ N ( 0 , V R k V T ) {\displaystyle p(\eta _{k})\approx N(0,VR_{k}V^{T})} . (23) Arvestades niisuguste lähendustega ja lubades ennustatava väärtuse nulliks, kasutatakse Kalmani filtri valemit e ^ k {\displaystyle {\hat {e}}_{k}} määramiseks: e ^ k = K k e ¯ z k {\displaystyle {\hat {e}}_{k}=K_{k}{\bar {e}}_{zk}} . (24) Asendades (24) tagasi valemisse (20) ja kasutades valemit (17) võib näha, et tegelikult me ei vajagi teist (oletuslikku) Kalmani filtrit: x ^ k = x ¯ k + K k e ¯ z k = x ¯ k + K k ( z k − z ¯ k ) {\displaystyle {\hat {x}}_{k}={\bar {x}}_{k}+K_{k}{\bar {e}}_{zk}={\bar {x}}_{k}+K_{k}(z_{k}-{\bar {z}}_{k})} . (25) Valemit (40) võib seejärel kasutada mõõtmiste uuendamiseks Laiendatud Kalmani filtris, kus ja tulevad valemitest (8) ja (9), ja Kalmani juurdekasv saadakse valemist (3) koos sobiva asendusega mõõtmiste vea koovariansile. Täielik Laiendatud Kalmani filtri valemite komplekt on järgmine: aja uuenduse valemid x ^ k − = f ( x ^ k − 1 , u k − 1 , 0 ) {\displaystyle {\hat {x}}_{k}^{-}=f({\hat {x}}_{k-1},u_{k-1},0)} (26) P k − = A k P k − 1 A k T + W k Q k − 1 W k T {\displaystyle P_{k}^{-}=A_{k}P_{k-1}A_{k}^{T}+W_{k}Q_{k-1}W_{k}^{T}} (27) mõõtmiste uuenduse valemid K k = P k − H k T ( H k P k − H k T + V k R k V k T ) − 1 {\displaystyle K_{k}=P_{k}^{-}H_{k}^{T}(H_{k}P_{k}^{-}H_{k}^{T}+V_{k}R_{k}V_{k}^{T})^{-1}} (28) x ^ k = x ^ k − + K k ( z k − h ( x ^ k − , 0 ) ) {\displaystyle {\hat {x}}_{k}={\hat {x}}_{k}^{-}+K_{k}(z_{k}-h({\hat {x}}_{k}^{-},0))} , (29) P k = ( I − K k H k ) P k − {\displaystyle P_{k}=(I-K_{k}H_{k})P_{k}^{-}} . (30) Alaindeks k {\displaystyle k} on seotud Jakobi maatriksitega A {\displaystyle A} , W {\displaystyle W} ja V {\displaystyle V} et tugevdada märget, et nad on erinevad igal aja etapil (ning seepärast alati uuesti arvutatud). Aja uuenduse valemid projekteerivad seisundi ja koodispersiooni määrangud eelmisest aja etapist k − 1 {\displaystyle k-1} järgmisse etappi k {\displaystyle k} . Funktsioon f {\displaystyle f} valemis (26) tuleb valemist (8), A k {\displaystyle A_{k}} ja W k {\displaystyle W_{k}} on protsessi Jakobi maatriksid ajahetkel k {\displaystyle k} , ja Q k {\displaystyle Q_{k}} on protsessi müra koodispersioon ajahetkel k {\displaystyle k} . Mõõtmiste uuenduse valemid korrigeerivad seisundi ja koodispersiooni määranguid mõõtmisega z k {\displaystyle z_{k}} . Funktsioon h {\displaystyle h} valemis (29) tuleb valemist (9), H k {\displaystyle H_{k}} ja V k {\displaystyle V_{k}} on mõõtmiste Jakobi maatriksid aja etapil k {\displaystyle k} , ja R k {\displaystyle R_{k}} on mõõtmiste müra koodispersioon aja etapil k {\displaystyle k} . R {\displaystyle R} alaindeks lubab selle muutumist koos iga uue mõõtmisega. Laiendatud Kalmani filtri oluline omadus on see, et Jakobi maatriks H k {\displaystyle H_{k}} Kalmani juurdekasvu valemis K k {\displaystyle K_{k}} võimaldab korrektselt levida ainult asjakohasel mõõtmiste informatsiooni komponendil. Niisugusel juhul annab Kalmani filter täiendatud üldise navigatsiooni tulemuse. Tuleb meeles piadada, et INS toetab virtuaalselt müratut väljundit, mis aeglaselt sumbub ajas. GPS omab minimaalset triivi, aga müra on INS mürast suurem. Kalmani filter kasutab statistilisi mudeleid mõlemast süsteemist, minimeerides optimaalselt nende vigade allikaid. See ongi Kalmani filtri kasutamise eeliseks GPS ja INS kombineerimisel. GPS vead võivad olla vastuvõtja kell, ionosfääri ja troposfääri viivitus, mitmeteelisus ja satelliitide efemeriidid ning kella vead. Inertsiaalsed ebatäpsused võivad sisaldada asukoha, kiiruse, orientatsiooni, güroskoobi, kiirendusmõõturi ja raskuskiirenduse vigu. Kalmani filter asub vastuvõtjas, oletuslikud vastuvõtja liikumise valemid asendavad inertsiaalset süsteemi. Liikumise võrrandid (tavaliselt pime-navigatsioon liikuva vastuvõtja jaoks) laiendavad valemit nii, et vastuvõtja trajektoor määrab vigade mudeli, mida on vaja Kalmani filtri jaoks. Siin on Kalmani filtri eeliseks (versus üksiku, fikseeritud vähimruutude lahendus) see, et need liikumise valemid siluvad GPS müra, saavutades nii vajaliku tulemuse. Olgu meil teostatud mõõtmine, mida iseloomustavad mõõtmine z 1 {\displaystyle z_{1}} ja selle dispersioon σ z 1 2 {\displaystyle \sigma _{z1}^{2}} , ning mida võib iseloomustada normaaljaotusfunktsiooniga N ( z 1 , σ z 1 2 ) {\displaystyle N(z_{1},\sigma _{z1}^{2})} , ning mille kohta võib märkida, et x ^ 1 = z 1 {\displaystyle {\hat {x}}_{1}=z_{1}} ja σ ^ 1 2 = σ z 1 2 {\displaystyle {\hat {\sigma }}_{1}^{2}=\sigma _{z1}^{2}} . Kui me teostame sama mõõtmise teist korda, siis seda mõõtmist iseloomistavad mõõtmine z 2 {\displaystyle z_{2}} ja selle dispersioon σ z 2 2 {\displaystyle \sigma _{z2}^{2}} . Küsimus on, kuidas määrata x ^ 2 {\displaystyle {\hat {x}}_{2}} ja σ ^ 2 2 {\displaystyle {\hat {\sigma }}_{2}^{2}} vastavalt meile teadaolevatele andmetele. Kombineerides hinnanguid, saame x ^ 2 = x ^ 1 + K 2 ( z 2 − x ^ 1 ) {\displaystyle {\hat {x}}_{2}={\hat {x}}_{1}+K_{2}(z_{2}-{\hat {x}}_{1})} K 2 = σ 1 2 σ 1 2 + σ z 2 2 {\displaystyle K_{2}={\frac {\sigma _{1}^{2}}{\sigma _{1}^{2}+\sigma _{z2}^{2}}}} Kombineerides dispersioone, saame 1 σ 2 2 = 1 σ 1 2 + 1 σ z 2 2 {\displaystyle {\frac {1}{\sigma _{2}^{2}}}={\frac {1}{\sigma _{1}^{2}}}+{\frac {1}{\sigma _{z2}^{2}}}} Kombineeritud määrang on x ^ = x ^ 2 {\displaystyle {\hat {x}}={\hat {x}}_{2}} ja σ ^ 2 = σ 2 2 {\displaystyle {\hat {\sigma }}^{2}=\sigma _{2}^{2}} , mis on reaalajas saadud kaalutud keskmine! Küsimus, mida Kalmani filter lahendab on järgmine: Me teame, kuidas süsteem käitub vastavalt süsteemi võrrandile, ja meil on teada asukoha mõõtmised, siis kuidas on võimalik kõige paremini määrata süsteemi muutujaid? Dünaamilistes süsteemides (süsteemid, mis muutuvad ajas) on muutujad väljendatud terminiga "state variables" ehk seisundi muutujad. Oletame et süsteemi muutujad, mis on väljendatud vektoriga x {\displaystyle {\textbf {x}}} , on seotud valemiga x k + 1 = A x k + w k {\displaystyle x_{k+1}=Ax_{k}+w_{k}} kus w k {\displaystyle w_{k}} on juhuslik protsessi müra, allindeksid vektorite juures tähistavad ajasamme. Näiteks vektor x {\displaystyle {\textbf {x}}} võiks koosneda asukohast p {\displaystyle p} ja kiirusest v {\displaystyle v} . Niisugusel juhul oleks süsteemi võrrand järgmine (Simon, 2004): ( p k + 1 v k + 1 ) = ( 1 T 0 1 ) ( p k v k ) + ( T 2 ∖ 2 T ) a k {\displaystyle {\binom {p_{k+1}}{v_{k+1}}}={\begin{pmatrix}1&T\\0&1\end{pmatrix}}{\binom {p_{k}}{v_{k}}}+{\binom {T^{2}\setminus 2}{T}}a_{k}} , kus a k {\displaystyle a_{k}} on juhuslik, ajast sõltuv pöörlemine ja T {\displaystyle T} on ajasamm k {\displaystyle k} ja k + 1 {\displaystyle k+1} vahel. Nüüd oletame, et meil on võimalik mõõta asukoht p {\displaystyle p} . Siis meie mõõtmine ajahetkel k {\displaystyle k} võib olla väljendatud z k = p k + v k {\displaystyle z_{k}=p_{k}+v_{k}} , kus v k {\displaystyle v_{k}} on juhuslik mõõtmise müra. Protsessi mudel: ( x k y k ) = ( 1 0 0 1 ) ( x k − 1 y k − 1 ) + ( ∼ x k − 1 ∼ y k − 1 ) {\displaystyle {\binom {x_{k}}{y_{k}}}={\begin{pmatrix}1&0\\0&1\end{pmatrix}}{\binom {x_{k-1}}{y_{k-1}}}+{\binom {\sim x_{k-1}}{\sim y_{k-1}}}} , mille võib ümber kirjutada järgmiselt: x ¯ k = A x ¯ k − 1 + w ¯ k − 1 {\displaystyle {\bar {x}}_{k}=A{\bar {x}}_{k-1}+{\bar {w}}_{k-1}} , kus x ¯ k {\displaystyle {\bar {x}}_{k}} on seisukord ajaepohhil S i s e s t a k {\displaystyle Sisestak} ; A {\displaystyle A} on seisundi üleminek; x ¯ k − 1 {\displaystyle {\bar {x}}_{k-1}} on seisund ajaepohhil k − 1 {\displaystyle k-1} ja w ¯ k − 1 {\displaystyle {\bar {w}}_{k-1}} on müra. Mõõtmiste mudel: ( u k v k ) = ( H x 0 0 H y ) ( x k y k ) + ( ≈ u k ≈ v k ) {\displaystyle {\binom {u_{k}}{v_{k}}}={\begin{pmatrix}H_{x}&0\\0&H_{y}\end{pmatrix}}{\binom {x_{k}}{y_{k}}}+{\binom {\approx u_{k}}{\approx v_{k}}}} , mille võib ümber kirjutada z ¯ k = H x ¯ k + v ¯ k {\displaystyle {\bar {z}}_{k}=H{\bar {x}}_{k}+{\bar {v}}_{k}} , kus z ¯ k {\displaystyle {\bar {z}}_{k}} on mõõtmine; H {\displaystyle H} on mõõtmise maatriks; x ¯ k {\displaystyle {\bar {x}}_{k}} on seisund ja v ¯ k {\displaystyle {\bar {v}}_{k}} on müra. Ettevalmistus filtreerimiseks: Kirjutame välja järgmised maatriksid: A = ( 1 0 0 1 ) {\displaystyle A={\begin{pmatrix}1&0\\0&1\end{pmatrix}}} seisundi üleminek Q = E [ w ¯ ∗ w ¯ T ] = ( Q x x 0 0 Q y y ) {\displaystyle Q=E[{\bar {w}}*{\bar {w}}^{T}]={\begin{pmatrix}Q_{xx}&0\\0Q_{yy}\end{pmatrix}}} protsessi müra koodispersioon R = E [ v ¯ ∗ v ¯ T ] = ( R x x 0 0 R y y ) {\displaystyle R=E[{\bar {v}}*{\bar {v}}^{T}]={\begin{pmatrix}R_{xx}&0\\0&R_{yy}\end{pmatrix}}} mõõtmise müra koodispersioon Initialiseerimine: x ¯ 0 = H z ¯ 0 {\displaystyle {\bar {x}}_{0}=H{\bar {z}}_{0}} P 0 = ( ε 0 0 ε ) {\displaystyle P_{0}={\begin{pmatrix}\varepsilon &0\\0&\varepsilon \end{pmatrix}}} Ennustus: x ¯ k − = A x ¯ k − 1 {\displaystyle {\bar {x}}_{k}^{-}=A{\bar {x}}_{k-1}} P k − = A P k − 1 A T + Q {\displaystyle P_{k}^{-}=AP_{k-1}A^{T}+Q} Korrigeerimine x ¯ k = x ¯ k − + K ( z ¯ k − H x ¯ k − ) {\displaystyle {\bar {x}}_{k}={\bar {x}}_{k}^{-}+K({\bar {z}}_{k}-H{\bar {x}}_{k}^{-})} P k = ( I − K H ) P k − {\displaystyle P_{k}=(I-KH)P_{k}^{-}} Kus z ¯ k {\displaystyle {\bar {z}}_{k}} on tegelik mõõtmine ja H x ¯ k − {\displaystyle H{\bar {x}}_{k}^{-}} on ennustatav mõõtmine. Kalmani juurdekasv arvutatakse K = P k − ( H P k − H T + R ) − 1 {\displaystyle K=P_{k}^{-}(HP_{k}^{-}H^{T}+R)^{-1}} , kus ( H P k − H T + R ) {\displaystyle (HP_{k}^{-}H^{T}+R)} on "denominaator" (ehk mõõtmiste ruum) Kokkuvõte: joonisel 2 on näidatud Kalmani filtri tööprotsess (http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf): Joonis 2. Kalmani filtri tööprotsess. Martin Vermeer (2004): "Methods of Navigation". Lecture materials. URL: http://www.hut.fi/~mvermeer/nav_en.pdf Greg Welch, Gary Bishop (2004): "An Introduction to the Kalman Filter". URL: http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf H. W. Sorenson (1970): "Least Squares estimation: from Gauss to Kalman". Reprinted from IEEE Spectrum, vol. 7, pp. 63–68, July 1970. URL: http://www.cs.unc.edu/~welch/media/pdf/Sorenson1970.pdf Maybeck, Peter S (1979): "Stochastic Models, Estimation and Control, Vol. 1". Academic Press Inc, 1979. URL: http://www.cs.unc.edu/~welch/kalman/maybeck.html R. E. Kalman (1960): "A New Approach to Linear Filtering and Prediction Problems". Transactions of the ASME–Journal of Basic Engineering, 82 (Series D): 35-45. URL: http://www.cs.unc.edu/~welch/media/pdf/Kalman1960.pdf Dan Simon (2001): "Kalman Filtering". Innovatia Software, 1998 – 2001. URL: http://www.innovatia.com/software/papers/kalman.htm. Greg Welch , Gary Bishop (2001): "An Introduction to the Kalman Filter". Course 8. SIGGRAPH, 2001. URL: http://www.cs.unc.edu/~welch/kalman/