Antti Somppi Paloittain päivittävä Kalmanin suodatin

Antti Somppi
Paloittain päivittävä Kalmanin suodatin
Kandidaatintyö
I
TIIVISTELMÄ
TAMPEREEN TEKNILLINEN YLIOPISTO
Teknis-luonnontieteellinen koulutusohjelma
TEKIJÄN NIMI: Antti Somppi
Kandidaatintyö 15 sivua, 3 liitesivua
Maaliskuu 2015
Pääaine: Matematiikka
Tarkastaja: Simo Ali-Löytty
Avainsanat: Kalmanin suodatin, epälineaarinen estimointi, Paloittain päivittävä Kalmanin suodatin
Kalmanin suodatin on algoritmi, joka laskee mittausten ja niitä kuvaavan mallin
avulla arvion näitä vastaavista todellisista arvoista. Tavoitteena on, että yhdistämällä mittaukset ja malli saadaan tarkennettua arviota todellisista arvoista sellaisessa
tilanteessa, jossa mittauksiin ja mallin antamiin tuloksiin tiedetään sisältyvän virheitä. Alkuperäinen Kalmanin suodatin on tehty laskemaan approksimaation tilanteessa, jossa niin malli kuin mittauksetkin ovat lineaarisia. Epälineaarisia tapauksia
varten on kehitetty alkuperäisen algoritmin pohjalta laajennoksia, joilla pyritään lisäämään algoritmin käyttömahdollisuuksia.
Tässä työssä tutustutetaan lukija erääseen tällaiseen laajennokseen, ja esitetään sen
lisäksi uusi tehokkaampi tapa ratkaista epälineaarisia estimointiongelmia. Työ pohjautuu Renato Zanettin vuonna 2012 tekemään julkaisuun RUF-algoritmista “Recursive Update Filtering for Nonlinear Estimation”, ja siinä osoitetaan uuden algoritmin etuja suhteessa vanhempaan algoritmiin käyttäen numeerista esimerkkiä, sekä
sitä havainnollistavia kuvia. Työssä todetaan RUF-algoritmin edut laskennallisen
vaativuuden sekä tulosten tarkkuuden suhteen verrattuna muihin epälineaarisia estimointiongelmia ratkaiseviin algoritmehin.
II
SISÄLLYS
1. Johdanto . . . . . . . . . . . . . . . .
2. Teoria . . . . . . . . . . . . . . . . . .
2.1 Ongelman asettelu . . . . . . . .
2.2 Laajennettu Kalmanin suodatin .
2.2.1 Algoritmin esittely . . . . . .
2.2.2 Esimerkki . . . . . . . . . . .
2.3 RUF Kalman -algoritmi . . . . .
2.3.1 Algoritmin esittely . . . . . .
2.3.2 Algoritmin johtaminen . . . .
2.3.3 Esimerkki . . . . . . . . . . .
3. Yhteenveto . . . . . . . . . . . . . . .
Lähteet . . . . . . . . . . . . . . . . . . .
A. Liite: EKF-esimerkin MATLAB-koodi
B. Liite: RUF-esimerkin MATLAB-koodi
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
1
3
3
5
5
6
8
8
8
12
13
15
16
17
III
TERMIT JA LYHENTEET MÄÄRITELMINEEN
f
Tilafunktio
h
Mittausfunktio
ti
Ajanhetki askeleella i
xi
Tilamuuttujan tila ajanhetkellä ti
x−
i
Tilamuuttujan tilan prioriestimaatti
x+
i
Tilamuuttujan tilan posterioriestimaatti
yi
Mittaus ajanhetkellä ti
ŷi
Mittauksen ennustettu arvo
wi
Tilamallin virhetermi
vi
Mittausmallin virhetermi
Qi
Mallifunktion kovarianssimatriisi
Ri
Mittafunktion kovarianssimatriisi
Ki
Algoritmin Kalmanin vahvistus
Φi
Mallifunktion derivaatan arvo ajanhetkellä ti x:n arvolla x̂−
i
Hk
Mittafunktion derivaatan arvo ajanhetkellä ti x:n arvolla x̂−
i
Pi
x:n (eli tilamuuttujan) keskineliövirhematriisi
Pi−
Prioriestimaattorin keskineliövirhematriisi
Pi+
Posteriestimaattorin keskineliövirhematriisi
Ck
Ristikovarianssi tilan xk ja mittauskohinan välillä
e1k
RUF-algoritmin ensimmäisen päivityspalan posteriorivirhe
e−
k
RUF-algoritmin päivityksen prioriestimointivirhe
MVUE
Harhaton minimivarianssiestimaattori (Minimum Variance Unbiased Estimate)
EKF
Laajennettu Kalmanin suodatin (Extended Kalman Filter)
IV
RUF
Paloittain päivittävä Kalmanin suodatin (Recursive Update Filter)
BLU
Paras harhaton lineaarinen (estimaattori) (Best Linear Unbiased)
EKF2
Toisen Asteen Laajennettu Kalmanin suodatin (Second order EKF)
UKF
Hajuton Kalmanin suodatin (Unscented Kalman Filter)
UT
Hajuton muunnos (Unscented Transformation)
IKF
Iteroitu Kalmanin suodatin (iterated Kalman filter)
Huom. Alaindeksi i viittaa tilamallin askeleeseen diskreetissä mallissa.
1
1.
JOHDANTO
Alkuperäinen Kalmanin suodatin -algoritmi julkaistiin vuonna 1960 [1]. Se on tullut
erittäin tunnetuksi ja antaa optimaalisen ratkaisun estimoitaessa lineaarisia ongemia. Globaali optimaalisuus saavutetaan kun käsitellään lineaarista mallia ja lineaarisia mittauksia, joiden virheet ja alkuperäinen estimointivirhe ovat normaalisti
jakautuneita. Mikäli virheet eivät ole normaalisti jakautuneita, klassinen Kalmanin
suodatin ei ole globaalisti optimaalinen, mutta silti lineaarinen harhaton minimivarianssi estimaattori (MVUE). [2] Kalmanin suodatinta voidaan soveltaa eri tavoilla sen mukaan, miten sitä tulkitaan. Bayesilaisella tulkinnalla haetaan jakaumia,
parhaan harhattoman lineaarisen estimaatin tulkinnalla minimoidaan posterioriestimaatin ja todellisen tilan neliöllistä keskivirhettä, ja deterministinen tulkinta jättää
pois oletukset virheiden jakaumista ja tulkitsee ongelmaa täysin deterministisenä
prosessina, jossa haetaan ratkaisua deterministiselle pienimmän neliösumman ongelmalle. [3]
Epälineaarisille estimointiongelmille on kehitetty monia laajennoksia Kalmanin
suodattimeen perustuen. Monet niistä perustuvat parhaan harhattoman lineaarisen
estimaatin laskemiseen. Näistä yksi yleisimpiä on laajennettu Kalmanin suodatin
(Extended Kalman filter, EKF). Laajennettu Kalmanin suodatin pyrkii linearisoimaan mittausten ja mallin epälineaarisuuksia. Algoritmissa oletetaan virheet pieniksi ja epälineaarisuuksia approksimoidaan niiden ensimmäisellä kertaluvulla, eli
approksimoimalla mittaus- ja tilamallia niiden Taylorin sarjojen kahdella ensimmäisellä termillä. Tämä toimii, mikäli epälineaarisuudet eivät ole kovin suuria. EKF:stä
on olemassa myös erilaisia variaatioita, kuten toisen asteen laajennettu Kalmanin
suodatin (Second Order Extended Kalman filter, EKF2). Tämä eroaa tavallisesta
EKF-suodattimesta vain siltä osin, että approksimaatioissa käytetään mallien Taylorin sarjojen toisen asteen termejä. Niin EKF:n kuin EKF2:nkin yksi puute on, että
niissä mittausfunktion arvon arvio lasketaan prioritilamuuttujan arvolla, ja mikäli
tämä arvio on väärin, ajautuu algoritmi heti alussa vääriin arvoihin. Toinen ongelma
on, että kun Kalmanin vahvistukselle lasketaan arvo, sen oletetaan olevan sama koko
päivitysaskeleella, vaikka 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, kuten myös tässä työs-
1. Johdanto
2
sä esiteltävä paloittain päivittävä suodatin vaativat laskuissa derivaattojen laskemista analyyttisesti sekä numeerisesti. On olemassa myös Kalmanin suodattimen laajennoksia, jotka eivät tätä vaadi. Esimerkiksi hajuton Kalmanin suodatin (Unscended
Kalman Filter, UKF) on tällainen. Se laskee numeerisesti integroimalla estimaattien arvot. UKF käyttää muunnosta, jota kutsutaan hajuttomaksi muunnokseksi
(Unscented Transformation, UT), jossa tilamuuttujan jakaumaa arvioidaan tekemällä mittausfunktion arvon odotusarvosta muunnos niin sanottujen sigmapisteiden avulla lasketuksi integraaliksi. Tämä arvio on tarkka mikäli mittausfunktio on
kolmannen asteen polynomi. Sigmapisteet lasketaan normaalijakaumasta, jonka odotusarvona on tilamuuttujan prioriestimaatti ja kovarianssina tilamuuttujan priorikovarianssi. Niiden määrittämiseen voidaan käyttää erilaisia menetelmiä tilanteesta
riippuen. Hajuton suodatin välttää EKF:n ja EKF2:n ongelman väärästä Kalmanin
vahvistuksesta valitsemalla sigmapisteet ympäri jakaumaa ja laskemalla vahvistuksen niistä. UKF:n algoritmi on muutoin sama kuin EKF:n, mutta posterioritilan ja
posteriorikovarianssin laskemisessa derivaatat korvataan hajuttomalla muunnoksella
lasketulla mittauksen odotusarvolla. Laskennalliselta vaativuudelta UKF on samaa
luokkaa EKF:n kanssa. Mahdollisia ongelmia UKF:ssä voi aiheuttaa se, että kovariansseiksi on mahdollista laskea negatiivisia arvoja, mutta tämä on vältettävissä
oikeanlaisella sigmapisteiden valinnalla.[4; 3; 5]
Edellä mainittujen lisäksi lisäksi on olemassa lukuisia muita algoritmeja, joilla
voidaan ratkoa epälineaarisia ongelmia. Näilläkin on omat heikkoutensa, esimerkiksi laskennallinen raskaus, joka on haittana mm. tehtäessä reaaliaikaista laskentaa.
Tässä työssä on tarkoituksena esitellä epälineaaristen estimointiongelmien ratkaisemiseen uusi menetelmä, jolla vältetään EKF:n ja muiden algoritmien heikkouksia.
Uuden algoritmin laskennallinen vaativuus on verrannollinen (N-1) kappaleeseen
EKF:n päivitysaskeleita. Algoritmi jakaa yhden päivitysaskeleen N kappaleeseen paloja, ja mitä useampia näitä on, sitä vähemmän algoritmi luottaa linearisointiin. [8]
Työssä esitellään EKF-algoritmi ja kyseinen uusi algoritmi, sekä verrataan niiden
toimivuutta esimerkkitapauksessa. Tämä työ on mukailee lyhennettynä Renato Zanettin vuonna 2012 tekemää julkaisua “Recursive Update Filtering for Nonlinear
Estimation”.
3
2.
TEORIA
2.1
Ongelman asettelu
Tarkoituksena tässä työssä on esitellä uudenlainen sovellustapa Kalmanin suodattimelle epälineaarissa tapauksissa. Työssä perehdytään suodattimeen, jota kutsutaan
vapaasti suomennettuna paloittain päivittäväksi Kalmanin suodattimeksi (Recursive Update Filter, RUF). Tarkoituksena on esitellä ja johtaa suodattimen algoritmi,
jossa epälineaarinen signaali approksimoidaan linearisoimalla päivitysaskel paloittain. Suodattimen perusajatus on tehdä lineaarinen estimaatti epälineaariselle mallille samankaltaisesti kuin laajennetulla Kalmanin suodattimella [6], mutta jakaa approksimaatiossa algoritmin päivitysaskel useampaan palaan, millä virheen toivotaan
pienentyvän siedettävän suuruiseksi, linearisoinnista huolimatta. Työssä käydään läpi myös EKF-algoritmin periaatteet ja sen antamia tuloksia, joihon RUF-algoritmin
antamia tuloksia verrataan yksinktertaisilla esimerkeillä MATLAB-koodina toteutettuna. Molemmista algoritmeista on olemassa diskreetisti ja jatkuvasti päivittävät
versiot. Tässä työssä perehdytään edellä mainittuihin.
Tila- ja mittamalleina tässä työssä käytetään funktioita f ja h, sekä niihin lisättyjä
virhetermejä w ja v. Yhtälömuodossa funktiot ovat seuraavanlaiset [7]:
xk = fk (xk−1 ) + wk ,
yk = hk (xk ) + vk .
(2.1)
Lineaariseen suodattimeen erona on funktioiden epälineaarisuus. Funktioista toinen
tai molemmat voivat olla epälineaarisia, kun taas lineaarisissa tapauksissa molemmat funktiot ovat lineaariset. Funktiot voivat muuttua askeleen mukaan, mihin viitataan alaindekseillä. Tässä työssä käsitellään ainoastaan tapauksia, joissa funktiot
ovat samoja joka askeleella. Virhetermit noudattavat nollakeskistä normaalijakaumaa kovarianssi(matriiseilla) Q ja R. Virheet eri ajanhetkillä ovat toisistaan riippumattomia.[9] Kovarianssit päivitetään jokaisella askeleella tilamuuttujien lisäksi.
Mittaukset oletetaan annetuiksi. Yleisestä tapauksesta poiketen tässä työssä käytetään skalaariesimerkkejä ongelman yksinkertaistamiseksi. Yleisessä tapauksessa tilavektorin ollessa m-pituinen ja mittausvektorin n-pituinen niiden virhetermit ovat
saman pituiset ja virheiden kovarianssimatriisit m x m - ja n x n -kokoiset. Tässäkin
2. Teoria
4
alaindeksi k viittaa askeleeseen ajanhetkella tk .
wk ∼ N(0, Qk ),
vk ∼ N(0, Rk ).
(2.2)
Melko yleinen tapa ratkaista epälineaaisia ongelmia on etsiä paras lineaarinen harhaton estimaattori, ns. BLU-estimaattori (best linear unbiased). Käytännössä tämä
tarkoittaa, että oletuksena ovat seuraavat yhtälöt [9]:
"
E
"
V
xk
yk
xk
yk
#!
#!
"
"
=
=
x̄k
ȳk
#
Pxxk Pxyk
Pxyk Pyyk
#
Tällöin tilan xk paras lineaarinen harhaton estimaattori on:
−1
x̂k = x̄k + Pxyk Pyy
(yk − ȳk ).
k
(2.3)
EKF-algoritmi ratkaisee tilamuuttujalle seuraavan askeleen priori- ja posterioriestimaatit, sekä odotetun mittaustuloksen. Jokaisella askeleella päivitetään kovarianssit molempien mallien kohinatermeille. Ongelmana EKF-algoritmissa on se, että
tarpeeksi epälineaarisissa tapauksissa algoritmin laskema posterioriestimaatti seuraavalla askeleella eroaa mittauksesta enemmän, kuin mitä hajonta on. Tämä johtuu siitä (kuten myöhemmin osoitetaan), että otetaan liian suuri askel mallifunktion
tangentin suuntaan ja päädytään kauaksi todellisesta funktion pisteestä. Tässä saatetaan päätyä tilanteeseen, jossa kohinaa sisältävä mittaus itsessään olisi tarkempi
estimaatti seuraavalle askeleelle, kuin algoritmin määrittämä estimaatti.
Ongelmana on siten löytää menetelmä, jolla päästään tarkempaan arvioon seuraavan askeleen tilamuuttujan arvosta. Tarkoituksena on siis korjata arviota siten, ettei todellisesta arvosta harhauduta, vaan parantaa algoritmin kykyä käsittellä myös
enemmän epälinaarisuutta. Yksinkertaistettuna: ollessamme tilamallin tilassa xk−1
pyrimme määrittämään tilamuuttujalle estimaatin tilassa xk . Tähän päästäksemme
tarvitsemme korjauksen EKF-algoritmin laskemaan posterioriestimaattoriin x+
k.
RUF-algoritmi lähtee etsimään parempaa estimaattia ottamalla pienemmän askeleen mallifunktion derivaatan suuntaan, jolloin tarkoituksena on pystyä seuraamaan
todellisen signaalin käyrää tarkemmin.
2. Teoria
2.2
2.2.1
5
Laajennettu Kalmanin suodatin
Algoritmin esittely
EKF -algoritmi eli laajennettu Kalmanin suodatin on ensimmäisiä Kalmanin suodattimen sovelluksia. Se kehitettiin pian alkuperäisen Kalmanin suodattimen jälkeen,
joka julkaistiin vuonna 1960. Tällöin oltiin huomattu, että Kalmanin suodattimelle
olisi olemassa paljon käyttökohteita, joita sillä ei kuitenkaan voida käsitellä. Tämä
johtuu siitä, että alkuperäinen suodatin olettaa mittaussignaalit lineaarisiksi ja sen
soveltamisessa käytetään lineaarisia malleja. Laajennettu Kalmanin suodatinta käytetään laajalti epälineaarisen estimoinnin työkaluna ja se olettaa estimointivirheet
pieniksi.[6] Perusajatus EKF:ssä on täysin sama kuin alkuperäisessä Kalmanin suodattimessa.
Seuraavan askeleen prioriestimaatin laskemiseen käytetään mallifuntiota f , ottaen
huomioon mallin epätarkkuuden kohinalla wk . Virhetermi noudattaa nollakeskistä
normaalijakaumaa, jonka kovarianssi(matriisina) on Qk .[7]
Annettujen funtioiden avulla lasketaan prioriestimaatti tilamuuttujalle sijoittamalla tilamalliin edellisen askeleen posterioriestimaatti sekä ennustettu mittaustulos
sijoittamalla mallifunktioon nykyisen askeleen prioriestimaatti:
+
−
x̂−
k = fk−1 (x̂k−1 ) ŷ = hk (x̂k ).
(2.4)
Kuten yleisessä ongelmanasettelussa mainittiin, funktioiden alaindeksit viittaavat
mallin askeleeseen. Funktiot voivat muuttua eri askeleilla, mutta tässä työssä käsitellään tapauksia, joissa näin ei ole. Seuraavassa vaiheessa tehdään lineaariapproksimaatio käyttäen tilamallin derivaattaa ja mittamallin derivaattaa. Tilafuntiota ja
mittamallin funktioita approksimoidaan niiden Taylorin sarjojen kahdella ensimmäisellä termillä.
Φk−1 ≈
δfk−1 (x) −
δx
x=x̂k−1
Hk ≈
δhk (x) .
δx x=x̂−k
(2.5)
Tilamallin priorikovarianssi(matriisi) seuraavassa tilassa saadaan nyt laskettua hyödyntämällä tilamallin derivaatan arvoa käyttäen tilamuuttujalle tässä nykyisen tilan
prioriestimaattia.
+
Pk− = Φk−1 Pk−1
ΦTk−1 + Qk−1 .
(2.6)
Kalmanin vahvistus lasketaan samaan tapaan kuin tavallisessa Kalmanin suodatin algoritmissa, mutta tilamallin ja mittamallin matriisit korvataan mitta- ja tilamallin
2. Teoria
6
Taulukko 2.1: EKF -algoritmin päivitysaskel, perustuu lähteeseen [7].
+
−
x̂−
k = f (x̂k−1 ), ŷk = h(x̂k )
δf (x, k − 1) Φk−1 ≈
−
δx
x=x̂k−1
δh(x, k) Hk ≈
−
δx
x=x̂k
−
+
Pk = Φk−1 Pk−1 ΦTk−1 + Qk−1
Kk = Pk− HkT (Hk Pk− HkT + Rk )−1
−
x̂+
k = x̂k + Kk (yk − ŷk )
Pk+ = (I − Kk Hk )Pk−
funktioiden derivaattojen arvoilla aiemmin lasketuissa pisteissä:
Kk =
2.2.2
Pk− HkT
.
Hk Pk− HkT + Rk
(2.7)
Esimerkki
Seuraava esimerkki kuvaa hyvin EKF -algoritmia käytettäessä mahdollisesti esiintyviä kompastuskiviä. [8] Käytetään selkeyden vuoksi yksinkertaista skalaariesimerkkiä. Oletetaan, että ollaan tilassa, jonka todellinen arvo on x = 3,5 ja prioriestimaatille on annettu arvio x̂− = 2,5. Myös prioriestimaatin kovarianssi on annettu ja sen
arvo on P − = 0,52 . Tällöin on tiedossa, että prioriesimaatin virhe on
x − x̂− = 3,5 − 2,5 = 1.
(2.8)
Kovarianssista nähdään, että keskihajonta on σ = 0,5, joten x̂− on kahden keskihajonnan päässä odotusarvostaan. Oletetaan että mittausfunktio on seuraavanlainen:
y = x3 + w
w ∼ N(0, 0,12 ).
(2.9)
Mittauksista laskemalla x:lle saataisiin siten seuraava estimaatti:
x̂M itta = y 1/3 .
(2.10)
Monte Carlo -simulointia käyttämällä tälle estimaatin virhekovarianssille PM itta saadaan laskettua arvo PM itta = 0,00272 . Mittafunktion derivaatan ja mittamallin virheen kovarianssin avulla voidaan myös approksimoida mittafunktion virheen kova-
2. Teoria
7
rianssia käyttämällä sen laskemisessa tilamuuttujan todellista arvoa ja saadaan lähes
sama arvo kuin Monte Carlo -simuloinnilla:
PM itta ≈
0,12
= 0,0027.
(3 · 3,52 )2
(2.11)
Kalmanin vahvistuksen laskemista varten lasketaan Hk :n lauseke auki:
3
δh(x, k) δx̂−
2
Hk ≈
− = k− = 3x̂−
k .
δx
x=x̂k
δx̂k
(2.12)
Nyt voidaan määrittää arvo Kalmanin vahvistukselle aiemmin mainitulla kaavalla
Kk = Pk− HkT [Hk Pk− HkT + Rk ]−1 = 0,1 · 3(x̂− )2 /[9(x̂− )4 · 0,12 + 0,12 ] = 0,0533.
(2.13)
Koska halutaan pitää esimerkki deterministisenä, oletetaan mittaus virheettömäksi,
eli y = x3 = 42,875. Tällöin algoritmin perusteella saadaan laskettua tilamuuttujalle
seuraava estimaatti x̂+ = 3,9532.
−
− 3
x̂+
EKF = x̂ + Kk (y − (x̂ ) ) = 3,9532.
(2.14)
Tästä nähdään, että estimaattorin virhe on 0,4532. Posteriorivirheen kovarianssiksi
saadaan laskettua 0,00532 .
P + = (1 − 3Kk (x̂− )2 )2 P − + Kk2 R = 0,00532 .
(2.15)
Tästä voidaan laskea, että esimaattorin virhe on noin 85-kertainen virheen keskihajontaan nähden. Virhe on siis erittäin suuri keskivirheeseen nähden. Virhe antaa
olettaa, että posterioriestimaatti olisi lähellä oikeata arvoa, mutta numeerisesta tuloksesta nähdään, ettei näin ole. Näin ollen algoritmin suorituskyky ei tässä tapauksessa ole hyvä. Seuraavassa edellä kuvattua esimerkkiä havainnollistava kuva.
2. Teoria
8
80
70
60
y=x3
prioriest.
posterioriest.
tod.sijainti
50
x, y
40
30
20
− 3
+
−
x̂+
k , (x̂k ) + Hk (x̂k − x̂k )
x̂− ,(x̂− )3
10
0
2
2.5
3
3.5
4
Kuva 2.1: EKF-algoritmilla laskettu päivitysaskel.
2.3
2.3.1
RUF Kalman -algoritmi
Algoritmin esittely
Tavallinen Kalmanin soudatin, sekä siihen perustuva EKF toimivat siten, että lineaarinen päivitysaskel otetaan käyttöön välittömästi kun estimaatti on saatu laskettua.
Paloittain päivittävä soudatin toimii siten, että päivitysaskel suoritetaan vähitellen
siten, että posterioriestimaatille lasketaan väliarvoja, joita käytetään seuraavassa
välivaiheessa prioriarvoina. [8] Tässä menetelmässä approksimaatiovaiheen Jacobin
matriisi voidaan laskea uudelleen ennen päivitystä, jolloin saadaan tarkemmin seurattua lineaarisella arviolla epälineaarista signaalia. Tässä kohtaa myös Kalmanin
vahvistus lasketaan uudelleen vastaamaan muuttunutta derivaattaa.
Paloittain päivittävän suodattimen etuna on se, että sitä käytettäessä ei välttämättä
tarvita kohinatermien jakaumaa. Mallifunktion ei tarvitse olla myöskään lineaarinen.
Jatkossa tullaan näyttämään, että RUF toimii paremmin kuin EKF tapauksissa joissa mittausfunktio on epälineaarinen. Tämän lisäksi rekursiivisella suodattimella vältetään Taylor-sarjan typistämisestä aiheutuva hajautuminen todellisesta funktiosta.
Algoritmi seuraa mittausfunktion gradienttia ja minimoi kovarianssin. Mittausten
ollessa lineaarisia algoritmi redusoituu laajennetuksi Kalman suodattimeksi. [8]
2.3.2
Algoritmin johtaminen
Kuten edellä jo todettiin, algoritmin ydinajatus on uuden askeleen lisääminen vaiheittain. Jokaisessa vaiheessa Jakobin matriisi H lasketaan uudelleen, mikä mahdol-
2. Teoria
9
listaa mittausepälineaarisuuksien tarkemman approksimoinnin lineaarisesti. Algoritmia varten merkitsemme muuttujalla Ck ristikovarianssia ajanhetkellä tk todellisen
tilan xk ja mittauskohinan välillä[10]:
T
Ck = E(xk − x̂−
k )vk .
(2.16)
Mittausmalli ja tilamalli määritellään aivan samoin kuin EKF-algoritmissäkin.
Päivitysaskeleet määritellään seuraavasti:
−
−
x̂+
k = x̂k + Kk (yk − Hk x̂k ).
(2.17)
yk = Hk xk + vk .
(2.18)
Optimaalinen minimivarianssiestimoinnin mukainen Kalmanin vahvistus on muotoa:
Kk = (Pk− HkT + Ck )(Hk Pk− HkT + Rk + Hk Ck + CkT HkT )−1 .
(2.19)
Posteriorivirhekovarianssimatriisi lasketaan seuraavasti:
Pk+ =(I − Kk Hk )Pk− (I − Kk Hk )T + Kk Rk KkT
− (I − Kk Hk )Ck KkT − Kk CkT (I − Kk Hk )T .
(2.20)
Kun tietyn askeleen mittaus on päivitetty kerran, saadaan posteriorivirheeksi ja
Kalmanin vahvistukseksi seuraava:
(1)
(1)
(1)
(1)
ek = xk − x̂k = (I − Kk Hk )e−
k − Kk vk .
(1)
Kk = Pk− HkT (Hk Pk− HkT + Rk )−1 .
(2.21)
(2.22)
Lausekkeessa 2.21 merkitään prioriestimointivirhettä e−
k :lla:
(1)
e−
k = xk − x̂k
(2.23)
(0)
Alkuvaiheessa kovarianssi Ck (merkitään myös Ck ) saa arvokseen nolla. Tämä
johtuu siitä, että tilan k prioritilamuuttuja ja tilan mittaus eivät ole toisistaan riip-
2. Teoria
10
puvia ensimmäisellä iteraatiolla. Kuitenkin seuraavan iteraation prioritilamuuttujan
laskemisessa käytetään kyseisen askeleen mittausdataa, jolloin tilamuuttujan virhe
ja mittausvirhe eivät ole toisistaan riippumattomia, jolloin Ck ei saa arvokseen nolla.Tästä johtuen ensimmäisen iteraation vahvistuksen lauseke saa ylläolevan muo(1)
don. ek :n ja vk :n väliseksi ristikovarianssiksi ensimmäisen vaiheen jälkeen saadaan:
(1)
Ck = −Kk Rk .
(2.24)
(1)
Pk+ :n lauseketta yksinkertaistamalla ja sijoittamalla Kk :n lauseke saadaan ensimmäisen vaiheen jälkeiselle kovarianssimatriisille:
(1)
(1)
Pk = (I − Kk Hk )Pk− − Kk CkT .
(2.25)
Suluissa olevat yläindeksit viittaavat tässä tietyn päivitysaskeleen vaiheeseen. Yllä
olevien yhtälöiden laskemisen jälkeen on teoriassa käyty siis yhden päivitysaskeleen
ensimmäinen vaihe. Vaiheiden määrä on vapaasti valittava parametri. Kuten edellä
(0)
mainittiin, ristikovarianssi Ck prioritilan ja mittausvirheen välillä on nolla, josta
(1)
seuraa, että Kk :n lauseke on hieman eri muotoa kuin myöhempien vahvistusten.
Tämä pätee ainoastaan optimaaliselle vahvistukselle. Jos nyt käsitellään sama alkuperäinen mittaus saaduilla kovariansseilla ja ristikovarianssilla, sekä alkuperäisellä
Jakobin matriisilla, käytetään priorikovarianssina Pk− edellisen vaiheen posterioriko(1)
varianssia Pk . Tällöin seuraavan vaiheen Kalmanin vahvistukseksi tulee:
(2)
(1)
(1)
(1)
(1)
(1)T
Kk = (Pk HkT + Ck )(Hk Pk − HkT + Rk + Hk Ck + Ck
HkT )(−1) .
(2.26)
Laskemalla edellinen lauseke auki saadaan tulokseksi nolla. Tämä johtuu siitä,
että ensimmäisessä vaiheessa oletettiin vahvistuksen arvo täydelliseksi, jolloin voidaan ajatella, että kaikki informaatio mittauksesta saatiin hyödynnettyä. Tässä tapauksessa muutosta ei tulisi tapahtua, sillä päivitysaskel saatiin jo tehtyä edellisessä
vaiheessa.
Mikäli ensimmäisen vaiheen päivitys ei olekaan optimaalinen, niin silloin vain osa
datasta hyödynnetään. Jos esimerkiksi Kalmanin vahvistus on vain puolet edellisen
esimerkin vahvistuksesta niin se saa arvon
(1)
Kk = 0,5Pk− HkT (Hk Pk− HkT + Rk )−1 .
(2)
(2.27)
Tässä tapauksessa myös Kk saa eri arvon, joka on eri kuin nolla. Seuraavassa vai(2)
heessa laskettavan vahvistuksen Kk arvo on sellainen, että askeleen kokonaisvai-
2. Teoria
11
kutus on sama kuin tavallisen Kalman suodattimen askeleessa. Mikäli päivitysaskel
jaetaan kolmeen vaiheeseen, niin sillon halutaan, että jokaisessa vaiheessa lisätään
kolmannes kokonaispäivityksestä. Tällöin ensimmäisen Kalmanin vahvistuksen kerroin on 1/3. Toisen vaiheen vahvistuksen kerroin on 1/2 ja viimeisen 1. Näin menetellään siksi, että ensimmäisen vaiheen jälkeen mittauksen sekä prioriestimaatin
ja Jakobin matriisin tulon erotuksesta on vähentynyt kolmannes (koska ensimmäisessä vaiheessa siirryttiin kolmannes päivitysaskeleen suuntaan) ja tällöin jäljellä
kyseisestä erotuksesta on kaksi kolmannesta. Kun kaksi kolmannesta kerrotaan puolikkaalla, saadaan kolmasosa, jolloin liikutaan taas kolmasosa uudelleen lasketun Jakobin suuntaan (eli edelleen approksimoidaan epälineaarista tapausta lineaarisesti
mutta ollaan tehty korjausta). Kun kolmannessa vaiheessa otetaan Kalmanin vahvistuksen kertoimeksi 1, niin päivityksestä on tehty jo kaksi kolmannesta, eli jäljellä
on kolmannes. Näin saatiin tehtyä yksi päivitysiteraatio kolmivaiheisesti siten, että
jokaisessa vaiheessa käytettiin sama osuus mittausdatasta ja korjattiin gradientin
suuntaa. Taulukossa 2.2 on esitettynä RUF-algoritmin yksi päivitysaskel.
Taulukko 2.2: RUF-algoritmin päivitysaskel [8].
(0)
(0)
(0)
Ck = 0, Pk = Pk− , x̂k = x̂−
k
For i = 1 to N
δh (i)
Hk =
δx x=x̂i−1
k
(i)
(i)
(i−1)
Wk = Hk Pk
(i)
(i)
(i−1)
Kk = γk (Pk
(i)
(i−1)
x̂k = x̂k
(i)
(i)T
+ Rk + Hk Ck
(i)
(i)T
+ Ck
Hk
Hk
(i−1)
(i)
(i)
(i)
(i−1)
Pk = (I − Kk Hk )Pk
(i)
(i)
(i)
(i)
(i)
(N )
(N )
(i)
(i−1)
Pk+ =Pk , x̂+
k = x̂k
(i)T
Hk
(i)
))
(i)
(i)
(i)
(i)T
(I − Kk Hk )T + Kk Rk Kk
(i)T
− (I − Kk Hk )Ck Kk
Ck = (I − Kk Hk )Ck
1
(i)
γk =
N +1−i
End For
(i−1)T
+ Ck
)(Wk )−1
(i−1)
+ Kk (yk − h(x̂k
(i−1)
(i)
(i)T
− Kk Ck
(i)
− Kk Rk
(i)
(i)
(I − Kk Hk )T
2. Teoria
2.3.3
12
Esimerkki
Esimerkkitapauksena käytetään täysin samaa tapausta, mitä aiempi EKF -algoritmin
esimerkki käsitteli. RUF -algoritmilla käyttäen kahta väliaskelta, saadaan posterioriestimaatiksi tilamuuttujalle arvo x̂+ = 3,5238. Posteriorin virhe on siis 0,0238.
Posteriorin virhekovarianssiksi saadaan P + = 0,00322 . Estimaatin virhe on siis noin
seitsenkertainen odotettuun keskihajontaan nähden, ainoastaan kahdella väliaskeleella laskettuna. Esimerkissä käytettiin ainoastaan kahta väliaskelta, jotta saatiin
selkeä kuvaaja. Käyttämällä kymmentä väliaskelta saataisiin estimaatin virheeksi
ainoastaan 0,0014 ja keskihajonnaksi 0,0028, jolloin virhe olisi vain puolet keskihajonnasta. Kuvassa 2.2 on esitettynä kaksivaiheisesti laskettu päivitysaskel RUFalgoritmillä.
50
45
40
y=x3
RUF
(x,y)
(1)
(1)
(2)
x̂k , (x̂k )3 + Hk ∆x̂k
(1)
(1)
x̂k , (x̂k )3
35
30
25
(1)
15
10
2.4
(1)
3
x̂k , (x̂−
k ) + Hk ∆x̂k
20
− 3
x̂−
k ,(x̂k )
2.6
2.8
3
3.2
3.4
3.6
Kuva 2.2: RUF -algoritmilla kaksivaiheisesti laskettu päivitysaskel.
13
3.
YHTEENVETO
Tässä kandidaatintyössä esiteltiin lukijalle yksi yleisimmin käytetyistä Kalmanin
suodatin -algoritmin epälineaarisista laajennoksista, laajennettu Kalmanin suodatin, sekä uusi tapa tehdä epälineaarista estimointia laskemalla päivitysaskel paloittain. Työssä käytiin läpi EKF -algoritmin toimintaperiaate ja esiteltiin paloittain
päivittävän suodattimen (RUF -algoritmi) toimintaa.
Algoritmien eroa havainnollistettiin numeerisella esimerkillä, josta kävi selvästi
ilmi RUF:n edut suhteessa EKF -algoritmiin. Esimerkin tapauksessa mittaus oletettiin tarkaksi. Kuvasta 2.1 nähdään, että tässä tapauksessa EKF -algoritmi laskee
prioriestimaatin oikein ja se asettuu oikealle kohdalle käyrää pisteeseen (x̂− , (x̂− )3 ).
Koska mittaus on huomattavasti tarkempi kuin prioriestimaatti (x̂− ), posterioriestimaatti lasketaan pääosin mittauksen perusteella. Tässä päivitys tapahtuu linearisoinnin perusteella vastaamaan mittausta, jolloin y-akselin arvo asettuu oikealle
kohdalle (kuten pitääkin kun mittaus oletettiin tarkaksi), mutta liian suuren Kalmanin vahvistuksen vaikutuksesta posterioriestimaatti harhautuu todellisesta arvosta.
EKF -algorimissa oletetaan myös Kalmanin vahvistuksen olevan sama koko päivitysvälillä, vaikka näin ei ole.
Verrattaessa RUF:a muihin Kalmanin suodattimen epälineaarisiin laajennuksiin
tyydytään toteamaan vain, että on tapauksia, joissa vanhemmat algorimit antavat
paremman tuloksen, mutta yleisesti RUF:lla saadaan tarkempia tuloksia, pienemmällä laskennallisella kuormituksella. Johdannossa mainittu hajuton Kalmanin suodatin tekee päivityksen samaan tyyliin kuin EKF ja EKF2, ja eroaa näistä ainoastaan siinä, että derivaattaa ei lasketa. UKF -suodatin voi joissain tapauksissa kärsiä
samoista ongelmista, kuin EKF:kin. Esimerkiksi tapauksessa, jossa mittaukset ovat
tarkkoja, voi UKF antaa ratkaisun, joka on näitä epätarkempi.
RUF -algoritmissa on käyttäjän valittavissa, kuinka moneen palaan päivitysaskeleet jaetaan, eli monessako osassa kukin päivitysaskel lasketaan. Tämä on tapauskohtaista ja sopivan määrän etsiminen on tasapainottelua tarkkuuden ja laskennallisen
raskauden välillä. Yleisesti voidaan sanoa, että sopiva määrä paloja on pienin mahdollinen, jolla saavutetaan haluttu tarkkuus. Esimerkkitapauksessa jo kahdella väliaskeleella laskettaessa päästiin estimaatin virheen ja posteriokeskivirheen suhteessa
kokoluokkaa pienempään arvoon kuin käyttämällä laajennettua Kalman suodatinta. Tästä voidaan päätellä, että erittäin pienellä laskennan lisäämisellä saadaan huo-
3. Yhteenveto
14
mattavasti parempi tulos. Kymmentä väliaskelta käyttämällä estimaatin virhe olisi
vain puolet posteriorikeskivirheestä, joten RUF -algoritmillä pystytään estimaattia
tarkentamaan huomattavasti lisäämällä päivitysaskeleen palojen määrää. Näin ollen
voidaan todeta RUF -algoritmin olevan esimerkkitapauksessa huomattavasti EKF
-algoritmia suorituskykyisempi. Koska paloittain päivittävä Kalman suodatin käyttää derivaattaa, kuten EKF ja EKF2:kin, niin tilanteet, joissa derivaatat saavat
arvokseen nollan, saattavat aiheuttaa sillekin ongelmia, mutta vaikutukset eivät ole
yhtä dramaattisia, sillä päivitys lasketaan vaiheittain. Olemassaolevista algoritmeista RUF:a eniten muistuttava on iteroitu Kalmanin suodatin (Iterated Kalman filter,
IKF). [8]
15
LÄHTEET
[1] R.E. Kalman “A new approach to linear filtering and prediction problems,”
J. Basic Eng. Mar. 1960, https://www.cs.unc.edu/~welch/kalman/media/
pdf/Kalman1960.pdf
[2] B. D. Tapley, B. E. Schutz, and G. H. Born, Statistical Orbit Determination.
Waltham, MA: Elsevier Academic Press, 2004.
[3] Ali-Löytty S., Gaussian Mixture Filters in Hybrid Positioning. Väitöskirja.
Tampere 2009. Tampereen Teknillinen Yliopisto. http://dspace.cc.tut.fi/
dpub/bitstream/handle/123456789/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:
//www.gatsby.ucl.ac.uk/~byron/nlds/julier04.pdf
[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, http://www.cs.berkeley.edu/~pabbeel/cs287-fa13/optreadings/
JulierUhlmann-UKF.pdf
[6] A. Gelb, Ed., Applied Optimal Estimation. Cambridge, MA: MIT press, 1996,
pp. 182-189, and pp. 190-191. Waltham, MA: Elsevier Academic Press, 2004.
[7] Grewal, M. ja Andrews, A. 1993. Kalman Filtering : theory and practice, 1.
painos. New Jersey, Prentice-Hall. 381 s.
[8] Zanetti, R. 2012. Recursive Update Filterin for Nonlinear Estimation
http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=
6096372&url=http%3A%2F%2Fieeexplore.ieee.org%2Fxpls%2Fabs_all.
jsp%3Farnumber%3D6096372
[9] Ali-Löytty S., Collin J. ja Sirola N.: Paikannuksen matematiikka 2010: http://
math.tut.fi/courses/MAT-45800/paikannuksen_matematiikka_2010b.pdf
[10] R. G. Brown, P. Y. Hwang, Introduction to Random Signals and Applied Kalman Filtering, 3rd ed. New York: Wiley 1997, pp. 348-350
16
A.
LIITE: EKF-ESIMERKIN MATLAB-KOODI
x0
x_
P_
R
H
=
=
=
=
=
K_ekf
y1
=
=
x_plus =
P_plus =
sigma =
3.5;
2.5;
0.5^2;
0.1^2;
3*x_^2;
%tilamuuttujan todellinen arvo
%tilamuuttujan prioriestimaatin arvo
%tilamuuttujan prioriestimaatin kovarianssi
%mittausmallin virheen kovarianssi
%mittausmallin Jacobin matriisin (derivaatan)
%arvo prioriestimaatilla laskettuna
P_*H/(H*P_*H’+R);
%EKF:n Kalmanin vahvistus
x0^3;
%Päivitetty mittaus, joka
%oletetaan tässä virheettömäksi
x_+K_ekf*(y1-h(x_)); %x:n posterioriestimaatti
(1-K_ekf*H)*P_;
%Posterioriestimaatin kovarianssi
sqrt(P_plus);
%Posterioriestimaatin keskivirhe
17
B.
LIITE: RUF-ESIMERKIN MATLAB-KOODI
N=10;
%Yhden päivitysaskeleen palojen määrä
%(vapaasti valittava)
R = 0.1^2;
%mittausmallin häiriön varianssi
W=zeros(N,1);
%apumuuttuja Kalmanin vahvistuksen laskemiseen
x_tod = 3.5;
%x:n todellinen arvo
C=zeros(N+1,1); %tilan x_k ja mittauskohinan ristikovarianssi
x_=zeros(N+1,1); %apumuuttujavektori, johon estimaatin väliarvot
%tallennetaan
x_(1)=2.5;
%esimerkkitapauksen tilamuuttujan arvon alkuarvaus
H=zeros(N+1,1); %apumuuttujavektori, johon tallennetaan mittaus%funktion derivaatan väliarvot.
H(1)=3*x_(1)^2; %Mittausfunktion derivaatan arvo alkuarvaustilassa
h=zeros(N+1,1); %apumuuttujavektori, johon tallennetaan paloissa
%mittausfunktion arvot kunkin palan posteriori%estimaatin arvolla laskettuna
h(1)=x_(1)^3;
%Arvio mittauksen alkuarvosta alkuarvaustilassa.
K=zeros(N,1);
%apuvektori, johon tallennetaan Kalmanin
%vahvistuksen arvot päivitysaskeleen
%eri paloissa/vaiheissa.
P=zeros(N+1,1); %apuvektori, johon tallennetaan päivitysaskeleen eri
%vaiheiden estimaattien kovarianssit.
P(1)=0.25;
%Kyseisen vektorin ensimmäiseen alkioon tulee
%prioriestimaatin kovarianssi.
y=42.875;
%Mittausarvo, joka oletettiin esimerkissä
% virheettömäksi (y = x_tod^3).
for i = 1:N
gamma=1/(N+1-i);
%Tässä silmukassa lasketaan
%päivitysaskeleen palat.
%Kalmanin vahvistuksen painokerroin,
%jolla kokonaisvahvistus jaetaan tasan
%päivitysaskeleen eri
B. Liite: RUF-esimerkin MATLAB-koodi
18
%vaiheiden kesken.
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;
end
x_post = x_(N+1);
P_post = P(N+1);
sigma
= sqrt(P_post);
%Tilan posterioriestimaatti tallentuu
%silmukassa x_ -vektorin viimeiseen alkioon,
%eli se on päivitysaskeleen viimeisen
%vaiheen tulos.
%Posterioriestimaatin kovarianssi tallentuu
% vastaavasti P-vektorin viimeiseen alkioon.
%Posterioriestimaatin keskivirhe