Studie av integration mellan rategyron och magnetkompass

Transkript

1 Studie av integration ellan rategron och agnetopass Eaensarbete utfört i Reglerteni vid Linöpings tenisa högsola av Sara Nilsson LITH-ISY-EX Eainator: Handledare: Anders Helersson Miael Aelsson Hans Bohlin ustaf Hendeb iii

2 iv

3 Avdelning, Institution Division, Departent Institutionen för ssteteni LINKÖPIN Datu Date 24-- Språ Language X Svensa/Swedish Engelsa/English Rapporttp Report categor Licentiatavhandling X Eaensarbete -uppsats D-uppsats Övrig rapport ISBN ISRN LITH-ISY-EX Serietitel och serienuer Title of series, nubering ISSN URL för eletronis version Titel Title Författare Author Studie av integration ellan rategron och agnetopass Stud of sensor fusion of rategros and agnetoeters Sara Nilsson Saanfattning Abstract This aster thesis is a stud on how a rategro triad, an acceleroeter triad, and a agnetoeter triad can be integrated into a navigation sste, estiating a vehicle s attitude, i.e. its roll, tipp, and heading angles. When onl a rategro triad is used to estiate a vehicle s attitude, a drift in the attitude occurs due to sensor errors. When an acceleroeter triad and a agnetoeter triad are used, an error in the vehicle s heading, appearing as a sine curve, depending on the heading, occurs. B integrating these sensor triads, the sensor errors have been estiated with a filter to iprove the estiated attitude s accurac. To investigate and evaluate the navigation sste, a siulation odel has been developed in Siulin/Matlab. The ipleentation has been ade using a Kalan filter where the sensor fusion taes place. Siulations for different scenarios have been ade and the results fro these siulations show that the drift in the vehicle s attitude is avoided. Ncelord Keword Navigation, Estiation, Kalan filtering, Rategro triad, Acceleroeter triad, Magnetoeter triad

4

5 ABSTRAT This aster thesis is a stud on how a rategro triad, an acceleroeter triad, and a agnetoeter triad can be integrated into a navigation sste, estiating a vehicle s attitude, i.e. its roll, tipp, and heading angles. When onl a rategro triad is used to estiate a vehicle s attitude, a drift in the attitude occurs due to sensor errors. When an acceleroeter triad and a agnetoeter triad are used, an error in the vehicle s heading, appearing as a sine curve, depending on the heading, occurs. B integrating these sensor triads, the sensor errors have been estiated with a filter to iprove the estiated attitude s accurac. To investigate and evaluate the navigation sste, a siulation odel has been developed in Siulin/Matlab. The ipleentation has been ade using a Kalan filter where the sensor fusion taes place. Siulations for different scenarios have been ade and the results fro these siulations show that the drift in the vehicle s attitude is avoided. Kewords: Navigation, Estiation, Kalan filtering, Rategro triad, Acceleroeter triad, Magnetoeter triad vi

6

7 SAMMANFATTNIN Eaensarbetet studerar hur en rategrotriad, en acceleroetertriad och en agnetoetertriad an integreras för att utvecla ett navigeringssste so estierar ett fordons attitd, dvs. dess roll-, tipp- och ursvinel. Då endast en rategrotriad används för att satta fordonets attitd uppstår drift i denna på grund av sensorfel. När endast en acceleroeter- och agnetoetertriad används uppstår ett fel i fordonets ursvinel so har foren av en sinusurva so beror av ursen. eno att integrera dessa sensorer an sensorfelen estieras ed hjälp av ett filter för att förbättra noggrannheten i den sattade attitden. För att undersöa och utvärdera detta navigeringssste har en siuleringsodell utveclats i Siulin/Matlab. Ipleenteringen har gjorts ed ett alanfilter där sensorfusion ser. Siuleringar har gjorts för olia scenarier och resultaten av dessa visar att attitddriften undvis. Ncelord: Navigering, Estiering, Kalanfiltrering, Rategrotriad, Acceleroetertriad, Magnetoetertriad vii

8

9 INNEHÅLL Inledning.... Bagrund....2 Sfte....3 Probledefinition....4 Metod Avgränsningar Rapportens disposition Navigering Definition av begreppet navigering Navigering Strning uidance Jordodeller Jordens geoetrisa for Jordens rotation Jordens gravitation Jordens agnetfält Ovärldsodell Terrängodell Färdväg Utdata Sensorer Allänt o sensorer Rategron Acceleroetrar Magnetoetrar Sensorfel Rategrotriaden Modellering av fordonets vinelhastighet Modellering av rategrotriaden Acceleroetertriaden Modellering av fordonets acceleration Modellering av acceleroetertriaden Magnetoetertriaden Magnetis deviation Modellering av agnetfält Modellering av agnetoetertriaden Sensorfusion Kapitelöversit Attitdberäningar Rategrotriadens attitdberäningar viii

10 4.2.2 Acceleroeter- och agnetoetertriadens attitdberäningar Definition av attitdfel Felodeller för attitd Rategrotriaden Acceleroetertriaden Magnetoetertriaden Slutlig tillståndsodell Kalanfiltrering Tillståndsodell Sapling av tillståndsodellen Kalanfiltret Etended alanfilter Felopensering Metod rodrift Korrigering av attitden Siuleringar och resultat Siuleringsodell Siuleringar Montearlo siulering Studerade felvariabler Scenarier Sensorfel och initialvärden Siuleringsresultat Återoppling och opensering Variering av rategrobias och rategrobrus Variering av agnetoeterbias Estiering av fordonets acceleration Siuleringsresultat från olia scenarier Slutsatser Resultat och slutsatser Siuleringsodell Resultat Förslag på fortsatt arbete Sensorfel Transporthastighet Magnetis urs Scenarier Kalanfiltrering... A Notation och betecningar... 3 A. Referensoordinatsste... 3 A.2 Fsialisa onstanter... 3 A.3 Kineati... 4 A.4 DM-saband... 5 i

11 B Referensoordinatsste... 6 B. Earth entered Inertial Frae... 6 B.2 Earth entered Earth Fied Frae... 6 B.3 eodetic Frae... 7 B.4 Local Level Frae... 8 B.5 hassi Frae... 8 B.6 Sensorplattfor... B.7 Rategrotriad... B.8 Acceleroetertriad... B.9 Magnetoetertriad... Vetortransforationer Sevoperatorn... 2

12

13 INLEDNIN I detta apitel besrivs bagrund och sfte ed eaensarbetet. Här återges även probledefinition sat etod och avgränsningar. I slutet av apitlet finns en besrivning av rapportens disposition.. Bagrund På AerotechTelub AB i Linöping bedrivs en studie gällande enla och robusta navigeringssste för bland annat soldater och stridsfordon. Ino raen för denna studie sall bland annat öjligheter och begränsningar vid integration ellan agnetoetrar, acceleroetrar och rategron studeras. Tidigare har förinställningar av fordons navigeringssste studerats en då detta ofta är en tidsrävande process finns ett behov av en bättre etod..2 Sfte Sftet ed eaensarbetet är att undersöa hur integrationen ellan nända sensorer an göras, vila tenier so an användas sat vila resultat so an uppnås. Resultaten av integrationen sall även jäföras ed vilen prestanda so uppnås då endast en rategrotriad respetive en acceleroeteroch en agnetoetertriad används..3 Probledefinition Uppgiften sftar till att utvecla en siuleringsodell och ett filter för appliationen navigering geno att integrera en rategrotriad, en acceleroetertriad sat en agnetoetertriad. För att unna odellera detta sste sa först en odell av jordens geoetrisa for specificeras och odelleras, varefter odeller av jordens rotation, gravitation och agnetfält an studeras. Vidare sa en rategrotriad, en acceleroetertriad sat en agnetoetertriad odelleras ed sensorfel i for av vitt brus och bias. Siuleringodellen sa utveclas i Matlab och Siulin. Efter siuleringsodellen ipleenterats sa erhållen prestanda sat egensaper för det fratagna filtret och eaniseringsodellen studeras geno siulering. Dessa resultat sall jäföras ed användning av endast rategron respetive agnetopass.

14 .4 Metod Efter en litteraturstudie härleds de saband so används för att beräna fordonets attitd ed ätningarna från rategrotriaden sat ed acceleroeter- och agnetoetertriaden och därefter studeras de fel so är av intresse. Utifrån detta härleds felodeller för hur felen fortplantas i ssteet och en tillståndsodell för detta ställs upp. För att däpa feltillväten studeras även vila fel so an återopplas för att reglera ssteet. Felen estieras ed hjälp av ett etended alanfilter. Siulering och verifiering av odellen utförs slutligen i Matlab och Siulin..5 Avgränsningar Det här eaensarbetet begränsas till att studera ett fordon so färdas på en liten ta i en platt värld, vilet edför att jordens transporthastighet an försuas. Ingen uppdatering av fordonets position oer att se och en följd av detta är att jordens gravitation, jordens priära agnetfält sat jordens rotationshastighet på den atuella positionen endast behöver beränas vid initiering av ssteet. Detta på grund av att dessa värdens förändringar är försubara när fordonet färdas på en liten ta. Eaensarbetet begränsas vidare till att endast studera en platt loal värld, dvs. en värld där inga berg och dalar eisterar utan tan är platt vilet edför att fordonets roll- och tippvinel alltid är noll. En tterligare begränsning är antagandet att agnetist norr saanfaller ed geografist norr, vilet edför att delinationen är noll och således inte behöver tas hänsn till. En odell för det jordagnetisa fältet sa ändå ipleenteras en jordens agnetfält i -led sätts därefter till noll. I eaensarbetet antas vidare att fordonets position och hastighet är givna observationer till eepel från en PS-ottagare. Dessa behöver inte odelleras utan antas vara oberoende observationer ontainerade av brus..6 Rapportens disposition I apitel 2 definieras navigering so är ett centralt begrepp i det här eaensarbetet. Där definieras även de jordodeller so används i eaensarbetet, dvs. odeller av jordens geoetrisa for, jordens rotation, jordens gravitation sat jordens agnetfält. I slutet av apitlet finns inforation o den ovärldsodell so används vid siuleringarna. 2

15 I apitel 3 besrivs de sensorer so används i eaensarbetet, dvs. rategron, acceleroetrar och agnetoetrar. Där besrivs även vila signaler dessa sensorer äter sat vila tper av sensorfel dessa signaler är behäftade ed. I första delen av apitel 4 återfinns de attitdberäningar so gjorts på utsignalerna från rategrotriaden respetive acceleroeter- och agnetoetertriaden. Därefter definieras attitdfel och felodeller för de båda beränade attitderna härleds och sätts saan till en slutgiltig tillståndsodell. I slutet av apitlet besrivs den allänna tillståndsforen sat alanfiltret, etended alanfilter och observerbarhetsriteriu för dessa båda. I apitel 5 besrivs vila siuleringar so gjorts. Där besrivs även de olia scenarier so siulerats sat vila värden på observationsbrus och processbrus i alanfiltret so är läpliga. Även storlesordningen på sensorfelen återges här. I apitel 6 redovisas de resultat so uppnås sat vila slutsatser so dragits utifrån dessa. I detta apitel återfinns även förslag på fortsatt arbete. I bilaga A besrivs den notation sat de betecningar so används i eaensarbetet. Bilaga B definierar de oordinatsste so används sat saband ellan dessa och i bilaga besrivs de vetortransforationer so behöver närare förlaring. 3

16 2 NAVIERIN I detta apitel definieras navigering, so är ett grundläggande begrepp i det här eaensarbetet. Därefter presenteras de jordodeller so används och slutligen förlaras även den ovärldsodell so används vid siuleringen. 2. Definition av begreppet navigering Begreppen navigera och navigering innebär i dagligt tal förågan att veta var an är och hur an sa röra sig för att oa till rätt plats. Det innebär bland annat att storheter so position, ritning och hastighet används. Vanligtvis brts det allänna begreppet navigering ner i tre na begrepp - navigering, strning och guidance. För er inforation o navigering hänvisas till []. 2.. Navigering Navigering innebär att endast det egna fordonets position, hastighet och attitd estieras. Även storheter so acceleration och vinelhastighet ränas so navigeringsstorheter Strning Strning innebär att fordonet påveras av ttre rafter på ett ontrollerat sätt för att det sa röra sig i en viss ritning eller anta ett ntt läge. Utan unsap o farostens navigeringsstorheter so position och hastighet är det oöjligt att stra farosten uidance uidance, so är ungefär detsaa so anövrering, innebär att alla etoder so an användas för att oa fra till hur farosten sa stras för att oa till önsad position används. För att åstadoa detta åste både unsaper o farostens navigering och strning användas. Storheter so avstånd och ritning till destination sat avstånd till önsat spår ellan två brtpunter är eepel på guidancestorheter. För att entdigt unna ange och tola navigations- och guidancestorheter behöver olia referenser att relatera till anges. Navigering av rdfaroster, så allad celestial navigering, har noralt solssteet so referens. Vid navigering av ett fordon på jorden, så allad terrestrial navigering, används vanligtvis jorden so referens. 4

17 2.2 Jordodeller En odell av jorden innebär en ateatis odell av jordens geoetri sat dess rotation, gravitationsfält och agnetfält. För att unna specificera de tre sistnända är det nödvändigt att först specificera en odell av jordens geoetrisa for. Denna behövs även för att ritningar och positioner sa unna besrivas entdigt. Jordodeller ligger alltid till grund för terrestial navigering Jordens geoetrisa for Enligt [] har ännisans sn på jordens for varit cet varierande geno tiderna. För cet länge sedan ansågs jorden vara platt, därefter sfäris och idag finns cet noggranna observationer av jordens for, en våggeodid ed ojän assfördelning. I tabell 2. nedan besrivs de historisa odellerna av jordens geoetrisa for. 5

18 Tabell 2. Historisa odeller av jordens for, []. År Modell Person/ivilisation Utveclingsetod > f.kr. Platt förtida Babloniern.fl. Felatig förståelse av dagliga observationer 9 f.kr. 3 f.kr. Konve dis Babloniern Observationer av sepp so försvinner under horisonten Sfärist lot Egpten Triangulering ot solen (Jorden ca. 5% för stor) 687 Ellipsoid Isaac Newton, England Mätningar på gravitations potentialer (felatig avplattningsfator ca 2%) 958 Svagt päronforad ellipsoid Ann Balie, USA Satellitätningar 96 Våggeodid Desond. King- Hele England.fl. Satellitätningar Dnais våggeodid ed ojän assfördelning Sa. Bose, USA.fl. Satellitätningar och flgätningar 6

19 I det här eaensarbetet används en ellipsoid odell av jordens for. Efterso denna odell är en relativt grov förenling av jordens for ger den upphov till relativt stora geoetrisa odellfel i vertiala positioner, vilet är en nacdel. En ellipsoidodell odellerar jordens geoetrisa for so en något tillplattad sfär. Foren genereras geno att en ellips roteras ring den indre aeln. Figur 2. nedan visar ett tvärsnitt av en ellipsoidis jordodell. eografisa nordpolen X (,R B ) (a,b) Tangentplan 9 ο + Φ EO Evatorplanet Φ EN Φ EO (R A,) Y Figur 2. Tvärsnitt av en ellipsoidis jordodell, []. I figuren ovan betecnar R A ellipsoidens stora ael, so ocså representerar evatorns radie, och R B ellipsoidens indre ael vilet otsvarar avståndet ellan polerna och evatorplanet. Figur 2. visar även sillnaden ellan geodetis latitud, Ф EO, och geocentris latitud, Ф EN. eocentris latitud är vineln ellan evatorplanet och en linje från origo till positionen (a,b). eodetis latitud är den latitud so noralt anges då latituden för en position anges. Den definieras so vineln ellan evatorplanet och noralen till tangentplanet vid positionen (a,b). I fortsättningen oer geodetis latitud betecnas ed Ф och benänas ed enbart latitud. Longitud betecnas λ. Låt r vara en ortsvetor från jordens ittpunt till en godtclig punt på ellipsoiden. Då gäller att alla r so tillhör referensellipsoiden uppfller följande evation 7

20 r R 2 2 B 2 2 r rz + +. (2.) 2 2 R R A A eno att införa variablerna geocentris latitud, geodetis latitud och longitud an evationen srivas på paraetris for enligt följande r r R R r R z B A A sinφ cosφ cosφ EN EN EN 2 R ( e )sinφ P cosλ R sinλ R P P cosφ cosλ cosφ sinλi (2.2) För en ellipsoidodell so besriver jordens geoetri gäller följande saband Avplattningsforfatorn: f a R A R R R A R B Ecentricitetforfatorn: e fa (2 fa ) 2 R Principalurvaturradien: Meridianurvaturradien: R R P A A - e R R 2 B A sin 2 ( e Φ 2 A M. ) ( e sin Φ) 2 Principalurvaturradien är röningsradien för en rörelse i öst- västlig ritning och eridianurvaturradien är jordradien längs en viss longitud. Det finns ett stort antal ellipsoidodeller so besriver jordens geoetri. En av de est använda odellerna är World eodetic Sste 984 (WS84), so är en bra global anpassning. Probleet ed denna odell är att loalt an ellipsoiden avvia ed upp till flera hundra eter från geodiden. I Sverige bruar en odell so heter Riets triangelnät 99 (RT9) användas. Denna odell är en ellipsoid so loalt ger bättre anslutning till geodiden och so siljer sig endast några eter från den sanna geodiden ino Sveriges gränser. Sillnaden ellan loalt och globalt anpassade ellipsoider illustreras nedan i figur

21 Anpassningsoråde lobalt anpassad ellipsoid Loalt anpassad ellipsoid eodid Figur 2.2 Loal anpassning av ellipsoid jordodell, []. I det här eaensarbetet används odellen WS84. Den har följande paraetrar: Jordens rotation R R f e A B a , ,344 /298, , Jorden roterar so beant runt sin egen vertiala ael. Det tar ett dgn för jorden att rotera ett helt varv. Det otsvarar ungefär 36 grader på 24 tiar och däred en vinelhastighet på 5 grader per tie. Ω e betecnar jordens rotationshastighet relativt tröghetsrden och ges av Ωe 7, rad/s. (2.3) För att uttrca jordens rotationshastighet behöver två oordinatsste, EI (I) och EEF (E), definieras. EI och EEF är båda oordinatsste so har origo i jordens ittpunt. Sillnaden ellan de är att EI är fi relativt avlägsna stjärnor edan EEF är fi ot jordens ta. Detaljerad inforation o oordinatssteen återfinns i bilaga B. och B.2. 9

22 För att definiera jordens rotationshastighet används DM-sabandet ellan EI och EEF so definieras i bilaga B. Detta saband definieras so r r r z E cos sin ( Ωet) sin( Ωet) ( Ω t) cos( Ω t) e e E I r r r z I. (2.4) Jordens rotationshastighet an nu uttrcas so en vetor ed hjälp av EI och EEF Ω E e ω E IE Ωe E IE Ω I e ω I IE I E Ωe För er detaljerad inforation o jordens rotation hänvisas till [] Jordens gravitation E IE. (2.5) En odell av jordens gravitation behövs för att besriva jordens gravitationsraft på olia ställen på jorden. ravitationsraften so påverar allt på jorden beror både på centripetalaccelerationen och på assattrationen från jorden. I [] ges en enel odell av jordens gravitation, so en funtion av latitud och altitud enligt följande ravitationen vid evatorn: Modell för jordgravitationen: Konstanter: 2 g 9,784 /s h 2 g g 2A + Bsin Φ, g R A g A + f + a, Ω R R B fa, A B e. 2 M

23 De övriga variablerna so ingår är: f a Avplattningsforfatorn, se avsnitt h Altitud eller höjd över referensellipsoiden, se figur a.2 i bilaga B.3. Ω e Jordens rotationshastighet, se avsnitt M Jordens assa, 5, g ravitationsonstanten, 6,67 - N 2 /g 2 R A, R B Kurvaturradier, se avsnitt Ф Latitud, se avsnitt g betecnar gravitationen uttrct i oordinatssteet EO, so finns närare besrivet i bilaga B.3. Enligt gravitationsodellen insar gravitationen när altituden öar och öar när latituden öar. Foreln är läplig att använda på lägre altituder eepelvis för argående fordon och flgplan, en gäller inte för en farost långt från jorden, eepelvis en satellit Jordens agnetfält En odell av jordens agnetfält är en teoretis odell so besriver agnetfältet ed avseende på geografis position. Jordens agnetfält härrör enligt [2] från flera ällor so saverar på olia sätt. I stort sett an agnetfältet delas upp i tre delar ed olia ursprung. Den största delen av fältet, priärfältet, alstras av eletrisa ströar i jordens ttre ärna so består av fltande järn. De andra två ällorna, av indre stra, är loala störningar från ströar och ferroagnetisa aterial i jordens antel sat eletrisa strösste i jonosfären och agnetosfären. I det här apitlet ges en odell för jordens priära agnetfält. Priärfältet, so utgör cira 9 procent av jordens agnetfält, alstras av eletrisa ströar i jordens ttre ärna av fltande järn. Dessa ströar fungerar so eletroagneter och producerar ett dipollinande agnetfält runt jorden. Detta fält varierar sata ed tiden vilet gör att de agnetisa polerna förflttas ontinuerligt relativt geografist norr. För att besriva priärfältet och dess förändring finns två globala odeller, World Magnetic Model (WMM) och International eoagnetic Reference Field (IRF), so odelleras i det här eaensarbetet och besrivs nedan. Under antagandet att atosfären är ice agnetis i en region nära jordens ta an agnetfältet enligt [2] besrivas so den negativa gradienten av den salära potentialen V enligt ( λ, t) B V r,θ, (2.6)

24 so satisfierar Laplace evation ( λ, t) so är en partiell differentialevation. ( θ, λ, t) 2 V r,θ, (2.7) r, är sfärisa oordinater och otsvarar avståndet från jordens ittpunt, geocentris olatitud (9 - latitud), longitud och tiden angiven i år. En lösning till Laplace evation i sfärisa oordinater är följande n ( n n ) Pn ( cosθ) a ( λ, t) a g () t cos( λ) + h () t sin( λ) V r,θ, ni + a n nea n a r n+ n a r n där a är en referensradie och ( cosθ) ( q () t cos( λ) + s () t sin( λ) ) P ( cosθ). n n n + (2.8) P n är Schidt seinoraliserade associerade Legendrefuntioner av grad n och ordning. För en närare besrivning av dessa funtioner hänvisas till [2]. Variablerna och är interna gaussoefficienter vid tiden t edan och är eterna gaussoefficienter vid tiden t. Sueringsindeen ni och ne betecnar g n s n antalet suationer för de interna respetive de eterna gaussoefficienterna. Enligt [2] är oefficienterna och h n q n q n a a h n och s cet indre än oefficienterna g n och därför an lösningen till Laplace evation approieras enligt n a r n ( n n ) Pn ( cosθ) ( λ, t) a g ( t) cos( λ) + h ( t) sin( λ) V r,θ, N n n+. (2.9) aussoefficienterna so g n () t och h n ( t) är funtioner av tiden och an srivas g h n n () t gn + g& n ( t t ) () t h + h& ( t t ) n n t t t + 5 t t t + 5 (2.) 2

25 där och är oefficienterna för priärfältet för det valda g n h n & n tidsintervallet och g och h & n är den seulära variationen för nästföljande feårsperiod. Detta edför att evation (2.9) an srivas so n ( n n ) Pn ( cosθ) ( λ, t) a g cos( λ) + h sin( λ) V r,θ, + a 3 n 8 n a r n+ a r n+ n ( g& ( t - t ) cos( λ) + h& ( t - t ) sin( λ) ) P ( cosθ). n n + n (2.) IRF-odellen för det priära agnetfältet truneras vid N 3 och odellen för den seulära variationen truneras vid N 8. aussoefficienterna g n och h n och oefficienterna för den seulära variationen g & n och &h n återfinns i [3]. Enligt (2.6) besrivs agnetfältet so den negativa gradienten av den salära potentialen V. Observationerna av jordens agnetfält an då beränas enligt följande z B B B θ λ r V r θ rsinθ V r 3 n V λ 3 n a r n+ 2 sinθ 3 n n a r dpn ( ( ) ( )) ( cosθ) g cos λ + h sin λ n a r n+ 2 n+ 2 n ( sin( λ) h cos( λ) ) P ( cosθ) g dθ ( ) P ( cosθ) ( n + ) g cos( λ) + h sin( λ) n och för seulär variation beränas observationerna enligt n n n n n n n (2.2) 3

26 & & & z B& θ B& λ sinθ B& r r 3 V& θ n V& λ rsinθ n+ 2 3 a n r V& r 3 n n a r dpn ( ( ) ( )) ( cosθ) g& cos λ + h& sin λ (& sin( λ) h& cos( λ) ) P ( cosθ) g Magnetfältsvetorn ges då av n+ 2 n n a r n n+ 2 n dθ ( ) P ( cosθ). ( n + ) g& cos( λ) + h& sin( λ) n n n n n n (2.3) z ( t) + & ( t t ) () t + & ( t t ) () t + & ( t t ). z z (2.4) Från agnetfältsvetorn an den totala och horisontella intensiteten sat inlination och delination beränas 2 2 Horisontell intensitet: H () t () t + 2 Total intensitet: F H 2 + () t z Inlination: Delination: z I arctan H D arctan ( t) ( t) (). t Delination betecnar sillnaden ellan geografis och agnetis nordritning och inlination betecnar agnetfältets vinel ot horisontalplanet på en given plats. 4

27 Efterso odellerna baseras på sfärisa oordinater åste en transforation från de geodetisa oordinaterna göras. Transforationen av de geodetisa h,, där h är höjden över havet, θ är olatituden och λ är r, beränas enligt följande oordinaterna ( θ, λ) longituden, till de sfärisa oordinaterna ( θ, λ) θ arctan r 2 h 2 + 2h R sin θ A + R cos θ B h + R A R A tan θ arctan R sin θ R cos θ h R R A + B + B B R Asin θ + R Bcos θ R Asin θ + R Bcos θ R sin θ + R cos θ R sin θ A + R cos θ B R sin θ + R cos θ A B A B tan θ (2.5) där h i de sista leden efterso endast ett argående fordon odelleras i det här eaensarbetet. 2.3 Ovärldsodell För att unna studera ett fordons beteende vid örning i olia terräng åste en ovärldsodell sapas. Det här avsnittet besriver översitligt hur detta går till ed hjälp av redan ipleenterade progra. För er inforation o ovärldsodellen hänvisas till [4] Terrängodell Det första oentet när ovärldsodellen sapas är att ta fra den ta so fordonet sa färdas på. Terrängodellen använder spline-interpolation för att anpassa en trediensionell ta till disreta punter so anges av användaren. För att denna ta, so representerar aren so fordonet åer på, sa vara ontinuerlig och ontinuerligt deriverbar två gånger används ubisa splines för tre diensioner för interpolerationen Färdväg För att definiera hur fordonet sa röra sig på den sapade tan åste ett antal punter so fordonet sa passera anges av användaren. För att sapa en urva so fordonet sa färdas längs sapas först en ubis splinefuntion i två diensioner utifrån de angivna punterna. Denna urva projiceras sedan på den trediensionella tan so redan sapats. 5

28 Efterso det är önsvärt att studera uppträdanden hos fordonet i olia hastigheter, accelerationer, vinelhastigheter och attitder har urvan eller vägen so fordonet rör sig längs ingen oppling till tiden. Istället anger användaren vilen hastighet fordonet sa ha i olia tidpunter. Notera att hastigheten alltid åste vara noll vid siuleringens start och slut. För att undvia disontinuerliga hastighetsförändringar anpassas även dessa punter ed en ubis splinefuntion Utdata Ovärldsodellen genererar fordonets rörelse geno att vid varje tidpunt returnera fordonets position, hastighet, acceleration, attitd och vinelhastighet. Fordonets rörelse besrivs ed hjälp av oordinatssteen EO, LL och HASSI (läs er o dessa oordinatsste i bilaga B). Från Siulin får fordonets chassi dessa storheter i for av en vetor ed 2 eleent so representerar följande utdata: Eleent -3 Position i oordinatssteet LL 4-6 Hastighet i oordinatssteet EO 7-9 Acceleration i oordinatssteet EO -8 Attitden, DM-sabandet ellan EO och HASSI 9-2 Vinelhastigheten i oordinatssteet 6

29 3 SENSORER I det här apitlet ges först en introdution till de sensorer so används i eaensarbetet. Därefter följer en närare besrivning av odelleringen av sensorerna och beräningar av fordonets attitd ed hjälp av utsignaler från dessa. 3. Allänt o sensorer De sensorer so används i eaensarbetet är so tidigare näns rategron, acceleroetrar och agnetoetrar. I det här avsnittet ges en översitlig besrivning av dessa sensorer och de fel so dessa genererar då de används. För er detaljerad besrivning av rategron och acceleroetrar sat dess sensorfel hänvisas till [] och [5]. 3.. Rategron Ett gro är en sensor so äter attitd eller vinelhastighet (eng. rate). Vad so äts beror på grots onstrution och vilen tp av gro det är. I det här eaensarbetet används rategron. Utsignalen från ett rategro är en sua av fordonets vinelhastighet och jordens rotationshastighet sat de sensorfel rategrot ger upphov till, i den ritning rategrots änslighetsael är placerad. De gron so odelleras har endast en änslighetsael. Detta innebär att o rotationsvetorn är ortogonal ot grots änslighetsael oer utsignalen att bli noll. För att äta en godtclig vinelhastighetsvetor behövs därför tre gron so placeras ortogonalt, en så allad rategrotriad Acceleroetrar En acceleroeter är en sensor so äter acceleration hos en ropp. Utsignalen från en acceleroeter allas för specifi raft och den består av den verliga accelerationen och jordens assattration efterso dessa två inte går att silja åt i ett slutet sste, sat de sensorfel acceleroetern ger upphov till. Acceleroetrar har generellt bara en änslighetsael och efterso både acceleration och assattrationsraft är definierade i tre diensioner, behövs tre acceleroetrar vars alar är orienterade ortogonalt ot varandra, en så allad acceleroetertriad, för att äta en specifi raft. 7

30 3..3 Magnetoetrar En agnetoeter är en sensor so äter agnetfältet på en position. Utsignalen från agnetoetern oer att vara stran hos agnetfältet på positionen so i sin tur är en sua av olia agnetfält so saverar. Dessa är först och fräst jordens priära agnetfält, loala anoalier och det agnetfält so genereras av fordonet. Även för en agnetoeter ontaineras utsignalen ed de sensorfel agnetoetern ger upphov till. Magnetoetrar har generellt bara en änslighetsael och efterso även agnetfält är definierat i tre diensioner, behövs tre agnetoetrar vars alar är orienterade ortogonalt ot varandra, en så allad agnetoetertriad, för att äta agnetfältet på en position Sensorfel Utsignalen från de sensorer so används innehåller tre tper av fel, biasfel, salfatorfel och brus. Då tre sensorer används för att äta signaler i olia ritningar an ett fjärde tp av fel uppstå, ortogonalitetsfel. Biasfel (so på svensa allas nollfel) aratäriseras av att utsignalen från en sensor är onstant och siljd från noll då ingen vinelhastighet, specifi raft respetive agnetfält finns att äta. Biasfelet för ett rategro är alltså en onstant utsignal so är siljd från noll då grot inte roterar. rots bias an ätas geno att orientera rategrot ed änslighetsaeln i öst- västlig ritning där jordrotationen inte är ätbar. Biasfelet för en acceleroeter är även den en onstant utsignal so är sild från noll då acceleroetern inte utsätts för någon specifi raft. Acceleroeterns bias an ätas geno att orientera acceleroetern så att änslighetsaeln är parallell ed horisontalplanet där gravitationen inte är ätbar. Även en agnetoeters utsignal har ett biasfel och även det en onstant utsignal so är siljd från noll då agnetoetern inte utsätts för något agnetfält. Salfatorfel är en annan tp av fel so utsignalerna från sensorerna ontaineras ed. Rategrots salfatorfel an alibreras geno att rotera grot ed en änd onstant rotationshastighet och därefter beräna voten ellan utsignalen och den ända referensen. Ett annat sätt att göra detta på är att orientera grot i en ritning där det änner av en änd del av jordrotationen och därefter beräna voten ellan utsignalen och den ända referensen. Salfatorfelet hos en acceleroeter an beränas geno att utsätta acceleroetern för en specifi raft. Detta an t.e. göras geno att orientera acceleroetern i vertial ritning och utsätta den för en raft och därefter vända på acceleroetern och låta den äta saa raft i otsatt ritning. 8

31 Salfatorfelet an därefter beränas utgående från dessa ätningar. Även en agnetoeters utsignal oer att salas ed en salfator. Den tredje tpen av fel so utsignalerna från sensorerna oer att innehålla är brus. I stort sett alla sensorers utsignaler blir ontainerade ed brus och detta gäller för både rategron och acceleroetrar sat agnetoetrar. Figur 3. nedan illustrerar hur de sensorfel so besrivits, bias, salfator sat vitt brus, påverar en signal Sann utsignal Bias Utsignal ed biasfel Utsignal ed bias, salfatorfel och vitt brus Figur 3. Illustration av hur sensorfel påverar en signal. Då tre sensorer används för att äta signaler i olia ritningar an so tidigare nänt en fjärde tp av fel uppstå, ortogonalitetsfel. Detta fel uppstår o änslighetsalarna hos sensorerna inte är eat ortogonala ot varandra. Mätningen av vinelhastigheten, den specifia raften respetive agnetfältet oer då att bli felatiga efterso sensorerna antas vara ortogonalt placerade. 9

32 3.2 Rategrotriaden I det här avsnittet besrivs först hur fordonets vinelhastighet odelleras. Därefter följer en besrivning av hur rategrotriaden odelleras sat hur utsignalen från denna oer att se ut Modellering av fordonets vinelhastighet eensat för alla tper av gron är att de har tröghetsrden so referens. Detta innebär bland annat att o appliationen räver en noggrannhet bättre än 5 grader per tie oer jordrotationen att behöva openseras bort från utdata. Alternativt an funtioner bggas in i grot och opensera för detta internt. Även den så allade transporthastigheten (eng. craftrate) oer att påvera ätningarna. Transporthastigheten är den vinelhastighet so observeras då ett fordon förflttar sig över jordtan och den an tecnas enligt följande v ω (3.) R + h där R är jordens radie, h är altituden och v är fordonets hastighet. Transporthastigheten definieras so vinelhastigheten hos oordinatssteet EO relativt EEF och betecnas ed ω. Den an enligt [] uttrcas so v v E v R + h ω λcosφ & p E v ω ω Φ& E E (3.2) R + h M ωez där och betecnar fordonets hastighet i - respetive -led uttrct i oordinatssteet EO. Då transporthastigheten oer att vara cet liten jäfört ed jordens rotation so i sin tur är cet liten jäfört ed fordonets vinelhastighet, an transporthastigheten approieras till noll. 2

33 Den vinelhastighet so äts av rategrotriaden är alltså en sua av fordonets egentliga vinelhastighet, jordens rotationshastighet sat transporthastigheten ω ω + ω + ω ω + ω I IE E IE (3.3) Jordens rotation besrivs närare i avsnitt Modellering av rategrotriaden Utsignal från rategrotriaden är enligt föregående stce en sua av fordonets egentliga vinelhastighet, jordens rotation och transporthastigheten sat de sensorfel so rategrotriaden själv ger upphov till. Utsignalen från ett rategro innehåller de sensorfel so besrivs närare i avsnitt Den verliga vinelhastigheten salas alltså ed en salfator rg och därefter oer vitt brus ed edelvärde noll, η rg, och bias, γ rg, att adderas till signalen. Även orrelerat brus adderas till utsignalen en denna tp av fel odelleras inte i eaensarbetet. Utsignalen får då följande utseende Felet från grot blir då R I R ( rg + ) ωi + ηrg γrg ω ~ +. (3.4) δ ω ~ ω + η + γ. (3.5) R I rg R I rg rg Hittills har endast fel so uppstår vid användning av enstaa gron studerats. Enligt avsnitt 3..4 uppstår även fel då de tre rategrona so används inte är idealt placerade relativt varandra. Rategrona är placerade i ett oordinatsste R enligt bilaga B.7. Rategrotriaden är i sin tur placerad på en sensorplattfor ed oordinatssteet PF enligt bilaga B.6. Efterso antagandet att origo för oordinatssteet R och PF saanfaller ed origo för HASSI odelleras endast två vridningsatriser ellan dessa oordinatsste. Rategrona behöver enligt avsnitt 3..4 inte vara placerade ortogonalt och DM-sabandet ellan R P PF och R betecnar därför ed / edan betecnar DM-sabandet P ellan oordinatssteen HASSI och PF. Sabandet ellan R, PF och HASSI odelleras då enligt följande ω / R I R P P ω I. (3.6) Detta edför att utsignalen från rategrotriaden odelleras enligt följande 2

34 I P - R - R P ( ) ( / ) ( + ) / ω + γ η ) ω ~ + P rg P I rg rg. (3.7) I eaensarbetet antas doc grona vara ortogonalt placerade och / betecnar då en enhetsatris. 3.3 Acceleroetertriaden I det här avsnittet besrivs först hur fordonets acceleration odelleras. Därefter följer en besrivning av hur acceleroetertriaden odelleras sat hur utsignalen från denna oer att se ut Modellering av fordonets acceleration Efterso det i slutna sste inte går att silja på acceleration, a, och assattration, g, består utsignalen från acceleroetern, so allas specifi raft och betecnas f, av båda dessa. För att äta den verliga accelerationen åste därför gravitationen openseras bort från utsignalen. Den specifia raft so är utsignal från acceleroetertriaden är alltså en differens ellan fordonets egentliga acceleration och jordens gravitationsraft. R P f a g. (3.8) Jordens gravitation besrivs närare i avsnitt Modellering av acceleroetertriaden Utsignal från acceleroetertriaden är enligt föregående stce en differens ellan fordonets egentliga acceleration och jordens gravitationsraft sat de sensorfel so acceleroetertriaden själv ger upphov till. Utsignalen från en acceleroeter innehåller de sensorfel so besrivs närare i avsnitt Den verliga accelerationen salas alltså ed en salfator acc och därefter oer vitt brus ed edelvärde noll, η acc, och bias, γ acc, att adderas till signalen. Även orrelerat brus adderas till utsignalen en denna tp av fel odelleras inte i eaensarbetet. Utsignalen får då följande utseende ~ f Felet från acceleroetern blir då A A ( acc + ) f + ηacc + γacc. (3.9) ~ A A δ f accf + ηacc + γ. (3.) 22 acc

35 Hittills har endast fel so uppstår vid användning av enstaa acceleroetrar studerats. Enligt avsnitt 3..4 uppstår även fel då de tre acceleroetrarna so används inte är idealt placerade relativt varandra. Acceleroetrarna är placerade i ett oordinatsste A enligt bilaga B.8. Acceleroetertriaden är i sin tur placerad på en sensorplattfor ed oordinatssteet PF enligt bilaga B.6. Efterso antagandet att origo för oordinatssteet A och PF saanfaller ed origo för HASSI odelleras endast två vridningsatriser ellan dessa oordinatsste. Acceleroetrarna behöver enligt avsnitt 3..4 inte vara placerade ortogonalt och DM-sabandet ellan PF och A betecnas P därför ed / edan betecnar DM-sabandet ellan A P oordinatssteen HASSI och PF. Sabandet ellan A, PF och HASSI odelleras då enligt följande A f / A P P f. (3.) Detta edför att utsignalen från rategrotriaden odelleras enligt följande ~ P - A - A P ( ) ( / ) ( + ) / f + γ + η η ) f P acc P acc acc + accc. (3.2) I eaensarbetet antas doc acceleroetrarna vara ortogonalt placerade A och / betecnar då en enhetsatris. P 3.4 Magnetoetertriaden I det här avsnittet ges först en besrivning av agnetis deviation. Därefter följer en besrivning av hur agnetoetertriaden odelleras sat hur utsignalen från denna ser ut Magnetis deviation Efterso ursen beränas utifrån det jordagnetisa fältets oponenter är det av stor vit att dessa an deteteras utan störningar från närliggande agnetisa ällor. Ferroagnetisa aterial i närheten av sensorerna an alltså bidra till ett säre resultat vid beräningen av fordonets urs. När ett ferroagnetist aterial placeras i ett liforigt agnetist fält oer detta att störa det agnetisa fältet geno att fältet rös. Figur 3.2 nedan illustrerar detta. 23

36 Figur 3.2 [7], Ett ferroagnetist aterial stör ett liforigt agnetfält geno att fältet rös och däred förstärs. Det är enligt [6] huvudsaligen två tper av agnetis deviation so orsaar störningar i det agnetfält so agnetoetrarna deteterar, jujärns- och hårdjärnsdeviation. Hårdjärnsdeviation orsaas av peranenta agneter eller agnetiserat järn eller stål i närheten av sensorplattforen. Dessa störällor är onstanta och placerade på ett fit avstånd relativt agnetoetrarna oavsett ursvinel. Hårdjärnsdeviation orsaar ett fel i ursvineln ed avseende på fordonets urs i for av sinusurva ed en period. Detta visas nedan i figur 3.3. Kursfel ed avseende på urs, hårdjärnsdeviation Kursfel grader Kursfel ed avseende på urs, jujärnsdeviation Kursfel grader Kursvinel Figur 3.3 Kursfel ed avseende på fordonets urs på grund av hårdjärnsdeviation (längst upp) och jujärnsdeviation (längst ner). 24

37 Magnetoetrarnas sensorfel i for av bias lassificeras so hårdjärnsdeviation och orsaar däred ett ursfel i for av en sinusurva ed en period. Efterso fordonet själv består till stora delar av järn påverar även det agnetfältet i for av hårdjärnsdeviation. Mjujärnsdeviation orsaas av att det jordagnetisa fältet störs av ogivande jua aterial i närheten av agnetoetrarna. På saa sätt so hårdjärnsdeviation röer jujärnsdeviation det jordagnetisa fältet. Sillnaden är att jujärnsdeviation resulterar i ett ursfel ed avseende på fordonets urs so har foren av en sinusurva ed två perioder enligt figur 3.3 ovan. Magnetoetrarnas sensorfel i for av salfator lassificeras so jujärnsdeviation och orsaar däred ett ursfel i for av en sinusurva ed två perioder. Det finns egentligen ingen sarp gräns ellan hårdjärns- och jujärnsdeviation en i regel är hårdjärnsdeviationen betdligt större än jujärnsdeviationen. Hårdjärnsdeviationen är även lättare att orrigera och de vanligaste reoendationerna är enligt [8] att hårdjärnsdeviationen sa orrigeras och i övrigt sa jujärnsegensaperna försöa undvias efterso dessa inte är lia noralt föreoande. Det här eaensarbetet begränsas till att estiera biasfelet i agnetoetrarna efterso dessa sensorfel är en av de största ällorna till agnetis deviation enligt resoneanget ovan. Även geografisa störningar i det jordagnetisa fältet påverar beräningen av fordonets ursvinel. Eepel på denna tp av störningar är att på loal nivå an ströar och ferroagnetisa aterial i jordens antel ha cet stor inveran på det totala agnetfältet. I alria oråden är loala anoalier på 2-4 vanliga. Eepelvis är det totala agnetfältet över Kirunaalen 75 nt vilet är 23 nt över det norala fältet i traten på ungefär 52 nt enligt [8]. Ett annat eepel är att även solvinden an ha en stor inveran på det totala agnetfältet. Eletrisa strösste i jonosfären och agnetosfären är ett resultat av saveran ellan jordens priärfält och partielstrålningen från solen. Öad partielstrålning från solen an orsaa cet stora förändringar i jordens agnetfält. Denna tp av störningar i det jordagnetisa fältet är inget so tas hänsn till i eaensarbetet Modellering av agnetfält Magnetfältet odelleras endast ed den odell av jordens priära agnetfält so besrivs närare i avsnitt De ttre störällor so finns, till eepel loala förändringar i agnetfältet och fordonets egen inveran på agnetfältet, odelleras inte. 25

38 3.4.3 Modellering av agnetoetertriaden Utsignalen från agnetoetertriaden är enligt föregående stce en sua av jordens priära agnetfält sat de sensorfel so agnetoetertriaden själv ger upphov till. Utsignalen från en agnetoeter innehåller de sensorfel so besrivs närare i avsnitt Det verliga agnetfältet salas alltså ed en salfator agn och därefter adderas vitt brus ed edelvärde noll, η agn, och bias, γ agn, till signalen. Även orrelerat brus adderas till utsignalen en denna tp av fel odelleras inte i eaensarbetet. Utsignalen får då följande utseende Felet från agnetoetern blir då M ( agn + ) + ηagn γagn M ~ +. (3.3) M M δ agn + ηagn + ~ γ. (3.4) agn Hittills har endast fel so uppstår vid användning av enstaa agnetoetrar studerats. Enligt avsnitt 3..4 oer även fel att uppstå då de tre agnetoetrarna so används inte är idealt placerade relativt varandra. Magnetoetrarna är placerade i ett oordinatsste MAN enligt bilaga B.9. Magnetoetertriaden är i sin tur placerad på en sensorplattfor ed oordinatssteet PF enligt bilaga B.6. Efterso antagandet att origo för oordinatssteet MAN och PF saanfaller ed origo för HASSI odelleras endast två vridningsatriser ellan dessa oordinatsste. Magnetoetrarna behöver enligt avsnitt 3..4 inte vara placerade ortogonalt och M P DM-sabandet ellan PF och MAN betecnas därför ed / edan betecnar DM-sabandet ellan oordinatssteen HASSI och PF. Sabandet ellan MAN, PF och HASSI odelleras då enligt följande M / M P P. (3.5) Detta edför att utsignalen från agnetoetertriaden odelleras so ~ P - M - M P ( ) ( / ) ( + ) / + γ η ) P agn P agn + P agn. (3.6) I eaensarbetet antas doc agnetoetrarna vara ortogonalt placerade och / betecnar då en enhetsatris. M P 26

39 4 SENSORFUSION I det här apitlet härleds uttrc för den attitd so beränas ed utsignalerna från rategrotriaden sat den attitd so beränas ed utsignalerna från acceleroeter- och agnetoetertriaden. Därefter definieras attitdfel och felodeller för de båda beränade attitderna härleds och sätts saan till en slutgiltig tillståndsodell. I slutet av apitlet besrivs den allänna tillståndsforen sat alanfiltret och etended alanfilter. 4. Kapitelöversit Det här avsnittet ger en översit över de beräningar och uttrc so härleds i det här apitlet sat hur de bggs saan till en siuleringsodell enligt figur 4. nedan. rotriad Acctriad Magntriad ω ~ δωˆ I I + ~ f ~ Attitdberäning Attitdberäning r Attitdfel r Kalanfilter δωˆ I r Attitdorrigering Figur 4. Illustration av siuleringsodellen. 27

40 De sensorer so används, en rategrotriad, en acceleroetertriad sat en agnetoetertriad, vila signaler de äter, sat vila fel dessa ätningar innehåller besrivs i apitel 3. I avsnitt 4.2. härleds saband för att beräna fordonets attitd ed utdata från rategrotriaden. Fordonets attitd beränas även ed utdata från acceleroetertriaden sat agnetoetertriaden och dessa saband finns besrivna i avsnitt Därefter beränas sillnaden ellan dessa två attitder i avsnitt 4.3. Sillnaden ellan de båda beränade attitderna observeras av ett alanfilter so estierar grobias och agnetoeterbias. Tillståndsodellen för detta alanfilter finns besriven i avsnitt I avsnitt 4.5 återfinns teori för alanfiltret sat etended alanfilter. För att undvia grodrift återopplas enligt avsnitt den av alanfiltret estierade grobiasen. Efter denna opensering innehåller fordonets urs-, roll och tippvinel fortfarande stationärfel. För att även undvia dessa fel orrigeras fordonets attitd efter alanfiltreringen enligt avsnitt Attitdberäningar I det här avsnittet återfinns de beräningar so gjorts på utsignalen från rategrotriaden sat beräningarna so gjorts på utsignalerna från acceleroeter- och agnetoetertriaden Rategrotriadens attitdberäningar Efterso det är fordonets attitd,, so sa beränas an följande differentialevation lösas & ω ω I + ω I ω ω I I ω I ω I (4.) ω I ω IE + ω E ω I E ω E IE 28

41 29 ω E där betecnar transporthastigheten och approieras ed noll enligt avsnitt Sevoperatorn betecnas ed och definieras i bilaga.. I saa bilaga definieras även differentialevationen. För att entdigt lösa denna differentialevation behövs ett initialvärde på. Detta värde hätas från beräningarna på acceleroeter- och agnetoetertriadens ätningar. Fordonets initiala attitd an enligt [8] estieras ed hjälp av följande tre saband ( ) g g g g ~ ~ ~ ~ ( ) ( ) ( ) ( ) ( ) ( ). ~ ~ ~ ~ T T T T T T g g g g (4.2) Dessa saband an srivas på atrisfor och an därefter lösas ut och beränas enligt följande ( ) ( ) ( ) ( ) ( ) ( ) ~ ~ ~ ~ T T T T T T g g g g ( ) ( ) ( ) ( ) ( ) ( ) T T T T T T ~ ~ ~ ~ g g g g (4.3) ravitation deteterad av acceleroetertriaden: z z f f f g g g ~ g g ravitationsvetorn enligt vald odell: z g g g g

42 Magnetfältsvetor deteterad av agnetoetertriaden: ~ z Magnetfältsvetor enligt vald odell:. z Efterso atrisen inte an garanteras vara ortogonal på grund av sensorfel och odellfel, an en ortogonaliserande algorit appliceras på den fraränade atrisen. Denna algorit an behöva upprepas några gånger o inte den ursprungliga atrisen är väldigt nära en ortogonal atris. ut T ( I in in ) in in + (4.4) 2 För ett stillastående fordon fungerar denna etod för att estiera fordonets attitd bra efterso den enda raft so verar på fordonet är gravitationsraften. Det går då att jäföra den specifia raft so är utsignal från acceleroetertriaden ed gravitationsvetorn enligt vald gravitationsodell. När fordonet färdas är fordonets egen acceleration en del av acceleroetertriadens utsignal och en säre estiering av attitden erhålls. Även loala anoalier i agnetfältet och det agnetfält so fordonet själv ger upphov till bidrar till en säre attitdestiering efterso dessa inte ingår i odellen av jordens agnetfält. Denna etod för att beräna fordonets attitd används därför endast so ett initialvärde till den givna differentialevationen (4.) Acceleroeter- och agnetoetertriadens attitdberäningar För att beräna fordonets attitd ed acceleroetertriaden och agnetoetertriaden beränas fordonets roll- och tippvinlar ed acceleroetertriadens ätningar och ursvineln ed agnetoetertriadens ätningar. Enligt [5] an detta göras enligt nedanstående beräningar. Utsignalerna från acceleroetertriaden definieras so 3

43 f f f z gsinθ g g z z cos θsinφ cos θcosφ. (4.5) Fordonets roll- och tippvinel an då beränas enligt följande f φ arctan fz (4.6) f θ arcsin. gz (4.7) För att beräna fordonets ursvinel används agnetoetertriadens ätningar. För att ursvineln sa unna beränas på detta sätt åste både roll- och tippvineln vara ända och i det här fallet hätas dessa vinlar från acceleroetertriadens roll- och tippberäningar. För att beräna ursvineln används följande saband där H och H beränas enligt H ψ arctan (4.8) H H cosθ H H z sinθ sinθ cosφ cosθ sinφ sinφ cosφ z cosθ + sinθ sinφ + zsinθ cosφ cosφ z sinφ. sinθ cosθ sin zcosθ cos + φ + φ (4.9) Efterso agnetist och geografist nord inte saanfaller och däred har olia ritningar används teren delination för att betecna sillnaden ellan geografis och agnetis nordritning. O delinationen är änd på den plats fordonet befinner sig an opassritningen därför enelt orrigeras för att den estierade ursvineln sa oa närare den sanna. 3

44 Kurs Kopassurs + Magnetis delination (4.) När roll-, tipp- och ursvinlarna beränats enligt ovan besrivna evationer an fordonets attitd beränas ed hjälp av DM-sabandet givet i appendi B Definition av attitdfel Att estiera attitden för ett fordon innebär att bestäa sabandet från oordinatssteet HASSI till oordinatssteet EO utgående från HASSI, under antagandet att positionsfelet är noll. Detta innebär att HASSI betratas so absolut referens och det estierade sabandet till EO definieras av ett saband ellan HASSI och ett ntt oordinatsste X-EO. Figur 4.2 illustrerar detta saband. EO HASSI X-EO Figur 4.2 Definition av attitdfel ellan oordinatssteen EO och X-EO. Detta saband an srivas ateatist enligt följande (4.) där de ingående atriserna benäns och relateras i terer av attitdeulervinlar enligt 32