Antti Somppi Paloittain päivittävä Kalmanin suodatin. Kandidaatintyö

Transkriptio

1 Antti Somppi Paloittain päivittävä Kalmanin suodatin Kandidaatintyö

2 I TIIVISTELMÄ TAMPEREEN TEKNILLINEN YLIOPISTO Tenis-luonnontieteellinen oulutusohjelma TEKIJÄN NIMI: Antti Somppi Kandidaatintyö 15 sivua, 3 liitesivua Maalisuu 2015 Pääaine: Matematiia Tarastaja: Simo Ali-Löytty Avainsanat: Kalmanin suodatin, epälineaarinen estimointi, Paloittain päivittävä Kalmanin suodatin Kalmanin suodatin on algoritmi, joa lasee mittausten ja niitä uvaavan mallin avulla arvion näitä vastaavista todellisista arvoista. Tavoitteena on, että yhdistämällä mittauset ja malli saadaan tarennettua arviota todellisista arvoista sellaisessa tilanteessa, jossa mittausiin ja mallin antamiin tulosiin tiedetään sisältyvän virheitä. Aluperäinen Kalmanin suodatin on tehty lasemaan approsimaation tilanteessa, jossa niin malli uin mittausetin ovat lineaarisia. Epälineaarisia tapausia varten on ehitetty aluperäisen algoritmin pohjalta laajennosia, joilla pyritään lisäämään algoritmin äyttömahdollisuusia. Tässä työssä tutustutetaan luija erääseen tällaiseen laajennoseen, ja esitetään sen lisäsi uusi tehoaampi tapa rataista epälineaarisia estimointiongelmia. Työ pohjautuu Renato Zanettin vuonna 2012 teemään julaisuun RUF-algoritmista Recursive Update Filtering for Nonlinear Estimation, ja siinä osoitetaan uuden algoritmin etuja suhteessa vanhempaan algoritmiin äyttäen numeerista esimeriä, seä sitä havainnollistavia uvia. Työssä todetaan RUF-algoritmin edut lasennallisen vaativuuden seä tulosten taruuden suhteen verrattuna muihin epälineaarisia estimointiongelmia rataiseviin algoritmehin.

3 II SISÄLLYS 1. Johdanto Teoria Ongelman asettelu Laajennettu Kalmanin suodatin Algoritmin esittely Esimeri RUF Kalman -algoritmi Algoritmin esittely Algoritmin johtaminen Esimeri Yhteenveto Lähteet A. Liite: EKF-esimerin MATLAB-oodi B. Liite: RUF-esimerin MATLAB-oodi

4 III TERMIT JA LYHENTEET MÄÄRITELMINEEN f h t i Tilafuntio Mittausfuntio Ajanheti aseleella i x i x i x + i Tilamuuttujan tila ajanhetellä t i Tilamuuttujan tilan prioriestimaatti Tilamuuttujan tilan posterioriestimaatti y i ŷ i w i v i Q i R i K i Mittaus ajanhetellä t i Mittausen ennustettu arvo Tilamallin virhetermi Mittausmallin virhetermi Mallifuntion ovarianssimatriisi Mittafuntion ovarianssimatriisi Algoritmin Kalmanin vahvistus Φ i Mallifuntion derivaatan arvo ajanhetellä t i x:n arvolla ˆx i H P i P i P + i C e 1 e MVUE EKF Mittafuntion derivaatan arvo ajanhetellä t i x:n arvolla ˆx i x:n (eli tilamuuttujan) esineliövirhematriisi Prioriestimaattorin esineliövirhematriisi Posteriestimaattorin esineliövirhematriisi Ristiovarianssi tilan x ja mittausohinan välillä RUF-algoritmin ensimmäisen päivityspalan posteriorivirhe RUF-algoritmin päivitysen prioriestimointivirhe Harhaton minimivarianssiestimaattori (Minimum Variance Unbiased Estimate) Laajennettu Kalmanin suodatin (Extended Kalman Filter)

5 IV RUF BLU EKF2 UKF UT IKF Paloittain päivittävä Kalmanin suodatin (Recursive Update Filter) Paras harhaton lineaarinen (estimaattori) (Best Linear Unbiased) Toisen Asteen Laajennettu Kalmanin suodatin (Second order EKF) Hajuton Kalmanin suodatin (Unscented Kalman Filter) Hajuton muunnos (Unscented Transformation) Iteroitu Kalmanin suodatin (iterated Kalman filter) Huom. Alaindesi i viittaa tilamallin aseleeseen disreetissä mallissa.

6 1 1. JOHDANTO Aluperäinen Kalmanin suodatin -algoritmi julaistiin vuonna 1960 [1]. Se on tullut erittäin tunnetusi ja antaa optimaalisen rataisun estimoitaessa lineaarisia ongemia. Globaali optimaalisuus saavutetaan un äsitellään lineaarista mallia ja lineaarisia mittausia, joiden virheet ja aluperäinen estimointivirhe ovat normaalisti jaautuneita. Miäli virheet eivät ole normaalisti jaautuneita, lassinen Kalmanin suodatin ei ole globaalisti optimaalinen, mutta silti lineaarinen harhaton minimivarianssi estimaattori (MVUE). [2] Kalmanin suodatinta voidaan soveltaa eri tavoilla sen muaan, miten sitä tulitaan. Bayesilaisella tulinnalla haetaan jaaumia, parhaan harhattoman lineaarisen estimaatin tulinnalla minimoidaan posterioriestimaatin ja todellisen tilan neliöllistä esivirhettä, ja deterministinen tulinta jättää pois oletuset virheiden jaaumista ja tulitsee ongelmaa täysin deterministisenä prosessina, jossa haetaan rataisua deterministiselle pienimmän neliösumman ongelmalle. [3] Epälineaarisille estimointiongelmille on ehitetty monia laajennosia Kalmanin suodattimeen perustuen. Monet niistä perustuvat parhaan harhattoman lineaarisen estimaatin lasemiseen. Näistä ysi yleisimpiä on laajennettu Kalmanin suodatin (Extended Kalman filter, EKF). Laajennettu Kalmanin suodatin pyrii linearisoimaan mittausten ja mallin epälineaarisuusia. Algoritmissa oletetaan virheet pienisi ja epälineaarisuusia approsimoidaan niiden ensimmäisellä ertaluvulla, eli approsimoimalla mittaus- ja tilamallia niiden Taylorin sarjojen ahdella ensimmäisellä termillä. Tämä toimii, miäli epälineaarisuudet eivät ole ovin suuria. EKF:stä on olemassa myös erilaisia variaatioita, uten toisen asteen laajennettu Kalmanin suodatin (Second Order Extended Kalman filter, EKF2). Tämä eroaa tavallisesta EKF-suodattimesta vain siltä osin, että approsimaatioissa äytetään mallien Taylorin sarjojen toisen asteen termejä. Niin EKF:n uin EKF2:nin ysi puute on, että niissä mittausfuntion arvon arvio lasetaan prioritilamuuttujan arvolla, ja miäli tämä arvio on väärin, ajautuu algoritmi heti alussa vääriin arvoihin. Toinen ongelma on, että un Kalmanin vahvistuselle lasetaan arvo, sen oletetaan olevan sama oo päivitysaseleella, vaia todellisuudessa se saattaa vaihdella. [3; 8] Tässä työssä esiteltävää uutta paloittain päivittävää Kalmanin suodatinta verrataan laajennettuun Kalmanin suodattimeen. Tavallinen ja toisen asteen laajennettu Kalmanin suodatin, uten myös tässä työs-

7 1. Johdanto 2 sä esiteltävä paloittain päivittävä suodatin vaativat lasuissa derivaattojen lasemista analyyttisesti seä numeerisesti. On olemassa myös Kalmanin suodattimen laajennosia, jota eivät tätä vaadi. Esimerisi hajuton Kalmanin suodatin (Unscended Kalman Filter, UKF) on tällainen. Se lasee numeerisesti integroimalla estimaattien arvot. UKF äyttää muunnosta, jota utsutaan hajuttomasi muunnosesi (Unscented Transformation, UT), jossa tilamuuttujan jaaumaa arvioidaan teemällä mittausfuntion arvon odotusarvosta muunnos niin sanottujen sigmapisteiden avulla lasetusi integraalisi. Tämä arvio on tara miäli mittausfuntio on olmannen asteen polynomi. Sigmapisteet lasetaan normaalijaaumasta, jona odotusarvona on tilamuuttujan prioriestimaatti ja ovarianssina tilamuuttujan prioriovarianssi. Niiden määrittämiseen voidaan äyttää erilaisia menetelmiä tilanteesta riippuen. Hajuton suodatin välttää EKF:n ja EKF2:n ongelman väärästä Kalmanin vahvistusesta valitsemalla sigmapisteet ympäri jaaumaa ja lasemalla vahvistusen niistä. UKF:n algoritmi on muutoin sama uin EKF:n, mutta posterioritilan ja posterioriovarianssin lasemisessa derivaatat orvataan hajuttomalla muunnosella lasetulla mittausen odotusarvolla. Lasennalliselta vaativuudelta UKF on samaa luoaa EKF:n anssa. Mahdollisia ongelmia UKF:ssä voi aiheuttaa se, että ovariansseisi on mahdollista lasea negatiivisia arvoja, mutta tämä on vältettävissä oieanlaisella sigmapisteiden valinnalla.[4; 3; 5] Edellä mainittujen lisäsi lisäsi on olemassa luuisia muita algoritmeja, joilla voidaan ratoa epälineaarisia ongelmia. Näilläin on omat heioutensa, esimerisi lasennallinen rasaus, joa on haittana mm. tehtäessä reaaliaiaista lasentaa. Tässä työssä on taroitusena esitellä epälineaaristen estimointiongelmien rataisemiseen uusi menetelmä, jolla vältetään EKF:n ja muiden algoritmien heiousia. Uuden algoritmin lasennallinen vaativuus on verrannollinen (N-1) appaleeseen EKF:n päivitysaseleita. Algoritmi jaaa yhden päivitysaseleen N appaleeseen paloja, ja mitä useampia näitä on, sitä vähemmän algoritmi luottaa linearisointiin. [8] Työssä esitellään EKF-algoritmi ja yseinen uusi algoritmi, seä verrataan niiden toimivuutta esimeritapausessa. Tämä työ on muailee lyhennettynä Renato Zanettin vuonna 2012 teemää julaisua Recursive Update Filtering for Nonlinear Estimation.

8 3 2. TEORIA 2.1 Ongelman asettelu Taroitusena tässä työssä on esitellä uudenlainen sovellustapa Kalmanin suodattimelle epälineaarissa tapausissa. Työssä perehdytään suodattimeen, jota utsutaan vapaasti suomennettuna paloittain päivittäväsi Kalmanin suodattimesi (Recursive Update Filter, RUF). Taroitusena on esitellä ja johtaa suodattimen algoritmi, jossa epälineaarinen signaali approsimoidaan linearisoimalla päivitysasel paloittain. Suodattimen perusajatus on tehdä lineaarinen estimaatti epälineaariselle mallille samanaltaisesti uin laajennetulla Kalmanin suodattimella [6], mutta jaaa approsimaatiossa algoritmin päivitysasel useampaan palaan, millä virheen toivotaan pienentyvän siedettävän suuruisesi, linearisoinnista huolimatta. Työssä äydään läpi myös EKF-algoritmin periaatteet ja sen antamia tulosia, joihon RUF-algoritmin antamia tulosia verrataan ysintertaisilla esimereillä MATLAB-oodina toteutettuna. Molemmista algoritmeista on olemassa disreetisti ja jatuvasti päivittävät versiot. Tässä työssä perehdytään edellä mainittuihin. Tila- ja mittamalleina tässä työssä äytetään funtioita f ja h, seä niihin lisättyjä virhetermejä w ja v. Yhtälömuodossa funtiot ovat seuraavanlaiset [7]: x = f (x 1 ) + w, y = h (x ) + v. (2.1) Lineaariseen suodattimeen erona on funtioiden epälineaarisuus. Funtioista toinen tai molemmat voivat olla epälineaarisia, un taas lineaarisissa tapausissa molemmat funtiot ovat lineaariset. Funtiot voivat muuttua aseleen muaan, mihin viitataan alaindeseillä. Tässä työssä äsitellään ainoastaan tapausia, joissa funtiot ovat samoja joa aseleella. Virhetermit noudattavat nollaesistä normaalijaaumaa ovarianssi(matriiseilla) Q ja R. Virheet eri ajanhetillä ovat toisistaan riippumattomia.[9] Kovarianssit päivitetään joaisella aseleella tilamuuttujien lisäsi. Mittauset oletetaan annetuisi. Yleisestä tapausesta poieten tässä työssä äytetään salaariesimerejä ongelman ysinertaistamisesi. Yleisessä tapausessa tilavetorin ollessa m-pituinen ja mittausvetorin n-pituinen niiden virhetermit ovat saman pituiset ja virheiden ovarianssimatriisit m x m - ja n x n -ooiset. Tässäin

9 2. Teoria 4 alaindesi viittaa aseleeseen ajanhetella t. w N(0, Q ), v N(0, R ). (2.2) Melo yleinen tapa rataista epälineaaisia ongelmia on etsiä paras lineaarinen harhaton estimaattori, ns. BLU-estimaattori (best linear unbiased). Käytännössä tämä taroittaa, että oletusena ovat seuraavat yhtälöt [9]: ([ E x y ]) [ ] x = ȳ ([ ]) [ ] V x y = P xx P xy P xy P yy Tällöin tilan x paras lineaarinen harhaton estimaattori on: ˆx = x + P xy P 1 yy (y ȳ ). (2.3) EKF-algoritmi rataisee tilamuuttujalle seuraavan aseleen priori- ja posterioriestimaatit, seä odotetun mittaustulosen. Joaisella aseleella päivitetään ovarianssit molempien mallien ohinatermeille. Ongelmana EKF-algoritmissa on se, että tarpeesi epälineaarisissa tapausissa algoritmin lasema posterioriestimaatti seuraavalla aseleella eroaa mittausesta enemmän, uin mitä hajonta on. Tämä johtuu siitä (uten myöhemmin osoitetaan), että otetaan liian suuri asel mallifuntion tangentin suuntaan ja päädytään auasi todellisesta funtion pisteestä. Tässä saatetaan päätyä tilanteeseen, jossa ohinaa sisältävä mittaus itsessään olisi tarempi estimaatti seuraavalle aseleelle, uin algoritmin määrittämä estimaatti. Ongelmana on siten löytää menetelmä, jolla päästään tarempaan arvioon seuraavan aseleen tilamuuttujan arvosta. Taroitusena on siis orjata arviota siten, ettei todellisesta arvosta harhauduta, vaan parantaa algoritmin yyä äsittellä myös enemmän epälinaarisuutta. Ysinertaistettuna: ollessamme tilamallin tilassa x 1 pyrimme määrittämään tilamuuttujalle estimaatin tilassa x. Tähän päästäsemme tarvitsemme orjausen EKF-algoritmin lasemaan posterioriestimaattoriin x +. RUF-algoritmi lähtee etsimään parempaa estimaattia ottamalla pienemmän aseleen mallifuntion derivaatan suuntaan, jolloin taroitusena on pystyä seuraamaan todellisen signaalin äyrää taremmin.

10 2. Teoria Laajennettu Kalmanin suodatin Algoritmin esittely EKF -algoritmi eli laajennettu Kalmanin suodatin on ensimmäisiä Kalmanin suodattimen sovellusia. Se ehitettiin pian aluperäisen Kalmanin suodattimen jäleen, joa julaistiin vuonna Tällöin oltiin huomattu, että Kalmanin suodattimelle olisi olemassa paljon äyttöohteita, joita sillä ei uitenaan voida äsitellä. Tämä johtuu siitä, että aluperäinen suodatin olettaa mittaussignaalit lineaarisisi ja sen soveltamisessa äytetään lineaarisia malleja. Laajennettu Kalmanin suodatinta äytetään laajalti epälineaarisen estimoinnin työaluna ja se olettaa estimointivirheet pienisi.[6] Perusajatus EKF:ssä on täysin sama uin aluperäisessä Kalmanin suodattimessa. Seuraavan aseleen prioriestimaatin lasemiseen äytetään mallifuntiota f, ottaen huomioon mallin epätaruuden ohinalla w. Virhetermi noudattaa nollaesistä normaalijaaumaa, jona ovarianssi(matriisina) on Q.[7] Annettujen funtioiden avulla lasetaan prioriestimaatti tilamuuttujalle sijoittamalla tilamalliin edellisen aseleen posterioriestimaatti seä ennustettu mittaustulos sijoittamalla mallifuntioon nyyisen aseleen prioriestimaatti: ˆx = f 1(ˆx + 1 ) ŷ = h (ˆx ). (2.4) Kuten yleisessä ongelmanasettelussa mainittiin, funtioiden alaindesit viittaavat mallin aseleeseen. Funtiot voivat muuttua eri aseleilla, mutta tässä työssä äsitellään tapausia, joissa näin ei ole. Seuraavassa vaiheessa tehdään lineaariapprosimaatio äyttäen tilamallin derivaattaa ja mittamallin derivaattaa. Tilafuntiota ja mittamallin funtioita approsimoidaan niiden Taylorin sarjojen ahdella ensimmäisellä termillä. Φ 1 δf 1(x) δx x=ˆx 1 H δh (x) δx x=ˆx. (2.5) Tilamallin prioriovarianssi(matriisi) seuraavassa tilassa saadaan nyt lasettua hyödyntämällä tilamallin derivaatan arvoa äyttäen tilamuuttujalle tässä nyyisen tilan prioriestimaattia. P = Φ 1P + 1 ΦT 1 + Q 1. (2.6) Kalmanin vahvistus lasetaan samaan tapaan uin tavallisessa Kalmanin suodatin - algoritmissa, mutta tilamallin ja mittamallin matriisit orvataan mitta- ja tilamallin

11 2. Teoria 6 Tauluo 2.1: EKF -algoritmin päivitysasel, perustuu lähteeseen [7]. ˆx = f(ˆx+ 1 ), ŷ = h(ˆx ) δf(x, 1) Φ 1 δx x=ˆx 1 δh(x, ) H δx P x=ˆx = Φ 1P + 1 ΦT 1 + Q 1 K = P HT (H P HT + R ) 1 ˆx + = ˆx + K (y ŷ ) P + = (I K H )P funtioiden derivaattojen arvoilla aiemmin lasetuissa pisteissä: K = P HT H P HT + R. (2.7) Esimeri Seuraava esimeri uvaa hyvin EKF -algoritmia äytettäessä mahdollisesti esiintyviä ompastusiviä. [8] Käytetään seleyden vuosi ysinertaista salaariesimeriä. Oletetaan, että ollaan tilassa, jona todellinen arvo on x = 3,5 ja prioriestimaatille on annettu arvio ˆx = 2,5. Myös prioriestimaatin ovarianssi on annettu ja sen arvo on P = 0,5 2. Tällöin on tiedossa, että prioriesimaatin virhe on x ˆx = 3,5 2,5 = 1. (2.8) Kovarianssista nähdään, että esihajonta on σ = 0,5, joten ˆx on ahden esihajonnan päässä odotusarvostaan. Oletetaan että mittausfuntio on seuraavanlainen: y = x 3 + w w N(0, 0,1 2 ). (2.9) Mittausista lasemalla x:lle saataisiin siten seuraava estimaatti: ˆx Mitta = y 1/3. (2.10) Monte Carlo -simulointia äyttämällä tälle estimaatin virheovarianssille P Mitta saadaan lasettua arvo P Mitta = 0, Mittafuntion derivaatan ja mittamallin virheen ovarianssin avulla voidaan myös approsimoida mittafuntion virheen ova-

12 2. Teoria 7 rianssia äyttämällä sen lasemisessa tilamuuttujan todellista arvoa ja saadaan lähes sama arvo uin Monte Carlo -simuloinnilla: P Mitta 0,1 2 = 0,0027. (2.11) (3 3,5 2 ) 2 Kalmanin vahvistusen lasemista varten lasetaan H :n lausee aui: H δh(x, ) δx x=ˆx = δˆx 3 δˆx = 3ˆx 2. (2.12) Nyt voidaan määrittää arvo Kalmanin vahvistuselle aiemmin mainitulla aavalla K = P HT [H P HT + R ] 1 = 0,1 3(ˆx ) 2 /[9(ˆx ) 4 0, ,1 2 ] = 0,0533. (2.13) Kosa halutaan pitää esimeri deterministisenä, oletetaan mittaus virheettömäsi, eli y = x 3 = 42,875. Tällöin algoritmin perusteella saadaan lasettua tilamuuttujalle seuraava estimaatti ˆx + = 3,9532. ˆx + EKF = ˆx + K (y (ˆx ) 3 ) = 3,9532. (2.14) Tästä nähdään, että estimaattorin virhe on 0,4532. Posteriorivirheen ovarianssisi saadaan lasettua 0, P + = (1 3K (ˆx ) 2 ) 2 P + KR 2 = 0, (2.15) Tästä voidaan lasea, että esimaattorin virhe on noin 85-ertainen virheen esihajontaan nähden. Virhe on siis erittäin suuri esivirheeseen nähden. Virhe antaa olettaa, että posterioriestimaatti olisi lähellä oieata arvoa, mutta numeerisesta tulosesta nähdään, ettei näin ole. Näin ollen algoritmin suoritusyy ei tässä tapausessa ole hyvä. Seuraavassa edellä uvattua esimeriä havainnollistava uva.

13 2. Teoria y=x 3 prioriest. posterioriest. tod.sijainti 50 x,y ˆx,(ˆx ) 3 ˆx +,(ˆx )3 +H (ˆx + ˆx ) Kuva 2.1: EKF-algoritmilla lasettu päivitysasel. 2.3 RUF Kalman -algoritmi Algoritmin esittely Tavallinen Kalmanin soudatin, seä siihen perustuva EKF toimivat siten, että lineaarinen päivitysasel otetaan äyttöön välittömästi un estimaatti on saatu lasettua. Paloittain päivittävä soudatin toimii siten, että päivitysasel suoritetaan vähitellen siten, että posterioriestimaatille lasetaan väliarvoja, joita äytetään seuraavassa välivaiheessa prioriarvoina. [8] Tässä menetelmässä approsimaatiovaiheen Jacobin matriisi voidaan lasea uudelleen ennen päivitystä, jolloin saadaan taremmin seurattua lineaarisella arviolla epälineaarista signaalia. Tässä ohtaa myös Kalmanin vahvistus lasetaan uudelleen vastaamaan muuttunutta derivaattaa. Paloittain päivittävän suodattimen etuna on se, että sitä äytettäessä ei välttämättä tarvita ohinatermien jaaumaa. Mallifuntion ei tarvitse olla myösään lineaarinen. Jatossa tullaan näyttämään, että RUF toimii paremmin uin EKF tapausissa joissa mittausfuntio on epälineaarinen. Tämän lisäsi reursiivisella suodattimella vältetään Taylor-sarjan typistämisestä aiheutuva hajautuminen todellisesta funtiosta. Algoritmi seuraa mittausfuntion gradienttia ja minimoi ovarianssin. Mittausten ollessa lineaarisia algoritmi redusoituu laajennetusi Kalman suodattimesi. [8] Algoritmin johtaminen Kuten edellä jo todettiin, algoritmin ydinajatus on uuden aseleen lisääminen vaiheittain. Joaisessa vaiheessa Jaobin matriisi H lasetaan uudelleen, miä mahdol-

14 2. Teoria 9 listaa mittausepälineaarisuusien taremman approsimoinnin lineaarisesti. Algoritmia varten meritsemme muuttujalla C ristiovarianssia ajanhetellä t todellisen tilan x ja mittausohinan välillä[10]: C = E(x ˆx )vt. (2.16) Mittausmalli ja tilamalli määritellään aivan samoin uin EKF-algoritmissäin. Päivitysaseleet määritellään seuraavasti: ˆx + = ˆx + K (y H ˆx ). (2.17) y = H x + v. (2.18) Optimaalinen minimivarianssiestimoinnin muainen Kalmanin vahvistus on muotoa: K = (P HT + C )(H P HT + R + H C + C T H T ) 1. (2.19) Posteriorivirheovarianssimatriisi lasetaan seuraavasti: P + =(I K H )P (I K H ) T + K R K T (I K H )C K T K C T (I K H ) T. (2.20) Kun tietyn aseleen mittaus on päivitetty erran, saadaan posteriorivirheesi ja Kalmanin vahvistusesi seuraava: e (1) = x ˆx (1) = (I K (1) H )e K(1) v. (2.21) K (1) = P HT (H P HT + R ) 1. (2.22) Lauseeessa 2.21 meritään prioriestimointivirhettä e :lla: e = x ˆx (1) (2.23) Aluvaiheessa ovarianssi C (meritään myös C (0) ) saa arvoseen nolla. Tämä johtuu siitä, että tilan prioritilamuuttuja ja tilan mittaus eivät ole toisistaan riip-

15 2. Teoria 10 puvia ensimmäisellä iteraatiolla. Kuitenin seuraavan iteraation prioritilamuuttujan lasemisessa äytetään yseisen aseleen mittausdataa, jolloin tilamuuttujan virhe ja mittausvirhe eivät ole toisistaan riippumattomia, jolloin C ei saa arvoseen nolla.tästä johtuen ensimmäisen iteraation vahvistusen lausee saa ylläolevan muodon. e (1) :n ja v :n välisesi ristiovarianssisi ensimmäisen vaiheen jäleen saadaan: C (1) = K R. (2.24) P + :n lauseetta ysinertaistamalla ja sijoittamalla K(1) :n lausee saadaan ensimmäisen vaiheen jäleiselle ovarianssimatriisille: P (1) = (I K (1) H )P K C T. (2.25) Suluissa olevat yläindesit viittaavat tässä tietyn päivitysaseleen vaiheeseen. Yllä olevien yhtälöiden lasemisen jäleen on teoriassa äyty siis yhden päivitysaseleen ensimmäinen vaihe. Vaiheiden määrä on vapaasti valittava parametri. Kuten edellä mainittiin, ristiovarianssi C (0) prioritilan ja mittausvirheen välillä on nolla, josta seuraa, että K (1) :n lausee on hieman eri muotoa uin myöhempien vahvistusten. Tämä pätee ainoastaan optimaaliselle vahvistuselle. Jos nyt äsitellään sama aluperäinen mittaus saaduilla ovariansseilla ja ristiovarianssilla, seä aluperäisellä Jaobin matriisilla, äytetään prioriovarianssina P edellisen vaiheen posterioriovarianssia P (1). Tällöin seuraavan vaiheen Kalmanin vahvistusesi tulee: K (2) = (P (1) HT + C (1) )(H P (1) H T + R + H C (1) + C (1)T H T ) ( 1). (2.26) Lasemalla edellinen lausee aui saadaan tulosesi nolla. Tämä johtuu siitä, että ensimmäisessä vaiheessa oletettiin vahvistusen arvo täydellisesi, jolloin voidaan ajatella, että aii informaatio mittausesta saatiin hyödynnettyä. Tässä tapausessa muutosta ei tulisi tapahtua, sillä päivitysasel saatiin jo tehtyä edellisessä vaiheessa. Miäli ensimmäisen vaiheen päivitys ei oleaan optimaalinen, niin silloin vain osa datasta hyödynnetään. Jos esimerisi Kalmanin vahvistus on vain puolet edellisen esimerin vahvistusesta niin se saa arvon K (1) = 0,5P HT (H P HT + R ) 1. (2.27) Tässä tapausessa myös K (2) saa eri arvon, joa on eri uin nolla. Seuraavassa vaiheessa lasettavan vahvistusen K (2) arvo on sellainen, että aseleen oonaisvai-

16 2. Teoria 11 utus on sama uin tavallisen Kalman suodattimen aseleessa. Miäli päivitysasel jaetaan olmeen vaiheeseen, niin sillon halutaan, että joaisessa vaiheessa lisätään olmannes oonaispäivitysestä. Tällöin ensimmäisen Kalmanin vahvistusen erroin on 1/3. Toisen vaiheen vahvistusen erroin on 1/2 ja viimeisen 1. Näin menetellään sisi, että ensimmäisen vaiheen jäleen mittausen seä prioriestimaatin ja Jaobin matriisin tulon erotusesta on vähentynyt olmannes (osa ensimmäisessä vaiheessa siirryttiin olmannes päivitysaseleen suuntaan) ja tällöin jäljellä yseisestä erotusesta on asi olmannesta. Kun asi olmannesta errotaan puoliaalla, saadaan olmasosa, jolloin liiutaan taas olmasosa uudelleen lasetun Jaobin suuntaan (eli edelleen approsimoidaan epälineaarista tapausta lineaarisesti mutta ollaan tehty orjausta). Kun olmannessa vaiheessa otetaan Kalmanin vahvistusen ertoimesi 1, niin päivitysestä on tehty jo asi olmannesta, eli jäljellä on olmannes. Näin saatiin tehtyä ysi päivitysiteraatio olmivaiheisesti siten, että joaisessa vaiheessa äytettiin sama osuus mittausdatasta ja orjattiin gradientin suuntaa. Tauluossa 2.2 on esitettynä RUF-algoritmin ysi päivitysasel. Tauluo 2.2: RUF-algoritmin päivitysasel [8]. C (0) = 0, P (0) = P, ˆx(0) = ˆx For i = 1 to N H (i) W (i) K (i) ˆx (i) P (i) C (i) γ (i) = End For P + = δh δx = H (i) = γ (i) x=ˆx i 1 P (i 1) (i 1) (P = ˆx (i 1) = (I K (i) (I K (i) = (I K (i) 1 N + 1 i (N) =P, ˆx + = ˆx(N) H (i)t H (i)t + R + H (i) C(i 1) + C (i 1) )(W (i) ) 1 + K (i) (y h(ˆx (i 1) )) H(i) (i 1) )P (I K (i) H(i) )C(i) K(i)T K (i) H(i) )C(i 1) K (i) R H(i) C(i)T + C (i 1)T H (i)t )T + K (i) R K (i)t (I K (i) H(i) )T

17 2. Teoria Esimeri Esimeritapausena äytetään täysin samaa tapausta, mitä aiempi EKF -algoritmin esimeri äsitteli. RUF -algoritmilla äyttäen ahta väliaselta, saadaan posterioriestimaatisi tilamuuttujalle arvo ˆx + = 3,5238. Posteriorin virhe on siis 0,0238. Posteriorin virheovarianssisi saadaan P + = 0, Estimaatin virhe on siis noin seitsenertainen odotettuun esihajontaan nähden, ainoastaan ahdella väliaseleella lasettuna. Esimerissä äytettiin ainoastaan ahta väliaselta, jotta saatiin seleä uvaaja. Käyttämällä ymmentä väliaselta saataisiin estimaatin virheesi ainoastaan 0,0014 ja esihajonnasi 0,0028, jolloin virhe olisi vain puolet esihajonnasta. Kuvassa 2.2 on esitettynä asivaiheisesti lasettu päivitysasel RUFalgoritmillä y=x 3 RUF (x,y) ˆx,(ˆx )3 ˆx (1),(ˆx(1) )3 +H ˆx (2) ˆx (1),(ˆx(1) )3 ˆx (1),(ˆx )3 +H ˆx (1) Kuva 2.2: RUF -algoritmilla asivaiheisesti lasettu päivitysasel.

18 13 3. YHTEENVETO Tässä andidaatintyössä esiteltiin luijalle ysi yleisimmin äytetyistä Kalmanin suodatin -algoritmin epälineaarisista laajennosista, laajennettu Kalmanin suodatin, seä uusi tapa tehdä epälineaarista estimointia lasemalla päivitysasel paloittain. Työssä äytiin läpi EKF -algoritmin toimintaperiaate ja esiteltiin paloittain päivittävän suodattimen (RUF -algoritmi) toimintaa. Algoritmien eroa havainnollistettiin numeerisella esimerillä, josta ävi selvästi ilmi RUF:n edut suhteessa EKF -algoritmiin. Esimerin tapausessa mittaus oletettiin tarasi. Kuvasta 2.1 nähdään, että tässä tapausessa EKF -algoritmi lasee prioriestimaatin oiein ja se asettuu oiealle ohdalle äyrää pisteeseen (ˆx, (ˆx ) 3 ). Kosa mittaus on huomattavasti tarempi uin prioriestimaatti (ˆx ), posterioriestimaatti lasetaan pääosin mittausen perusteella. Tässä päivitys tapahtuu linearisoinnin perusteella vastaamaan mittausta, jolloin y-aselin arvo asettuu oiealle ohdalle (uten pitääin un mittaus oletettiin tarasi), mutta liian suuren Kalmanin vahvistusen vaiutusesta posterioriestimaatti harhautuu todellisesta arvosta. EKF -algorimissa oletetaan myös Kalmanin vahvistusen olevan sama oo päivitysvälillä, vaia näin ei ole. Verrattaessa RUF:a muihin Kalmanin suodattimen epälineaarisiin laajennusiin tyydytään toteamaan vain, että on tapausia, joissa vanhemmat algorimit antavat paremman tulosen, mutta yleisesti RUF:lla saadaan tarempia tulosia, pienemmällä lasennallisella uormitusella. Johdannossa mainittu hajuton Kalmanin suodatin teee päivitysen samaan tyyliin uin EKF ja EKF2, ja eroaa näistä ainoastaan siinä, että derivaattaa ei laseta. UKF -suodatin voi joissain tapausissa ärsiä samoista ongelmista, uin EKF:in. Esimerisi tapausessa, jossa mittauset ovat taroja, voi UKF antaa rataisun, joa on näitä epätarempi. RUF -algoritmissa on äyttäjän valittavissa, uina moneen palaan päivitysaseleet jaetaan, eli monessao osassa uin päivitysasel lasetaan. Tämä on tapausohtaista ja sopivan määrän etsiminen on tasapainottelua taruuden ja lasennallisen rasauden välillä. Yleisesti voidaan sanoa, että sopiva määrä paloja on pienin mahdollinen, jolla saavutetaan haluttu taruus. Esimeritapausessa jo ahdella väliaseleella lasettaessa päästiin estimaatin virheen ja posterioesivirheen suhteessa ooluoaa pienempään arvoon uin äyttämällä laajennettua Kalman suodatinta. Tästä voidaan päätellä, että erittäin pienellä lasennan lisäämisellä saadaan huo-

19 3. Yhteenveto 14 mattavasti parempi tulos. Kymmentä väliaselta äyttämällä estimaatin virhe olisi vain puolet posterioriesivirheestä, joten RUF -algoritmillä pystytään estimaattia tarentamaan huomattavasti lisäämällä päivitysaseleen palojen määrää. Näin ollen voidaan todeta RUF -algoritmin olevan esimeritapausessa huomattavasti EKF -algoritmia suoritusyyisempi. Kosa paloittain päivittävä Kalman suodatin äyttää derivaattaa, uten EKF ja EKF2:in, niin tilanteet, joissa derivaatat saavat arvoseen nollan, saattavat aiheuttaa sillein ongelmia, mutta vaiutuset eivät ole yhtä dramaattisia, sillä päivitys lasetaan vaiheittain. Olemassaolevista algoritmeista RUF:a eniten muistuttava on iteroitu Kalmanin suodatin (Iterated Kalman filter, IKF). [8]

20 15 LÄHTEET [1] R.E. Kalman A new approach to linear filtering and prediction problems, J. Basic Eng. Mar. 1960, pdf/kalman1960.pdf [2] B. D. Tapley, B. E. Schutz, and G. H. Born, Statistical Orbit Determination. Waltham, MA: Elsevier Academic Press, [3] Ali-Löytty S., Gaussian Mixture Filters in Hybrid Positioning. Väitösirja. Tampere Tampereen Tenillinen Yliopisto. dpub/bitstream/handle/ /219/ali-loytty.pdf [4] S. J. Julier ja J. K. Uhlmann, Unscended Filtering and Nonlinear Estimation, Invited Paper, Proceedings of the IEEE vol. 92, no. 3, march 2004, http: // [5] S. J. Julier ja J. K. Uhlmann, A new extension of the Kalman Filter to nonlinear systems. In Proceeding of AeroSense: the 11th international symposium on aerospace/defence sensing, simulation and controls, 1997, JulierUhlmann-UKF.pdf [6] A. Gelb, Ed., Applied Optimal Estimation. Cambridge, MA: MIT press, 1996, pp , and pp Waltham, MA: Elsevier Academic Press, [7] Grewal, M. ja Andrews, A Kalman Filtering : theory and practice, 1. painos. New Jersey, Prentice-Hall. 381 s. [8] Zanetti, R Recursive Update Filterin for Nonlinear Estimation &url=http%3A%2F%2Fieeexplore.ieee.org%2Fxpls%2Fabs_all. jsp%3farnumber%3d [9] Ali-Löytty S., Collin J. ja Sirola N.: Paiannusen matematiia 2010: math.tut.fi/courses/mat-45800/paiannusen_matematiia_2010b.pdf [10] R. G. Brown, P. Y. Hwang, Introduction to Random Signals and Applied Kalman Filtering, 3rd ed. New Yor: Wiley 1997, pp

21 16 A. LIITE: EKF-ESIMERKIN MATLAB-KOODI x0 = 3.5; %tilamuuttujan todellinen arvo x_ = 2.5; %tilamuuttujan prioriestimaatin arvo P_ = 0.5^2; %tilamuuttujan prioriestimaatin ovarianssi R = 0.1^2; %mittausmallin virheen ovarianssi H = 3*x_^2; %mittausmallin Jacobin matriisin (derivaatan) %arvo prioriestimaatilla lasettuna K_ef = P_*H/(H*P_*H +R); %EKF:n Kalmanin vahvistus y1 = x0^3; %Päivitetty mittaus, joa %oletetaan tässä virheettömäsi x_plus = x_+k_ef*(y1-h(x_)); %x:n posterioriestimaatti P_plus = (1-K_ef*H)*P_; %Posterioriestimaatin ovarianssi sigma = sqrt(p_plus); %Posterioriestimaatin esivirhe

22 17 B. LIITE: RUF-ESIMERKIN MATLAB-KOODI N=10; %Yhden päivitysaseleen palojen määrä %(vapaasti valittava) R = 0.1^2; %mittausmallin häiriön varianssi W=zeros(N,1); %apumuuttuja Kalmanin vahvistusen lasemiseen x_tod = 3.5; %x:n todellinen arvo C=zeros(N+1,1); %tilan x_ ja mittausohinan ristiovarianssi x_=zeros(n+1,1); %apumuuttujavetori, johon estimaatin väliarvot %tallennetaan x_(1)=2.5; %esimeritapausen tilamuuttujan arvon aluarvaus H=zeros(N+1,1); %apumuuttujavetori, johon tallennetaan mittaus- %funtion derivaatan väliarvot. H(1)=3*x_(1)^2; %Mittausfuntion derivaatan arvo aluarvaustilassa h=zeros(n+1,1); %apumuuttujavetori, johon tallennetaan paloissa %mittausfuntion arvot unin palan posteriori- %estimaatin arvolla lasettuna h(1)=x_(1)^3; %Arvio mittausen aluarvosta aluarvaustilassa. K=zeros(N,1); %apuvetori, johon tallennetaan Kalmanin %vahvistusen arvot päivitysaseleen %eri paloissa/vaiheissa. P=zeros(N+1,1); %apuvetori, johon tallennetaan päivitysaseleen eri %vaiheiden estimaattien ovarianssit. P(1)=0.25; %Kyseisen vetorin ensimmäiseen alioon tulee %prioriestimaatin ovarianssi. y=42.875; %Mittausarvo, joa oletettiin esimerissä % virheettömäsi (y = x_tod^3). for i = 1:N gamma=1/(n+1-i); %Tässä silmuassa lasetaan %päivitysaseleen palat. %Kalmanin vahvistusen painoerroin, %jolla oonaisvahvistus jaetaan tasan %päivitysaseleen eri

23 B. Liite: RUF-esimerin MATLAB-oodi 18 end %vaiheiden esen. W(i)=H(i)*P(i)*H(i)+R+H(i)*C(i)+C(i)*H(i); K(i)=gamma(i)*(P(i)*H(i)+C(i))/W(i); x_(i+1)=x_(i)+k(i)*(z-h(i)); C(i+1)=(1-K(i)*H(i))*C(i)-K(i)*R; P(i+1)=(1-K(i)*H(i))*P(i)*(1-K(i)*H(i)) +K(i)*R*K(i) -(1-K(i)*H(i))*C(i+1)*K(i)-K(i)*C(i+1)*(1-K(i)*H(i)); H(i+1)=3*x_(i+1)^2; h(i+1)=x_(i+1)^3; x_post = x_(n+1); %Tilan posterioriestimaatti tallentuu %silmuassa x_ -vetorin viimeiseen alioon, %eli se on päivitysaseleen viimeisen %vaiheen tulos. P_post = P(N+1); %Posterioriestimaatin ovarianssi tallentuu % vastaavasti P-vetorin viimeiseen alioon. sigma = sqrt(p_post); %Posterioriestimaatin esivirhe