En simuleringsmiljö för distribuerad navigering

En simuleringsmiljö för distribuerad navigering

Examensarbete utfört i Reglerteknik och Kommunikationssystem av

Rickard Färnemyhr

LITH-ISY-EX-3286-2002 Linköping 2002

En simuleringsmiljö för distribuerad navigering

Examensarbete utfört i Reglerteknik och Kommunikationssystem vid Linköpings tekniska högskola

av

Rickard Färnemyhr

LITH-ISY-EX-3286-2002

Handledare: Hans Bohlin

Per-Johan Nordlund

Examinator: Fredrik Gustafsson

Avdelning, Institution Division, Department Institutionen för Systemteknik 581 83 LINKÖPING Datum 2002-05-31 Språk Language Rapporttyp Report category ISBN X Svenska/Swedish Engelska/English Licentiatavhandling X Examensarbete LITH-ISY-EX-3286-2002 C-uppsats

D-uppsats Serietitel och serienummer Title of series, numbering Övrig rapport

____

URL för elektronisk version

http://www.ep.liu.se/exjobb/isy/2002/3286/ Titel

Title

En simuleringsmiljö för distribuerad navigering A simulation environment for distributed navigation Författare

Author

Rickard Färnemyhr

Abstract

This master thesis studies distributed navigation which is a function implemented in a future network based combat information system to improve the accuracy in navigation for 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 reckoning performance through the backup system that consists of an odometer and a magnetic compass. Dead reckoning means a drift in the position that makes the accuracy in the navigation worse.

The distributed navigation function uses position and navigation data with measurements between the vehicles to estimate the errors and uncertainties in positions, which are used to improve the accuracy in position for the vehicles.

To investigate and demonstrate distributed navigation, a simulation environment has been produced in Matlab. The environment is general so different navigation systems can be used and studied and also dynamical so a further development is possible.

The simulation environment has been used to investigate and evaluate an implementation of distributed navigation. The implementation has been made using a central filter where fusion takes place of all navigation data and measurements. This filter has been realized with help of Kalman filter theory, in which all vehicles are put together in a state space model. Simulations have been performed for different scenarios and the result of these show that the drift in position is avoided.

Keyword

Sammanfattning

Examensarbetet studerar distribuerad navigering som är en funktion implementerad i ett framtida nätverksbaserat stridsledningssystem för att förbättra noggrannheten i navigering hos stridsfordon i en mekaniserad bataljon, framförallt vid händelse av ett GPS-bortfall. Vid ett eventuellt GPS-bortfall erhåller fordonen dödräkningsprestanda genom det reservsystem som består av en odometer och magnetkompass. Dödräkning innebär en positionsdrift som försämrar noggrannheten i navigeringen.

Funktionen distribuerad navigering använder positions- och navigeringsdata samt inmätningsdata mellan fordonen för att estimera positionsfel och positionsosäkerheter som sedan används för att förbättra noggrannheten i fordonens position.

För att undersöka och demonstrera distribuerad navigering har en simuleringsmiljö tagits fram i Matlab. Miljön är generell så att olika navigeringssystem ska kunna användas och studeras, och även dynamisk så att en vidareutveckling av miljön ska vara möjlig.

Simuleringsmiljön har använts för att undersöka och utvärdera en implementering av distribuerad navigering. Implementeringen har gjorts med användandet av ett centralt filter där fusion sker av alla navigerings- och inmätningsdata. Detta filter har realiserats med hjälp av kalmanfilterteori, där alla fordon har sammanfogats i en tillståndsmodell. Simuleringar har gjorts för olika scenarier och resultatet av dessa visar att positionsdriften undviks.

Abstract

This master thesis studies distributed navigation which is a function implemented in a future network based combat information system to improve the accuracy in navigation for 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 reckoning performance through the backup system that consists of an odometer and a magnetic compass. Dead reckoning means a drift in the position that makes the accuracy in the navigation worse.

The distributed navigation function uses position and navigation data with measurements between the vehicles to estimate the errors and uncertainties in positions, which are used to improve the accuracy in position for the vehicles.

To investigate and demonstrate distributed navigation, a simulation environment has been produced in Matlab. The environment is general so different navigation systems can be used and studied and also dynamical so a further development is possible.

The simulation environment has been used to investigate and evaluate an implementation of distributed navigation. The implementation has been made using a central filter where fusion takes place of all navigation data and measurements. This filter has been realized with help of Kalman filter theory, in which all vehicles are put together in a state space model. Simulations have been performed for different scenarios and the result of these show that the drift in position is avoided.

Förord

Denna rapport utgör den skriftliga redovisningen av mitt examensarbetet på Civilingenjörsutbildningen Teknisk fysik och Elektroteknik vid Linköpings Tekniska Högskola.

Detta examensarbete har utförts hos AerotechTelub. AerotechTelub ingår i Saab-gruppen och erbjuder kvalificerade tekniska tjänster, underhåll och kundanpassade systemlösningar inom informationsteknik, elektronik och farkostteknik till totalförsvaret, verk och myndigheter samt valda nischer inom näringslivet. Antal anställda är ungefär 2600 och finns på orterna Arboga, Linköping, Växjö, Stockholm och ytterligare på 20 orter i Sverige samt i Danmark, Finland och Tyskland. Mer information finns på www.aerotechtelub.se

Jag vill här tacka min handledare på AerotechTelub AB, Hans Bohlin och min handledare vid Linköpings Tekniska Högskola, Per-Johan Nordlund för den hjälp jag har fått under tiden jag har utfört mitt examensarbete.

Innehållsförteckning

1 Inledning 1 1.1 Bakgrund ... 1 1.2 Uppgift ... 1 1.3 Rapportens uppläggning... 1 2 Navigering 2 2.1 Allmänt... 2 2.2 Navigeringssystem... 2 2.3 Val av navigeringssystem... 3 2.3.1 Allmänt ... 3 2.3.2 Autonomt / Icke-autonomt... 4 2.3.3 Passivt / Aktivt ... 4 2.3.4 Utnyttjande av integritetsövervakning ... 4 2.3.5 Hög / Låg uppdateringshastighet ... 5 2.3.6 Hög / Låg dynamik... 5 2.3.7 Robusthet ... 5 2.3.8 Kostnad ... 5 2.4 Exempel på navigeringssystem ... 5 2.4.1 Översikt... 5 2.4.2 Tröghetsnavigeringssystem ... 5 2.4.3 Satellitnavigeringssystem ... 6 2.4.4 Stjärnnavigeringssystem ... 6 2.4.5 Terrängnavigeringssystem ... 6 2.4.6 Hybridnavigeringssystem ... 7 3 Distribuerad navigering 8 3.1 Idé ... 8

3.2 Användningsområde och syfte ... 8

3.2.1 Användningsområde... 8

3.2.2 Förbättrad navigering ... 8

3.3 Inmätningar ... 9

3.3.1 Allmänt ... 9

3.3.2 Acceptanstest ... 9

3.4 En implementering av Distribuerad navigering ... 9

3.5 Exempel på navigeringsuppdatering hos ett fordon ... 10

4 Simuleringsmiljö 13 4.1 Allmänt... 13 4.2 Förenklingar ... 13 4.3 Modulen Scenario ... 14 4.4 Modulen Trajectory ... 14 4.5 Modulen Navigeringssystem... 14 4.5.1 Mekaniseringsmodell ... 14 4.5.2 Felmodell ... 15 4.5.3 Odometer ... 15 4.5.4 Magnetkompass... 16 4.5.5 Slutlig felmodell... 16

4.5.6 Felbudget ... 17 4.6 Modulen Operatör... 17 4.7 Modulen Sikte ... 18 4.7.1 Allmänt ... 18 4.7.2 Felmodeller ... 18 4.7.3 Felbudgetar ... 18 4.8 Modulen Nätverk ... 19 4.9 Modulen Filter ... 19 4.10 Modulen Uppdatering ... 19

5 Filter för distribuerad navigering 20 5.1 Översikt ... 20 5.2 Tidsuppdateringsmodell... 20 5.3 Tillståndsreducering... 21 5.4 Mätuppdateringsmodell ... 23 5.5 Tillståndsmodell ... 25 5.6 Ej implementerade mätuppdateringsmodeller ... 26 5.7 Flödesdiagram ... 27 6 Simuleringar 30 6.1 Allmänt... 30 6.2 Studerade felmått ... 30 6.3 Scenarier... 31 6.4 Sammanfattning av undersökningen... 33 6.5 Simuleringar ... 33 6.6 Scenario 1 ... 34 6.6.1 Översikt... 34 6.6.2 Estimering av position ... 34

6.6.3 Estimering av skalfaktor och kompassfel ... 36

6.6.4 Osäkerheter ... 37

6.6.5 Simuleringsresultat ... 39

6.7 Scenario 2 A ... 39

6.7.1 Översikt... 39

6.7.2 Estimering av position ... 39

6.7.3 Estimering av skalfaktor och kompassfel ... 41

6.7.4 Osäkerheter ... 43

6.7.5 Simuleringsresultat ... 44

6.8 Scenario 2 B ... 45

6.8.1 Översikt... 45

6.8.2 Estimering av position ... 45

6.8.3 Estimering av skalfaktor och kompassfel ... 46

6.8.4 Simuleringsresultat ... 47

6.9 Scenario 3 ... 47

6.9.1 Översikt... 47

6.9.2 Estimering av position ... 47

6.9.3 Estimering av skalfaktor och kompassfel ... 49

6.9.4 Osäkerheter ... 50

6.9.5 Simuleringsresultat ... 50

7 Slutsatser 51

7.1 Simuleringsmiljön... 51

7.2 Implementeringen av distribuerad navigering... 51

7.2.1 Resultat ... 51

7.2.2 Utvärdering av resultatet... 51

7.3 Förslag till fortsatt arbete ... 52

8 Referenser 53 Appendix A Kalmanfiltrering 54 A.1 Tillståndsmodell ... 54

A.2 Kalmanfiltret... 54

A.3 Shaping filter ... 55

Appendix B Nomenklatur 57 B.1 Storheter ... 57

B.2 Stokastiska storheter ... 58

1 Inledning

1.1 Bakgrund

Varje militärt stridsfordon har ett navigeringssystem. Dessa system är av olika noggrannhet, allt från enkla dödräknande system och Global Positioning System (GPS) till högkvalitativa tröghetsnavigeringssystem (TNS). Det ultimata vore att varje fordon var utrustat med ett sådant högkvalitativt TNS, men på grund av bl.a. höga kostnader omöjliggörs detta.

På grund av den höga kostnaden kan alltså inte alla stridsfordonen i en mekaniserad bataljon vara utrustat med ett TNS, utan måste förlita sig på ett enkelt dödräknande system och GPS. Problemet är dock att GPS är relativt lätt att störa ut och man är beroende av yttre påverkbara källor vilket gör att man inte bör förlita sig på GPS som primärt navigeringssystem. GPS är alltså inte robust och autonomt, vilket är önskvärt i militära applikationer.

Då lösningen med GPS har nackdelar vill man hitta en annan. En sådan lösning är distribuerad navigering.

1.2 Uppgift

Att utarbeta en simuleringsmiljö för demonstration och undersökning av distribuerad navigering. Simuleringsmiljön ska vara generell så att olika navigeringssystem ska kunna användas och studeras. Den ska även vara dynamisk så att en vidareutveckling av miljön är möjlig. Simuleringsmiljön ska skrivas i Matlab.

Även utarbeta ett program/filter där en implementering av distribuerad navigering sker och utvärdera programmet/filtret med simuleringar.

1.3 Rapportens

uppläggning

Efter en inledande beskrivning av navigering och val av olika navigeringssystem i kapitel 2 görs en genomgång av distribuerad navigering i kapitel 3. Där finns även ett förslag på en implementering av distribuerad navigering.

I kapitel 4 beskrivs simuleringsmiljön med tillhörande moduler och härledningar av de felmodeller som används av simuleringsprogrammet. Härledningen och modellbygget av det filter som förslaget på en implementering innebär finns i kapitel 5. Där finns även arbetsgången för filtret beskrivet.

I kapitel 6 undersöks och utvärderas filtret enligt angivna felmått. I slutet av det kapitlet finns en sammanfattning av simuleringsresultaten. Slutsatserna som är dragna i detta examensarbete finns i kapitel 7.

Appendix A beskrivs kortfattat den kalmanfilterteori som används. Appendix B innehåller nomenklaturen.

2 Navigering

2.1 Allmänt

Navigering innebär kunskapen om vart man befinner och hur man rör sig relativt något utvalt referenssystem. Denna kunskap innefattas i olika navigeringsstorheter som

• position och höjd • hastighet

• acceleration • attityd

• attitydvinkelhastighet

Det finns även andra storheter, men det är dessa som är av primärt intresse.

Själva ordet navigering eller då navigation kommer från det latinska ordet navigato, som betyder sjöresa, men i dag har ordet en vidare betydelse.

De olika navigeringsstorheterna måste anges relativt ett referenssystem för att man entydigt ska kunna tolka storheterna. Vid navigering av en farkost på jorden så används normalt jorden som referens. Detta kallas för terrestial navigering. Vid navigering av rymdfarkoster har man istället för jorden solsystemet, andra planeter etc. som referens. Denna typ av navigering kallas celestial navigering.

En fullständig beskrivning av en farkosts position och rörelse vid en fix tidpunkt relativt en specificerad referens sammanfattas ibland med begreppet farkostens kinematiska tillstånd. De ovan angivna navigeringsstorheterna vid en given tidpunkt utgör en del av farkostens kinematiska tillstånd. Med detta i baktanke kan en definition av navigering formuleras

Definition Navigering är en funktion som mäter en farkosts rörelse och beräknar en del av en farkosts kinematiska tillstånd.

Ett nära besläktat begrepp till navigering är guidance, som använder resultatet från navigering. Guidance innebär att man försöker hitta till en angiven destination/mål och även denna kunskap innefattas i olika storheter som

• avstånd till destination/mål • riktning till destination/mål • hastighet relativt destination/mål

Informationen om navigering och navigeringssystemen är hämtad från [3].

2.2 Navigeringssystem

Ett navigeringssystem har till uppgift att kontinuerligt generera data om farkostens läge och position, dvs. navigeringsstorheterna. Följande definition för navigeringssystemet beskriver dess funktionalitet

Definition Ett navigationssystem är en apparat som mäter och beräknar en del av en farkosts navigationsstorheter relaterade till en referens.

För att kunna beräkna navigationsstorheterna måste navigationssystemet få tillgång till någon typ av information om farkostens läge och/eller rörelse relativt någon referens. Informationen finns i den utdata som generas från navigeringssensorerna. Dessa referenser (olika sensorer

kan ha olika referenser) är oftast inte den referens som navigationsberäkningarna sker i. Men med hjälp av modeller mellan olika referenser kan konverteringar göras och om referenserna rör sig i förhållande till varandra måste navigationssystemet kontinuerligt göra beräkningar av sambandet mellan referenserna (detta kallas här för instrumentering) för att sedan göra konverteringen. Den referens som navigationssystemet utför sina beräkningar i är oftast inte heller den referens som navigeringsstorheterna ska presenteras i. Då krävs det ytterligare beräkningar och en konvertering innan presentationen kan ske. Nedan i figur 2.1 illustreras detta. Navigerings-sensorer Navigations-presentation Navigations-dator Nav. rådata Navigations-storheter Navigeringssystem

Figur 2.1. Blockschema vid navigering.

2.3

Val av navigeringssystem

2.3.1 Allmänt

I kapitel 1.1 nämns problem och egenskaper hos navigeringssystem som spelar in när man ska utrusta ett militärt stridsfordon med ett navigeringssystem. Här görs en mer systematiserad genomgång av egenskaper och karaktäristik som bör beaktas vid valet av navigationssystem för ett visst fordon eller applikation. Egenskaperna och karaktäristik är följande

• Autonomt / Icke-autonomt • Passivt / Aktivt • Utnyttjande av integritetsövervakning • Hög / Låg uppdateringshastighet • Hög / Låg dynamik • Robusthet

• Kostnad (inköp, installation och underhåll)

Det finns olika navigationsprinciper olika navigationssystem kan klassificeras av. Beroende på princip erhålls vissa egenskaper och karaktäristik enligt ovan.

Navigeringsprincipen är den metod med vilken navigationssystemet beräknar navigationsstorheterna. Man skiljer mellan tre olika principer, dödräkning, fixpunktsuppdatering och guidance (om guidance ska räknas som en navigeringsprincip eller inte beror på hur man ser på det, i vilket fall som helst är det nära besläktat med navigering).

Dödräkning har sitt historiska ursprung från navigering av fartyg. Med kompass och logg som sensorer mättes kurs och fart. Logg är en sensor som mäter fartygets fart relativt vattnet. Med kartor som innehåller information om havsströmmar kompenserades observationen från loggen så att man får farten relativt jorden. Men hjälp av dessa data kan positionen sedan integreras fram. Själva principen går alltså ut på att från givna data integrera, en eller flera gånger beroende på vilken utdata man har, fram positionen. I det tidsdiskreta fallet blir det en summation.

Fixpunktsuppdatering innebär att man genom inmätningar mot kända objekt uppdaterar sin position. Dessa objekt kan vara av olika slag, allt från satelliter, stjärnor och kända objekt i naturen. Den enklaste formen av fixpunktsnavigering är navigering med hjälp av kartor. Guidance beskrivs kortfattat i kapitel 2.1.

Värt att påpeka är att inget navigeringssystem är perfekt och det finns även inga perfekta navigeringssensorer. Detta medför att fel uppstår vid navigeringsberäkningar. Man får helt enkelt betrakta de navigeringsstorheter som navigeringssystemet ger ifrån sig som skattningar eller estimat. Detta innebär att navigeringsstorheterna är behäftade med ett fel som är okänt. Det finns i princip tre olika felkällor i navigeringsberäkningarna.

1. Felaktigheter i referensmodellerna. Detta medför att instrumenteringen blir felaktig. 2. Sensorfel. Sensorerna innehåller fel som påverkar navigeringsberäkningarna. Det

vanligaste felet är mätbrus, som alltid finns närvarade vid mätningar.

3. Numeriska fel. Vid navigationsberäkningarna måste sensorsignalerna samplas och diskretiseras vilket medför felaktigheter.

De numeriska felen är oftast försumbara.

Den kunskap man kan ha om felen är deras karaktäristik. Detta beror på vilken navigeringsprincip som utnyttjas och vilket navigeringssystem som används och vilka navigeringsstorheter som betraktas. Tex. så karaktäriseras felet i ett dödräknande system, t.ex. TNS, av att positionsfelet växer med körd sträcka till skillnad från ett fixpunktsuppdaterande system, t.ex. GPS, som är begränsat men brusigt. Dessa två navigeringssystem beskrivs i kapitel 2.4.

2.3.2 Autonomt / Icke-autonomt

Ett navigeringssystem karaktäriseras som autonomt om dess navigeringssensorer är oberoende och bortkopplade från yttre påverkbara källor. Detta innebär att navigeringssystemet ej kan störas ut eller på annat sätt luras genom yttre påverkan. Exempel på yttre källors påverkan är telesignaler som stör sensorer (sändare) i ett navigeringssystem.

I militära applikationer är ett autonomt navigeringssystem att föredra.

2.3.3 Passivt / Aktivt

Ett navigeringssystem karaktäriseras som passivt om det ej sänder ut signaler eller på annat sätt kommunicerar med omvärlden.

I militära applikationer är ett passivt navigeringssystem att föredra då risken finns att ett aktivt navigeringssystem kan röja en farkost närvaro eller än värre pejlas in.

2.3.4 Utnyttjande av integritetsövervakning

Integritetsövervakning innebär att navigeringssystemet kan upptäcka fel i navigeringssensorerna eller i utdata. Detta kan realiseras på olika sätt. Ett enkelt sätt är att bedöma rimligheten i utdata.

Ett vanligare och bättre sätt är att om möjligt utnyttja redundant information i navigeringssystemet. Den redundanta informationen kan jämföras inbördes med varandra för att på så sätt upptäcka orimligheter. För att detta ska fungera måste naturligtvis tillgång till redundant information finnas.

2.3.5 Hög / Låg uppdateringshastighet

Hur ofta navigeringssystemet ger ifrån sig utdata karaktäriserar också navigeringssystemet. Uppdateringshastigheten som krävs skiljer sig åt beroende på i vilken applikation navigeringssystem används.

Ett närliggande problem är eventuella tidsfördröjningar i utdata från navigeringssystemet. Hur stort detta problem är beror på hur fort fordonet rör sig. Detta kan i vissa applikationer reduceras något genom att tidsstämpla utdata.

2.3.6 Hög / Låg dynamik

Ett problem som kan uppstå i ett navigeringssystem är eftersläpning i utdata. Det är kopplat till navigeringssystemets dynamik. Med dynamik menas här navigeringssystemets förmåga att följa med i kraftiga manövrer som farkosten utför. Har navigeringssystemet hög dynamik följer navigeringssystemet snabbt med och en eftersläpning undviks och tvärtom annars. Det finns även en konflikt mellan om utdata från navigeringssystemet har eftersläpning eller är brusigt. Om man vill minska på bruset kan man lågpassfiltrera utdata, men detta ger bieffekten att man får mer eftersläpning. Detta kan accepteras om navigeringssystemet har hög dynamik.

2.3.7 Robusthet

Ett navigeringssystems förmåga att motstå medvetna eller omedvetna störningar benämns med robusthet. Detta begrepp är nära kopplat till om navigeringssystemet är autonomt eller inte. Är navigeringssystemet autonomt minskar risken för att störningar kan påverka navigeringssystemet.

Finns det redundant information i navigeringssystemet ökar givetvis robustheten, eftersom vissa navigeringssensorer kan störas ut, men navigeringssystemet fungerar ändå, visserligen med sämre noggrannhet.

2.3.8 Kostnad

Även om ett navigeringssystem har hög prestanda enligt ovanstående egenskaper så spelar även kostnaden in vid valet av navigeringssystem, ju bättre navigeringssystem desto högre kostnad. I kostnaden innefattas delar som inköp, installation och underhåll.

2.4

Exempel på navigeringssystem

2.4.1 Översikt

Här beskrivs några olika navigeringssystem med avseende på ovanstående egenskaper och även på vilka olika sensorreferenser, referensmodeller och instrumenteringar som används. 2.4.2 Tröghetsnavigeringssystem

Sensorerna i ett tröghetsnavigeringssystem (TNS) mäter acceleration och attitydvinkelhastighet relativt tröghetsrymden. Dessa måste sedan konverteras till jordens referens vid terrestialnavigering. Vid beräkning av hastighet och position utnyttjas sedan dödräkning.

Ett TNS har följande egenskaper och karaktäristik: • Autonomt

• Hög uppdateringshastighet • Hög dynamik

• Bra robusthet • Hög kostnad

Att TNS är autonomt och passivt gör att det lämpar sig bra för militära applikationer. 2.4.3 Satellitnavigeringssystem

Det mest kända satellitnavigeringssystemet är Global Positioning System (GPS). GPS använder sig av icke geostationära satelliter. Navigeringssensorerna (GPS antennen) mäter avstånd och relativ hastighet till en satellit. Detta innebär att navigeringssensorn har satellitens bana som referens. Här måste en instrumentering göras mellan satelliten och jorden för att sedan göra en konvertering. Flera satelliter används samtidigt. GPS använder fixpunktsuppdatering som navigeringsprincip.

Ett GPS har följande egenskaper och karaktäristik: • Icke-autonomt • Passivt • Låg uppdateringshastighet • Relativt låg dynamik • Dålig robusthet • Låg kostnad 2.4.4 Stjärnnavigeringssystem

Ett stjärnnavigeringssystem använder stjärnorna för att beräkna navigationsstorheterna. De navigationssensorer som används är videokamera med bildbehandling. En gammal sensor är annars den klassiska sextanten.

Ett stjärnnavigeringssystem har följande egenskaper och karaktäristik: • Autonomt

• Passivt

Ett exempel på var ett stjärnnavigeringssystem används är i det militära flygplanet B2 från USA.

2.4.5 Terrängnavigeringssystem

Ett terrängnavigeringssystem utnyttjar fixpunktsuppdatering som navigationsprincip, där inmätningar görs mot kända objekt i terrängen. I moderna terrängnavigeringssystem används topografisk radarhöjdmätare och barometer som navigeringssensorer. Höjddata jämförs sedan med en topografisk databas där korrelationsanalys används som metod för att avgöra var man är.

Ett terrängnavigeringssystem av ovan beskrivna typ har följande egenskaper och karaktäristik: • Aktivt

2.4.6 Hybridnavigeringssystem

Alla olika navigeringssystem har sina fördelar och nackdelar. Idén med ett hybridnavigeringssystem är att utnyttja det bästa från de olika navigeringssystemen. Man använder alltså två (eller i princip hur många man vill) navigeringssystem som kompletterar varandra. Det vanligaste är att använda TNS och GPS. De båda systemen kompletterar varandra bra genom att det ena har sina fördelar där den andra har sina nackdelar.

En ytterligare fördel är att man får tillgång till redundant information. Då kan ett system för integritetsövervakning användas. De övriga egenskaper och karaktäristik ett hybridnavigeringssystem har beror på vilka navigeringssystem man förenar.

3 Distribuerad

navigering

3.1 Idé

Distribuerad navigering är en funktion implementerad i ett framtida nätverksbaserat stridsledningssystem, främst tänkt för stridsfordon i en mekaniserad bataljon eller för stridsbåtar i en amfibiebataljon. Funktionen använder följande information som finns (eller bör finnas) i det framtida nätverksbaserade stridsledningssystemet:

• positions- och navigeringsdata

• igenkännings- och identifieringsdata (laser- eller radiobaserad IK) • mål och inmätningsdata från sikten och radiobaserad avståndsmätningar

All den här informationen används i ett filter för att estimera positionsfel och positionsosäkerheter för att sedan utnyttjas för att förbättra fordonens position.

3.2

Användningsområde och syfte

3.2.1 Användningsområde

Användningsområden för distribuerad navigering kan ske främst i mekaniserade bataljoner och amfibiebataljoner. I detta examensarbete ligger tyngdpunkten på en mekaniserad bataljon.

3.2.2 Förbättrad navigering

De fordon som ingår i en mekaniserad bataljon använder idag främst GPS och ett dödräknande navigeringssystem som reserv för att beräkna position. Det dödräknande systemet består av en odometer och en magnetkompass. Se kapitel 4.51 för hur detta dödräknande system fungerar.

Då GPS är relativt enkelt att störa ut och att man är beroende av yttre påverkbara källor så bör man inte förlita sig på GPS som primärt navigeringssystem. GPS är alltså inte robust och autonomt, vilket är önskvärt i militära applikationer. Det reservsystem som då finns tillgängligt vid händelse av GPS-bortfall är det dödräknande systemet. Problemet med detta system är att positionsfelet växer med körd sträcka (storleksordning 30 m per körd kilometer). Ett alternativ till det dödräknade systemet med odometer och magnetkompass är ett högpresterande TNS. Visserligen finns även här en positionsdrift, men den är betydligt mindre (storleksordning 1 m per körd kilometer). Problemet är dock den höga kostnad det innebär att utrusta varje fordon i en mekaniserad bataljon med ett högpresterande TNS.

Men används distribuerad navigering som hjälpsystem till det dödräknade systemet med odometer och magnetkompass kommer navigeringsprestanda att förbättras. Används dessa två tillsammans får man ett hybridnavigeringssystem som utnyttjar två olika navigeringsprinciper, dödräkning och en sorts relativ fixpunktsuppdatering. Det relativa innebär en fixpunktsuppdatering mot ett fordon med en bättre positionssäkerhet. Genom inmätningarna mellan fordonen i en mekaniserad bataljon fördelas den bättre navigeringsprestanda som vissa fordon har ut till alla. Detta förutsätter givetvis att det finns fordon med bättre navigeringssystem, som t.ex. högpresterande TNS, i bataljonen. Hur mycket navigeringsprestanda kommer att ökas beror på möjligheten och benägenheten hos fordonen att göra inmätningar mot varandra.

De estimerade positionsfelen som filtret producerar sänds ut över nätverket till alla ingående fordon som då kan korrigera sin egen position.

3.3 Inmätningar

3.3.1 Allmänt

En av förutsättningarna för att distribuerad navigering ska fungera är att inmätningar mellan fordonen sker. Man kan skilja mellan tre olika typer av inmätningar

• samtidig bärings- och avståndsinmätning • avståndsinmätning

• bäringsinmätning

Naturligtvis är det bäst när det sker en samtidig bärings- och avståndsinmätning beroende på att positionsosäkerheten för ett fordon minskar i alla riktningar (se mer hur det fungerar i kapitel 3.5 och tillhörande figurer).

3.3.2 Acceptanstest

Innan det centrala filtret får ta del av en inmätning mellan två fordon måste ett acceptanstest utföras. Detta för att inte filtret ska luras med falska data. Testet består av två olika krav som måste vara uppfyllda.

1. LoS (Line of Sight) krav. Genom att i en digital 3D-karta testa om det finns LoS mellan de två ingående fordonen i mätningen kan en inmätning accepteras.

2. Med hjälp av de korrigerade positionerna för de två ingående fordonen kan rimligheten i inmätningsdata bedömas. Vid markant avvikelse kan en inmätning diskvalificeras.

3.4

En implementering av Distribuerad navigering

En implementering av funktionen distribuerad navigering i ett stridsledningssystem kan göras dels decentraliserat, dels centraliserat. Används ett centraliserat filter placeras det i ett fordon som innehåller navigeringscentralen, men det kan även placeras redundant i alla fordon i händelse att fordonet med navigeringscentralen faller bort.

I det här examensarbetet studeras ett centralt filter där fusion sker av alla navigerings- och inmätningsdata. Inmätningsdata måste dock genomgå ett acceptanstest innan filtret får ta del av det, se mer i kapitel 3.3.2. Resultatet av denna fusion ska vara estimat av positionsfel och positionsosäkerheter av alla ingående fordon. Vid en implementering kan olika metoder användas, t.ex. med användning av

• linjär regression • kalmanfilter

Det första förslaget är oklart om det fungerar och har ej behandlats i detta examensarbete.Vad och hur ett kalmanfilter fungerar beskrivs i appendix A.

I kalmanfiltret finns alla fordon med ett lågpresterande navigationssystem representerade. Fordonen med ett högpresterande navigeringssystem (TNS) antas vara ideala och finns inte representerade. Detta antagande gäller för implementeringen av funktionen distribuerad navigering i detta examensarbete. Detta antagande medför att de estimat som filtret producerar är felaktiga, då de är relativt de högpresterande navigeringssystemen (TNS). Hur bra estimaten är beror på hur stora fel de högpresterande navigeringssystemen (TNS) har.

De fordon som har ett högpresterande navigeringssystem (TNS) ingår alltså inte i kalmanfiltret och medför också att mätmatrisen påverkas. Vid en mätning där ett sådant fordon ingår, blir mätningen en ”exakt” mätning, se mer i kapitel 5.4.

Ett val man också gör är huruvida de estimeringar som kalmanfiltret producerar ska vara återkopplade in i de lågpresterande navigeringssystemen. Problem kan uppstå med instabilitet, då metoden kräver mycket bra estimat. Denna metod går under namnet closed loop. Denna metod har dock inte valts utan i den valda implementeringen sker korrigeringarna med hjälp av estimaten från kalmanfiltret på utdata från navigeringssystemen. Navigeringssystemen själva påverkas alltså inte av korrigeringarna. Denna metod går under namnet open loop. Vid en decentraliserad implementering av funktionen distribuerad navigering kan kalmanfilter även användas. Då kan ett kalmanfilter användas i varje fordon och ett informationsutbyte sker mellan dessa kalmanfilter. Problem kan uppstå då man använder denna teknik. Det som kan ske är att man använder information flera gånger så att en falsk förbättring sker. Problemet kan eventuellt undvikas med en variant av kalmanfiltret, ett så kallat informationsfilter (se [4] för mer om informationsfilter). Detta har ej undersökts vidare.

För att kalmanfiltret ska kunna appliceras krävs modeller av fordonens navigeringssystem. Även olika modeller för hur en inmätning ska behandlas av kalmanfiltret. När ett fordon vill deltaga i systemet för distribuerad navigering måste kalmanfiltret få kunskap om vilket navigeringssystem fordonet har och vilken noggrannhet det har för att rätt modell ska kunna användas. Även information om sikten och om annan inmätningsutrustning måste tillföras kalmanfiltret för att de rätta modellerna för inmätningarna används.

Framtagning av modeller av fordonens navigeringssystem och av olika modeller av inmätningar och även hur kalmanfiltret fungerar beskrivs i kapitel 5.

3.5

Exempel på navigeringsuppdatering hos ett fordon

Nedan följer ett litet konstruerat scenario med tre fordon för att visa hur principen med distribuerad navigering fungerar. Olika typer av inmätningar används för att visa hur positionen förbättras beroende på typen av inmätning.

• Steg 1. Figur 3.1 visar tre fordon med olika navigeringssystem. Två olika typer av inmätningar sker och ett informationsutbyte sker över ett nätverk.

• Steg 2. Mellan steg 1 och 2 producerar det centrala filtret i navigeringscentralen positionskorrigeringar och nya positionsosäkerheter. Figur 3.2 visar de nya positionerna och positionsosäkerheterna efter uppdateringen.

• Steg 3. Mellan steg 2 och 3 rör sig fordonen och positionsosäkerheterna ökar. Figur 3.3 visar hur en ny bäringsinmätning mellan fordon 2 och 3 sker vid steg 3 och ett nytt informationsutbyte över nätverket.

• Steg 4. Nu har det centrala filtret på nytt estimerat fram positioner och positionsosäkerheter och en uppdatering har skett, se figur 3.4. Nu har positionen och positionsosäkerheten för fordon 3 förbättrats i alla riktningar genom en serie olika inmätningar mellan fordonen i ”bataljonen”. Även positionen för fordon 2 påverkas men visas inte.

Figurserien 3.1 – 3.4 visar också vilken position navigeringssystemen estimerar, dessa är dock inte så bra men förbättras genom inmätningarna.

Fordon 1 TNS Innehåller Navigeringscentral Fordon 2 Dödräknande Fordon 3

Dödräknande Fordonets position

Vad navigeringssystemet estimerar Fordonets position

Sanning Avståndsinmätning

Bäringsinmätning

Positionsosäkerhet

Figur 3.1. Fordonen vid steg 1.

Fordon 1

Fordon 3 Fordon 2

Fordon1

Fordon3

Fordon 2

Bäringsinmätning

Track

Figur 3.3. Fordonen vid steg 3.

Fordon 1

Fordon 3 Fordon 2

4 Simuleringsmiljö

4.1 Allmänt

Idén med distribuerad navigering kommer att undersökas och demonstreras i en simuleringsmiljö som ska efterlikna verkligheten så mycket som möjligt. Naturligtvis så måste man göra många förenklingar och approximationer, men syftet är ändå att simuleringsmiljön ska vara så verklighetstrogen att rimliga slutsatser kan dras.

Önskvärt skulle även vara att kunna verifiera simuleringsmiljön mot verkliga test, för att förvissa sig om simuleringsmiljöns giltighet. Men detta har inte rymts inom ramen för examensarbetet.

Figur 4.1 visar uppbyggnaden av simuleringsmodellen. De streckade boxarna implementeras ej. Omvärlden består av en platt miljö med obefintlig terräng. De streckade flödena har ingen verkan.

Nedan följer de förenklingar som har gjorts och sedan hur de olika modulerna i simuleringsmodellen fungerar. Nav system Trajectory Scenario sann position sann heading tidpunkter positioner tid est. position est. heading Sikte Line of Sight Detektion est. avstånd est. bäring Terräng Nätverk Uppdaterat Navdata avstånd bäring Filter Operatör inmätnings tidpunkter LoS inmätning målidentifiering grid GIS magn filterparametrar tidsfördröjningar ”realtid” off-line korrigeringar Uppdatering Fordon

Figur 4.1. Visar simuleringsmodellen.

4.2 Förenklingar

De olika fordonstyperna i simuleringsmiljön behandlas olika. De fordon som har ett högpresterande navigeringssystem (TNS) betraktas som ideala. Detta innebär att navigeringssystemet inte ger felaktiga värden.

För fordonen med ett lågpresterande navigeringssystem (enkelt dödräknande) används en modell för feltillväxten. På så sätt genereras felaktiga navigeringsvärden. Mer om det i kapitel 4.5.

4.3 Modulen

Scenario

Scenario skickar data för varje fordon bestående av en vektor med brytpunkter (nordliga och östliga koordinater). Även en vektor med tidsangivelser för varje brytpunkt skickas. Mellan varje brytpunkt rör sig fordonet på en rak linje, detta för att förenkla beräkningarna. Detta kan naturligtvis ses som en begränsning, men använder man tillräckligt många brytpunkter så kan vilken färdväg som helst beskrivas med godtycklig noggrannhet.

4.4 Modulen

Trajectory

Från de data som scenariot ger ska koordinater för varje tidssteg som simulatorn stegas fram med beräknas. Även hastigheter och kurs ska beräknas. För varje tidssteg mellan två brytpunkter är hastigheten och kursen konstant. Detta ska göras för varje fordon.

För att underlätta beräkningarna så införs ett villkor på data från scenariot. Tidsintervallet mellan två brytpunkter måste vara jämt delbart med det tidssteg simulatorn stegas fram med.

4.5 Modulen

Navigeringssystem

4.5.1 Mekaniseringsmodell

För att simulera navigeringssystemen används en felmodell. För att ta fram en felmodell används en mekaniseringsmodell. Denna beskriver vilka relationer som råder mellan fordonets kinematiska tillstånd. Detta gäller bara de fordon som har en lågpresterande navigeringssystem, de andra betraktas som ideala och har därför inget behov av en modell för simuleringar.

Det koordinatsystem som används i mekaniseringsmodellen är ett högersystem med (North, East, Down) som x-, y- och z-axlar.

Det dödräknande systemet använder sig av två olika navigeringssensorer, en odometer och en magnetkompass. Navigationssystemet uttrycks då på följande diskreta form där den är uppdelad i nordlig respektive östlig koordinat,

, sin cos 1 1 k k k k k k k k d e e d n n ψ ψ ∆ + = ∆ + = + + (4.1) där , 1 k k k d d d = − ∆ ₊ (4.2)

där d är distans och ψ är kursen.

Kursen ψ antas vara konstant under tidsintervallet .

1 k

k t

t

T = ₊ − (4.3)

k d k ψ T

⋅

cos

⋅

sin

1 −

+

T T

+

+

×

×

T k n k e

Figur 4.2. Blockschema över mekaniseringsmodellen.

4.5.2 Felmodell

Då modulen trajectory generar de sanna värdena på koordinaterna vill man ha en modell för felen i koordinaterna, en felmodell. Dessa fel adderas sedan på de sanna värdena för att simulera utdata från navigeringssystemet. För att ta fram en sådan modell kan man först derivera ekvationerna (4.1) med avseende på alla ingående variabler och man får följande

k k k k k k k k k d n d n n n ψ ψ ψ sin cos 1 1 1 1 ∆ − = ∂ ∂ = ∆ ∂ ∂ = ∂ ∂ + + + (4.4)

för nordlig koordinat och

k k k k k k k k d e d e e e ψ ψ ψ cos sin 1 1 1 1 ∆ = ∂ ∂ = ∆ ∂ ∂ = ∂ ∂ + + + (4.5)

för östlig koordinat. Utnyttjar man dessa derivator, d.v.s. (4.4) och (4.5), i en 1:a ordningens linjärisering av felfortplantningen fås följande ekvationer

. cos sin sin cos 1 1 k k k k k k k k k k k k k k d d e e d d n n δψ ψ δ ψ δ δ δψ ψ δ ψ δ δ ∆ + ∆ + = ∆ − ∆ + = + + (4.6) Ekvationer i (4.6) beskriver hur felen i navigeringssensorerna fortplantas vidare till

koordinaterna.

4.5.3 Odometer

Odometern mäter distansen fordonet kör. Det fel som odometern gör modelleras med ett skalfaktorfel , d k d = ∆ ∆ δ (4.7)

där ∆d är körd distans och k är skalfaktorn. Skalfaktorn är olika för olika fordon. 4.5.4 Magnetkompass

Magnetkompassen mäter kursen mot magnetiska norr. Problemet är att man önskar ha kursen ψ mot geografiska norr. Skillnaden mellan dessa två betecknas deklination. Deklinationen beror på förändringar och störningar i jordens magnetfält. Vissa av dessa kan man förutse och ta hänsyn till, men ett kvarvarande fel finns och betecknas med

,

0

δψ (4.8)

vilket är gemensamt för alla fordon.

Ett annat problem är att fordonet själv stör magnetfältet. Det är magnetiska material i fordonet som orsakar magnetisk deviation. Detta har modellerats som ett fel som beror på fordonets kurs. Felet betecknas med

),δψ_dev =Asin(ψ +B (4.9)

där felet har modellerats som en sinuskurva med amplituden A och fasförskjutningen B . Deviationsfelet är olika för olika fordon. Uttrycket (4.9) kan approximeras och delas upp i två delar, ψ δψidccos (4.10) och , sinψ δψids (4.11)

där beteckningen id står för att felet är unikt för varje fordon.

Ett ytterligare fel som tillkommer är ett kalibreringsfel. Detta betecknas med ,

id

δψ (4.12)

och även detta fel är olika för olika fordon.

Samlas nu alla dessa olika fel ihop fås följande totala felmodell för magnetkompassen . sin cos 0 δψ δψ ψ δψ ψ δψ δψ = + id + idc + ids (4.13) 4.5.5 Slutlig felmodell

Införs nu de felmodeller som är framtagna för navigeringssensorerna i (4.6) fås följande ekvationer ). sin cos ( cos sin ) sin cos ( sin cos 0 1 0 1 k ids k idc id k k k k k k k ids k idc id k k k k k k d d k e e d d k n n ψ δψ ψ δψ δψ δψ ψ ψ δ δ ψ δψ ψ δψ δψ δψ ψ ψ δ δ + + + ⋅ ∆ + ∆ + = + + + ⋅ ∆ − ∆ + = + + (4.14) Införs nu beteckningarna k k k e k k k n d r d r ψ ψ sin cos , , ∆ = ∆ ∆ = ∆ (4.15) blir ekvationerna (4.14)

). sin cos ( ) ( ) sin cos ( ) ( , 0 , , 1 , 0 , , 1 k ids k idc k n id k n k e k k k ids k idc k e id k e k n k k r r k r e e r r k r n n ψ δψ ψ δψ δψ δψ δ δ ψ δψ ψ δψ δψ δψ δ δ + ∆ + + ∆ + ∆ + = + ∆ − + ∆ − ∆ + = + + (4.16)

Ekvationerna i (4.16) används för att simulera felen i navigeringssystemet.

Värden på k, δψid, δψidc och δψids slumpas fram för varje fordon och δψ0 för alla fordon.

För att få rimliga värden som har verklighetsanknytning används felbudgeten från kapitel 4.5.6 som underlag. Felen beräknas för varje tidssteg och ackumuleras i varje brytpunkt. Detta beräknas för varje fordon som har ett lågpresterande navigeringssystem. För de navigeringssystem som betraktas som ideala adderas inget fel.

Utdata från navigeringssystemet skickas till nätverket. 4.5.6 Felbudget

Värdena på parametrarna, k, δψ₀, δψ_id, δψ_idc och δψ_ids har man ingen exakt kunskap om. Värdena kan ej direkt mätas, utan kan bara i viss mån estimeras. Men erfarenhetsmässigt finns det ändå en viss kunskap, en kunskap om deras statistiska egenskaper. Dessa kan utryckas i en felbudget , ) , ( ) , ( ) , ( ) , ( ) , ( 0 0 0 ids idc ids ids ids idc idc idc id id id k k m N m N m N m N m N k ψ ψ ψ ψ ψ ψ ψ ψ ψ ψ σ σ σ δψ σ δψ σ δψ σ δψ σ = ∈ ∈ ∈ ∈ ∈ (4.17) där ]. [ 5 . 1 ] [ 6 . 0 ] [ 2 . 0 01 . 0 ] [ 0 0 0 0 rad rad rad rad m m m m m ids idc id k ids idc id k = = = = = = = = = = ψ ψ ψ ψ ψ ψ ψ ψ σ σ σ σ σ (4.18)

4.6 Modulen

Operatör

Operatören avgör om en inmätning mot ett annat fordon ska göras. Huruvida ett sådant beslut tas eller inte beror på en slumpmodell. Modellen som används är enkel, där en tidpunkt för inmätningen slumpas fram utifrån en likformig sannolikhetsfördelning på ett angivet tidsintervall.

Även andra modeller kan användas där t.ex. positionsosäkerheten spelar in. Ju större osäkerheten i positionen är desto större är sannolikheten att operatören beslutar att göra en inmätning.

När en inmätning ska göras skickas en signal till modulen siktet som utför inmätningen.

4.7 Modulen

Sikte

4.7.1 Allmänt

I simuleringsmodellen kan siktet utföra inmätningar som består av avstånd och bäring mellan två fordon. Här används de sanna värdena på positionskoordinaterna från modulen trajectory för att räkna ut de sanna värdena på avstånd och bäring. För att simulera verkligheten ska nu fel adderas till dessa sanna värden, då siktet inte ger exakta mätvärden utan är behäftade med vissa fel.

Även andra inmätningar skulle kunna utföras, t.ex. bara en avståndsmätning eller bara en bäringsmätning. Detta har dock inte implementerats.

4.7.2 Felmodeller

En felmodell för en mätning av bäringen mot ett fordon ser ut som följande

ϕ

ε δϕ δψ

δϕ = + 0+ (4.19)

där δψ är felet i kurs, δϕ0 är ett offsetfel och εϕ som är ett mätbrus. Felet i kurs påverkar

också siktet vilket medför att ett fordon med ett lågpresterande navigationssystem får en sämre bäringsinmätning än ett fordon med ett högpresterande navigationssystem.

En felmodell för en mätning av avståndet ser liknande ut. Den är

r

r

r δ ε

δ = 0+ (4.20)

där δr₀ är ett offsetfel och ε_r är mätbrus.

Värdena på δϕ₀ och δr₀ slumpas fram för varje sikte. Även här används en felbudget, se kapitel 4.7.3, som underlag för att få rimliga värden med verklighetsanknytning.

Utdata från siktet skickas till modulen nätverk. 4.7.3 Felbudgetar

Felbudgeten för bäringen är erfarenhetsmässigt

), , ( ) , ( 0 0 0 ϕ ε ϕ ε ϕ ϕ ϕ σ δε σ δϕ m N m N ∈ ∈ (4.21) där ]. [ 1 . 0 ] [ 1 . 0 ] [ 0 0 0 rad rad rad m m = = = = ϕ ε ϕ ϕ ε ϕ σ σ (4.22)

Felbudgeten för δψ fås från felbudgeten av magnetkompassen. Felbudgeten för avståndet är erfarenhetsmässigt

), , ( ) , ( _{0 0} 0 r r r r r m N m N r ε ε σ δε σ δ ∈ ∈ (4.23)

där ]. [ 2 ] [ 2 ] [ 0 0 0 m m m m m r r r r = = = = ε ε σ σ (4.24)

4.8 Modulen

Nätverk

Nätverket transporterar utdata från navigeringssystemet och från siktet till filtret. Tillbaka transporteras positionskorrigeringar till uppdatering. Detta gäller för varje fordon.

Nätverket består bara av en identitet, d.v.s. inga tidsfördröjningar finns eller några andra störningar inverkar. I en mer realistisk situation finns det naturligtvis både tidsfördröjningar och andra störningar, men en sådan modell ligger utanför ramen för detta examensarbete.

4.9 Modulen

Filter

Filtret tillhör egentligen inte simuleringsmodellen utan här implementeras Distribuerad navigering. En implementering har tagits fram och härledningen av den finns i kapitel 5. Indata till filtret är positioner från alla ingående fordon och även data från de inmätningar fordonen gör mot varandra. Utdata från filtret är positionskorrigeringar och positionsosäkerheter.

4.10 Modulen

Uppdatering

Denna modul korrigerar positionerna från navigeringssystemen med hjälp av utdata från filtret.

5 Filter

för

distribuerad

navigering

5.1 Översikt

För att kunna använda ett kalmanfilter behövs en tillståndsmodell av systemet. Tillståndsmodellen består av två olika delar, en tidsuppdateringsmodell och en mätuppdateringsmodell.

Först tas en tidsuppdateringsmodell fram för ett fordon och en mätuppdateringsmodell för en inmätning. Alla tidsuppdateringsmodeller för varje fordon sätts ihop till en gemensam tidsuppdateringsmodell och alla mätuppdateringsmodeller för alla inmätningar som sker vid samma tidpunkt sätts ihop till en gemensam mätuppdateringsmodell. Båda dessa modeller sätts sedan ihop till en större gemensam tillståndsmodell.

I kapitel 5.7 beskrivs tillvägagångssättet för kalmanfiltret.

5.2 Tidsuppdateringsmodell

Utifrån den felmodell som är härledd i kapitel 4.5.2 har en tidsuppdateringsmodell tagits fram för varje fordon i filtret. Ekvationerna för felmodellen är

), sin cos ( ) ( ) sin cos ( ) ( , 0 , , 1 , 0 , , 1 k ids k idc k n id k n k e k k k ids k idc k e id k e k n k k r r k r e e r r k r n n ψ δψ ψ δψ δψ δψ δ δ ψ δψ ψ δψ δψ δψ δ δ + ∆ + + ∆ + ∆ + = + ∆ − + ∆ − ∆ + = + + (5.1) där . , 1 , , , 1 , , k e k e k e k n k n k n r r r r r r − = ∆ − = ∆ + + (5.2) Naturligtvis har man inte tillgång till ∆r_n,k, ∆r_e,koch ψ_k. Istället används de värden

navigationssystemet räknar fram.

Den enda kunskapen som finns om parametrarna i ekvationerna, k,(δψ0+δψid), δψidc och ids

δψ är deras stokastiska egenskaper

. ) , ( ) , ( ) , ( ) , ( ) , ( 0 0 0 ids idc ids ids ids idc idc idc id id id k k m N m N m N m N m N k ψ ψ ψ ψ ψ ψ ψ ψ ψ ψ σ σ σ δψ σ δψ σ δψ σ δψ σ = ∈ ∈ ∈ ∈ ∈ (5.3)

Nu adderas δψ0 och δψid ihop till en parameter för att underlätta notationen och den

. ) , ( 2 0 2 0 id kt id kt kt kt kt m m m m N ψ ψ ψ ψ ψ ψ ψ ψ σ σ σ σ δψ + = + = ∈ (5.4)

Om nu alla parametrar betraktas som stokastiska processer fås följande tidsuppdateringsmodell . sin sin cos cos 1 0 0 1 , , , 1 k ids k n e k idc k n e k kt k n e k k e n k k r r r r r r k r r e n e n δψ ψ ψ δψ ψ ψ δψ δ δ δ δ       ∆ ∆ − +       ∆ ∆ − +       ∆ ∆ − +       ∆ ∆ +             =       + (5.5)

Problemet är att de stokastiska processerna k , _k δψkt ,k, δψidc,k och δψids ,k inte är

okorrelerade med sig själva över tiden, vilket man vill ha i en tillståndsmodell. Detta inses genom att undersöka processernas stokastiska egenskaper, exempelvis k _k

[

]

, ) ( 0 ] [ 1 2 k k k T k k k k k t t T k k E T R k E m − = = = = = + + σ (5.6)

där T alltså är tidsintervallet mellan tidstegen filtret arbetar i. Även för de andra stokastiska processerna blir resultatet likartat.

För att åtgärda detta problem kan ett shaping filter användas, se appendix A.3. En stokastisk variabel får följande shaping filter

2 1 ) 0 ( =σ = + x k k R x x (5.7) Detta innebär att ett tillstånd måste tillföras för varje stokastisk variabel.

Utökas nu tidsuppdateringsmodellen med fyra ytterligare tillstånd så undviker man de korrelerade processerna. Modellen blir då

. 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 sin cos 1 0 sin cos 0 1 1 ids k idc kt k n n n e e e e n k ids idc id k e n r r r r r r r r k e n                                         ∆ ∆ ∆ ∆ ∆ − ∆ − ∆ − ∆ =                     + δψ δψ δψ δ δ ψ ψ ψ ψ δψ δψ δψ δ δ (5.8)

Startvärdena på varianserna på de utökade tillstånden fås från felbudgeten i kapitel 4.5.6.

5.3 Tillståndsreducering

Modellen i (5.8) har inte använts p.g.a. av observerbarhetsproblem. Antalet tillstånd är för stort. Lösningen på detta är att reducera modellkomplexiteten genom att minska antalet tillstånd.

För att få bättre observerbarhet slås alla fel som har med magnetkompassen att göra ihop. Felet blir då . sin cosψ δψ ψ δψ δψ δψ = kt+ idc + ids (5.9)

Genom detta reduceras antal tillstånd med två och bättre observerbarhet erhålls.

Parametern δψ får ett beroende av kursen ψ och andra stokastiska egenskaper. Dess väntevärde är

[ ]

[

]

[

] [

]

[

]

, 0 sin cos sin cos sin cos = + + = + + = + + = = ψ ψ ψ δψ ψ δψ δψ ψ δψ ψ δψ δψ δψ ψ ψ δψ ids idc m m m E E E E E m kt ids idc kt ids idc kt (5.10)

och variansen blir

[

]

[

]

[ ] [ ]

[ ]

. sin cos ) sin cos ( ) sin cos ( 2 2 2 2 2 2 2 2 idc kt ids idc kt ids idc kt ids idc kt E E E E E σ σ ψ δψ ψ δψ δψ ψ δψ ψ δψ δψ ψ δψ ψ δψ δψ δψδψ σδψ + = + + = + + ⋅ + + = = (5.11)

Även denna parameter betraktas som en stokastisk process med väntevärde

[ ]

=0, =E k m_δψ δψ (5.12) och kovariansen

[

]

[

]

[

]

[

]

[

]

. sin sin cos cos ) sin cos ( ) sin cos ( ) ( , , , , , , , , , , , , T k ids k ids T k k T k idc k idc T k k T k kt k kt T k T k ids T k T k idc T k kt k k ids k k idc k kt T k k E E E E E T R + + + + + + + + + + + + + = + + ⋅ + + = = δψ δψ ψ ψ δψ δψ ψ ψ δψ δψ ψ δψ ψ δψ δψ ψ δψ ψ δψ δψ δψ δψ δψ (5.13) Med , k k T k ψ θ ψ ₊ = + (5.14)

där θ_k är förändringen i kurs under tidsintervallet T =t_k+1−t_kblir kovariansen

. cos )) sin( sin ) cos( (cos ) ( 2 2 2 2 k k k k k k k idc kt idc kt T R θ σ σ θ ψ ψ θ ψ ψ σ σ ψ ψ ψ ψ δψ + = + + + + = (5.15) Denna process är inte okorrelerad med sig själv över tiden utan även här måste ett shaping

filter användas. Se appendix A.3 för mer om shaping filter. Ett shaping filter på följande form används , 1 k k k k k x z w G x F x = + = + (5.16)

där w är vitt brus med varians 1. Här måste _k F och _k G bestämmas. Först multipliceras _k

(5.16) med x i både högerled och vänsterled och väntevärdet tas _k

[

]

[ ]

[

]

. cos ) ( cos 2 2 2 2 2 2 2 2 1 idc kt idc kt idc kt idc kt k k k k k k k k k k k k F F w x E G x x E F x x E ψ ψ ψ ψ ψ ψ ψ ψ σ σ θ σ σ σ σ θ σ σ + + = + = + + = + (5.17)

För att bestämma G kvadreras _k x_k+1 i (5.16) och väntevärde tas

[

]

[ ]

[

]

. ) cos 1 ( 2 ) cos 1 ( ) ( 2 2 2 2 2 4 2 2 2 2 2 2 2 2 1 1 idc kt idc kt idc idc kt idc kt k k k k k k k k k k k k k G G F w w E G x x E F x x E ψ ψ ψ ψ ψ ψ ψ ψ ψ σ σ θ σ σ θ σ σ σ σ σ + − + − = + + = + + = + + (5.18)

Hela tidsuppdateringsmodellen blir nu

. 0 0 0 0 0 0 0 1 0 0 1 0 0 1 1 k k k k n e e n k w G k e n F r r r r k e n             +                         ∆ ∆ ∆ − ∆ =             + δψ δ δ δψ δ δ (5.19)

Mer kortfattat kan modellen skrivas som

, ) ( ) ( ) ( ) ( ) ( 1 i i k i k i k i k A x B w_k x ₊ = + (5.20)

där index i anger fordonet.

5.4 Mätuppdateringsmodell

I simuleringsmiljön har alla fordon ett sikte med vilket bäring och avstånd kan mätas samtidigt. Nedan tas en modell fram för hur kalmanfiltret använder en sådan inmätning.

Följande beteckningar, se figur 5.1, används vid en inmätning. Vinkeln ϕ är bäringen mot målet och r är avståndet.

ϕ r Fordon i Fordon j N E

Fordon i mäter alltså mot fordon j och de mätvärden man erhåller är . ~ sin ~ ~ cos ~       ϕ ϕ r r (5.21) Dessa mätvärden innehåller fel, både i r~ och i ϕ~ , som påverkar de sanna värdena enligt

. cos sin sin cos sin cos ~ sin ~ ~ cos ~             − +       =       δϕ δ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ r r r r r r r (5.22) Detta uttryck fås om man deriverar

, sin cos       ϕ ϕ r r (5.23) och gör en 1:a ordningens linjärisering.

Positionerna som navigeringssystemen ger är . ~ ~       e n (5.24) Dessa innehåller också fel

. ~ ~       +       =       e n e n e n δ δ (5.25) Bildar man nu skillnaden mellan de uppmätta värdena, (5.21) och (5.24), så får man följande

mätuppdateringsmodell , 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 r r r e n e n e e n n e e n n r r r r r e e r n n r z z δϕ δ ϕ ϕ ϕ ϕ δ δ δ δ δ δ δ δ δϕ δ ϕ ϕ ϕ ϕ ϕ ϕ ϕ ϕ (5.26)

där index i anger fordonet, vilket också gäller genomgående i resten av kapitlet.

Vid en mätning där ett fordon med ett högpresterande navigeringssystem ingår blir mätuppdateringsmodellen annorlunda, vilket även påpekades i kapitel 3.4. Den blir

, cos sin sin cos ) ( ) (             − +       ± =       δϕ δ ϕ ϕ ϕ ϕ δ δ r r r e n z z e n (5.27) där plus- eller minustecknet beror på vem som mäter mot vem. Plustecken om fordonet med

det lågpresterande navigeringssystem mäter in det fordonet med det högpresterande navigeringssystemet och tvärtom annars.

      − ϕ ϕ ϕ ϕ cos sin sin cos r r (5.28) har man inte naturligtvis inte tillgång till de sanna värdena r och ϕ, så man blir tvungen att

använda . ~ ~ ϕ ϕ ≈ ≈r r (5.29) Vid flera inmätningar vid samma tidpunkt utökas mätuppdateringsmodellen.

Mer kortfattat kan modellen skrivas som

, ~ cos ~ ~ sin ~ sin ~ ~ cos 0 0 0 1 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 1 0 0 ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) (       =       − =       − − = + = i i l k k l k j i j i l k l k l k k l k l k r v r r D C v D x C z δϕ δ ϕ ϕ ϕ ϕ K K K K (5.30)

där index l anger mätningen och x är här hela tillståndsvektorn för filtret. k

5.5 Tillståndsmodell

Sätter man nu ihop tidsuppdateringsmodellerna (5.20) för varje fordon som ingår i filtret och mätuppdateringsmodellen (5.30) får man en tillståndsmodell. Från mätuppdateringsmodellen ser man att det är bara vissa tillstånd som påverkas av en mätning beroende på om en inmätning har gjorts mellan dessa fordon. Man får alltså en tidsberoende mätmatris.

. 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ) ( ) 2 ( ) 1 ( ) ( ) 2 ( ) 1 ( ) ( ) 2 ( ) 1 ( ) ( ) 2 ( ) 1 ( ) ( ) 2 ( ) 1 ( ) ( ) 2 ( ) 1 ( ) ( ) 2 ( ) 1 ( ) ( ) 2 ( ) 1 ( ) ( ) 2 ( ) 1 ( 1 ) ( ) 2 ( ) 1 ( k L k L k N k L k L k N k N k N k N k N v v v D D D x x x C C C z z z w w w B B B x x x A A A x x x                             +                             =                                           +                             =               + M O M M L L M M M M O M M L L M O M M L L M (5.31)

5.6

Ej implementerade mätuppdateringsmodeller

När man vid en inmätning mellan två fordon bara mäter avståndet eller bäringen får man två andra mätuppdateringsmodeller. Dessa är dock inte implementerade men beskrivs här nedan. När fordon i gör en avståndsmätning mot fordon j enligt figur 5.1 erhåller man mätvärdet

. ~

r (5.32)

Detta mätvärde innehåller ett fel,

.

~ _{r r}

r = +δ (5.33)

Positionerna som navigeringssystemen ger är . ~ ~       e n (5.34) Dessa innehåller också fel

. ~ ~       +       =       e n e n e n δ δ (5.35) Positionerna som de båda fordonen ger uppskattar även avståndet mellan fordonen och kan då

skrivas på följande form

, ~ nav nav r r r = +δ (5.36) där r är . ) ( ) (n(j) n(i) 2 e(j) e(i) 2 r= − + − (5.37)

Ett uttryck för δr_nav fås om man deriverar (5.37) och gör en 1:a ordningens linjärisering

. sin cos sin sin cos cos ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( r e e r n n e e n n r i j i j i j i j nav − = − = − + − = ϕ ϕ δ ϕ δ ϕ δ ϕ δ ϕ δ (5.38)

Bildar man nu skillnaden mellan de uppmätta värdena, (5.32) och (5.34), så får man följande mätuppdateringsmodell

(

cos sin cos sin

)

.

) ( ~ ~ ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( i j j i i i nav nav i nav r r e n e n r r r r r r r r z δ δ δ δ δ ϕ ϕ ϕ ϕ δ δ δ δ +               − − = + − = + − + = − = (5.39)

Vid beräkning av matrisen