Tuomo Mäki-Marttunen Stokastiset ja tavalliset differentiaaliyhtälöt inertiapaikannuksessa

Transkriptio

1 TAMPEREEN TEKNILLINEN YLIOPISTO Luonnontieteiden ja ympäristöteniian tiedeunta Tuomo Mäi-Marttunen Stoastiset ja tavalliset differentiaaliyhtälöt inertiapaiannusessa Diplomityö Aihe hyväsytty tiedeuntaneuvostossa Tarastaja: professori Robert Piché (TTY)

2 Alusanat Tämä työ on tehty Tampereen tenillisellä yliopistolla matematiian laitosen henilöohtaisen paiannusen algoritmien tutimusryhmässä vuoden 2008 aiana Noian rahoittamana. Tahdon iittää diplomityöni tarastajaa Robert Pichéä seä Niilo Sirolaa, joa myös osallistui työn taristuseen. Lisäsi tahdon iittää Simo Ali-Löyttyä suodatusta osevista hedelmällisistä esusteluista seä Ville Honavirtaa ja Tommi Perälää irjoitusasua osevista vineistä ja esusteluista. Kiitos myös muille ryhmämme jäsenille, ystävilleni ja erityisesti perheelleni aiesta tuestanne. Tampereella, 22. loauuta 2008 Tuomo Mäi-Marttunen ii

3 Tiivistelmä TAMPEREEN TEKNILLINEN YLIOPISTO Luonnontieteiden ja ympäristöteniian tiedeunta Matematiian laitos Mäi-Marttunen, Tuomo: Stoastiset ja tavalliset differentiaaliyhtälöt inertiapaiannusessa. Diplomityö, 52 sivua ja 11 liitesivua Tarastaja: professori Robert Piché Käsitellään tiedeuntaneuvostossa marrasuussa 2008 Inertiapaiannusongelmaa, jossa oletettuina mittausina on ulmanopeus- ja iihtyvyysmittausia, tutitaan olmesta eri lähtöohdasta. Ensimmäinen lähtöohta, jossa mittausvirheiden oletetaan puuttuvan oonaan, johtaa vetorisuunnistusongelmaan (dead reconing), jona rataisemisessa eseisessä osassa on asennon inemaattisen tilanyhtälön rataiseminen. Eri asennon esitystavoilla (rotaatiomatriisi, ysiövaterni ja Eulerin ulmat) on erilaiset inemaattiset tilanyhtälöt, ja eri Runge-Kutta-menetelmiä testataan simulaatiossa näihin tilanyhtälöihin. Todetaan Runge-Kutta-menetelmien suppeneminen testitapausessa aseloon pienentyessä ja todetaan Eulerin ulmia äyttävän rataisijan ärsivän tilanyhtälönsä singulaarisuudesta. Toinen lähtöohta johtaa myös vetorisuunnistusongelmaan, mutta tässä tarastellaan myös estimointivirhettä. Rataistavana on stoastinen differentiaaliyhtälö, jolle esitetään asi rataisualgoritmia: Euler-Maruyama- seä toisen asteen heio Taylor-menetelmä. Algoritmeja verrataan simulaatiossa, ja todetaan niiden suppeneminen testitapausessa aseloon pienentyessä. Kolmannessa lähtöohdassa inertiapaiannusta tarastellaan suodatusongelmana. Ongelmalle johdetaan mittausmalli seä vaihtoehtoisia stoastisen differentiaaliyhtälön rataisemiseen perustuvia approsimatiivisia tilamalleja. Esitetään asi suodatuseen sopivaa algoritmia: partielisuodatin (bootstrap filter) ja laajennettu Kalman-suodatin. Tiettyjen tilamallien todetaan täyttävän partielisuodattimen suppenemiseen vaadittavat ehdot. Esitettyjä suodattimia vertaillaan esenään simulaatiossa. iii

4 Abstract TAMPERE UNIVERSITY OF TECHNOLOGY Faculty of Science and Environmental Engineering Department of Mathematics Mäi-Marttunen, Tuomo: Stochastic and ordinary differential equations in inertial positioning. Master of Science Thesis, 52 pages and 11 pages appendices Examiner: professor Robert Piché Evaluated by the Faculty Council in November 2008 Inertial positioning problem with angular velocity and acceleration measurements as assumed measurements is considered from three different bases. The first basis in which measurement errors are presumed absent leads to a dead reconing problem, in which solving the inematic equation of attitude plays a major role. Different parametrizations of attitude (rotation matrix, unit quaternion and Euler angles) have different inematic equations, and different Runge-Kutta schemes are tested on these inematic equations. Runge-Kutta schemes are found to converge and the solvers using Euler angles parametrization are found to suffer from the singularity of the inematic equation. The second basis leads to a dead reconing problem too, but in this basis also the estimation error is examined. Stochastic differential equations are formed, and two numerical solving methods for them are presented: Euler-Maruyama scheme and second order wea Taylor scheme. The methods are compared in a simulation and the order of their rates of convergence is found to accord with the theory. In the third basis inertial positioning is considered as a filtering problem. A measurement model and alternative approximative state models based on solving the relevant stochastic differential equation are derived. Two algorithms for solving the filtering problem are presented: the bootstrap filter and the extended Kalman filter. Certain state models are found to satisfy the convergence conditions of the bootstrap filter. The presented filters are compared with each other in a simulation. iv

5 Sisältö Alusanat Tiivistelmä Abstract Lyhenteet Symbolit ii iii iv vii viii 1 Johdanto 1 2 Deterministinen inertiapaiannus Vetorit ja oordinaatistot Asennon esitystapoja Rotaatiomatriisi Kvaternit Eulerin ulmat Kinemaattisen tilanyhtälön rataiseminen Esplisiittiset Runge-Kutta-rataisumenetelmät Implisiittiset Runge-Kutta-menetelmät Simulaatio Paian rataiseminen Virheen etenemisyhtälöt ja niiden rataiseminen Virheen etenemisyhtälö asennon rataisemisessa Stoastisen differentiaaliyhtälön rataiseminen Euler-Maruyama-menetelmä Toisen asteen heio Taylor-menetelmä Simulaatio Virheen etenemisyhtälö paiarataisussa v

6 ABSTRACT vi 4 Bayesiläinen suodatus inertiapaiannusessa Bayesiläisen suodatusen taustaa Inertiapaiannusen tila- ja mittausmalli Tila- ja mittausmalli RotM-asennon sisältävälle tilamuuttujalle Tila- ja mittausmalli SymE-asennon sisältävälle tilamuuttujalle Partielisuodatin Laajennettu Kalman-suodatin Simulaatio Yhteenveto 50 6 Liitteet Koordinaatistonvaihtomatriisin ominaisuudet Kulmanopeuden johto simulaatiossa Toisen asteen heio Taylor-menetelmä Toisen asteen heio Taylor-menetelmä x:n suhteen lineaarisille yhtälöille Toisen asteen heio Taylor-menetelmä RotM-tilamallissa Toisen asteen heio Taylor-menetelmä SymE-tilamallissa

7 Lyhenteet EA Eulerin ulmat, Eulerin ulmien esitystapa (Euler Angles) EKF laajennettu Kalman-suodatin (Extended Kalman Filter) EM Euler-Maruyama GPS maailmanlaajuinen paiannusjärjestelmä (Global Positioning System) IMU inertiamittausysiö (Inertial Measurement Unit) ODE tavallinen differentiaaliyhtälö (Ordinary Differential Equation) PF partielisuodatin (Particle Filter) RK1, RK2, RK4 Ensimmäisen, toisen ja neljännen asteen Runge-Kutta-menetelmä RK2 IMPL implisiittinen RK2-menetelmä RK2 PROJ projisoiva RK2-menetelmä (s. sivu 14) RotM rotaatiomatriisiesitystapa (Rotation Matrix representation) SDE stoastinen differentiaaliyhtälö (Stochastic Differential Equation) SymE symmetristen Eulerin parametrien esitystapa (Symmetric Euler parameters) T2 toisen asteen heio Taylor-menetelmä (second order wea Taylor scheme) vii

8 Symbolit a, b, c, x, y, z salaari- tai pystyvetorimuuttujia A, B, C, X, Y, Z matriiseja a, b, c, x, y, z salaari- tai pystyvetoriarvoisia satunnaismuuttujia f, g, h salaari- tai pystyvetoriarvoisia funtioita f, g, h salaari- tai pystyvetoriarvoisia satunnaismuuttujan funtioita u, v, w vetoreita p, q vaterneja a t, b t, c t, x t, y t, z t a t,b t,c t,x t,y t,z t ȧ t, ḃt, ċ t a (i), b (i), c (i) a (i 1:i 2 ) A ij, B ij, C ij ajan suhteen muuttuvia salaari- tai pystyvetorimuuttujia stoastisia prosesseja muuttujan a t, b t tai c t aiaderivaatta pystyvetorin a, b tai c i:s alio pystyvetorin a alioiden i 1, i 1 + 1,..., i 2 muodostama pystyvetori matriisin A, B tai C i:nnen rivin j:s alio A i1 :i 2,j 1 :j 2 matriisin A rivien i 1, i 1 + 1,...,i 2 saraeiden j 1, j 1 + 1,...,j 2 muodostama osamatriisi A T matriisin A transpoosi Määr. i =, Lause j =, () = on määritelmän i, lauseen j tai yhtälön nojalla yhtä suuri uin ( u 1, u 2, u 3 ) vetoreiden u 1, u 2 ja u 3 määrittämä oordinaatisto viii

9 SYMBOLIT ix a b vetoreiden a ja b välinen pistetulo (2-)normi ( a, b) vetoreiden a ja b välinen pienin epänegatiivinen ulma a b vetoreiden a ja b välinen ristitulo C U W rotaatiomatriisi oordinaatiostosta W oordinaatistoon U (a ) luuolmion a muodostama vinosymmetrinen 3 3-matriisi, s. sivu 5 R reaaliluujen jouo R d d-ulotteisten reaaliluupystyvetoreiden jouo R m n m riviä oreiden ja n saraetta leveiden reaaliluumatriisien jouo H vaternien jouo onjugaattioperaattori R u,α iertofuntio, joa iertää annettua vetoria vetorin u ympäri ulman α verran L v iertofuntio, joa iertää annettua puhdasta vaternia vaternin v imaginääriosan ympäri v:n reaaliosan verran Ω luuolmiosta ω muodostettu vinosymmetrinen 4 4-matriisi, s. sivu 9 Fr Frobenius-normi 0 m n m n-nollamatriisi I, I n identiteettimatriisi, n n-identiteettimatriisi F x, F y, F z satunnaismuuttujan x, y tai z ertymäfuntio f x, f y, f z satunnaismuuttujan x, y tai z tiheysfuntio E(x), Var(x) satunnaismuuttujan x odotusarvo ja varianssi N(µ, Σ) normaalijaauma, jona odotusarvo on µ ja varianssi Σ W t vect f x y x [i], w [i] δ x valoisen ohinan prosessi Kronecerin tulo vetorointioperaatio satunnaismuuttujan x ehdollinen tiheysfuntio satunnaismuuttujalla y ehdollistettuna partielipilven i:s alio ja sen paino Diracin delta-funtio pisteessä x

10 Luu 1 Johdanto Paiannusella taroitetaan sellaisen tilan estimointia, joa sisältää tiedon paiannettavan ohteen (esineen tai olennon) paiasta jossain muodossa. Inertiapaiannusella taroitetaan paiannusta, jossa äytetään hyväsi appaleen inertiaominaisuutta eli sitä, että appale säilyttää nopeutensa ja ulmanopeutensa ellei miään uloinen voima tai momentti vaiuta siihen [20, s. 10]. Inertiapaiannusen mahdollistavat liieen tunnistavat laitteet, inertiasensorit, joista täreimmät ovat iihtyvyys- ja ulmanopeusanturit. Tässä työssä tarastellaan inertiapaiannusen matemaattista taustaa ja menetelmiä. Oletusena on, että paiannettavan ohteen alutila (asento, nopeus, paia) on tiedossa, ja että ohteessa on iinteästi asennettuna olme iihtyvyysanturia seä olme ulmanopeusanturia. Kiihtyvyysanturit ovat toisiaan vastaan ohtisuorassa siten, että ysi mittaa iihtyvyyttä laitteen x-suunnassa (a (1) ), toinen y-suunnassa (a (2) ) ja olmas z-suunnassa (a (3) ). Vastaavasti ulmanopeusanturit on asennettu siten, että ysi mittaa laitteen ulmanopeutta oman x-aselinsa ympäri (ω (1) ), toinen y-aselin ympäri (ω (2) ) ja olmas z- aselin ympäri (ω (3) ). Tällaista antureiden muodostamaa oonaisuutta utsutaan inertiamittausysiösi (IMU) [20, s. 11]. Näiden antureiden antaman mittausdatan pohjalta tehtävää paiannusta tarastellaan olmesta eri lähtöohdasta, ja nämä lähtöohdat tu- 1

11 LUKU 1. JOHDANTO 2 losineen muodostavat tämän työn olme seuraavaa luua. Ensimmäisessä lähtöohdassa oletetaan antureiden mittauset virheettömisi, jolloin oo ongelma on deterministinen. Tällä taroitetaan tässä sitä, että muana ei ole satunnaisuutta aiheuttavia termejä, vaan mitatut arvot ovat absoluuttisen taroja. Toisessa lähtöohdassa oletetaan, että mitattavan suureen (iihtyvyys, ulmanopeus) oiea arvo on satunnaismuuttuja, joa on tietyllä tavalla jaautunut mitatun arvon ympärille. Kolmannessa lähtöohdassa laitteen ulmanopeus ja iihtyvyys ovat osana appaleen tilaa uvaavaa tilamuuttujaa, joa on satunnaismuuttuja, ja jona epävarmuus mittausten välisenä aiana asvaa. Tässä lähtöohdassa mitatun suureen atsotaan olevan tietyllä tavalla jaautunut vastaavan oiean suureen ympärille, ja tilan jaaumaa saatujen mittausten ehdolla pyritään estimoimaan. Lisäsi oletetaan, että tilalla on join tiedossa oleva alujaauma. Lähtöohdista ahta ensimmäistä voidaan utsua nimellä vetorisuunnistus (engl. dead reconing), ja niistä jälimmäisessä esitytään virheen etenemiseen. Kolmatta lähtöohtaa taas voidaan utsua Bayesiläisesi lähtöohdasi, sillä se pitää sisällään Bayesiläisen suodatusen ongelmanasettelun. Ensimmäisen lähtöohdan ongelmanasettelu johtaa differentiaaliyhtälöihin ja toisen ja olmannen lähtöohdan ongelmanasettelu stoastisiin differentiaaliyhtälöihin, mistä johtuu työn nimi. Työn tavoitteena on uvata näiden olmen eri lähtöohdan muodostamia inertiapaiannusongelmia matemaattisesti yhtenäisellä merintätavalla seä luoda perusta inertiapaiannusjärjestelmän liittämisesi muun tyyppisiä mittausia äyttäviin järjestelmiin (esim. GPS:ään). Toisena tavoitteena on verrata yleisesti äytettyjä differentiaaliyhtälön rataisumenetelmiä eräisiin sofistioituneempiin menetelmiin, joita ei inertiapaiannussovellusissa tavallisesti äytetä, seä arvioida, olisivato viimesimainitut äyttöelpoisempia uin edelliset. Kolmantena tavoitteena mainittaoon pyrimys päästä inertiapaiannusongelman ytimeen ja tunnistaa inertiapaiannusen eseisimpien ongelmien aiheuttajat. Poieten valtaosasta inertiapaiannusta äsittelevää tutimusta ja irjallisuutta (esim. [8], [22], [20]), tässä työssä ei tarastella eriseen tilamuuttujien virheitä, vaan virhe sisältyy tilamuuttujaan tämän ollessa satunnaismuuttuja. Lisäsi paiannusongelmaa pelistetään jättämällä painovoima ja maapallon pyöriminen tarastelun ulopuolelle, samaten haastava alibrointiongelma [4, s. 44] sivuutetaan olettamalla mahdolliset mittausvirheet nollaesisisi. Tässä työssä tarastellaan pelän inertiamittausysiön avulla tehtävää paiantamista, mutta tavallisesti sovellusissa inertiamittausysiöä äytetään esimerisi satelliittipaiannusjärjestelmän (GPS) [20] rinnalla tai muuta saatavilla olevaa informaatiota, esimerisi artta- tai seinätietoa [25], apuna äyttäen.

12 Luu 2 Deterministinen inertiapaiannus Tässä appaleessa äsitellään inertiapaiannusta, jossa aii saatavat mittauset - ulmanopeusmittauset ja iihtyvyysmittauset - oletetaan virheettömisi. Tehtävänämme on määrittää paiannettavan ohteen paia näitä saatuja mittausia seä tiettyä alutilaa hyväsi äyttäen. Paia on vetorisuure, samoin uin nopeus ja iihtyvyys. Tarastellaan seuraavasi taremmin, mitä tämä taroittaa. 2.1 Vetorit ja oordinaatistot Määritellään vetori v objetina, jolla on suunta ja pituus. Vetorin esittämisesi numeerisesti tarvitaan oordinaatisto. Koordinaatisto määritellään olmen toisiaan vastaan ohtisuorassa olevan ysiövetorin olmiona U = ( u 1, u 2, u 3 ), ja vetorin v esitys 3

13 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 4 tässä oordinaatistossa on luuolmio coord U ( v) = v u 1 v u 2 v u 3, (2.1) missä ahden vetorin välinen pistetulo on määritelty niiden välisen ulman ja pituusien avulla seuraavasti: a b = a b cos ( a, b). Se, että luuolmio (2.1) on vetorin v esitys, taroittaa, että v = ( v u 1 ) u 1 + ( v u 2 ) u 2 + ( v u 3 ) u 3. (2.2) Tässä työssä äsitellään vain olmiulotteisia oieaätisesti ortogonaalisia oordinaatistoja, eli aiille oordinaatistoille U = ( u 1, u 2, u 3 ) pätee: u 1 u 2 = u 3, u 2 u 3 = u 1 ja u 3 u 1 = u 2, missä ahden vetorin a ja b välinen ristitulo a b on määritelty vetorina, joa on ohtisuorassa vetoreita a ja b vastaan oiean äden säännön suuntaan ja jona pituus on a b sin ( a, b). Tarastellaan mielivaltaisen vetorin v esitysten suhdetta eri oordinaatistoissa U = ( u 1, u 2, u 3 ) ja W = ( w 1, w 2, w 3 ): coord U ( v) (2.2) ( ) = coord U ( v w1 ) w 1 + ( v w 2 ) w 2 + ( v w 3 ) w 3 ( ) ( v w1 ) w 1 + ( v w 2 ) w 2 + ( v w 3 ) w 3 u1 ( ) ( v w1 ) w 1 + ( v w 2 ) w 2 + ( v w 3 ) w 3 u2 ( ) ( v w1 ) w 1 + ( v w 2 ) w 2 + ( v w 3 ) w 3 u3 u 1 w 1 u 1 w 2 u 1 w 3 v w 1 = u 2 w 1 u 2 w 2 u 2 w 3 v w 2 (2.1) = u 3 w 1 u 3 w 2 u 3 w 3 = C U Wcoord W ( v), v w 3 missä C U W = u 1 w 1 u 1 w 2 u 1 w 3 u 2 w 1 u 2 w 2 u 2 w 3. u 3 w 1 u 3 w 2 u 3 w 3 Kosa oordinaatistot U ja W ovat oieaätisiä ja osa vetorit u i ja w i ysiövetoreita, niin CW U on ortogonaalinen ja det CU W = 1(s. liite 6.1). Tällaista matriisia utsutaan rotaatiomatriisisi.

14 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 5 Tässä työssä äytämme ahta oordinaatistoa: Inertiaoordinaatistoa I, jossa Newtonin lait ovat voimassa, seä paiannettavaan laitteeseen sidottua oordinaatistoa B, jossa iihtyvyys- ja ulmanopeusmittauset tehdään. Rotaatiomatriisi CB I ertoo, miten laitteeseen sidottu oordinaatisto on suuntautunut inertiaoordinaatistoon nähden, toisin sanoen, missä asennossa B on I:hin nähden. Määrittelemmein laitteen asennon parametrina tai parametrijouona, joa määrittää rotaatiomatriisin CB I. Halutessamme rataista paian joudumme oo ajan olemaan selvillä siitä, missä asennossa laite on, jotta tiedämme minä suuntainen iihtyvyys on inertiaoordinaatistossa. Tarastellaan seuraavasi, millaisia eri parametrisointeja eli esitystapoja asennolle on ja millaisia ovat niiden tilanyhtälöt, eli yhteydet laitteessa mitattuun ulmanopeuteen. 2.2 Asennon esitystapoja Rotaatiomatriisi Ysinertaisin parametrisointi rotaatiomatriisille on rotaatiomatriisi itse. Kutsumme esitystapaa, jossa asento esitetään rotaatiomatriisina rotaatiomatriisiesitystavasi (RotM) [17, s. 85]. Rotaatiomatriisiesitystavassa asento on 9-alioinen matriisi (R R 3 3 ), mutta ortogonaalisuusehto R T R = RR T = I 3 pienentää vapausasteiden määrän olmeen [17, s. 86]. Kinemaattinen tilanyhtälö rotaatiomatriisille on: missä ω = ω 1 ω 2 ω 3 Ṙ = R(ω ) (2.3) on appaleeseen sidotun oordinaatiston ulmanopeus inertiaoordinaatiston suhteen, ja (ω ) on siten määritelty matriisi, että matriisilla ertominen vastaa ristituloa, ts. a R 3 : ω a = (ω )a [21, s.21]. Pienellä sievennysellä matriisille (ω ) saadaan ysiäsitteinen muoto (ω ) = 0 ω 3 ω 2 ω 3 0 ω 1 ω 2 ω 1 0. (2.4)

15 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS Kvaternit Toinen yleisesti äytetty esitystapa rotaatiolle on ysiövaterni. Määritellään ensisi vaternien algebra. Määritelmä 1 Kvaternien algebra oostuu vaternien jouosta H = {q 0 + iq 1 + jq 2 + q 3 q i R, i = 0,..., 3} ja siinä on määritelty summa ja ertolasu seuraavasti: Jos p = p 0 + ip 1 + jp 2 + p 3 ja q = q 0 + iq 1 + jq 2 + q 3, niin p + q = p 0 + q 0 + i(p 1 + q 1 ) + j(p 2 + q 2 ) + (p 3 + q 3 ) pq = p 0 q 0 p 1 q 1 p 2 q 2 p 3 q 3 + i(p 0 q 1 + p 1 q 0 + p 2 q 3 p 3 q 2 ) + j(p 0 q 2 p 1 q 3 + p 2 q 0 + p 3 q 1 ) + (p 0 q 3 + p 1 q 2 p 2 q 1 + p 3 q 0 ) [15, s ] Määritelmästä seuraa, että jouo {1, i, j, } on vaternien jouon anta, ja että i 2 = j 2 = 2 = 1 ja ij =, j = i, i = j. On huomattava, että vaterniertolasu on assosiatiivinen, mutta ei ommutatiivinen. Kvaternia q 0 + iq 1 + jq 2 + q 3 sanotaan puhtaasi, jos q 0 = 0. Kvaterneille on määritelty onjugaattioperaatio seuraavasti: q 0 + iq 1 + jq 2 + q 3 = q 0 iq 1 jq 2 q 3. Kvaterninormi on määritelty seuraavasti: q 0 + iq 1 + jq 2 + q 3 = q q2 1 + q2 2 + q2 3 Kvaterni q on ysiövaterni jos ja vain jos q = 1. Joaiselle ysiövaternille on olemassa α R ja puhdas ysiövaterni u s.e. [15, s. 118] q = cosα + u sin α. Reaaliluu α ja puhdas ysiövaterni u ovat ysiäsitteiset, jos vaadimme, että α [ π, π[. Määritellään seuraavasi ierto olmiulotteisessa vetoriavaruudessa.

16 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 7 u A R u,α ( A) Kuva 2.1: Vetoriavaruuden appale A V ja sen uva R u,α ( A) Määritelmä 2 Oloon V olmiulotteinen vetoriavaruus, ja u V ja α R. Kiertofuntio R u,α : V V on siten määritelty funtio, että vetori R u,α ( v) saadaan iertämällä vetoria v vetorin u suuntaisen aselin ympäri ulman α verran vastapäivään aselivetorin päästä atsottuna. Tällaisen ierron voidaan päätellä olevan lineaarinen operaatio: on selvää, että vetorin reaaliluvulla ertominen voidaan tehdä joo iertämisen jäleen tai sitä ennen, saaden tulosesi ummastain operaatiosta saman vetorin. Samaten asi vetoria voidaan summata yhtä hyvin ennen uin jäleen ierto-operaation. Lisäsi ierto-operaatio säilyttää oieaätisyyden tämän luija voi päätellä pyörittelemällä oieaa ättään: asettaessa olme oiean äden sormea toisiaan ohtisuoraan niiden esinäinen asema toisiinsa nähden eli oieaätisyys pysyy muuttumattomana, vaia itse ättä pyöriteltäisiin ympäriinsä. Ysiövaternien avulla voidaan luoda määritelmän 2 muainen ierto-operaatioiden jouo puhtaiden vaternien jouossa Im H = {iq 1 + jq 2 + q 3 q i R, i = 1,..., 3}. Jos nimittäin q = cosα+u sin α, missä u on puhdas ysiövaterni, niin funtio L q : Im H Im H, missä L q (v) = qv q, (2.5) iertää puhdasta vaternia v ulman 2α verran puhtaan vaternin u ympäri [15, s. 130]. Jos siis samaistamme puhtaiden vaternien annan {i, j, } reaalivetoriavaruuden luonnollisen annan { e 1, e 2, e 3 } anssa, niin puhtaalle vaternille v = iv 1 + jv 2 + v 3 suoritettava operaatio L cos α+u sin α vastaa reaalivetoriavaruuden vetorin v = v 1 e 1 + v 2 e 2 + v 3 e 3 iertämistä vetorin u = u 1 e 1 + u 2 e 2 + u 3 e 3 suuntaisen aselin ympäri ulman 2α verran.

17 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 8 Seuraava lause ertoo, että luonnollisantaisen vetorin ierto jonin iinteän aselin ympäri tietyn ulman verran vastaa vetorin oordinaattiesitysen ertomista ortogonaalisella matriisilla, jona determinantti on 1. Lause 1 Oloon R u,α määritelmän 2 muainen iertofuntio. Tällöin on olemassa ortogonaalinen matriisi R u,α s.e. det R u,α = 1 ja joaiselle v V : Kyseinen matriisi on coord ( e1, e 2, e 3 ) (R u,α ( v)) = R u,α coord ( e1, e 2, e 3 )( v). R u,α = I 3 + sin α(u ) + (1 cosα)(u ) 2, (2.6) 0 u 3 u 2 missä u = coord ( e1, e 2, e 3 )( u) ja (u ) = u 3 0 u 1 u 2 u 1 0 Vastaavasti, jos R on ortogonaalinen matriisi s.e. det R = 1, niin on olemassa u V ja α R s.e. v 1, v 2, v 3 R : R v 1 v 2 v 3 = coord ( e1, e 2, e 3 ) (R u,α (v 1 e 1 + v 2 e 2 + v 3 e 3 )). Todistus: Aluosa: Oloon u V, α R ja oloon A = ( a 1, a 2, a 3 ) s. e. a i = R u,α ( e i ), i = 1,...,3, missä E = ( e 1, e 2, e 3 ) on luonnollisen annan määräämä oordinaatisto. Tällöin myös A on ortogonaalinen oieaätinen oordinaatisto. Oloon v V ja oloon v 1 v 2 = coord E ( v). Kosa iertofuntio on lineaarinen, niin v 3 R u,α ( v) = R u,α (v 1 e 1 + v 2 e 2 + v 3 e 3 ) = v 1 R u,α ( e 1 ) + v 2 R u,α ( e 2 ) + v 3 R u,α ( e 3 ) = v 1 a 1 + v 2 a 2 + v 3 a 3. Siispä coord E R u,α ( v) = CA E coord A(R u,α ( v)) = C E A v 1 v 2 v 3 = C E A coord E( v) (2.7)

18 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 9 Bortz [3, s. 62] osoittaa lasuaavan C E A = I 3+sin α(u )+(1 cosα)(u ) 2, jolloin aluosa on todistettu. Kuipers [15, s. 52] todistaa lauseen loppuosan äyttäen notaatiota, jossa samaistetaan vetori ja sen esitys luonnollisessa annassa. Lauseen nojalla siis iertoaselivetorin ja iertoulman yhdelmä määrittää rotaatiomatriisin ja päinvastoin. Kosa taas ysiövaterni voidaan samaistaa iertoaselivetorin ja -iertoulman yhdelmän anssa, niin ysiövaterni on eräs parametrisointi rotaatiomatriisille. Kutsumme tällaista esitystapaa Eulerin symmetrisisi parametreisi (SymE). Tämä asennon esitystapa on neliulotteinen, mutta ysiövaterniehdon q = 1 johdosta vapausasteita tässäin esitystavassa on vain olme. Esitystapa ei ole uitenaan ysiäsitteinen, sillä joaiselle ysiövaternille q funtiot L q ja L q ovat yhtälön (2.5) nojalla identtiset, ja näin ollen yhtä rotaatiomatriisia vastaa asi ysiövaternia. Kinemaattinen tilanyhtälö tälle asentovaternille on [21, s. 21] q = 1 qw, (2.8) 2 missä w on ulmanopeusluuolmioa vastaava puhdas vaterni w = iω 1 + jω 2 + ω 3 Kun esitämme asentovaternia q = q 0 +iq 1 +jq 2 +q 3 luuneliönä q = q 1 q 2 q 3, saamme tilanyhtälön muotoon q 1 q 2 q 3 q 0 Määr. 1 = 1 2 ω 1 q 0 + ω 3 q 2 ω 2 q 3 ω 2 q 0 ω 3 q 1 + ω 1 q 3 ω 3 q 0 + ω 2 q 1 ω 1 q 2 ω 1 q 1 ω 2 q 2 ω 3 q 3 0 ω 3 ω 2 ω 1 = 1 ω 3 0 ω 1 ω 2 2 ω 2 ω 1 0 ω 3 ω 1 ω 2 ω 3 0 q 0 q 1 q 2 q 3 q 0. Eli lyhyesti: missä q = 1 Ωq, (2.9) 2 0 ω 3 ω 2 ω 1 ω 3 0 ω 1 ω 2 Ω = ω 2 ω 1 0 ω 3. (2.10) ω 1 ω 2 ω 3 0

19 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS Eulerin ulmat Eulerin ulmat ovat sellaista iertosarjaa vastaavat ulmat, jossa suoritetaan olme perääistä iertoa oordinaattiaselien ympäri, uin ierto tietyn ulman verran. Koordinaattiaselit voidaan valita vapaasti uitenin siten, että perääiset iertoaselit eivät ole osaan sama oordinaattiaseli. Näin ollen eri iertojärjestysiä on 12 appaletta. Tarastellaan iertosarjaa, jossa suoritetaan olme iertoa oordinaattiaselien ympäri järjestysessä x y z: ensisi ierretään x-aselin ympäri ulman ϕ verran, sitten y- aselin ympäri ulman θ verran ja lopusi z-aselin ympäri ulman ψ verran. Tällainen iertosarja vastaa funtiota R ϕ,θ,ψ : V V : Siispä v V : missä R ϕ,θ,ψ = R e3, ψ R e2,θ R e1, ϕ. coord ( e1, e 2, e 3 ) (R ϕ,θ,ψ ( v)) = coord ( e1, e 2, e 3 ) (R e3, ψ(r e2,θ(r e1, ϕ( v)))) Lause 1 = R e3, ψr e2,θr e1, ϕcoord ( e1, e 2, e 3 )( v) = Q ϕ,θ,ψ coord ( e1, e 2, e 3 )( v), (2.11) Q ϕ,θ,ψ = R e3, ψr e2,θr e1, ϕ cosϕ sin ϕ 0 cosθ 0 sin θ = sin ϕ cosϕ cos ψ sin ψ. (2.12) sin θ 0 cosθ 0 sin ψ cosψ Matriisi Q ϕ,θ,ψ on ortogonaalisten matriisien tulona ortogonaalinen, ja det Q ϕ,θ,ψ = det R e3, ψ det R e2,θ det R e1, ϕ = 1 3 = 1. Siis aiille ϕ, θ, ψ R matriisi Q ϕ,θ,ψ on rotaatiomatriisi. Vastaavasti joaiselle rotaatiomatriisille R on olemassa ulmat ϕ, θ, ψ R s.e. tämä rotaatiomatriisi voidaan esittää yllä uvattuna rotaatiomatriisitulona [17, s. 90]: R = R e3, ψr e2,θr e1, ϕ. Näin ollen Eulerin ulmat ϕ, θ ja ψ ovat eräs asennon esitystapa. Näitä parametreja utsutaan josus myös Cardanin ulmisi, mutta tässä työssä äytämme edellistä nimitystä.

20 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 11 Eulerin ulmien (EA) inemaattinen tilanyhtälö on seuraava trigonometrinen differentiaaliyhtälö [17, s. 97]: ϕ θ ψ = (ω 2 sin ψ ω 1 cos ψ)/ cosθ (ω 2 cos ψ + ω 1 sin ψ) ω 3 + (ω 1 cosψ ω 2 sin ψ) tanθ. (2.13) 2.3 Kinemaattisen tilanyhtälön rataiseminen Kullain asennon esitystavalla on siis inemaattinen tilanyhtälö, joa on muotoa: ẋ = ˆf(ω, x), missä x on rataistava asento ja ω on appaleeseen sidotun oordinaatiston ulmanopeusmittaus, joa oletetaan tässä virheettömäsi. Tähän asti olemme jättäneet ajan huomioimatta tilanyhtälöissä, mutta otetaan se nyt muaan tarasteluun. Oletusena on, että voimme tehdä ulmanopeusmittausen millä ajanhetellä tahansa. Tilanyhtälö on muotoa: ẋ t = ˆf(ω(t), x t ) (2.14) tai lyhyesti ẋ t = f(t, x t ). (2.15) Aluasento x 0 oletetaan tunnetusi. Kyseessä on siis tavallinen ensimmäisen ertaluoan differentiaaliyhtälö. On selvää, että paiannusen altaisissa ongelmissa, missä yhtälön (2.15) funtio f pitää sisällään mittausdataa (tässä tapausessa ulmanopeusmittausia), ei mitään analyyttista rataisumenetelmää voida äyttää. Yleisin tapa tällaisen differentiaaliyhtälön numeeriseen rataisemiseen on jonin Runge-Kutta-menetelmän äyttö. Runge-Kutta-menetelmät voidaan yleistää attamaan seä implisiittiset että esplisiittiset Runge-Kutta-menetelmät. Seuraavassa määrittelemme Haireria ja muita [10, s. 205] muaillen yleisen, esplisiittisen ja implisiittisen Runge-Kutta-menetelmän. i 1 Määritelmä 3 Oloon a ij, b i R, un i, j = 1,..., s ja oloon c i = a ij. Rataisume- j=1

21 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 12 netelmää i x n+1 s = f(t n + c i h, x n + h a ij j ), i = 1,..., s s = x n + h b i i j=1 utsutaan s:n vaiheen Runge-Kutta-menetelmäsi aluehto-ongelmalle (2.15). Kun i j : a ij = 0, on yseessä esplisiittinen Runge-Kutta-menetelmä. Muussa tapausessa puhumme implisiittisestä Runge-Kutta-menetelmästä. Rataisumenetelmää sanotaan p-asteisesi, jos tarpeesi sileille 1 funtioille f pätee: x(t 0 + h) x 1 Kh p+1, (2.16) eli toisin sanoen jos taran rataisun x(t 0 + h):n ja x 1 :n Taylorin sarjaehitelmien p:n ensimmäisen termin ertoimet ovat samat [10, s. 134]. Runge-Kutta-menetelmiä esitetään usein seuraavanlaisen tauluon avulla: c 1 a 11 a 12 a a 1s c 2 a 21 a 22 a a 2s c 3 a 31 a 32 a a 3s..... c s a s1 a s2 a s3... a ss b 1 b 2 b 3... b s Seuraavassa osiossa tutitaan muutamia tapauseen sopivia esplisiittisiä Runge-Kuttamenetelmiä Esplisiittiset Runge-Kutta-rataisumenetelmät Määritelmän 3 nojalla esplisiittisten RK-menetelmien erroinmatriisin a a 1s a s1... a ss diagonaali ja sen yläpuoliset aliot ovat nollia. Tavan muaan emme meritse näitä nollia tauluoon vaan jätämme ne tyhjisi. Nämä nollat aiheuttavat sen, että aselpäivitysessä 1 Tarpeesi sileillä funtioilla taroitetaan tässä sellaisia funtioita f, joiden aii p:nnen asteen derivaatat ovat olemassa ja jatuvia (s [10, s. 157]).

22 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 13 ei tarvitse rataista mitään yhtälöä (toisin uin implisiittisten Runge-Kutta-menetelmien aselpäivitysessä), vaan uin luu i, i = 1,...,s määräytyy suoralla lasulla aiemmista ertoimista j, j = 1,..., i 1. Simulaatioissa äytetään yhden, ahden ja neljän vaiheen Runge-Kutta-menetelmiä, jota ovat samalla ensimmäisen, toisen ja neljännen asteen rataisumenetelmiä. Kutsumme näitä menetelmiä RK1-, RK2- ja RK4-menetelmäsi. Alla näyvät menetelmien errointauluot: RK1 RK2 RK (2.17) RK1 on siis tunnettu Eulerin menetelmä. RK2 puolestaan tunnetaan nimellä esplisiittinen välipistemenetelmä, ja RK4-menetelmää utsutaan josus yleisyytensä vuosi neljännen asteen Runge-Kutta-menetelmäsi tai ysinertaisesti Runge-Kuttamenetelmäsi. Määrittelemme avaruuden R m n moniston tässä työssä urana, eli avaruuden R m n jouona, joa voidaan ilmaista jonin funtion f : R m n R avulla muodossa {x R m n f(x) = 0 } [19, s. 14]. Erityisesti tarastelemme monistoja M RotM = { R R 3 3 R T R I 3 = 0, det R 1 = 0 } ja M SymE = { q R 4 q 1 = 0 }. Näistä ensimmäinen on siis rotaatiomatriisien monisto ja toinen ysiövaterneja vastaavien luuneliöjen monisto. Rataistessa RotM- tai SymE-tyyppisen asennon inemaattista tilanyhtälöä ((2.3) tai (2.9)) tulee ottaa huomioon, että oiea rataisu on jatuvasti monistolla M i (i = RotM tai SymE). Esplisiittiset Runge-Kutta-menetelmät eivät säilytä ortogonaalisuutta eivätä normia; ts. vaia asentomuuttuja hetellä t 0 uuluisi monistoon M i, niin rataistu asentomuuttuja seuraavalla ajanhetellä t 1 ei välttämättä uitenaan uuluu monistoon M i. Tämän orjaamisesi voidaan ullain ajanhetellä projisoida rataistu asento halutulle monistolle M i. SymE:n tapausessa tämä tehdään ysinertaisesti jaamalla va-

23 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 14 terni omalla normillaan, jolloin tulosena on ysiövaterni. RotM:n tapausessa taas äytetään hyväsi QR-hajotelmaa seuraavasti. Mielivaltaisen neliömatriisin A projetio ortogonaalisten matriisien jouoon saadaan sen singulaariarvohajotelmasta: jos A = USV T on matriisin A singulaariarvohajotelma, niin UV T = argmin A Q Fr, Q Q missä Q on ortogonaalisten matriisien jouo ja Fr on Frobenius-normi [7, s. 442]. Singulaariarvohajotelman laseminen on uitenin melo rasasta, minä vuosi toivomme saavamme approsimaation ortogonaalimatriisiprojetiosta muulla tavoin. Käyin ilmi, että QR-hajotelman ortogonaalinen matriisi Q on tietyissä tapausissa melo lähellä projetiota UV T. Sun [24, s. 388] osoittaa, että un A on tarpeesi lähellä ortogonaalista matriisia, niin A Q Fr < 5 A UV T Fr, missä A = QR on sellainen matriisin A QRhajotelma, jossa yläolmiomatsiisin R diagonaalialiot ovat positiivisia. Ehto tarpeesi lähellä on, että A T A I , (2.18) missä 2 on matriisin 2-normi eli spetraalinormi [24, s. 388]. Oletamme tässä työssä ulmanopeusmittausia tehtävän niin pienin aiavälein, että ehto (2.18) pysyy voimassa, unhan asento projisoidaan joa ajanhetellä. Tällöin siis myös normi A UV T Fr on pieni, mistä seuraa, että myös normi A Q Fr on pieni. Käytämme siis ortogonaalisesi projisoidun asentomatriisin approsimaationa QRhajotelmasta saatavaa ortogonaalista matriisia. Kyseinen hajotelma voidaan lasea Gram- Schmidt-algoritmilla. Meritsemme näitä menetelmiä, missä ensin rataistaan tulevan ajanheten asento esplisiittisellä Runge-Kutta-menetelmällä ja sen jäleen projisoidaan toivotulle monistolle, lyhenteillä RK1 PROJ, RK2 PROJ ja RK4 PROJ Implisiittiset Runge-Kutta-menetelmät Tässä työssä esitymme vain yhteen implisiittiseen Runge-Kutta-menetelmään: implisiittiseen välipistemenetelmään. Tämän errointauluo on seuraava: RK2 IMPL (2.19) 1

24 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 15 Eli aselpäivitys on: missä x 1 = x 0 + h, = f(t 0 + h 2, x 0 + h 2 ). Menetelmän äyttämisen edellytysenä on, että muuttuja voidaan rataista yo. yhtälöstä. Vaia menetelmä on vain yhden vaiheen RK-menetelmä, se on uitenin toista astetta [11, s. 142]. Tästä syystä meritsemme menetelmää lyhenteellä RK2 IMPL. Käytämme implisiittistä välipistemenetelmää vain rotaatiomatriisiesitystavan RotM ja vaterniesitystavan SymE inemaattisten tilanyhtälöiden rataisemiseen. Jos tarastellaan RotM-esitystavan asentomatriisin sijaan tämän transpoosia, saadaan seä RotMettä SymE-esitystavan inemaattinen tilanyhtälö muotoon Ẋ t = S(t)X t, missä matriisiarvoinen funtio S(t) on vinosymmetrinen aiilla ajanhetillä, ja rataistava muuttuja X t on joo 3 3- (RotM) tai 4 1-matriisi (SymE). Tällaiselle systeemille implisiittinen välipistemenetelmä antaa ortogonaalisen rataisun aiilla ajanhetillä, eli i > 1 : Xi T X i = I, unhan vain aluasento on ortogonaalinen, eli X0 TX 0 = I [6, s. 268]. SymE-esitystavan tapausessa ortogonaalisuusehto on yhtäpitävä ysiönormiehdon anssa, sillä q R 4 : q 2 = 1 q T q = 1 = I 1. Näin ollen implisiittinen välipistemenetelmä säilyttää asentovaternin ysiönormin inemaattisessa tilanyhtälössä (2.9). RotM-esitystavan asentomatriisin transpoosin Ri T ortogonaalisuudesta seuraa myös itse asentomatriisin R i ortogonaalisuus, joten implisiittinen välipistemenetelmä säilyttää ortogonaalisuuden myös inemaattisessa tilanyhtälössä (2.3). Eulerin ulmien tapausessa emme saavuttaisi implisiittisen välipistemenetelmän äytöllä vastaavaa hyötyä uin RotM- ja SymE-esitystavoissa, sillä siinä mahdollisten asentojen monisto on oo avaruus R 3. Lisäsi soveltaminen olisi hanalaa, sillä tilanyhtälö ei ole lineaarinen. Tarastellaan rataisumenetelmää rotaatiomatriisin (RotM) tapausessa. Aselpäivitys rotaatiomatriisille on seuraava: missä K = R 1 = R 0 + hk, (R 0 + h2 ) K Ω t0 +. h 2

25 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 16 Viimeisestä yhtälöstä voidaan rataista matriisi K: ( K = R 0 Ω t0 + I h 3 h Ω t ) h Saadaan siis R 1 = R 0 ( I 3 + hω t0 + h 2 ( I 3 1 ) ) 1 2 Ω t 0 +. h 2 Vastaavasti ysiövaterniesitystavan SymE inemaattiselle tilanyhtälölle (2.9) implisiittisen välipistemenetelmän aselpäivitys saa muodon q 1 = q 0 + h 1, missä 1 = 1 2 Ω t 0 + h 2 ( q 0 + h ) 2 1. Rataistaan viimeisestä 1 ja sijoitetaan edelliseen, jolloin aselpäivitys voidaan irjoittaa lyhysti: q 1 = ( I 4 + h 2 (I 4 h ) 4 Ω t 0 + ) 1 Ω h t0 + q h Simulaatio Seuraavasi simuloimme asennon rataisemista. Realistisen tapausen mallintamisen sijaan raennamme simulaation siten, että se on tarpeesi (mutta ei liian) haasteellinen, ja ennen aiea sellaisesi, että sen rataisu voidaan lasea tarasti. Lähdetään siis liieelle simuloitujen asentojen tarasta rataisusta. Oloon asennon rotaatiomatriisi ajanhetellä t: cos θ(t) 0 sin θ(t) R(t) = cos ψ(t) sin ψ(t), (2.20) sin θ(t) 0 cos θ(t) 0 sin ψ(t) cos ψ(t) missä θ : R R ja ψ : R R ovat sileitä funtioita. Nähdään, että riippumatta funtioiden θ ja ψ valinnasta matriisi R(t) on rotaatiomatriisi aiilla ajanhetillä. Rotaatiomatriisin inemaattisesta tilanyhtälöstä (2.3) saadaan puolittain matriisilla R T ertomalla: (ω ) = R T Ṙ. Tämä aui ertomalla (s. liite 6.2) saadaan: 0 θ sin ψ θ cosψ (ω ) = θ sin ψ 0 ψ, θ cosψ ψ 0

26 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 17 mistä ulmanopeusluuolmiosi saadaan (2.4) nojalla ψ ω = θ cos ψ. (2.21) θ sin ψ Valitaan funtiot θ ja ψ seuraavasti: θ(t) = e 0.2(t 2.5)2 ψ(t) = (t 2.5) 2, (2.22) missä t [0, 5]. On huomattava, että θ saa arvon π ahdella eri ajanhetellä. Näillä 2 ajanhetillä Eulerin ulmien inemaattinen tilanyhtälö (2.13) on singulaarinen. Vaia rataistua asentoa vastaava θ ei osaan saisiaan taralleen arvoa π, on rajallisen lasentataruuden vuosi oletettavaa, että tilanyhtälön (2.13) rataisija ärsii tästä 2 singulaarisuudesta. Rataistaan siis inemaattiset tilanyhtälöt (2.3), (2.9) ja (2.13) MATLAB-ohjelmiston avulla äyttäen erilaisia edellä uvattuja Runge-Kutta-rataisumenetelmiä ja eripituisia disretointivälejä eli aselpituusia h. Kunin rataisijan alutilana on oiea asento R 0 = R(0) (2.23) muunnettuna yseisen rataisijan asentomuuttujasi. Rataistujen asentojen vertailu suoritetaan RotM-esitystavassa, osa näistä olmesta esitystavasta se on ainoa, joa on ysiäsitteinen. Muutamme siis rataistut SymE- ja EA-asennot RotM-asennosi äyttäen sopivia [17, s ] muunneltuja 2 muunnosaavoja, ja lasemme unin rataistun asentomatriisin ˆR(t) Frobenius-etäisyyden oieasta asentomatriisista R(t). Kuvassa 2.2 on esitetty eri tavoilla rataistun asennon masimivirhettä disretointivälin h funtiona, eli ullein disretointivälille ja rataisumenetelmälle on lasettu ullein ajanhetelle R(t) ˆR(t) Fr ja otettu näistä suurin arvo ja piirretty uvaan. Lisäsi on tarasteltu rataistujen asentojen ortogonaalisuutta lasemalla ullein ajanhetelle R(t) T R(t) I 3 Fr seä näiden suurin arvo. Kutsumme tätä suuretta masimiortogonaalisuusvirheesi. 2 Nitsche ja Knicmeyer [17] äyttävät artielissaan RotM-esitystavassa inemaattista tilanyhtälöä Ṙ N&K = (ω N&K )R N&K. Transponoimalla yhtälö puolittain ja meritsemällä R = R T N&K ja ω = ω N&K saadaan Savagen [21] äyttämä tuttu yhtälö Ṙ = ṘT N&K = RT N&K (ω N&K ) T = R T N&K ( ω N&K ) = R(ω ). Käyttäessämme siis artielissa esitettyjä muunnosaavoja transponoimme saadun (tai lähtö-) rotaatiomatriisin, ja äyttäessämme artielissa esiteltyä Eulerin ulmien inemaattista tilanyhtälöä (2.13), äytämme ulmanopeuden ω sijaan tämän vastaluuolmioa ω.

27 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 18 Asennon masimivirhe disretointivälin funtiona Fr 10 6 ROTM RK1 SYME RK1 EA RK1 SYME RK1 PROJ ROTM RK1 PROJ EA RK2 ROTM RK2 ROTM RK2 PROJ ROTM RK2 IMPL SYME RK2 SYME RK2 PROJ SYME RK2 IMPL EA RK ROTM RK4 SYME RK h / s 10 2 Asentomatriisin masimiortogonaalisuusvirhe disretointivälin funtiona ROTM RK1 SYME RK Fr ROTM RK2 SYME RK ROTM RK SYME RK SYME RK2 PROJ ROTM RK2 PROJ EA RK1,2,4 SYME RK2 IMPL ROTM RK2 IMPL h / s Kuva 2.2: Rataistujen asentomatriisien Frobenius-etäisyyden masimi oieasta asentomatriisista seä masimiortogonaalisuusvirhe

28 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 19 Kuvan perusteella todetaan, että rataistut asennot todella näyttävät suppenevan oieaan asentoon aselooa pienennettäessä, neljännen asteen menetelmät nopeammin uin toisen asteen menetelmät, jota taas suppenevat nopeammin uin ensimmäisen asteen menetelmät. Lisäsi nähdään, että RotM RK1- ja SymE RK1-rataisijoiden ortogonaalisuusvirhe on melo suuri, ja näiden projisoivat versiot RotM RK1 PROJ ja SymE RK1 PROJ parantavatin rataisuja merittävästi. Sen sijaan toisen asteen rataisijat RotM RK2 ja SymE RK2 eivät tuota paljon parempaa tulosta, vaia ne projisoitaisiinin. Neljännen asteen projisoivien rataisijoiden virheitä ei ole tähän uvaan piirretty, sillä ne eivät silmämääräisesti eroaisi tavallisten RK4-rataisijoiden virheistä. Implisiittisen välipistemenetelmän rataisut ovat jonin verran tarempia uin vastaavat toisen asteen esplisiittiset rataisijat, ja niiden rataisut pysyvät automaattisesti ortogonaalisina, lasentataruuden rajoissa. Neljännen asteen rataisijat suppenevat hyvin nopeasti verrattuna muihin, josin EA RK4 näyttää hyvin epävaaalta. Tämän uten muidenin EA-rataisijoiden epävaaus johtuu jo aiemmin mainitusta singulaarisuudesta. 2.4 Paian rataiseminen Palataan vielä tämän luvun lopussa lyhyesti itse ongelmaan, eli paiannuseen. Kappaleessa 2.3 on esitetty tapoja rataista asento ullain ajanhetellä äyttäen hyväsi ulmanopeusmittausia. Jos tiedetään laitteen asento ullain ajanhetellä seä alupaia ja -nopeus, voidaan saatavien iihtyvyysmittausten perusteella rataista laitteen paia ullain ajanhetellä. Oloon r(t), v(t) ja a(t) laitteen paia-, nopeus ja iihtyvyysvetori ajan funtiona ja r I (t) = coord I ( r(t)) v I (t) = coord I ( v(t)) a I (t) = coord I ( a(t)) näiden oordinaatit inertiaoordinaatistossa. Paia-, nopeus- ja iihtyvyysoordinaattien väliset yhteydet ovat: v I (t) = a I (t) (2.24) ja ṙ I (t) = v I (t), (2.25) joten laitteen oordinaatit inertiaoordinaatistossa saadaan ahdesti integroimalla iihtyvyys. Kiihtyvyys inertiaoordinaatistossa taas saadaan oordinaatistomuunnosella a I (t) = C I B (t)a B(t) (2.26)

29 LUKU 2. DETERMINISTINEN INERTIAPAIKANNUS 20 missä a B (t) on iihtyvyys laitteen omassa oordinaatistossa ajanhetellä t ja CB I (t) on laitteen asennon määräävä rotaatiomatriisi ajanhetellä t. Miäli laitteeseen ei vaiuta miään painovoima, niin a B (t) on laitteen mittaama iihtyvyys muussa tapausessa laitteen mittaama iihtyvyys on a B (t) g B (t) [1, s. 54], missä g B (t) on laitteeseen ohdistuvien gravitaatiovoimien aiheuttama iihtyvyys laitteen oordinaatistossa ajanhetellä t. Tässä työssä oletamme ysinertaisuuden vuosi, ettei laitteeseen vaiuta miään gravitaatiovoima. Saadaan siis: r I (t) = r I (0) + = r I (0) + t 0 t 0 v I (u)du ( v I (0) + = r I (0) + v I (0)t + u 0 t u 0 0 ) a I (w)dw du C I B (w)a B(w)dwdu. Kasoisintegraali voidaan lasea numeeristen integrointimenetelmien avulla. Toinen [ vaihtoehto ] paian rataisemiseen on oota paia ja nopeus yhdesi tilamuuttujasi, jolloin yhtälöistä (2.24)-(2.26) saadaan muodostettua differentiaaliyhtälö r I (t) v I (t) [ ṙ I (t) v I (t) ] = [ v I (t) C I B (t)a B(t) Saatu differentiaaliyhtälö saa siis syötteenään asentomatriisin CB I seä iihtyvyysmittausen ullain ajanhetellä. Tämä differentiaaliyhtälö voidaan rataista äyttäen tässä appaleessa esitettyjä differentiaaliyhtälön rataisumenetelmiä. Kolmas vaihtoehto paian rataisemiseen on oota paia, nopeus ja asento yhdesi tilamuuttujasi, muodostaa tälle niin iään differentiaaliyhtälö ja rataista se äyttäen jotain appaleessa esitettyä rataisumenetelmää. Tämä tapa ei siis vaadi asennon rataisemista etuäteen, vaan aii olme muuttujaa saadaan rataistua yhtä aiaa. ].

30 Luu 3 Virheen etenemisyhtälöt ja niiden rataiseminen Tässä luvussa tarvitaan todennäöisyyslasentaa, joten määritellään alusi muutama perusäsite [13, s. 3-5]: Määritelmä 4 Kolmio (Φ, F, P) on todennäöisyysavaruus, jos Φ on aleistapausten jouo, F on mitallisten jouojen jouo ja P on Kolmogorovin asioomat täyttävä mitta. Määritelmä 5 Oloon (Φ, F, P) todennäöisyysavaruus. Satunnaismuuttuja x on mitallinen uvaus x : Φ R d, eli x 1 (O) = {ϕ Φ x(ϕ) O} F joaiselle avoimelle O R d, missä x 1 (O) on jouon O aluuva. Satunnaismuuttujan jaaumasta puhuttaessa taroitetaan yleensä sen ertymä- tai tiheysfuntiota, jota määritellään seuraavasti: [13, s. 6-7] 21

31 LUKU 3. VIRHEEN ETENEMISYHTÄLÖT JA NIIDEN RATKAISEMINEN 22 Määritelmä 6 Satunnaismuuttujan x : Φ R d ertymäfuntio on F x (x) = P({ϕ x(ϕ) x}) = P(x x), missä moniulotteinen pienempi-tai-yhtäsuuri-uin-relaatio määritellään x 1. x d y 1. y d x 1 y 1... x d y d. Jos on olemassa sellainen funtio f x : R d [0, ), että x R d : F x (x) = xd x1 f x (u)du 1 du d, niin satunnaismuuttuja x on jatuva ja f x on satunnaismuuttujan x tiheysfuntio. Satunnaismuuttujaa uvaavia tunnusluuja ovat sen odotusarvo ja varianssi [13, s. 12]: Määritelmä 7 Jatuvan satunnaismuuttujan x : Φ R d odotusarvo on E(x) = xf x (x)dx ja varianssi Var(x) = (x E(x))(x E(x)) T f x (x)dx. Tämän työn annalta tärein jatuva jaauma on normaalijaauma, joa määritellään seuraavasti [13, s. 19]: Määritelmä 8 Oloon x jatuva satunnaismuuttuja avaruudessa R d, ja oloon µ R d ja Σ R d d symmetrinen ja positiividefiniitti. Jos satunnaismuuttujan x tiheysfuntio on f x (x) = 1 det(2πσ) e 1 2 (x µ)t Σ 1 (x µ), niin x on normaalijaautunut parametrein µ ja Σ. Jos x on normaalijaautunut parametrein µ ja Σ, niin sanomme, että x noudattaa normaalijaaumaa N(µ, Σ), ja meritsemme tätä x N(µ, Σ). Normaalijaauman parametrit µ ja Σ ovat samalla sen odotusarvo ja varianssi. Satunnaismuuttujien esinäisen suhteen uvaamisesi äytetään yhteis- ja marginaalijaauman seä satunnaismuuttujien riippumattomuuden äsitteitä: [13, s ]

32 LUKU 3. VIRHEEN ETENEMISYHTÄLÖT JA NIIDEN RATKAISEMINEN 23 Määritelmä 9 Satunnaismuuttujien x 1,...,x yhteisjaauman ertymäfuntio on Yhteisjaaumasta saatavia jaaumia F x1,...,x (x 1,...,x ) = P(x 1 x 1,...,x x ). F xi (x i ) = F x1,...,x (,...,, x i,,..., ) = lim x j j i F x1,...,x (x 1,..., x ), i {1,..., } sanotaan marginaalijaaumisi. Määritelmä 10 Satunnaismuuttujat x 1,...,x ovat riippumattomia, jos F x1,...,x (x 1,...,x ) = F xi (x i ). (3.1) Jatuville satunnaismuuttujille x 1,...,x yhden muuttujan marginaalijaauman tiheysfuntio voidaan lasea yhtälön f xi (x i ) = f x1,...,x (x 1,..., x )dx 1 dx i 1 dx i+1 dx (3.2) avulla. Lisäsi riippumattomuusehto voidaan irjoittaa yhtälön (3.1) anssa evivalenttiin muotoon [13, s ] f x1,...,x (x 1,...,x ) = Määritellään seuraavasi stoastinen prosessi: f xi (x i ). (3.3) Määritelmä 11 Oloon (Φ, F, P) todennäöisyysavaruus ja T parametrijouo. Stoastinen prosessi on sellainen uvaus x : Φ T R d, että joaisella iinteällä t T x(, t) on satunnaismuuttuja. Parametrijouo T on yleensä join aiajana, esimerisi T = [0, ). Välin [T 0, T 1 ] T disretointi on sellainen järjestetty jouo τ = (t 0,...,t ) että T 0 = t 0 < t 1 <... < t = T 1. Jos τ = (t 0,...,t ) on disretointi, niin yhteisjaaumaa F xτ (x 1,..., x ) = F xt0,...,x t (x 1,...,x ) sanotaan prosessin x t äärellisulotteisesi jaaumasi [13, s.33]. Stoastisia prosesseja x t ja y t, t T sanotaan toistensa versioisi, jos P({ϕ x t (ϕ) = y t (ϕ)}) = 1 t T [18, s. 14].

33 LUKU 3. VIRHEEN ETENEMISYHTÄLÖT JA NIIDEN RATKAISEMINEN Virheen etenemisyhtälö asennon rataisemisessa Tässä luvussa tarastelemme, mitä tapahtuu, un oletamme mittausten sisältävän stoastisen virhetermin. Lähdemme liieelle samanaltaisesta tehtävänasettelusta uin luvussa 2, mutta ilman oletusta, että aii mittauset ovat virheettömiä. Sen sijaan oletamme, että ullain ajanhetellä saatava mittaus on vain arvio todellisesta iihtyvyyden tai ulmanopeuden arvosta. Tarastellaan asennon inemaattista tilanyhtälöä. Luvussa 2 totesimme, että ullein esitystavalle asennon x t inemaattinen tilanyhtälö saadaan muotoon ẋ t = ˆf(ω(t), x t ) (2.14) Oletettaessa, että mittauset ovat ohinaisia, voidaan ohinatermi lisätä ulmanopeuden arvoon, jolloin yhtälö voidaan irjoittaa intuitiivisesti muotoon ẋ t = ˆf(ω(t) + ohina,x t ). (3.4) Kohina on siis olmiulotteista, ja oletamme sen omponenttien olevan riippumattomia toisistaan. On huomattava, että otettaessa ohinatermi muaan tietyn ajanheten asento x t vaihtuu satunnaismuuttujasi x t ja differentiaaliyhtälön rataiseva funtio x t, t [0, ) vaihtuu stoastisen differentiaaliyhtälön rataisevasi stoastisesi prosessisi x t, t [0, ). Toivomme löytävämme stoastisen prosessin W t = W (1) t W (2) t W (3) t siten, että ohina voitaisiin irjoittaa muotoon αw t, missä α olisi vaio ja stoastisella prosessilla W t olisi seuraavat ominaisuudet: t 1 t 2 W t1 ja W t2 ovat riippumattomia. W t on stationäärinen, eli jaauma F Wt1 +t,...,w t +t ei riipu ajasta t. t : E(W t ) = ja Var(W t ) = I 3. Tällaista stoastista prosessia ei uitenaan ole olemassa, mutta Øsendal toteaa, että on olemassa ehdot täyttävä yleistetty stoastinen prosessi (s. [18, s ]). Tällaista prosessia utsutaan valoisen ohinan prosessisi. [18, s. 21] Voimme siis irjoittaa yhtälön (3.4) muotoon: ẋ t = ˆf(ω(t) + αw t,x t ). (3.5)

34 LUKU 3. VIRHEEN ETENEMISYHTÄLÖT JA NIIDEN RATKAISEMINEN 25 Lisättäessä tällainen virhetermi inemaattisiin tilanyhtälöihin (2.3), (2.9) ja (2.13), saadaan Ṙ t = R t (ω(t) ) + αr t αr t q t = 1 2 Ω(t)q t + α 2 W (1) t αr t W (2) t W (3) t, (3.6) q tw (1) t + α q tw (2) t α q tw (3) t (3.7) ja ϕ t θ t = ψ t (ω 2 (t) sin ψ t ω 1 (t) cosψ t )/ cosθ t (ω 2 (t) cosψ t + ω 1 (t) sin ψ t ) ω 3 (t) + (ω 1 cosψ t ω 2 (t) sin ψ t ) tanθ t cos ψ t sin ψ t cos θ t + α sin ψ t W (1) cos θ t 0 t + α cosψ t W (2) t + α 0 W (3) t (3.8) cos ψ t tan θ t sin ψ t tan θ t 1 Näistä rotaatiomatriisiesitystavan yhtälö (3.6) on hieman hanala, osa siinä rataistava muuttuja on matriisiarvoinen satunnaismuuttuja. Päästäsemme tästä ongelmasta eroon äytämme hyväsemme vetorointia vect ja Kronecerin tuloa. Vetorointi määritellään yleisesti funtiona, joa muuttaa mielivaltaisen matriisin pystyvetorisi pinoamalla matriisin saraeet päälleäin. Eli jos meritsemme A = [A 1:n,1 A 1:n,m ] R n m, niin vect [A 1:n,1...A 1:n,m ] = A 1:n,1. A 1:n,m. (3.9) Kronecerin tulo taas on ahdelle mielivaltaiselle matriisille suoritettava bilineaarinen

35 LUKU 3. VIRHEEN ETENEMISYHTÄLÖT JA NIIDEN RATKAISEMINEN 26 operaatio : R n m R p q R np mq, joa määritellään: A 11 B A 1n B A B =.., A m1 B A mn B missä A = A 11 A 1n. A m1 A mn. Rm n ja B = B 11 B 1q. B p1 B pq. Rp q. Vetoroinnilla ja Kronecerin tulolla on yhteys vect(axb) = (B T A) vect X, (3.10) joa pätee aiille matriiseille A, B, X, joille tulo AXB on määritelty. Tässä työssä vetoroimme pelästään 3 3-matriiseja, ja niinpä määrittelemme vetoroinnin funtiona vect : R 3 3 R 9, joa toteuttaa yhtälön (3.9). Määriteltyämme lähtöjouon dimension vetorointi on bijetiivinen operaatio, ja niinpä voimme vetoroida yhtälön (3.6): ρ t = vectṙt = vect R t (ω(t) ) + αr t W (1) t αr t W (2) t + αr t W (3) t. (3.11) Vetorointi on selvästi lineaarinen operaatio, eli vect(αa + βb) = α vect A + β vect B, un α, β R ja A, B R 3 3. Näin ollen (3.11) saa muodon: vect ρ t = vect (I 3 R t (ω(t) )) + α vect I 3 R t W (1) t α vect I 3 R t W (2) t + α vect I 3 R t W (3) t. (3.12) Matriisit R t on errottu vasemmalta identiteettimatriisilla seuraavasta syystä: soveltamalla yhtälöä (3.10) näihin identiteettimatriisilla errottuihin termeihin saamme yhtälön

36 LUKU 3. VIRHEEN ETENEMISYHTÄLÖT JA NIIDEN RATKAISEMINEN 27 (3.12) muotoon: ρ t = ( ) (ω(t) ) T I 3 ρt + α I 3 ρ t W (1) t α I 3 ρ t W (2) t + α I 3 ρ t W (3) t, (3.13) missä ρ t = vectr t. Saamme siis unin asennon esitystavan stoastisen inemaattisen tilanyhtälön (3.13, 3.7, 3.8) muotoon tai yleisemmässä muodossa ẋ t = ˆf(ω(t),x t ) + ẋ t = f(t,x t ) ĝ i (x t )W (i) t (3.14) g i (t,x t )W (i) t, (3.15) missä funtiot f ja g i, i = 1,..., 3 ovat pystyvetoriarvoisia ajan ja asentomuuttujan funtioita. Esitystavoissa RotM ja SymE funtiot f ja g i ovat asentomuuttujan x t suhteen lineaarisia, un taas Eulerin ulmilla ne ovat trigonometrisia. Funtioiden lineaarisuudesta on merittävää hyötyä myöhemmin rataisualgoritmien muodostamisessa. Yhtälö (3.15) tulitaan yleensä Itô-integraaliyhtälönä x t = x 0 + t 0 tai differentiaalimuodossa [18, s.63] f(s,x s )ds + 3 t 0 g i (s,x s )db (i) s (3.16) dx t = f(t,x t )dt + 3 g i (t,x t )db (i) t. (3.17) Kutsumme yhtälöitä ( ) yhteisnimellä stoastinen differentiaaliyhtälö josin yleensä tällä termillä viittaamme (3.17):n muotoiseen yhtälöön. Itô-yhtälöitä (3.16) ja (3.17) utsumme virheen etenemisyhtälöisi, sillä ne uvaavat, miten asennon virhe tai epävarmuus ehittyy ajan myötä. Yhtälöissä esiintyvät stoastiset prosessit {B (i) t } t [0, ) ovat toisistaan riippumattomia Brownin liieitä. Integraali t 1 t 0 f(t)db t on Itô-integraali, un B t on Brownin liie [18, s.63]. Brownin liie on stoastinen prosessi, jolla on seuraavat ominaisuudet [14, s. 40]: