En simuleringsmiljö för distribuerad navigering

Transkript

1 En simuleingsmiljö fö distibuead navigeing Eamensabete utföt i Regleteni och Kommuniationssystem av Ricad Fänemyh LITH-ISY-EX-386- Linöping

2

3 En simuleingsmiljö fö distibuead navigeing Eamensabete utföt i Regleteni och Kommuniationssystem vid Linöpings tenisa högsola av Ricad Fänemyh LITH-ISY-EX-386- Handledae: Eaminato: Hans Bohlin Pe-Johan Nodlund Fedi Gustafsson Linöping, 6 maj

4

5 Avdelning, Institution Division, Depatment Datum -5-3 Institutionen fö Systemteni LINKÖPING Spå Language X Svensa/Swedish Engelsa/English Rappottyp Repot categoy Licentiatavhandling X Eamensabete C-uppsats D-uppsats Övig appot ISBN LITH-ISY-EX-386- Seietitel och seienumme Title of seies, numbeing URL fö eletonis vesion Titel Title Föfattae Autho En simuleingsmiljö fö distibuead navigeing A simulation envionment fo distibuted navigation Ricad Fänemyh Abstact This maste thesis studies distibuted navigation which is a function implemented in a futue netwo based combat infomation system to impove the accuacy in navigation fo combat vehicles in a mechanized battalion, above all in the event of loss of GPS. In the event of loss of the GPS the vehicles obtain dead econing pefomance though the bacup system that consists of an odomete and a magnetic compass. Dead econing means a dift in the position that maes the accuacy in the navigation wose. The distibuted navigation function uses position and navigation data with measuements between the vehicles to estimate the eos and uncetainties in positions, which ae used to impove the accuacy in position fo the vehicles. To investigate and demonstate distibuted navigation, a simulation envionment has been poduced in Matlab. The envionment is geneal so diffeent navigation systems can be used and studied and also dynamical so a futhe development is possible. The simulation envionment has been used to investigate and evaluate an implementation of distibuted navigation. The implementation has been made using a cental filte whee fusion taes place of all navigation data and measuements. This filte has been ealized with help of Kalman filte theoy, in which all vehicles ae put togethe in a state space model. Simulations have been pefomed fo diffeent scenaios and the esult of these show that the dift in position is avoided. Keywod Navigation, Estimation, Kalman filte

6

7 Sammanfattning Eamensabetet studea distibuead navigeing som ä en funtion implementead i ett famtida nätvesbaseat stidsledningssystem fö att föbätta noggannheten i navigeing hos stidsfodon i en meanisead bataljon, famföallt vid händelse av ett GPS-botfall. Vid ett eventuellt GPS-botfall ehålle fodonen dödäningspestanda genom det esevsystem som bestå av en odomete och magnetompass. Dödäning innebä en positionsdift som fösäma noggannheten i navigeingen. Funtionen distibuead navigeing använde positions- och navigeingsdata samt inmätningsdata mellan fodonen fö att estimea positionsfel och positionsosäehete som sedan används fö att föbätta noggannheten i fodonens position. Fö att undesöa och demonstea distibuead navigeing ha en simuleingsmiljö tagits fam i Matlab. Miljön ä geneell så att olia navigeingssystem sa unna användas och studeas, och även dynamis så att en vidaeutvecling av miljön sa vaa möjlig. Simuleingsmiljön ha använts fö att undesöa och utvädea en implementeing av distibuead navigeing. Implementeingen ha gjots med användandet av ett centalt filte dä fusion se av alla navigeings- och inmätningsdata. Detta filte ha ealiseats med hjälp av almanfilteteoi, dä alla fodon ha sammanfogats i en tillståndsmodell. Simuleinga ha gjots fö olia scenaie och esultatet av dessa visa att positionsdiften undvis. Nycelod: Navigeing, Estimeing, Kalmanfilte

8 Abstact This maste thesis studies distibuted navigation which is a function implemented in a futue netwo based combat infomation system to impove the accuacy in navigation fo combat vehicles in a mechanized battalion, above all in the event of loss of GPS. In the event of loss of the GPS the vehicles obtain dead econing pefomance though the bacup system that consists of an odomete and a magnetic compass. Dead econing means a dift in the position that maes the accuacy in the navigation wose. The distibuted navigation function uses position and navigation data with measuements between the vehicles to estimate the eos and uncetainties in positions, which ae used to impove the accuacy in position fo the vehicles. To investigate and demonstate distibuted navigation, a simulation envionment has been poduced in Matlab. The envionment is geneal so diffeent navigation systems can be used and studied and also dynamical so a futhe development is possible. The simulation envionment has been used to investigate and evaluate an implementation of distibuted navigation. The implementation has been made using a cental filte whee fusion taes place of all navigation data and measuements. This filte has been ealized with help of Kalman filte theoy, in which all vehicles ae put togethe in a state space model. Simulations have been pefomed fo diffeent scenaios and the esult of these show that the dift in position is avoided. Keywods: Navigation, Estimation, Kalman filte

9 Föod Denna appot utgö den siftliga edovisningen av mitt eamensabetet på Civilingenjösutbildningen Tenis fysi och Eletoteni vid Linöpings Tenisa Högsola. Detta eamensabete ha utföts hos AeotechTelub. AeotechTelub ingå i Saab-guppen och ebjude valificeade tenisa tjänste, undehåll och undanpassade systemlösninga inom infomationsteni, eletoni och faostteni till totalfösvaet, ve och myndighete samt valda nische inom näingslivet. Antal anställda ä ungefä 6 och finns på otena Aboga, Linöping, Väjö, Stocholm och ytteligae på ote i Sveige samt i Danma, Finland och Tysland. Me infomation finns på Jag vill hä taca min handledae på AeotechTelub AB, Hans Bohlin och min handledae vid Linöpings Tenisa Högsola, Pe-Johan Nodlund fö den hjälp jag ha fått unde tiden jag ha utföt mitt eamensabete. Ricad Fänemyh, -5-6

10

11 Innehållsfötecning Inledning. Bagund.... Uppgift....3 Rappotens uppläggning... Navigeing. Allmänt.... Navigeingssystem....3 Val av navigeingssystem Allmänt Autonomt / Ice-autonomt Passivt / Ativt Utnyttjande av integitetsövevaning Hög / Låg uppdateingshastighet Hög / Låg dynami Robusthet Kostnad Eempel på navigeingssystem Övesit Töghetsnavigeingssystem Satellitnavigeingssystem Stjännavigeingssystem Teängnavigeingssystem Hybidnavigeingssystem Distibuead navigeing 8 3. Idé Användningsomåde och syfte Användningsomåde Föbättad navigeing Inmätninga Allmänt Acceptanstest En implementeing av Distibuead navigeing Eempel på navigeingsuppdateing hos ett fodon... 4 Simuleingsmiljö 3 4. Allmänt Föenlinga Modulen Scenaio Modulen Tajectoy Modulen Navigeingssystem Meaniseingsmodell Felmodell Odomete Magnetompass Slutlig felmodell... 6

12 4.5.6 Felbudget Modulen Opeatö Modulen Site Allmänt Felmodelle Felbudgeta Modulen Nätve Modulen Filte Modulen Uppdateing Filte fö distibuead navigeing 5. Övesit Tidsuppdateingsmodell Tillståndseduceing Mätuppdateingsmodell Tillståndsmodell Ej implementeade mätuppdateingsmodelle Flödesdiagam Simuleinga 3 6. Allmänt Studeade felmått Scenaie Sammanfattning av undesöningen Simuleinga Scenaio Övesit Estimeing av position Estimeing av salfato och ompassfel Osäehete Simuleingsesultat Scenaio A Övesit Estimeing av position Estimeing av salfato och ompassfel Osäehete Simuleingsesultat Scenaio B Övesit Estimeing av position Estimeing av salfato och ompassfel Simuleingsesultat Scenaio Övesit Estimeing av position Estimeing av salfato och ompassfel Osäehete Simuleingsesultat Sammanfattning av simuleingsesultat... 5

13 7 Slutsatse 5 7. Simuleingsmiljön Implementeingen av distibuead navigeing Resultat Utvädeing av esultatet Föslag till fotsatt abete Refeense 53 Appendi A Kalmanfilteing 54 A. Tillståndsmodell A. Kalmanfiltet A.3 Shaping filte Appendi B Nomenlatu 57 B. Stohete B. Stoastisa stohete B.3 Övig notation... 58

14 Inledning. Bagund Vaje milität stidsfodon ha ett navigeingssystem. Dessa system ä av olia noggannhet, allt fån enla dödänande system och Global Positioning System GPS till högvalitativa töghetsnavigeingssystem TNS. Det ultimata voe att vaje fodon va utustat med ett sådant högvalitativt TNS, men på gund av bl.a. höga ostnade omöjliggös detta. På gund av den höga ostnaden an alltså inte alla stidsfodonen i en meanisead bataljon vaa utustat med ett TNS, utan måste fölita sig på ett enelt dödänande system och GPS. Poblemet ä doc att GPS ä elativt lätt att stöa ut och man ä beoende av ytte påvebaa ällo vilet gö att man inte bö fölita sig på GPS som pimät navigeingssystem. GPS ä alltså inte obust och autonomt, vilet ä önsvät i militäa appliatione. Då lösningen med GPS ha nacdela vill man hitta en annan. En sådan lösning ä distibuead navigeing.. Uppgift Att utabeta en simuleingsmiljö fö demonstation och undesöning av distibuead navigeing. Simuleingsmiljön sa vaa geneell så att olia navigeingssystem sa unna användas och studeas. Den sa även vaa dynamis så att en vidaeutvecling av miljön ä möjlig. Simuleingsmiljön sa sivas i Matlab. Även utabeta ett pogam/filte dä en implementeing av distibuead navigeing se och utvädea pogammet/filtet med simuleinga..3 Rappotens uppläggning Efte en inledande besivning av navigeing och val av olia navigeingssystem i apitel gös en genomgång av distibuead navigeing i apitel 3. Dä finns även ett föslag på en implementeing av distibuead navigeing. I apitel 4 besivs simuleingsmiljön med tillhöande module och häledninga av de felmodelle som används av simuleingspogammet. Häledningen och modellbygget av det filte som föslaget på en implementeing innebä finns i apitel 5. Dä finns även abetsgången fö filtet besivet. I apitel 6 undesös och utvädeas filtet enligt angivna felmått. I slutet av det apitlet finns en sammanfattning av simuleingsesultaten. Slutsatsena som ä dagna i detta eamensabete finns i apitel 7. Appendi A besivs otfattat den almanfilteteoi som används. Appendi B innehålle nomenlatuen.

15 Navigeing. Allmänt Navigeing innebä unsapen om vat man befinne och hu man ö sig elativt något utvalt efeenssystem. Denna unsap innefattas i olia navigeingsstohete som position och höjd hastighet acceleation attityd attitydvinelhastighet Det finns även anda stohete, men det ä dessa som ä av pimät intesse. Själva odet navigeing elle då navigation omme fån det latinsa odet navigato, som betyde sjöesa, men i dag ha odet en vidae betydelse. De olia navigeingsstohetena måste anges elativt ett efeenssystem fö att man entydigt sa unna tola stohetena. Vid navigeing av en faost på joden så används nomalt joden som efeens. Detta allas fö teestial navigeing. Vid navigeing av ymdfaoste ha man istället fö joden solsystemet, anda planete etc. som efeens. Denna typ av navigeing allas celestial navigeing. En fullständig besivning av en faosts position och öelse vid en fi tidpunt elativt en specificead efeens sammanfattas ibland med begeppet faostens inematisa tillstånd. De ovan angivna navigeingsstohetena vid en given tidpunt utgö en del av faostens inematisa tillstånd. Med detta i batane an en definition av navigeing fomuleas Definition Navigeing ä en funtion som mäte en faosts öelse och beäna en del av en faosts inematisa tillstånd. Ett näa beslätat begepp till navigeing ä guidance, som använde esultatet fån navigeing. Guidance innebä att man fösöe hitta till en angiven destination/mål och även denna unsap innefattas i olia stohete som avstånd till destination/mål itning till destination/mål hastighet elativt destination/mål Infomationen om navigeing och navigeingssystemen ä hämtad fån [3].. Navigeingssystem Ett navigeingssystem ha till uppgift att ontinueligt geneea data om faostens läge och position, dvs. navigeingsstohetena. Följande definition fö navigeingssystemet besive dess funtionalitet Definition Ett navigationssystem ä en appaat som mäte och beäna en del av en faosts navigationsstohete elateade till en efeens. Fö att unna beäna navigationsstohetena måste navigationssystemet få tillgång till någon typ av infomation om faostens läge och/elle öelse elativt någon efeens. Infomationen finns i den utdata som geneas fån navigeingssensoena. Dessa efeense olia sensoe

16 an ha olia efeense ä oftast inte den efeens som navigationsbeäningana se i. Men med hjälp av modelle mellan olia efeense an onveteinga göas och om efeensena ö sig i föhållande till vaanda måste navigationssystemet ontinueligt göa beäninga av sambandet mellan efeensena detta allas hä fö instumenteing fö att sedan göa onveteingen. Den efeens som navigationssystemet utfö sina beäninga i ä oftast inte helle den efeens som navigeingsstohetena sa pesenteas i. Då ävs det ytteligae beäninga och en onveteing innan pesentationen an se. Nedan i figu. illusteas detta. Navigeingssystem Nav. ådata Navigationspesentation Navigationsdato Navigationsstohete Navigeingssensoe.3 Val av navigeingssystem Figu.. Blocschema vid navigeing..3. Allmänt I apitel. nämns poblem och egensape hos navigeingssystem som spela in nä man sa utusta ett milität stidsfodon med ett navigeingssystem. Hä gös en me systematisead genomgång av egensape och aatäisti som bö beatas vid valet av navigationssystem fö ett visst fodon elle appliation. Egensapena och aatäisti ä följande Autonomt / Ice-autonomt Passivt / Ativt Utnyttjande av integitetsövevaning Hög / Låg uppdateingshastighet Hög / Låg dynami Robusthet Kostnad inöp, installation och undehåll Det finns olia navigationspincipe olia navigationssystem an lassificeas av. Beoende på pincip ehålls vissa egensape och aatäisti enligt ovan. Navigeingspincipen ä den metod med vilen navigationssystemet beäna navigationsstohetena. Man silje mellan te olia pincipe, dödäning, fipuntsuppdateing och guidance om guidance sa änas som en navigeingspincip elle inte beo på hu man se på det, i vilet fall som helst ä det näa beslätat med navigeing. Dödäning ha sitt histoisa uspung fån navigeing av fatyg. Med ompass och logg som sensoe mättes us och fat. Logg ä en senso som mäte fatygets fat elativt vattnet. Med ato som innehålle infomation om havsstömma ompenseades obsevationen fån loggen så att man få faten elativt joden. Men hjälp av dessa data an positionen sedan integeas fam. Själva pincipen gå alltså ut på att fån givna data integea, en elle flea gånge beoende på vilen utdata man ha, fam positionen. I det tidsdiseta fallet bli det en summation. 3

17 Fipuntsuppdateing innebä att man genom inmätninga mot ända objet uppdatea sin position. Dessa objet an vaa av olia slag, allt fån satellite, stjäno och ända objet i natuen. Den enlaste fomen av fipuntsnavigeing ä navigeing med hjälp av ato. Guidance besivs otfattat i apitel.. Vät att påpea ä att inget navigeingssystem ä pefet och det finns även inga pefeta navigeingssensoe. Detta medfö att fel uppstå vid navigeingsbeäninga. Man få helt enelt betata de navigeingsstohete som navigeingssystemet ge ifån sig som sattninga elle estimat. Detta innebä att navigeingsstohetena ä behäftade med ett fel som ä oänt. Det finns i pincip te olia felällo i navigeingsbeäningana.. Felatighete i efeensmodellena. Detta medfö att instumenteingen bli felatig.. Sensofel. Sensoena innehålle fel som påvea navigeingsbeäningana. Det vanligaste felet ä mätbus, som alltid finns nävaade vid mätninga. 3. Numeisa fel. Vid navigationsbeäningana måste sensosignalena samplas och disetiseas vilet medfö felatighete. De numeisa felen ä oftast fösumbaa. Den unsap man an ha om felen ä deas aatäisti. Detta beo på vilen navigeingspincip som utnyttjas och vilet navigeingssystem som används och vila navigeingsstohete som betatas. Te. så aatäiseas felet i ett dödänande system, t.e. TNS, av att positionsfelet väe med öd stäca till sillnad fån ett fipuntsuppdateande system, t.e. GPS, som ä begänsat men busigt. Dessa två navigeingssystem besivs i apitel Autonomt / Ice-autonomt Ett navigeingssystem aatäiseas som autonomt om dess navigeingssensoe ä obeoende och botopplade fån ytte påvebaa ällo. Detta innebä att navigeingssystemet ej an stöas ut elle på annat sätt luas genom ytte påvean. Eempel på ytte ällos påvean ä telesignale som stö sensoe sändae i ett navigeingssystem. I militäa appliatione ä ett autonomt navigeingssystem att föeda..3.3 Passivt / Ativt Ett navigeingssystem aatäiseas som passivt om det ej sände ut signale elle på annat sätt ommunicea med omvälden. I militäa appliatione ä ett passivt navigeingssystem att föeda då isen finns att ett ativt navigeingssystem an öja en faost nävao elle än väe pejlas in..3.4 Utnyttjande av integitetsövevaning Integitetsövevaning innebä att navigeingssystemet an upptäca fel i navigeingssensoena elle i utdata. Detta an ealiseas på olia sätt. Ett enelt sätt ä att bedöma imligheten i utdata. Ett vanligae och bätte sätt ä att om möjligt utnyttja edundant infomation i navigeingssystemet. Den edundanta infomationen an jämföas inbödes med vaanda fö att på så sätt upptäca oimlighete. Fö att detta sa fungea måste natuligtvis tillgång till edundant infomation finnas. 4

18 .3.5 Hög / Låg uppdateingshastighet Hu ofta navigeingssystemet ge ifån sig utdata aatäisea ocså navigeingssystemet. Uppdateingshastigheten som ävs silje sig åt beoende på i vilen appliation navigeingssystem används. Ett näliggande poblem ä eventuella tidsfödöjninga i utdata fån navigeingssystemet. Hu stot detta poblem ä beo på hu fot fodonet ö sig. Detta an i vissa appliatione educeas något genom att tidsstämpla utdata..3.6 Hög / Låg dynami Ett poblem som an uppstå i ett navigeingssystem ä eftesläpning i utdata. Det ä opplat till navigeingssystemets dynami. Med dynami menas hä navigeingssystemets fömåga att följa med i aftiga manöve som faosten utfö. Ha navigeingssystemet hög dynami följe navigeingssystemet snabbt med och en eftesläpning undvis och tvätom annas. Det finns även en onflit mellan om utdata fån navigeingssystemet ha eftesläpning elle ä busigt. Om man vill minsa på buset an man lågpassfiltea utdata, men detta ge bieffeten att man få me eftesläpning. Detta an accepteas om navigeingssystemet ha hög dynami..3.7 Robusthet Ett navigeingssystems fömåga att motstå medvetna elle omedvetna stöninga benämns med obusthet. Detta begepp ä näa opplat till om navigeingssystemet ä autonomt elle inte. Ä navigeingssystemet autonomt minsa isen fö att stöninga an påvea navigeingssystemet. Finns det edundant infomation i navigeingssystemet öa givetvis obustheten, eftesom vissa navigeingssensoe an stöas ut, men navigeingssystemet fungea ändå, visseligen med säme noggannhet..3.8 Kostnad Även om ett navigeingssystem ha hög pestanda enligt ovanstående egensape så spela även ostnaden in vid valet av navigeingssystem, ju bätte navigeingssystem desto höge ostnad. I ostnaden innefattas dela som inöp, installation och undehåll..4 Eempel på navigeingssystem.4. Övesit Hä besivs någa olia navigeingssystem med avseende på ovanstående egensape och även på vila olia sensoefeense, efeensmodelle och instumenteinga som används..4. Töghetsnavigeingssystem Sensoena i ett töghetsnavigeingssystem TNS mäte acceleation och attitydvinelhastighet elativt töghetsymden. Dessa måste sedan onveteas till jodens efeens vid teestialnavigeing. Vid beäning av hastighet och position utnyttjas sedan dödäning. Ett TNS ha följande egensape och aatäisti: Autonomt Passivt 5

19 Hög uppdateingshastighet Hög dynami Ba obusthet Hög ostnad Att TNS ä autonomt och passivt gö att det lämpa sig ba fö militäa appliatione..4.3 Satellitnavigeingssystem Det mest ända satellitnavigeingssystemet ä Global Positioning System GPS. GPS använde sig av ice geostationäa satellite. Navigeingssensoena GPS antennen mäte avstånd och elativ hastighet till en satellit. Detta innebä att navigeingssenson ha satellitens bana som efeens. Hä måste en instumenteing göas mellan satelliten och joden fö att sedan göa en onveteing. Flea satellite används samtidigt. GPS använde fipuntsuppdateing som navigeingspincip. Ett GPS ha följande egensape och aatäisti: Ice-autonomt Passivt Låg uppdateingshastighet Relativt låg dynami Dålig obusthet Låg ostnad.4.4 Stjännavigeingssystem Ett stjännavigeingssystem använde stjänona fö att beäna navigationsstohetena. De navigationssensoe som används ä videoamea med bildbehandling. En gammal senso ä annas den lassisa setanten. Ett stjännavigeingssystem ha följande egensape och aatäisti: Autonomt Passivt Ett eempel på va ett stjännavigeingssystem används ä i det militäa flygplanet B fån USA..4.5 Teängnavigeingssystem Ett teängnavigeingssystem utnyttja fipuntsuppdateing som navigationspincip, dä inmätninga gös mot ända objet i teängen. I modena teängnavigeingssystem används topogafis adahöjdmätae och baomete som navigeingssensoe. Höjddata jämfös sedan med en topogafis databas dä oelationsanalys används som metod fö att avgöa va man ä. Ett teängnavigeingssystem av ovan besivna typ ha följande egensape och aatäisti: Ativt Ett eempel på ett teängnavigeingssystem ä SAABs TERNAV system. 6

20 .4.6 Hybidnavigeingssystem Alla olia navigeingssystem ha sina födela och nacdela. Idén med ett hybidnavigeingssystem ä att utnyttja det bästa fån de olia navigeingssystemen. Man använde alltså två elle i pincip hu många man vill navigeingssystem som omplettea vaanda. Det vanligaste ä att använda TNS och GPS. De båda systemen omplettea vaanda ba genom att det ena ha sina födela dä den anda ha sina nacdela. En ytteligae födel ä att man få tillgång till edundant infomation. Då an ett system fö integitetsövevaning användas. De öviga egensape och aatäisti ett hybidnavigeingssystem ha beo på vila navigeingssystem man föena. 7

21 3 Distibuead navigeing 3. Idé Distibuead navigeing ä en funtion implementead i ett famtida nätvesbaseat stidsledningssystem, fämst tänt fö stidsfodon i en meanisead bataljon elle fö stidsbåta i en amfibiebataljon. Funtionen använde följande infomation som finns elle bö finnas i det famtida nätvesbaseade stidsledningssystemet: positions- och navigeingsdata igenännings- och identifieingsdata lase- elle adiobasead IK mål och inmätningsdata fån siten och adiobasead avståndsmätninga All den hä infomationen används i ett filte fö att estimea positionsfel och positionsosäehete fö att sedan utnyttjas fö att föbätta fodonens position. 3. Användningsomåde och syfte 3.. Användningsomåde Användningsomåden fö distibuead navigeing an se fämst i meaniseade bataljone och amfibiebataljone. I detta eamensabete ligge tyngdpunten på en meanisead bataljon. 3.. Föbättad navigeing De fodon som ingå i en meanisead bataljon använde idag fämst GPS och ett dödänande navigeingssystem som esev fö att beäna position. Det dödänande systemet bestå av en odomete och en magnetompass. Se apitel 4.5 fö hu detta dödänande system fungea. Då GPS ä elativt enelt att stöa ut och att man ä beoende av ytte påvebaa ällo så bö man inte fölita sig på GPS som pimät navigeingssystem. GPS ä alltså inte obust och autonomt, vilet ä önsvät i militäa appliatione. Det esevsystem som då finns tillgängligt vid händelse av GPS-botfall ä det dödänande systemet. Poblemet med detta system ä att positionsfelet väe med öd stäca stolesodning 3 m pe öd ilomete. Ett altenativ till det dödänade systemet med odomete och magnetompass ä ett högpesteande TNS. Visseligen finns även hä en positionsdift, men den ä betydligt minde stolesodning m pe öd ilomete. Poblemet ä doc den höga ostnad det innebä att utusta vaje fodon i en meanisead bataljon med ett högpesteande TNS. Men används distibuead navigeing som hjälpsystem till det dödänade systemet med odomete och magnetompass omme navigeingspestanda att föbättas. Används dessa två tillsammans få man ett hybidnavigeingssystem som utnyttja två olia navigeingspincipe, dödäning och en sots elativ fipuntsuppdateing. Det elativa innebä en fipuntsuppdateing mot ett fodon med en bätte positionssäehet. Genom inmätningana mellan fodonen i en meanisead bataljon födelas den bätte navigeingspestanda som vissa fodon ha ut till alla. Detta föutsätte givetvis att det finns fodon med bätte navigeingssystem, som t.e. högpesteande TNS, i bataljonen. Hu mycet navigeingspestanda omme att öas beo på möjligheten och benägenheten hos fodonen att göa inmätninga mot vaanda. De estimeade positionsfelen som filtet poducea sänds ut öve nätveet till alla ingående fodon som då an oigea sin egen position. 8

22 3.3 Inmätninga 3.3. Allmänt En av föutsättningana fö att distibuead navigeing sa fungea ä att inmätninga mellan fodonen se. Man an silja mellan te olia type av inmätninga samtidig bäings- och avståndsinmätning avståndsinmätning bäingsinmätning Natuligtvis ä det bäst nä det se en samtidig bäings- och avståndsinmätning beoende på att positionsosäeheten fö ett fodon minsa i alla itninga se me hu det fungea i apitel 3.5 och tillhöande figue Acceptanstest Innan det centala filtet få ta del av en inmätning mellan två fodon måste ett acceptanstest utföas. Detta fö att inte filtet sa luas med falsa data. Testet bestå av två olia av som måste vaa uppfyllda.. LoS Line of Sight av. Genom att i en digital 3D-ata testa om det finns LoS mellan de två ingående fodonen i mätningen an en inmätning accepteas.. Med hjälp av de oigeade positionena fö de två ingående fodonen an imligheten i inmätningsdata bedömas. Vid maant avvielse an en inmätning disvalificeas. 3.4 En implementeing av Distibuead navigeing En implementeing av funtionen distibuead navigeing i ett stidsledningssystem an göas dels decentaliseat, dels centaliseat. Används ett centaliseat filte placeas det i ett fodon som innehålle navigeingscentalen, men det an även placeas edundant i alla fodon i händelse att fodonet med navigeingscentalen falle bot. I det hä eamensabetet studeas ett centalt filte dä fusion se av alla navigeings- och inmätningsdata. Inmätningsdata måste doc genomgå ett acceptanstest innan filtet få ta del av det, se me i apitel Resultatet av denna fusion sa vaa estimat av positionsfel och positionsosäehete av alla ingående fodon. Vid en implementeing an olia metode användas, t.e. med användning av linjä egession almanfilte Det fösta föslaget ä olat om det fungea och ha ej behandlats i detta eamensabete.vad och hu ett almanfilte fungea besivs i appendi A. I almanfiltet finns alla fodon med ett lågpesteande navigationssystem epesenteade. Fodonen med ett högpesteande navigeingssystem TNS antas vaa ideala och finns inte epesenteade. Detta antagande gälle fö implementeingen av funtionen distibuead navigeing i detta eamensabete. Detta antagande medfö att de estimat som filtet poducea ä felatiga, då de ä elativt de högpesteande navigeingssystemen TNS. Hu ba estimaten ä beo på hu stoa fel de högpesteande navigeingssystemen TNS ha. 9

23 De fodon som ha ett högpesteande navigeingssystem TNS ingå alltså inte i almanfiltet och medfö ocså att mätmatisen påveas. Vid en mätning dä ett sådant fodon ingå, bli mätningen en eat mätning, se me i apitel 5.4. Ett val man ocså gö ä huuvida de estimeinga som almanfiltet poducea sa vaa åteopplade in i de lågpesteande navigeingssystemen. Poblem an uppstå med instabilitet, då metoden äve mycet ba estimat. Denna metod gå unde namnet closed loop. Denna metod ha doc inte valts utan i den valda implementeingen se oigeingana med hjälp av estimaten fån almanfiltet på utdata fån navigeingssystemen. Navigeingssystemen själva påveas alltså inte av oigeingana. Denna metod gå unde namnet open loop. Vid en decentalisead implementeing av funtionen distibuead navigeing an almanfilte även användas. Då an ett almanfilte användas i vaje fodon och ett infomationsutbyte se mellan dessa almanfilte. Poblem an uppstå då man använde denna teni. Det som an se ä att man använde infomation flea gånge så att en fals föbätting se. Poblemet an eventuellt undvias med en vaiant av almanfiltet, ett så allat infomationsfilte se [4] fö me om infomationsfilte. Detta ha ej undesöts vidae. Fö att almanfiltet sa unna appliceas ävs modelle av fodonens navigeingssystem. Även olia modelle fö hu en inmätning sa behandlas av almanfiltet. Nä ett fodon vill deltaga i systemet fö distibuead navigeing måste almanfiltet få unsap om vilet navigeingssystem fodonet ha och vilen noggannhet det ha fö att ätt modell sa unna användas. Även infomation om siten och om annan inmätningsutustning måste tillföas almanfiltet fö att de ätta modellena fö inmätningana används. Famtagning av modelle av fodonens navigeingssystem och av olia modelle av inmätninga och även hu almanfiltet fungea besivs i apitel Eempel på navigeingsuppdateing hos ett fodon Nedan följe ett litet onstueat scenaio med te fodon fö att visa hu pincipen med distibuead navigeing fungea. Olia type av inmätninga används fö att visa hu positionen föbättas beoende på typen av inmätning. Steg. Figu 3. visa te fodon med olia navigeingssystem. Två olia type av inmätninga se och ett infomationsutbyte se öve ett nätve. Steg. Mellan steg och poducea det centala filtet i navigeingscentalen positionsoigeinga och nya positionsosäehete. Figu 3. visa de nya positionena och positionsosäehetena efte uppdateingen. Steg 3. Mellan steg och 3 ö sig fodonen och positionsosäehetena öa. Figu 3.3 visa hu en ny bäingsinmätning mellan fodon och 3 se vid steg 3 och ett nytt infomationsutbyte öve nätveet. Steg 4. Nu ha det centala filtet på nytt estimeat fam positione och positionsosäehete och en uppdateing ha sett, se figu 3.4. Nu ha positionen och positionsosäeheten fö fodon 3 föbättats i alla itninga genom en seie olia inmätninga mellan fodonen i bataljonen. Även positionen fö fodon påveas men visas inte. Figuseien visa ocså vilen position navigeingssystemen estimea, dessa ä doc inte så ba men föbättas genom inmätningana.

24 Bäingsinmätning Fodon Dödänande Fodon TNS Innehålle Navigeingscental Avståndsinmätning Positionsosäehet Fodon 3 Dödänande Fodonets position Vad navigeingssystemet estimea Figu 3.. Fodonen vid steg. Fodonets position Sanning Fodon Fodon Fodon 3 Figu 3.. Fodonen vid steg.

25 Fodon Fodon Bäingsinmätning Tac Fodon 3 Figu 3.3. Fodonen vid steg 3. Fodon Fodon Fodon 3 Figu 3.4. Fodonen vid steg 4.

26 4 Simuleingsmiljö 4. Allmänt Idén med distibuead navigeing omme att undesöas och demonsteas i en simuleingsmiljö som sa eftelina veligheten så mycet som möjligt. Natuligtvis så måste man göa många föenlinga och appoimatione, men syftet ä ändå att simuleingsmiljön sa vaa så velighetstogen att imliga slutsatse an das. Önsvät sulle även vaa att unna veifiea simuleingsmiljön mot veliga test, fö att fövissa sig om simuleingsmiljöns giltighet. Men detta ha inte ymts inom amen fö eamensabetet. Figu 4. visa uppbyggnaden av simuleingsmodellen. De stecade boana implementeas ej. Omvälden bestå av en platt miljö med obefintlig teäng. De stecade flödena ha ingen vean. Nedan följe de föenlinga som ha gjots och sedan hu de olia modulena i simuleingsmodellen fungea. Fodon inmätnings tidpunte Opeatö inmätning målidentifieing Site est. avstånd est. bäing tid Tajectoy sann position sann heading Nav system est. position est. heading Nätve Filte tidpunte positione LoS avstånd bäing oigeinga Scenaio Line of Sight Detetion tidsfödöjninga filtepaameta Uppdateing gid GIS magn Teäng Uppdateat Navdata ealtid off-line Figu 4.. Visa simuleingsmodellen. 4. Föenlinga De olia fodonstypena i simuleingsmiljön behandlas olia. De fodon som ha ett högpesteande navigeingssystem TNS betatas som ideala. Detta innebä att navigeingssystemet inte ge felatiga väden. Fö fodonen med ett lågpesteande navigeingssystem enelt dödänande används en modell fö feltillväten. På så sätt geneeas felatiga navigeingsväden. Me om det i apitel

27 4.3 Modulen Scenaio Scenaio sica data fö vaje fodon bestående av en veto med bytpunte nodliga och östliga oodinate. Även en veto med tidsangivelse fö vaje bytpunt sicas. Mellan vaje bytpunt ö sig fodonet på en a linje, detta fö att föenla beäningana. Detta an natuligtvis ses som en begänsning, men använde man tilläcligt många bytpunte så an vilen fädväg som helst besivas med godtyclig noggannhet. 4.4 Modulen Tajectoy Fån de data som scenaiot ge sa oodinate fö vaje tidssteg som simulaton stegas fam med beänas. Även hastighete och us sa beänas. Fö vaje tidssteg mellan två bytpunte ä hastigheten och usen onstant. Detta sa göas fö vaje fodon. Fö att undelätta beäningana så infös ett villo på data fån scenaiot. Tidsintevallet mellan två bytpunte måste vaa jämt delbat med det tidssteg simulaton stegas fam med. 4.5 Modulen Navigeingssystem 4.5. Meaniseingsmodell Fö att simulea navigeingssystemen används en felmodell. Fö att ta fam en felmodell används en meaniseingsmodell. Denna besive vila elatione som åde mellan fodonets inematisa tillstånd. Detta gälle baa de fodon som ha en lågpesteande navigeingssystem, de anda betatas som ideala och ha däfö inget behov av en modell fö simuleinga. Det oodinatsystem som används i meaniseingsmodellen ä ett högesystem med Noth, East, Down som -, y- och z-ala. Det dödänande systemet använde sig av två olia navigeingssensoe, en odomete och en magnetompass. Navigationssystemet uttycs då på följande diseta fom dä den ä uppdelad i nodlig espetive östlig oodinat, dä dä d ä distans och ä usen. n e n e d d cos sin, 4. d d d, 4. Kusen antas vaa onstant unde tidsintevallet T t t 4.3. Meaniseingsmodellen an ocså epesenteas i ett blocschema, se figu 4.. 4

28 d T T cos sin T T n e Figu 4.. Blocschema öve meaniseingsmodellen Felmodell Då modulen tajectoy genea de sanna vädena på oodinatena vill man ha en modell fö felen i oodinatena, en felmodell. Dessa fel addeas sedan på de sanna vädena fö att simulea utdata fån navigeingssystemet. Fö att ta fam en sådan modell an man föst deivea evationena 4. med avseende på alla ingående vaiable och man få följande fö nodlig oodinat och n n n d n e e e d e cos d d sin sin cos fö östlig oodinat. Utnyttja man dessa deivato, d.v.s. 4.4 och 4.5, i en :a odningens linjäiseing av felfotplantningen fås följande evatione δn δn cos δ d d sin δ 4.6 δe δe sin δ d d cos δ. Evatione i 4.6 besive hu felen i navigeingssensoena fotplantas vidae till oodinatena Odomete Odometen mäte distansen fodonet ö. Det fel som odometen gö modelleas med ett salfatofel δ d d, 4.7 5

29 dä d ä öd distans och ä salfaton. Salfaton ä olia fö olia fodon Magnetompass Magnetompassen mäte usen mot magnetisa no. Poblemet ä att man önsa ha usen mot geogafisa no. Sillnaden mellan dessa två betecnas delination. Delinationen beo på föändinga och stöninga i jodens magnetfält. Vissa av dessa an man föutse och ta hänsyn till, men ett vavaande fel finns och betecnas med δ, 4.8 vilet ä gemensamt fö alla fodon. Ett annat poblem ä att fodonet själv stö magnetfältet. Det ä magnetisa mateial i fodonet som osaa magnetis deviation. Detta ha modelleats som ett fel som beo på fodonets us. Felet betecnas med δ Asin B, 4.9 dev dä felet ha modelleats som en sinusuva med amplituden A och fasfösjutningen B. Deviationsfelet ä olia fö olia fodon. Uttycet 4.9 an appoimeas och delas upp i två dela, och δ cos idc 4. δ ids sin, 4. dä betecningen id stå fö att felet ä unit fö vaje fodon. Ett ytteligae fel som tillomme ä ett alibeingsfel. Detta betecnas med δ id, 4. och även detta fel ä olia fö olia fodon. Samlas nu alla dessa olia fel ihop fås följande totala felmodell fö magnetompassen δ δ δ δ cos δ sin. 4.3 id idc ids Slutlig felmodell Infös nu de felmodelle som ä famtagna fö navigeingssensoena i 4.6 fås följande evatione δn δe Infös nu betecningana bli evationena 4.4 δn cos d δ δ δe sin d δ δ id id δ n, e, δ idc d idc d d sin cos δ d cos cos δ cos sin ids ids sin sin

30 δn δe δn δe e, n, n, δ e, δ idc idc e, cos δ n, cos δ δ δ sin δ δ ids ids id id sin. Evationena i 4.6 används fö att simulea felen i navigeingssystemet. 4.6 Väden på, δ id, δ idc och δ ids slumpas fam fö vaje fodon och δ fö alla fodon. Fö att få imliga väden som ha velighetsannytning används felbudgeten fån apitel som undelag. Felen beänas fö vaje tidssteg och acumuleas i vaje bytpunt. Detta beänas fö vaje fodon som ha ett lågpesteande navigeingssystem. Fö de navigeingssystem som betatas som ideala addeas inget fel. Utdata fån navigeingssystemet sicas till nätveet Felbudget Vädena på paametana,, δ, δ id, δ idc och δ ids ha man ingen eat unsap om. Vädena an ej diet mätas, utan an baa i viss mån estimeas. Men efaenhetsmässigt finns det ändå en viss unsap, en unsap om deas statistisa egensape. Dessa an utycas i en felbudget dä m σ σ m id idc N m δ N m δ δ σ m id idc ids idc N m N m σ ids,, σ δ N m id σ. σ..6 σ ids m idc [ ad] [ ad].5 id idc ids, σ m, σ, σ, σ [ ad]. id ids idc ids [ ad] Modulen Opeatö Opeatöen avgö om en inmätning mot ett annat fodon sa göas. Huuvida ett sådant beslut tas elle inte beo på en slumpmodell. Modellen som används ä enel, dä en tidpunt fö inmätningen slumpas fam utifån en lifomig sannolihetsfödelning på ett angivet tidsintevall. Även anda modelle an användas dä t.e. positionsosäeheten spela in. Ju stöe osäeheten i positionen ä desto stöe ä sannoliheten att opeatöen besluta att göa en inmätning. 7

31 Nä en inmätning sa göas sicas en signal till modulen sitet som utfö inmätningen. 4.7 Modulen Site 4.7. Allmänt I simuleingsmodellen an sitet utföa inmätninga som bestå av avstånd och bäing mellan två fodon. Hä används de sanna vädena på positionsoodinatena fån modulen tajectoy fö att äna ut de sanna vädena på avstånd och bäing. Fö att simulea veligheten sa nu fel addeas till dessa sanna väden, då sitet inte ge eata mätväden utan ä behäftade med vissa fel. Även anda inmätninga sulle unna utföas, t.e. baa en avståndsmätning elle baa en bäingsmätning. Detta ha doc inte implementeats Felmodelle En felmodell fö en mätning av bäingen mot ett fodon se ut som följande δ δ δ 4.9 dä δ ä felet i us, δ ä ett offsetfel och ε som ä ett mätbus. Felet i us påvea ocså sitet vilet medfö att ett fodon med ett lågpesteande navigationssystem få en säme bäingsinmätning än ett fodon med ett högpesteande navigationssystem. En felmodell fö en mätning av avståndet se linande ut. Den ä dä δ ä ett offsetfel och ε ä mätbus. ε δ δ ε 4. Vädena på δ och δ slumpas fam fö vaje site. Även hä används en felbudget, se apitel 4.7.3, som undelag fö att få imliga väden med velighetsannytning. Utdata fån sitet sicas till modulen nätve Felbudgeta Felbudgeten fö bäingen ä efaenhetsmässigt δ N m δε N m ε, σ, σ ε, 4. dä m [ ad] σ. [ ad ] 4. σ ε m ε. [ ad ]. Felbudgeten fö δ fås fån felbudgeten av magnetompassen. Felbudgeten fö avståndet ä efaenhetsmässigt δ N m, σ δε N m,, ε σ ε 4.3 8

32 dä m σ [ m] 4.4 σ ε m ε [ m]. [ m] 4.8 Modulen Nätve Nätveet tanspotea utdata fån navigeingssystemet och fån sitet till filtet. Tillbaa tanspoteas positionsoigeinga till uppdateing. Detta gälle fö vaje fodon. Nätveet bestå baa av en identitet, d.v.s. inga tidsfödöjninga finns elle någa anda stöninga invea. I en me ealistis situation finns det natuligtvis både tidsfödöjninga och anda stöninga, men en sådan modell ligge utanfö amen fö detta eamensabete. 4.9 Modulen Filte Filtet tillhö egentligen inte simuleingsmodellen utan hä implementeas Distibuead navigeing. En implementeing ha tagits fam och häledningen av den finns i apitel 5. Indata till filtet ä positione fån alla ingående fodon och även data fån de inmätninga fodonen gö mot vaanda. Utdata fån filtet ä positionsoigeinga och positionsosäehete. 4. Modulen Uppdateing Denna modul oigea positionena fån navigeingssystemen med hjälp av utdata fån filtet. 9

33 5 Filte fö distibuead navigeing 5. Övesit Fö att unna använda ett almanfilte behövs en tillståndsmodell av systemet. Tillståndsmodellen bestå av två olia dela, en tidsuppdateingsmodell och en mätuppdateingsmodell. Föst tas en tidsuppdateingsmodell fam fö ett fodon och en mätuppdateingsmodell fö en inmätning. Alla tidsuppdateingsmodelle fö vaje fodon sätts ihop till en gemensam tidsuppdateingsmodell och alla mätuppdateingsmodelle fö alla inmätninga som se vid samma tidpunt sätts ihop till en gemensam mätuppdateingsmodell. Båda dessa modelle sätts sedan ihop till en stöe gemensam tillståndsmodell. I apitel 5.7 besivs tillvägagångssättet fö almanfiltet. 5. Tidsuppdateingsmodell Utifån den felmodell som ä häledd i apitel 4.5. ha en tidsuppdateingsmodell tagits fam fö vaje fodon i filtet. Evationena fö felmodellen ä δn δn n, e, δ δ id e, δ idc cos δ ids sin 5. δe δe e, n, δ δ id δ cos δ sin, dä n, Natuligtvis ha man inte tillgång till navigationssystemet äna fam. n, idc n, n, e, e, e, n,, e,. ids 5. och. Istället används de väden Den enda unsapen som finns om paametana i evationena,, δ δ id, δ idc och δ ids ä deas stoastisa egensape N m δ N m δ N m δ δ σ id idc ids idc N m N m σ ids., σ id idc ids, σ, σ, σ, σ id idc ids 5.3 Nu addeas δ och δ id ihop till en paamete fö att undelätta notationen och den betecnas med δ och ha de statistisa egensapena t

34 ., id t id t t t t m m m m N σ σ σ σ δ 5.4 Om nu alla paameta betatas som stoastisa pocesse fås följande tidsuppdateingsmodell. sin sin cos cos,,, ids n e idc n e t n e e n e n e n δ δ δ δ δ δ δ 5.5 Poblemet ä att de stoastisa pocessena, t, δ, idc, δ och ids, δ inte ä ooeleade med sig själva öve tiden, vilet man vill ha i en tillståndsmodell. Detta inses genom att undesöa pocessenas stoastisa egensape, eempelvis [ ], ] [ T t t T E T R E m σ 5.6 dä T alltså ä tidsintevallet mellan tidstegen filtet abeta i. Även fö de anda stoastisa pocessena bli esultatet liatat. Fö att åtgäda detta poblem an ett shaping filte användas, se appendi A.3. En stoastis vaiabel få följande shaping filte σ R 5.7 Detta innebä att ett tillstånd måste tillföas fö vaje stoastis vaiabel. Utöas nu tidsuppdateingsmodellen med fya ytteligae tillstånd så undvie man de oeleade pocessena. Modellen bli då. sin cos sin cos ids idc t n n n e e e e n ids idc id e n e n δ δ δ δ δ δ δ δ δ δ 5.8 Statvädena på vaiansena på de utöade tillstånden fås fån felbudgeten i apitel Tillståndseduceing Modellen i 5.8 ha inte använts p.g.a. av obsevebahetspoblem. Antalet tillstånd ä fö stot. Lösningen på detta ä att educea modellompleiteten genom att minsa antalet tillstånd.

35 Fö att få bätte obsevebahet slås alla fel som ha med magnetompassen att göa ihop. Felet bli då δ δ δ cos δ sin. 5.9 t idc Genom detta educeas antal tillstånd med två och bätte obsevebahet ehålls. Paameten δ få ett beoende av usen och anda stoastisa egensape. Dess vänteväde ä och vaiansen bli m δ σ δ E E E m, E E ids [ δ ] [ δ t δ idc cos δ ids sin ] [ δ ] E[ δ ] cos E[ δ ] t δ E t idc m idc cos m ids sin [ δδ ] [ δ [ δ ] E[ δ ] cos E[ δ ] t t t t σ σ δ idc. δ idc idc cos δ cos δ idc ids ids sin ] ids sin ids sin Även denna paamete betatas som en stoastis pocess med vänteväde och ovaiansen Med R δ T E E δ E m [ δ δ ] [ δ ], sin δ E 5. [ δ t, δ idc, cos δ ids, sin t, T δ idc, T cos T δ ids, T sin T ] [ δ δ ] cos cos E[ δ δ ] t, sin sin T t, T T E[ δ δ ]. ids, ids, T T idc, idc, T 5.3 θ, 5.4 T dä θ ä föändingen i us unde tidsintevallet R δ T σ σ t t σ σ idc idc cosθ. T t t bli ovaiansen cos cos θ sin sin θ 5.5 Denna pocess ä inte ooelead med sig själv öve tiden utan även hä måste ett shaping filte användas. Se appendi A.3 fö me om shaping filte. Ett shaping filte på följande fom används z F, G w 5.6

36 3 dä w ä vitt bus med vaians. Hä måste F och G bestämmas. Föst multipliceas 5.6 med i både högeled och vänsteled och väntevädet tas [ ] [ ] [ ]. cos cos idc t idc t idc t idc t F F w E G E F E σ σ θ σ σ σ σ θ σ σ 5.7 Fö att bestämma G vadeas i 5.6 och vänteväde tas [ ] [ ] [ ]. cos cos 4 idc t idc t idc idc t idc t G G F w w E G E F E σ σ θ σ σ θ σ σ σ σ σ 5.8 Hela tidsuppdateingsmodellen bli nu. n e e n w G e n F e n δ δ δ δ δ δ 5.9 Me otfattat an modellen sivas som, i i i i i w B A 5. dä inde i ange fodonet. 5.4 Mätuppdateingsmodell I simuleingsmiljön ha alla fodon ett site med vilet bäing och avstånd an mätas samtidigt. Nedan tas en modell fam fö hu almanfiltet använde en sådan inmätning. Följande betecninga, se figu 5., används vid en inmätning. Vineln ä bäingen mot målet och ä avståndet. Fodon i Fodon j N E Figu 5.. Visa betecningana vid en inmätning.

37 4 Fodon i mäte alltså mot fodon j och de mätväden man ehålle ä. ~ ~ sin ~ ~ cos 5. Dessa mätväden innehålle fel, både i ~ och i ~, som påvea de sanna vädena enligt. cos sin sin cos sin cos ~ ~ sin ~ ~ cos δ δ 5. Detta uttyc fås om man deivea, sin cos 5.3 och gö en :a odningens linjäiseing. Positionena som navigeingssystemen ge ä. ~ ~ e n 5.4 Dessa innehålle ocså fel. ~ ~ e n e n e n δ δ 5.5 Bilda man nu sillnaden mellan de uppmätta vädena, 5. och 5.4, så få man följande mätuppdateingsmodell, cos sin sin cos cos sin sin cos sin cos ~ ~ ~ sin ~ ~ ~ ~ cos ~ i i j j i i i j i j i j i j i i i j i j e n e n e n e e n n e e n n e e n n z z δ δ δ δ δ δ δ δ δ δ δ δ 5.6 dä inde i ange fodonet, vilet ocså gälle genomgående i esten av apitlet. Vid en mätning dä ett fodon med ett högpesteande navigeingssystem ingå bli mätuppdateingsmodellen annolunda, vilet även påpeades i apitel 3.4. Den bli, cos sin sin cos ± δ δ δ δ e n z z e n 5.7 dä plus- elle minustecnet beo på vem som mäte mot vem. Plustecen om fodonet med det lågpesteande navigeingssystem mäte in det fodonet med det högpesteande navigeingssystemet och tvätom annas. Vid beäning av matisen

38 5 cos sin sin cos 5.8 ha man inte natuligtvis inte tillgång till de sanna vädena och, så man bli tvungen att använda ~. ~ 5.9 Vid flea inmätninga vid samma tidpunt utöas mätuppdateingsmodellen. Me otfattat an modellen sivas som, ~ ~ cos ~ sin ~ ~ sin ~ cos i i l l j i j i l l l l l v D C v D C z δ δ K K K K 5.3 dä inde l ange mätningen och ä hä hela tillståndsveton fö filtet. 5.5 Tillståndsmodell Sätte man nu ihop tidsuppdateingsmodellena 5. fö vaje fodon som ingå i filtet och mätuppdateingsmodellen 5.3 få man en tillståndsmodell. Fån mätuppdateingsmodellen se man att det ä baa vissa tillstånd som påveas av en mätning beoende på om en inmätning ha gjots mellan dessa fodon. Man få alltså en tidsbeoende mätmatis.. L L N L L N N N N N v v v D D D C C C z z z w w w B B B A A A M O M M L L M M M M O M M L L M O M M L L M 5.3 Antalet fodon ä N och antalet mätninga vid tidpunten ä L.

39 5.6 Ej implementeade mätuppdateingsmodelle Nä man vid en inmätning mellan två fodon baa mäte avståndet elle bäingen få man två anda mätuppdateingsmodelle. Dessa ä doc inte implementeade men besivs hä nedan. Nä fodon i gö en avståndsmätning mot fodon j enligt figu 5. ehålle man mätvädet ~. 5.3 Detta mätväde innehålle ett fel, ~ δ Positionena som navigeingssystemen ge ä n ~ ~ e Dessa innehålle ocså fel n ~ n δn ~ e e δe Positionena som de båda fodonen ge uppsatta även avståndet mellan fodonen och an då sivas på följande fom ~ δ, 5.36 dä ä nav nav j i j i n n e e Ett uttyc fö δ nav fås om man deivea 5.37 och gö en :a odningens linjäiseing δ nav n cos e sin cosδn j j n e j i i. cosδn i sinδe j sinδe i 5.38 Bilda man nu sillnaden mellan de uppmätta vädena, 5.3 och 5.34, så få man följande mätuppdateingsmodell z ~ ~ δ nav nav δ Vid beäning av matisen i δ δ i nav δn δe δn δe i i i cos sin cos sin δ. j j 5.39 cos sin cos sin, 5.4 6

40 ha man natuligtvis inte tillgång till de sanna vädena utan man an använda nˆ cos eˆ sin j j i nˆ ~ i eˆ ~, 5.4 dä nˆ och ê ä oigeade positione fån filtet. Nä fodon i gö en bäingsinmätning mot fodon j enligt figu 5. fås enligt samma metod som ovan en mätuppdateingsmodell z ~ ~ δ nav nav δ sin Hä vid beäning av matisen i δ δ sin använde man linande som ovan ~ i cos nˆ nav cos nˆ sin sin eˆ δn cos δe δn δe eˆ j i j i i i cos,. j j δ i Flödesdiagam Kalmanfiltet abeta i två olia fase, en tidsuppdateingsfas och en mätuppdateingsfas. Detta besivs i appendi A, dä även ytteligae teoi ges. Fö att åsådliggöa hu almanfiltets algoitm i denna implementeing abeta visas de olia fasena i flödesdiagam. Vissa dela ä doc inte implementeade, men visas ändå. Det fösta diagammet, se figu 5., visa algoitmens övegipande flöde. Hä se man de två olia fasena. Tidsuppdateingsfasen av algoitmen an bytas ne och visas i figu 5.3. Hä beänas de stohete som algoitmen använde fö att utföa tidsuppdateingen. Hu detta gös beo på hu tidsuppdateingsmodellen se ut fö det ingående fodonet. I figu 5.4 visas mätuppdateingsfasen. Hu den abeta beo på vilen typ av inmätning som ha utföts. Även flea inmätninga unde samma tidpunt an behandlas. Byte man nu ne fasen ytteligae så visa figu 5.5 hu stohetena som används i mätuppdateingen beänas. Beäningana beo på vila navigeingssystem de fodon ha som ingå i en inmätning. 7

41 Data inmatning Tiduppdateings fas Nej Finns inmätninga? Ja Mätuppdateings fas Data utmatning Figu 5.. De olia fasena i algoitmen fö almanfiltet. Switch av navsytem Dödä. Anda Hämta filte paameta Uppdateing av A-matis Ej implementeat Uppdateing av B-matis Tidsuppdateingsmodelle Loop Alla fodon Tiduppdateing av filtet Figu 5.3. Tidsuppdateingsfasen av almanfiltet. 8

42 Range och Bäing Switch av inmätningstyp Range Bäing Beäning av filteväden Ej implementeat Ej implementeat Loop Alla mätninga Mätuppdateing av filtet Figu 5.4. Mätsuppdateingsfasen av almanfiltet. Dödä. mot Dödä. Dödä. mot TNS Switch av fodonstype TNS mot Dödä. TNS mot TNS Behandlas ej Hämta filte paameta Hämta filte paameta Hämta filte paameta Uppdateing av C-matis Uppdateing av C-matis Uppdateing av C-matis Beäning av vaiansen R Beäning av vaiansen R Beäning av vaiansen R Mätuppdateingsmodelle Mätuppdateingsmodelle Beäning av Z-matis Figu 5.5. Den nedbutna mätsuppdateingsfasen av almanfiltet. 9