Institutionen för systemteknik

Transkript

1 Institutionen för systemteknik Department of Electrical Engineering Examensarbete Sensorfusion av GPS och IMU för en racingtillämpning Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Anders Blomfeldt Rasmus Haverstad LITH-ISY-EX--08/4143--SE Linköping 2008 Department of Electrical Engineering Linköpings universitet SE Linköping, Sweden Linköpings tekniska högskola Linköpings universitet Linköping

2

3 Sensorfusion av GPS och IMU för en racingtillämpning Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Anders Blomfeldt Rasmus Haverstad LITH-ISY-EX--08/4143--SE Handledare: Examinator: Johan Sjöberg isy, Linköpings universitet Anders Birgersson Zetiq Development Fredrik Gustafsson isy, Linköpings universitet Linköping, 2 juli, 2008

4

5 Avdelning, Institution Division, Department Division of Automatic Control Department of Electrical Engineering Linköpings universitet SE Linköping, Sweden Datum Date Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport ISBN ISRN LITH-ISY-EX--08/4143--SE Serietitel och serienummer Title of series, numbering ISSN URL för elektronisk version Titel Title Sensorfusion av GPS och IMU för en racingtillämpning Sensor Fusion of GPS and IMU for a Racing Application Författare Author Anders Blomfeldt Rasmus Haverstad Sammanfattning Abstract AIMS AB is a Swedish company that develops and sells inertial sensor systems. A new product is under research which basically is an aided inertial navigation system where unwanted drift is eliminated. This can be done by integrating a GPSreciever into the system and fuse measurement data from GPS and IMU. Such a product is useful in racing, where engineers always are searching for the optimal performance and the shortest lap times. Today the only feedback the engineers and mechanics get from their work is the driver s comments and lap times. It would be of great help if it was possible to see the behaviour of the car directly in technical terms. The focus of this master thesis is fusing a GPS receiver and an IMU using an Extended Kalman Filter (EKF). The main task of the EKF is to estimate the errors of the intertial measurements and correct the measurements to eliminate drift in velocity and position. To verify the system, data collection was done togheter with the team G2 Racing and Tony Ring. The measurements were done in Tony s Ferrari F430 Pista during two days at the Paul Ricard circuit in southern France. From the results possibilites to further estimate interesting entities were evaluated. Nyckelord Keywords sensorfusion, GPS, IMU

6

7 Abstract AIMS AB is a Swedish company that develops and sells inertial sensor systems. A new product is under research which basically is an aided inertial navigation system where unwanted drift is eliminated. This can be done by integrating a GPS-reciever into the system and fuse measurement data from GPS and IMU. Such a product is useful in racing, where engineers always are searching for the optimal performance and the shortest lap times. Today the only feedback the engineers and mechanics get from their work is the driver s comments and lap times. It would be of great help if it was possible to see the behaviour of the car directly in technical terms. The focus of this master thesis is fusing a GPS receiver and an IMU using an Extended Kalman Filter (EKF). The main task of the EKF is to estimate the errors of the intertial measurements and correct the measurements to eliminate drift in velocity and position. To verify the system, data collection was done togheter with the team G2 Racing and Tony Ring. The measurements were done in Tony s Ferrari F430 Pista during two days at the Paul Ricard circuit in southern France. From the results possibilites to further estimate interesting entities were evaluated. Sammanfattning AIMS AB är ett företag som tillverkar och säljer olika typer av tröghetsnavigeringssystem. En ny produkt med ett tröghetsnavigeringssystem stöttat av GPS är under utveckling. Med sensorfusion kan GPS- och IMU-mätningar integreras genom användning av ett Extended Kalman Filter (EKF). Ett sådant system kan komma till användning inom racing där strävan efter optimal prestanda och korta varvtider är stor. Den information ingenjörerna får om bilens beteende är ofta enbart förarens kommentarer. Förarens beskrivning kan ofta vara svår att översätta till tekniska termer som behövs för att enkelt justera inställningar och nå önskade egenskaper. Det skulle vara till stor hjälp om ingenjörerna direkt fick tillgång till teknisk information om fordonets beteende. I detta examensarbete ligger fokus på sensorfusion av GPS- och IMU-mätningar genom användning av ett Extended Kalman Filter (EKF). Huvuduppgiften för kalmanfiltret är att skatta fel i mätningar av accelerationer och vinkelhastigheter så att korrigering för dessa kan göras och därmed drift i position elimineras. För verifiering av systemet har datainsamling utförts tillsammans med teamet G2 Racing och Tony Ring. Mätningnarna är gjorda i Tonys Ferrari F430 Pista under två dagar på banan Paul Ricard i södra Frankrike. Ur resultatet undersöks även möjligheterna till vidare skattning av intressanta data som kan vara till hjälp för ingenjörerna i ett racingteam. v

8

9 Tack Vi vill tacka AIMS för att vi fått möjligheten att göra detta intressanta och lärorika examensarbete. Ett stort tack även till vår handledare Dr. Johan Sjöberg för gott stöd och givande diskussioner samt till vår examinator Prof. Fredrik Gustafsson på institutionen för systemteknik vid Linköpings Universitet. Tack även till annan personal som hjälpt oss under arbetets gång. Dessutom riktas ett stort tack till Tony Ring och G2 Racing för visat intresse av vårt arbete och det mycket goda samarbetet vid insamling av mätdata. Ett stort tack ägnas teamets tekniska chef Stefan Lehmann. Besöket på Paul Ricard är ett minne för livet. vii

10

11 Innehåll 1 Introduktion Bakgrund Problemformulering Mål Grundläggande positionering Positionering med IMU Mätning av accelerationer Mätning av vinkelhastigheter Felkällor Positionering med GPS Principen för positionering med GPS GPS-signaler Felkällor Mätning av hastighet Differentiell GPS (DGPS) Integration Navigationssystem Koordinatsystem World Geodetic System 1984 (WGS84) Transformationer Attitydrepresentation Eulervinklar Direction Cosine Matrix, Cb n TNS-ekvationer Generell navigationsekvation Ekvationer i navigationsramen Feldynamik i navigationsramen Sammanfattning av ekvationer Integration av GPS och IMU Sensormodeller Mätmodell för accelerometrar Mätmodell för gyron ix

12 x Innehåll Mätmodell för GPS Kalmanfiltret Introduktion Filtermodell Extended Kalman Filter Trimning av filterparametrar Observerbarhet Tolkning av IMUns mätvärden Initiering Implementering Feltillståndsmodell TNS-modell Hävarm Icke-holonomiska villkor Hårdvara IMU GPS Resultat och Analys Datainsamling Monteringsfel Resultat Positionsstöttning Icke-Holonomiska Villkor Felanalys Observerbarhet Diskussion Mervärden Problem Fortsatt arbete Slutsatser 63

13 Nomenklatur Operator Betydelse ( ) Mätvärde ( ) ˆ Skattning ( ) Predikterat värde ( ) Tidsderivata Kryssprodukt [ ] Skevsymetrisk matris E [ ] Väntevärde f( ) Funktion av [ ] Skevsymmetrisk form av kryssprodukt diag() Diagonalmatris Akronym Betydelse DCM Direction Cosine Matrix ECEF Earth Centered Earth Fixed EKF Extended Kalman Filter GPS Global Positioning System IMU Inertial Measurement Unit TNS Tröghetsnavigeringssystem INS Inertial Navigation System KF Kalman Filter MEMS Micro Electro Mechanical System NED North-East-Down-koordinatsystem WGS84 World Geodic System

14 2 Nomenklatur Symbol Betydelse x k Tillståndsvektor vid tidpunkt k x k+1 Tillståndvektor vid tidpunkt k +1 u Insignal y Mätsignal p Position v Hastighet Ψ Eulervinklar φ Roll θ Pitch ψ Yaw δp Fel i position δv Fel i hastighet δψ Fel i eulervinklar δa Bias i accelerometrar δω Bias i gyron Ω Skevsymmetrisk form av ω Rω,a b Transformationsmatris för vinkelhastigheter mellan koordinatsystem a och b x, y, z Koordinataxlar i ett kroppsfixt system N,E,D Koordinataxlar i ett NED-system a N,a E,a D Accelerationer i respektive koordinataxel i ett NED-system ω x,ω y,ω z Vinkelhastigheter i respektive koordinataxel u Insignal A, B, C Matriser i tillståndsmodellen (kontinuerlig tid) F, G, H Matriser i tillståndsmodellen (diskret tid) F TNS,G TNS Matris med tröghetsdynamik Q, R Kovariansmatriser och designvariabler i kalmanfiltret P Av kalmanfiltret skattad kovariansmatris I n Enhetsmatris av storlek n 0 m n nollmatris med m rader och n kolumner Ca b Rotationsmatris mellan koordinatsystem a och b R M,R T Avstånd till ekvatorialplanet respektive jordens rotationsaxel R E Jordens ekvatorialradie f Specifika krafter [m/s] g Gravitationsvektor ϕ Latitud λ Longitud h Altitud (höjd över havsnivån)

15 Kapitel 1 Introduktion 1.1 Bakgrund Positionering av olika slag har ett brett användningsområde idag. Det kan handla om allt från att hitta i skogen eller i trafiken till att få självgående fordon att navigera. När GPS (Global Positioning System) utvecklades av det amerikanska försvaret fick man tillgång till ett globalt system som med enkel utrustning kan användas för att mäta både hastighet och position. I många tillämpningar ger GPS mer än tillräcklig noggrannhet, men i vissa typer av system behövs bättre precision. En annan typ av sensor som är användbar vid positionering är en IMU (Inertial Measurement Unit). En IMU mäter accelerationer och vinkelhastigheter varur dess position och attityd kan beräknas. Noggranna IMUer är i allmänhet dyra och strävan efter att bygga system som inte får kosta allt för mycket har lett till utveckling av billigare typer av sensorer. En MEMS-IMU (Micro Electro Mechanical System - Inertial Measurement Unit) är en typ av IMU som är betydligt billigare än de allra bästa systemen. Priset för detta är försämrad prestanda hos sensorerna. 1.2 Problemformulering Inom banracing kan noggranna mätningar av fordonets position och rörelser ge värdefull information om dess beteende och för autonoma farkoster krävs någon form av positionsbestämning för att de ska kunna navigera. Olika typer av positioneringssystem har olika svagheter. Genom att utnyttja flera sensorer med olika styrkor och svagheter kan bättre skattningar göras än vad som är möjligt med de respektive systemen var för sig. 1.3 Mål Företaget AIMS vill ta fram ett system för att positionera ett fordon så bra som möjligt. Detta ska göras genom att integrera en eller flera IMUer med en GPS- 3

16 4 Introduktion mottagare med hjälp av ett Extended Kalman Filter (EKF). I detta examensarbete tas ett filter för fusioneringen av sensordata fram. En studie ska därefter göras om vilka intressanta parametrar som kan beräknas ur de data som mäts. Fokus ligger inte på att ta fram ett filter som kan köras online, men det ska vara lätt att modifiera algoritmerna så att realtidsberäkning av intressanta parametrar kan göras.

17 Kapitel 2 Grundläggande positionering I detta kapitel ges en översiktlig introduktion till de tekniker för positionering som kommer att användas i arbetet. Till en början beskrivs principerna för hur en MEMS-IMU fungerar. Därefter ges en introduktion till GPS-systemet. Till sist diskuteras hur kompensering för de brister som finns i respektive teknik kan genomföras genom att integrera IMU och GPS i ett gemensamt system. 2.1 Positionering med IMU Ett sätt att bestämma position för ett objekt är att kontinuerligt mäta dess acceleration. Positionen fås genom att integrera den uppmätta accelerationen två gånger med avseende på tiden. Ett enkelt exempel är ett tåg som kan röra sig i båda riktningarna längs ett rakt spår. Genom att mäta accelerationen och integrera två gånger får man positionen för tåget på spåret. För att positionera ett objekt i tre dimensioner behöver accelerationer mätas i tre riktningar. I de fall ett objekt även kan rotera måste kännedom finnas kring hur det kroppsfasta koordinatsystemet är relaterat till inertialramen. I varje tidpunkt måste vinklarna mellan systemens koordinataxlar kännas till. Ett sätt att beräkna vinklarna är att integrera de mätta vinkelhastigheterna. Inledningsvis beskrivs hur accelerationer och vinkelhastigheter kan mätas Mätning av accelerationer Newtons andra lag säger att en kropps acceleration är proportionell mot de sammanlagda krafterna som verkar på den, med kroppens massa som proportionalitetskonstant. Matematiskt kan detta skrivas som F = ma. För att mäta accelerationer kan man tänka sig en anordning med en fjäder med fjäderkonstanten k och en massa m. När anordningen utsätts för en acceleration gör massans tröghetsmoment att fjädern tänjs. Fjäderns kraft verkande på massan är proportionell mot fjäderns förlängning, δ som F = kδ. Genom att mäta förlängningen kan accelerationen beräknas som a = F/m = kδ/m. I grunden används denna princip i en accelerometer av MEMS-typ, men själva mätningen kan göras på ett flertal 5

18 6 Grundläggande positionering olika sätt. Exempelvis kan istället för användning av en fjäder, ett magnetiskt fält låtas påverka massan för att hålla den på plats. Styrkan på magnetfältet är proportionell mot den kraft massan utsätts för vid acceleration. Om vetskap om magnetfältets styrka finns kan kraften beräknas, varur accelerationen sedan fås. Det vanliga är att använda tre stycken endimensionella accelerometrar monterade ortogonalt mot varandra. (Titterton och Weston, 2004) Mätning av vinkelhastigheter En sensor som är lämplig för att mäta rotationsrörelse är ett gyro. Gyrot mäter vinkelhastighet vilken genom en integration ger en vinkel. Principen för ett gyro kan se väldigt olika ut, men i grunden utnyttjas tröghetsmomentet hos en kropp som roterar med hög hastighet. När en sådan utsätts för en rotation kring en annan axel än kroppens rotationsaxel skapas en coriolisacceleration som kan mätas. För en IMU av MEMS-typ används ofta ett vibrationsgyro som huvudsakligen skiljer sig från grundprincipen genom att den så kallade kontrollmassan vibrerar istället för roterar. Ett gyro för varje axel som objektet kan rotera kring behövs och precis som för accelerometrar monteras vanligen tre gyron ortogonalt mot varandra. (Titterton och Weston, 2004) Felkällor Om det i någon sensor existerar ett fel, integreras detta fel hela tiden och på sikt blir felet i position stort. Ett sådant fel kan till exempel vara en bias, det vill säga att en accelerometer mäter en acceleration med ett konstant fel eller att ett gyro mäter en vinkelhastighet även när IMUn ligger stilla. Detta leder till att positionen, som är en dubbelintegral av accelerationen, driver iväg. Ett annat fel som kan förekomma är skalfel. Sensorerna ger då ett mätvärde som är proportionellt mot den verkliga mätstorheten med en faktor skild från ett. Båda dessa sensorfel är vanliga problem, framför allt i billigare positioneringssystem, och reducering av dessa fel är av stor betydelse när sensorerna integreras i ett mätsystem. (Grewal m.fl., 2007) 2.2 Positionering med GPS GPS-systemet är ett av flera system av typen Global Navigation Satellite System (GNSS). Systemet består av 24 eller fler (32 i februari 2008, varav 31 fungerande) satelliter som kretsar i sex olika banor runt jorden på en höjd av ungefär km över jordytan. Satelliter på denna höjd fullgör ett varv runt jorden på ungefär 12 timmar. Satelliterna är utspridda i sina banor på ett sådant sätt att en mottagare, var som helst på den del av jordytan där systemet är tänkt att användas, teoretiskt ska ha kontakt med minst fyra satelliter samtidigt. I verkligheten finns det mycket som kan vara i vägen för kommunikationen mellan satellit och mottagare. Berg och träd i naturen samt byggnader i städer kan vara tillräckligt för att någon satellit ska försvinna ur räckhåll för en mottagare. (Grewal m.fl., 2007)

19 2.2 Positionering med GPS Principen för positionering med GPS Ombord på varje GPS-satellit finns ett atomur med mycket hög noggrannhet. Principiellt beräknas mottagarens position genom att den tar emot ett meddelande som innehåller den tid då meddelandet skickades från satelliten. Genom att jämföra tidsdata med mottagarens klocka vid den tidpunkt som signalen kom fram kan avståndet till satelliten beräknas. Det beräknade avståndet till vardera satellit innehåller, på grund av bland annat osäkerheter i satelliternas klockor och störningar i jonosfären, en viss osäkerhet. Figur 2.1 visar hur osäkerheten i avstånd till flera satelliter skapar ett område inom vilket mottagaren med säkerhet befinner sig. Figur 2.1. Positionering i 2D. Mottagaren befinner sig någonstans i det grå området. För att beräkna en position behövs generellt data från fyra satelliter. De tre koordinaterna som ska bestämmas ger tre okända variabler, men det faktum att mottagarens klocka inte är exakt introducerar en fjärde frihetsgrad, vilken kräver ytterligare en satellit för att kunna elimineras. (Misra och Enge, 2006) GPS-signaler Meddelandesignaler skickas kontinuerligt från alla satelliter, och för att kunna skilja på vilken satellit som skickat en specifik signal används Code Division Multiplexing (CDM). CDM fungerar så att signalen förutom en tidpunkt även innehåller en kod som identifierar från vilken satellit signalen kommer. Varje satellit skickar signaler på två olika frekvenser. En anledning till att två frekvenser används är att signaler tidsfördröjs när de skickas från satelliten till mottagaren på jorden. Signaler med olika frekvens tidsfördröjs olika mycket och genom att mäta skillnaden

20 8 Grundläggande positionering i tidsfördröjning kan kompensering av tidsfördröjningen genomföras och därmed fås en noggrannare positionsbestämning. Den ena bärvågen, L 1 -bandet, har en frekvens på 1575,42 MHz och den andra, L 2 -bandet, använder sig av frekvensen 1227,6 MHz. På L 1 -bandet moduleras två olika koder; en kod som innehåller en grov positionsangivelse (coarse acquisition code, C/A) och en kod med högre positionsupplösning (precision code, P). På L 2 -bandet skickas endast P-koden. P- koden är krypterad och kan bara användas i USAs militära applikationer, medan C/A-koden är fritt tillgänglig och används av alla GPS-mottagare. (Grewal m.fl., 2007) Felkällor Flera felkällor finns som försämrar resultatet av en positionsmätning med GPS. Nedan beskrivs kort de största felkällorna som förekommer i dagens GPS-system. Utförligare beskrivning av felen kan till exempel hittas i Grewal m.fl. (2007) eller Schumacher (2006). I atmosfären gör förekomsten av bland annat vattenånga och laddade partiklar att signalerna fördröjs på sin väg mot jordytan. Satelliterna avviker hela tiden lite från sina tänkta banor, vilket leder till felaktigt avstånd mellan satellit och mottagare. Satelliternas klockor är mycket noggranna, men innehåller små fel. Då avståndet till en satellit beräknas genom att multiplicera ljusets hastighet med tiden för signalen att ta sig till mottagaren, ger till exempel ett fel på en mikrosekund ett fel i avstånd på 300 meter. Innan en GPS-signal når mottagaren kan den studsa mot kringliggande objekt, till exempel byggnader. Detta gör att samma signal når mottagaren flera gånger men har färdats olika väg, och mottagaren skattar då två olika positioner. Fenomenet kallas flervägsutbredning och vissa dyrare mottagare har inbyggda filter som försöker kompensera för detta. Till sist finns också termiskt brus som antingen kan komma från omgivningen eller skapas i mottagaren själv Mätning av hastighet En GPS kan inte enbart mäta position. Många mottagare kan också mäta hastigheten med vilken den färdas. Det enklaste sättet att mäta hastigheten är att approximera derivatan av de mätta positionerna. Eftersom dessa innehåller ett okänt fel ger dock detta ett opålitligt resultat. Ett bättre sätt för en GPS att mäta hastighet bygger på dopplereffekten. Den kända frekvensen hos bärvågen jämförs med den frekvens som mottagaren mäter. Efter att ha kompenserat för satelliternas rörelse i sina banor runt jorden samt jordens rotation kan en hastighet beräknas. Det framräknade värdet på hastighet är relativt pålitligt vilket gör det lämpligt att använda till exempel som stöttning i ett GPS-TNS-system. (Grewal m.fl., 2007)

21 2.3 Integration Differentiell GPS (DGPS) Genom att placera en basstation med GPS-mottagare på marken och noggrant mäta in dess position kan mottagaren skatta parametrar för de flesta av de fel som beskrevs i avsnitt De mätbara felen är starkt korrelerade i ett lokalt område. Om basstationen sänder ut justeringsparametrar till mottagare i närheten kan därmed mottagaren korrigera för dessa fel och en betydligt bättre positionsangivelse kan ges. Alla mottagare har inte funktionalitet för DGPS och mottagare som hanterar DGPS är oftast betydligt dyrare än enklare mottagare. (Grewal m.fl., 2007) 2.3 Integration Förekomsten av fel i sensorer försämrar naturligtvis ett mätsystems prestanda. Om sensorer med olika typer av fel kombineras, kan styrkan i den ena sensorn eliminera inverkan av svagheter i den andra. En tröghetssensor mäter, som beskrivet ovan, accelerationer och vinkelhastigheter. Ett fel i ett sådant mätvärde som integreras till en position ger upphov till att positionen driver. En GPS mäter däremot en absolut position. Ett fel i denna mätning ger inte upphov till någon drift, istället fås en position med viss osäkerhet. Integrering av GPS och IMU motiveras väl av att de styrkor och svagheter som existerar hos de båda systemen kompletterar varandra på ett utmärkt sätt. (Grewal m.fl., 2007) Genom att modellera felparametrar hos sensorerna kan en optimal skattning av dessa göras med hjälp av ett kalmanfilter. Figur 2.2 visar en enkel systemskiss över hur ett system av GPS och IMU kan integreras. Mätningarna från GPS-mottagare och IMU tas in i systmet, där tröghetsberäkningar på IMU-data görs och ett kalmanfilter skattar intressanta tillstånd. IMU GPS INS-beräkningar och kalmanfilter Tillståndsskattningar Figur 2.2. Integration av GPS och IMU i ett positioneringssystem.

22

23 Kapitel 3 Navigationssystem I ett navigatonssystem med GPS och IMU fås mätstorheter i olika koordinatsystem som måste transformeras till en lämpling ram för bearbetning. I detta kapitel beskrivs först de vanligaste koordinatsystemen som används vid navigering. Sedan följer definitioner och härledning av de ekvationer som används för att få önskad data i navigationsramen. Dessa ekvationer ligger till grund för de diskretiserade ekvationerna som senare implementeras i kalmanfiltret. Under härledningen tas de fullständiga ekvationerna fram för att slutligen, om möjligt, förenklas genom att ta bort försumbara variabler. 3.1 Koordinatsystem För olika typer av tillämpningar behöver olika hänsyn tas till påverkande källor, detta tillsammans med vilka typer av sensorer som används är en avgörande faktor för vilka koordinatsystem som skall komma att användas av positioneringssystemet. Nedan listas några vanliga koordinatsystem som används vid positionering. Alla koordinatsystem är ortonormala och kartesiska om inget annat anges. Earth Center Inertial (i): En inertialram defineras som en ram där Newtons rörelselagar kan tillämpas. För att dessa lagar ska gälla får inte systemet befinna sig i ett accelererande eller roterande tillstånd. För globala positioneringssystem används ofta en inertialram med origo i jordens centrum och som är ickeroterande med hänsyn till fixa stjärnor. Se figur 3.1. Earth Centered Earth Fixed (e): Detta koordinatsystem har sitt origo i jordens masscentrum. z-axeln löper längs jordens rotationsaxel och x- och y- axlarna ligger båda i ekvatorialplanet där x-axeln skär nollmeridianen. Det är ett högersystem där z-axeln sammanfaller med z-axeln i (i). Detta koordinatsystem roterar relativt ECI med en hastighet av ω ie =7, rad/s. Se figur 3.1. Navigationsram (n): Origo för detta koordinatsystem är fixt i marken och defineras ur en initialposition som till exempel fås från GPS. Axlarna är riktade 11

24 12 Navigationssystem i nordlig och östlig riktning samt nedåt, vilket är ett så kallat NED-system (North-East-Down). För tillämpningar begränsade till ett litet geografiskt område kan detta ofta anses vara en inertialram. Se figur 3.1. Figur 3.1. I navigationsramen används ett system där axlarna pekar mot norr, öst respektive nedåt - ett NED-system (North-East-Down). Kroppsfixt (b): Ett koordinatsystem som är fixt i fordonet med origo i dess masscentrum. Figur 3.2 illustrerar hur ett sådant koordinatsystem är definierat. Koordinataxlarna sammanfaller med axlarna för fordonets roll, pitch respektive yaw. Det är ett högersystem där z-axeln pekar nedåt. Figur 3.2. Ett kroppsfixt koordinatsystem. x-axeln är riktad rakt framåt medan y-axeln är riktad åt höger och z-axeln nedåt. Ännu ett koordinatsystem kan förekomma då mätinstrumenten inte är monterade i fordonets masscentrum. I denna rapport antas att detta koordinatsystems origo sammanfaller med det kroppsfixa systemets origo. Att mätinstrumentens axlar inte

25 3.2 World Geodetic System 1984 (WGS84) 13 sammanfaller med det kroppsfixa koordinatsystemets axlar tas hänsyn till genom att rotera allt mätdata från mätsystemet till det kroppsfixa koordinatsystemet. Detta beskrivs närmare i avsnitt Longitud och latitud är vinklar uttryckta i (e), vilket gör det enkelt att utifrån mätvärden från en GPS få positioner uttryckta i detta system. Mätdata från IMU i form av accelerationer och vinkelhastigheter fås i förhållande till (i). För applikationer som används över ett större geografiskt område eller över en längre tid måste även hänsyn tas till jordens rotation vilket skiljer (e) och (i) från varandra. I vissa tillämpningar kan dock bidraget från jordens rotation anses tillräckligt litet för att försummas. Tillsvidare beskrivs de fullständiga ekvationerna och obetydliga variabler försummas i slutet. 3.2 World Geodetic System 1984 (WGS84) GPS använder sig av koordinatsystemet WGS84 som är en internationell standard. WGS84 bygger på en modell av jorden där den genomsnittliga havsnivån approximeras med en ellipsoid. Koordinatsystemets axlar sammanfaller med koordinataxlarna i (e), vilket gör det enkelt att få positioner uttryckta i detta system. En sådan modell ger upphov till lokala variationer i gravitationen och en gravitationsvektor som inte är ortogonal mot jordytan. För globala positioneringssystem där stor noggrannhet efterfrågas krävs en god modell av jordens gravitationsfält. För det tillämpningsområde denna rapport behandlar där fordonet rör sig över en relativt liten geografisk yta kan gravitationen och jordradien antas konstant och beräknas utifrån positionen för navigationsramen. Värdet på jordens rotationshastighet då WGS84 används är 7, rad/s, se Grewal m.fl. (2007). Longitud λ: Longituden är noll vid Greenwich-meridianen och når 360 grader vid fullföljt varv österut kring jorden. Longituden talar därmed om hur långt österut eller västerut man befinner sig relativt Greenwich-meridianen. Se figur 3.1. Latitud ϕ: Latituden är noll vid ekvatorn och når 90 grader vid nordpolen och -90 grader vid sydpolen och talar om hur långt från ekvatorn man befinner sig. Se figur 3.1. Höjd h: Höjd i meter över havsnivån. I WGS84 används ett antal parametrar som beskriver jordens form och rörelse. Dessa parametrar definieras nedan. WGS84-parametrar Ekvatorialradie,R E = m (3.1a) Polradie,R P = m (3.1b) Jorden excentritet,e = (3.1c) ω e = 7, rad/s (3.1d)

26 14 Navigationssystem R M = R T = R E (1 e 2 ) (1 e 2 sin 2 (ϕ)) 3/2 (3.1e) R E (1 e2 sin 2 (ϕ)) (3.1f) där R M och R T representerar avståndet till ekvatorialplan respektive jordens rotationsaxel från lokal position. För att översätta en position angiven i WGS84-systenet till en lokal navigationsram behövs en relation mellan dessa två koordinatsystem. Denna definieras i ekvationerna (3.2). Transformation till lokala koordinater n Δp N n Δp E = R M Δϕ (3.2a) = R T cos(δϕ)δλ (3.2b) 3.3 Transformationer För det tillämpningsområde som detta arbete berör, där rörelseområdet är begränsat till ett litet geografiskt område, är det intuitivt att använda (e) som inertialram för att enkelt transformera GPS-data till navigationsramen. Genom att presentera beräknad data i navigationsramen är det enkelt att tolka fordonets position och hastighet och positionen kan lätt jämföras med en karta över området. Position från GPS fås enkelt utifrån definitionen av WGS84 tillsammans med mätvärden från GPS. Den lokala postionen i navigationsramen uppdateras utifrån ändringar av longitud och latitud genom ekvation (3.2) där R M och R T beräknas en gång initialt genom ekvationerna (3.1) och sedan antas konstanta över den yta man rör sig på. Vid sidan av GPS-mätningar måste mätvärden från IMU transformeras till navigationsramen, vilket sker genom en rotation. Det finns flera metoder att representera attityd på. De tre vanligast förekommande är eulervinklar, DCM (Direction Cosine Matrix) samt kvaternioner. Eulervinklar lämpar sig bra för applikationen i detta arbete och är även lättast att tolka. Därför implementeras eulervinklar senare i modellen som beskrivs i avsnitt 4.2. Valet av eulervinklar medför visserligen en risk för singulariteter, men denna risk innebär ingen begränsning i praktiken i denna tillämpning, vilket motiveras senare i kapitlet. Fordonet och IMUn roterar relativt navigationsramen och mätvärden måste därmed kontinuerligt transformeras från (b) till (n). Detta görs genom att rotera mätdata med en rotationssmatris, Cb n, där nedsänkt index representerar vilket koordinatsystem rotation sker från och upphöjt index representerar vilket koordinatsystem rotation sker till. Oberoende av metod att representera attityden, måste Cb n alltid beräknas för att göra transformationen från koordinatsystemet för mätutrustningen till önskat koordinatsystem för beräkning.

27 3.3 Transformationer 15 Rotationen som utförs med Cb n är i grunden tre olika rotationer, en kring varje koordinataxel. Rotationen kring de kroppsfasta x-, y- ochz-axlarna kallas roll, pitch respektive yaw, se figur 3.2. Varje rotation representeras av en matris. De tre matriserna R x, R y och R z svarar mot rotationen kring x-, y- respektive z-axeln och definieras enligt R x (φ) = cos(φ) sin(φ) 0 sin(φ) cos(φ) (3.3) R y (θ) = cos(θ) 0 sin(θ) sin(θ) 0 cos(θ) (3.4) R z (ψ) = cos(ψ) sin(ψ) 0 sin(ψ) cos(ψ) DCM från koordinatsystem (n) till (b) ges av: (3.5) C b n = R x (φ)r y (θ)r z (ψ) (3.6) Transformationen från (b) till (n) fås genom inversen av (3.6) och eftersom Cb n ortonormal fås inversen ur transponatet. är C n b =(Cb n) T = R z (ψ) T R y (θ) T R x (φ) T = cos(θ) cos(ψ) cos(φ) sin(ψ) cos(φ) sin(ψ) + sin(φ) sin(θ) cos(ψ) cos(φ) cos(ψ) + sin(φ) sin(θ) sin(ψ) sin(φ) cos(ψ) + cos(φ) sin(θ) cos(ψ) sin(φ) cos(ψ) + cos(φ) sin(θ) sin(ψ) sin(φ) sin(φ) cos(ψ) cos(φ) cos(θ) (3.7) Eftersom det Cb n = 1 för alla vinklar bevaras volymen vilket innebär att ingen omskalning sker vid transformationen.

28 16 Navigationssystem 3.4 Attitydrepresentation I detta avsnitt beskrivs metoder för representation av attityd. Den mest intuitiva metoden är eulervinklar vilken används i detta arbete. Eulervinklar medför dock en risk för singulariteter vilket beskrivs senare. Ett annat alternativ är uppdatering av attityden med Direction Cosine Matrix (DCM) vilken även den beskrivs i detta kapitel. Den trejde metoden är kvaternioner vilken inte beskrivs här. För beskrivning av kvaternioner se istället Titterton och Weston (2004). Implementeringsmässigt utförs kryssprodukter genom att utnyttja den skevsymmetriska formen enligt a ω = a zω y a y ω z a x ω z a z ω x a y ω x a x ω y = 0 ω z ω y ω z 0 ω x ω y ω x 0 a =Ωa (3.8) där a är en godtycklig tredimensionell vektor och Ω den skevsymmetriska formen av ω Eulervinklar Med denna metod lagras attityden i en vektor [φ θψ] T och vinkelhastigheter som mäts av IMUn används för att uppdatera denna vektor. Detta görs genom att först transformera vinkelhastigheter till navigationsramen och sedan integrera dessa. Med de beräknade eulervinklarna kan sedan Cb n tas fram. Genom att transformera T vinkelhastigheter uttryckta i (b) till (n) fås derivatan av eulervinkarna [ φ θ ψ] i navigationsramen. Eftersom rotation med eulervinklar sker i bestämd ordning måste detta tas hänsyn till när [ φ θ ψ] beräknas. Följande ekvationssystem fås för denna transformation. (Titterton och Weston, 2004) ω x φ 0 ω y = 0 + R x (φ) 0 θ + R x (φ)r y (θ) (3.9) ω z ψ b n n n Använder vi oss av (3.3)-(3.5) kan ekvationssystemet skrivas som ω x 1 sin(θ) 0 φ ω y = 0 cos(θ) cos(θ) sin(φ) ω z sin(φ) 0 cos(θ) cos(φ) θ ψ b n (3.10) Genom att multiplicera med inversen av ovanstående transformationsmatris på båda sidor av ekvationen kan attityden i navigationsramen lösas ut och följande system av differentialekvationer erhålls φ θ ψ n = 1 sin(φ) tan(θ) cos(φ) tan(θ) 0 cos(φ) sin(φ) 0 sin(φ) sec(θ) cos(φ) sec(θ) ω x ω y ω z b = Rω,b n ω x ω y ω z b (3.11)

29 3.4 Attitydrepresentation 17 Singulariteterna som eulervinklar medför ses i (3.11). Då cos(θ) = 0 är sec(θ) = 1 cos θ odefinierad. Detta skulle innebära att fordonet åker i en riktning rakt uppåt eller rakt nedåt vilket oftast inte sker i ett markfordon. För system tillämpade på till exempel krigsflygplan är andra alternativ att föredra, till exempel kvaternioner eller uppdatering av attityden genom Cb n Direction Cosine Matrix, C n b Ett alternativ till eulervinkelrepresentationen är att låta Cb n -matrisen representera attityden. Denna metod använder sig till skillnad från eulervinkelrepresentationen av tidsderivatan av Cb n. Nackdelen med denna metod är att den kräver sex olika tillstånd istället för eulervinkelrepresentationens tre tillstånd. Matrisen Cb n innehåller nio obekanta element men i och med dess ortonormala egenskap kan tre av dessa beräknas ur de andra sex. Denna metod har testats och resulterat i goda resultat. Om eulervinkarna önskas fås dessa ur Cb n genom nedanstående ekvationer. (Titterton och Weston, 2004) ( ) c32 φ = arctan (3.12) c 33 θ = arcsin( c 31 ) (3.13) ( ) c21 ψ = arctan (3.14) där c ij representerar elementen i Cb n och i och j representerar rad respektive kolumn, 1 i, j 3. Då det kroppsfixa systemet vrider sig mäts en vinkelhastighet som kan utnyttjas för att uppdatera Cb n. För små vinklar används första ordningens taylorutveckling av de trigonometriska funktionerna, cos(α) 1ochsin(α) α för att approximera Cb n. Det ger Cb n 1 ψ θ ψ 1 φ (3.15) θ φ 1 Vinkelhastigheterna i (b) kan representeras med en skevsymmetrisk matris Ω b bn. Indexen betecknar rotation från (b) till (n) uttryckt i (b). Den vinkeländring som sker i (b) under ett sampel kan skrivas ωbn b Δt vilka är små då sampelfrekvensen är hög. Det innebär att på skevsymmetrisk form fås Ω b bnδt. Rotationsmatrisen i tidpunkt t + Δt kan enligt Titterton och Weston (2004) formuleras som där ΔΨ = 0 Δψ Δθ Δψ 0 Δφ Δθ Δφ 0 c 11 C n b (t +Δt) =C n b (t)(i + ΔΨ) (3.16) = 0 ω z Δt ω y Δt ω z Δt 0 ω x Δt ω y Δt ω x Δt 0 =Ω b bnδt (3.17)

30 18 Navigationssystem där Δφ, Δθ och Δψ är de små vinkeländringarna som sker under en sampelperiod. Vidare kan derivatan av Cb n formuleras som Ċb n (t) = Cb n lim b (t) Δt 0 Δt = Cb n lim bn (t)δt) Cn b (t) Δt 0 Δt = Cb n (t)ω b bn(t) (3.18) Matrisen C n b beräknas på diskret form ur följande ekvation C n b (k +1)=C n b (k)+ċn b (k)δt = C n b (k)(i +Ω b bn(k)δt ) (3.19) Detta sätt att representera attityden på medför att Cb n av beräkningstekniska skäl kan förlora sin ortonormala egenskap och för att korrigera för detta kan normalisering enligt Titterton och Weston (2004) genomföras enligt C n b := Cb n I Cn b Cn b 2 T C n b (3.20) 3.5 TNS-ekvationer I detta avsnitt härleds de ekvationer som beskriver dynamiken för tröghetsnavigeringssystemet. Först tas den generella ekvationen fram för att sedan anpassas till navigationsramen. Även feldynamiken för de olika tillstånden tas fram då den ligger till grund för modellen i kalmanfiltret Generell navigationsekvation I detta avsnitt härleds de generella differentialekvationerna som beskriver förhållandet mellan insignalerna från IMUn och utsignalerna ur TNS-systemet (position, hastighet, attityd). Låt p i och p a vara två tredimensionella vektorer som beskriver en position i ett inertialsystem (i) respektive ett godtyckligt koordinatsystem (a). Se figur 3.3. y i p i y a p a x a r i x i Figur 3.3. Vektorn p uttryckt i koordinatsystem (i) respektive (a).

31 3.5 TNS-ekvationer 19 Förhållandet mellan koordinaterna för denna position i de olika koordinatsystemen kan beskrivas med följande ekvation p i = C i ap a + r i (3.21) där C i a relaterar koordinatsystemens attityd och r i är en vektor mellan koordinatsystemens origon. Deriveras (3.21) två gånger med avseende på tiden fås Deriveras (3.18) fås p i = C i a p a +2Ċi aṗ a + C i ap a + r i (3.22) C i a = Ċi aω a ia + C i a Ω a ia = C i a(ω a iaω a ia + Ω a ia) (3.23) Vektorn p i kan delas upp i g i +f i där g är gravitationsvektorn och f är de specifika krafterna som verkar på koordinatsystemet (a). Utnyttjas detta tillsammans med insättning av ekvationerna (3.18) och (3.23) i (3.22) fås C i a p a +2C i aω a iaṗ a + C i a Ω a iap a + C i aω a iaω a iap a + r i = g i + f i (3.24) Transformering till koordinatsystem (a) från koordinatsystem (i) genom multiplicering med Ci a på båda sidor och omskrivning för att få ut p a ger oss slutligen ekvationen p a = g a + f a 2Ω a iaṗ a Ω a iap a Ω a iaω a iap a r a (3.25) Detta är den generella navigationsekvationen i ett godtycklig koordinatsystem (a). Denna ekvation kan användas för globala positioneringssystem och vid ett tillämpningsområde där det geografiska området begränsas till en liten yta kan några av termerna försummas. Används (i)-ramen som definierats i avsnitt 3.1 som inertialram och navigationsekvationerna implementeras i (e)-ramen sammanfaller de båda koordinatsystemens origo vilket innebär att r i = 0. I tillämpningen som denna rapport behandlar där en lokal navigationsram används som intertialram representerar r i avståndet mellan intertialramens och navigationsramens origo Ekvationer i navigationsramen Detta avsnitt beskriver de i föregående avsnitt framtagna ekvationerna anpassade för systemets tillämpning. Då mätningar från både accelerometrar och gyron fås relativt inertialramen (i) kommer dessa att påverkas av såväl jordens rotation som gravitation. Eftersom beräkningar sker i navigationsramen måste mätningarna kompenseras för dessa. Modell av jordens rotation Jordens rotationshastighet relativt (i) ges av ω e ie =(0 0 ω e ) T (3.26)

32 20 Navigationssystem där ω e är jordens rotationshastighet, se ekvation (3.1d). Genom nedanstående transformation fås vinkelhastigheten av jordens rotation uttryckt i (n) där ϕ är latituden. Eftersom navigationsramen är fix på jordytan gäller att ωen n =0. ω n in = ω n ie + ω n en = C n e ω e ie +0=(ω e cos ϕ 0 ω e sin ϕ) T (3.27) Beräkningen av ωin n görs initialt utifrån position mätt av GPS och antas konstant. Vinkelhastigheten som gyrona mäter i (b) ges av Den kompenserade vinkelhastigheten fås då som Gravitationsmodell ω b ib = ω b nb + ω b in = ω b nb + C b nω n in (3.28) ω b nb = ω b ib C b nω n in (3.29) Gravitationen varierar beroende av position på jorden. En enkel modell används här tagen ur Titterton och Weston (2004), och denna beräkning görs initialt i implementationen med hjälp av den med GPSen mätta startpositionen. g(0)=9, (1 + 5, sin 2 (2ϕ)) (3.30) Med modeller för jordens rotation och gravitation kan nu (3.25) uttryckas i navigationsramen med de specifika krafterna verkande på det kroppsfixa systemet, f b. p n = g n + C n b f b 2Ω n inṗ n Ω n inp n Ω n inω n inp n + r n (3.31) Ekvation (3.31) kan förenklas genom att utelämna euleraccelerationen som representaras av den fjärde termen i högerledet. Euleraccelerationen försummas då förändringen av jordens rotationshastighet antas vara obefintlig. Den femte termen är centripetalkraften orsakad av jordens rotation och den kan utelämnas på grund av den låga känsligheten hos mätutrustningen. Eftersom positionen av fordonet uttrycks relativt navigationsramen och inte relativt intertialramen kan r n försummas. Efter dessa antaganden fås p n = g n + C n b f b 2Ω n inṗ n (3.32) Ekvationerna som härletts i detta avsnitt kan sammanfattas i figur Feldynamik i navigationsramen Genom kalibrering av IMUn kan bias och skalfel i accelerometrar kompenseras för, men mätninganra kommer ändå innehålla fel som varierar över tiden bland annat på grund av temperaturförändringar. Det ger upphov till fel även i skattningarna av position och hastighet. Felet i position bestäms utifrån skillnaden mellan GPS-mätning och TNS-skattning, vilket är relaterat till hastigheten och attityden genom en uppsättning differentialekvationer. Dessa ekvationer ligger till grund för

33 3.5 TNS-ekvationer 21 Gravitationsmodell Accelerometrar Gyron b ib b nb b a n Rb C n b a n v n 2 n n in v v n n p n v b in b C n n in n C e e ie Figur 3.4. Blockschema över TNS-beräkningarna. kalmanfiltret då loosely coupled implementering används, vilket vi kommer till i avsnitt 4.2. Ekvationerna beskrivs i detta avsnitt där termerna i (3.31) byts ut till motsvarande skattad ( ) ˆ respektive mätt ( ) storhet. Feldynamik för hastighet Genom att substituera termerna i (3.31) till skattad respektive mätt storhet fås ˆv n = g n + Ĉn b f b 2Ω n inˆv n (3.33) Då vinkelfelen är små kan, enligt Titterton och Weston (2004), förhållandet mellan skattad och sann Cb n skrivas som Ĉ n b =(I [δψ ])C n b (3.34) där [δψ ] är en skevsymmetrisk form av attitydfelen δψ. 0 δψ δθ [δψ ] = δψ 0 δφ (3.35) δθ δφ 0 Genom att definera felet δ( ) som skillnaden mellan det riktiga värdet och det skattade värdet ( ) ˆ för respektive term och sätta in detta i ekvation (3.33) fås v n δ v n = g n +(I [δψ ]C n b (f b δf b ) 2Ω n in(v n δv n ) (3.36) Utvecklas denna ekvation fås v n δ v n = g n + Cb n f b 2Ω n inv n Cb n δf b [δψ ]Cb n f b +[δψ ]δf b +2Ω n } {{ } inδv n v n (3.37) Vidare ger detta en ekvation för δ v n där andra ordningens termer försummats

34 22 Navigationssystem δ v n = C n b δf b +[δψ ]C n b f b 2Ω n inδv n (3.38) Vidare är C n b f b = f n och skriver vi f n på skevsymmetrisk form [f n ] fås δ v n = C n b δfb +[δψ ](C n b f b ) 2Ω n in δvn = C n b δfb [f n ]δψ 2Ω n in δvn (3.39) Feldynamik för attityd Fel i attityd uppkommer vid fel i mätningnar av vinkelhastigheter. Transformering av dessa fel till navigationsramen sker på samma sätt som för vinkelhastigheter. Derivatan av felet δψ ges av δ Ψ = δ φ δ θ δ ψ = och δψ fås genom en integration. 1 sin(φ) tan(θ) cos(φ) tan(θ) 0 cos(φ) sin(φ) 0 sin(φ) sec(θ) cos(φ) sec(θ) δω x δω y δω z (3.40) Sammanfattning av ekvationer Här sammanfattas de kontinuerliga ekvationerna som implementerats i navigationssystemet och härletts tidigare i detta kapitel. TNS-dynamik ṗ n = v n v n = g n + C n b f b 2Ω n in vn Ψ n = R n ω,b ωb (3.41) Feldynamik δṗ n = δv n δ v n = C n b δfb [f n ]δψ 2Ω n in δvn δ Ψ n = R n ω,b δωb (3.42)

35 Kapitel 4 Integration av GPS och IMU I detta kapitel beskrivs hur fusionen av GPS- och IMU-data genomförs. Kapitlet inleds med en beskrivning av sensormodeller och följs av teori för kalmanfiltret. Sedan följer härledning av den kalmanfiltermodell som implementerats. 4.1 Sensormodeller I detta avsnitt härleds nödvändiga sensormodeller. Avsnittet är uppdelat i tre olika delar där varje sensortyp behandlas separat. Därefter följer en sammanfattning där mätekvationerna sätts ihop till en mätmodell. Precis som systemmodellen måste sensormodellerna diskretiseras, eftersom det inte är möjligt att erhålla mätdata kontinuerligt. Denna diskretisering görs i avsnitt 4.2, där sedan en färdig mätmodell ställs upp och implementeras i kalmanfiltret Mätmodell för accelerometrar En accelerometer mäter den totala acceleration som den utsätts för. Denna består av gravitionen och rörelser hos den kropp som accelerometern är monterad på. Normalt är det bara den senare som är av intresse. Accelerometrarna kan dock inte skilja de två åt. Istället måste kompensering för detta göras i beräkningarna. En enkel modell för mätningen kan skrivas som ã b = a b g b (4.1) där a b är den acceleration som ger upphov till rörelse hos fordonet och g b är gravitationsvektorn. Index b anger att storheterna uttrycks i det kroppsfixa (body- ) koordinatsystemet. Ett problem med sensorer är att de alltid innehåller fel. Några kända typer av fel som kan modelleras beskrivs nedan. Bias innebär att mätvärdena innehåller en offset som varierar långsamt med tiden. 23

36 24 Integration av GPS och IMU Skalfel gör att proportionalitetskonstanten mellan verklig storhet och dess mätning är skild från ett. Monteringsfel betyder att sensorerna inte sitter monterade ortogonalt mot varandra. Olinjäriteter i sensorerna innebär att kurvan som relaterar faktisk storhet till dess mätning inte är linjär, vilket den i sensormodellen antas vara. Temperaturberoenden gör att sensorns mätvärden till viss del beror på temperaturen på sensorn. Brus är en stokastisk process som adderas till mätningen. Om alla dessa fel tas med i tillståndsmodellen leder det snabbt till att tillstånden inte kan skattas, ingen observerbarhet fås (se avsnitt 4.2.5). Vanligt är att bara modellera brus, bias och eventuellt skalfel. Om vi infogar mätbruset v a N(0,R a ) och biasen δa i ekvation (4.1) får vi sensormodellen ã b = a b g b + δa + v a (4.2) Ekvation (4.2) är en modell i det kroppsfixa koordinatsystemet. Önskas mätningen istället beskriven i ett annat koordinatsystem, till exempel en navigationsram, måste en rotation motsvarande skillnaden mellan de två systemen göras, med rotationsmatrisen C n b. De tre accelerometrarna är av samma modell och antas därför ha samma bruskaraktäristik. Vidare antas det inte finnas någon korrelation mellan accelerometrarnas mätvärden. Den del av kovariansmatrisen som motsvarar accelerometrarna blir därmed diagonalmatrisen diag(σ 2 acc σ 2 acc σ 2 acc). Om accelerometrarna inte sitter ortogonalt kan detta modelleras som en korellation mellan accelerometrarna, se Skog (2007), men till detta tas ingen hänsyn i det här arbetet Mätmodell för gyron Ett gyro mäter vinkelhastigheten hos den kropp som det är monterat i, i förhållande till en inertialram. En enkel sensormodell blir ω b = ω b ib (4.3) De typer av fel som nämndes för accelerometrarna kan även förekomma i gyrona. Om vi tar med brus och bias i mätmodellen blir mätekvationen ω b = ω b ib + δω + v ω (4.4) där kovariansmatrisen för bruset av samma anledning som för accelerometrarna antas vara en diagonalmatris med samma varians i alla axlar, det vill säga diag(σ 2 gyro σ 2 gyro σ 2 gyro).

37 4.2 Kalmanfiltret Mätmodell för GPS De fel som nämndes i avsnitt modelleras tillsammans som normalfördelat vitt brus, v GP S, med väntevärde noll. En mätekvation som beskriver GPS-mätningarna ges av y = p e + v GP S (4.5) där alltså v GP S är en normalfördelad 3-dimensionell variabel med E[v GP S ]= Kalmanfiltret I detta avsnitt ges en introduktion som motiverar användandet av kalmanfiltret. Först presenteras den grundläggande teorin och därefter följer förklaringar av de aspekter som behöver tas hänsyn till innan implementering kan ske. Ingen härledning av de ekvationer som filtret bygger på presenteras. För en mer komplett framställning, se till exempel Gustafsson m.fl. (2001) eller Gustafsson (2000). Modellstrukturen för hopkopplingen av IMU- och GPS-mätningar i ett kalmanfilter kan se ut på tre olika sätt; uncoupled, loosely coupled och tightly coupled, se figur 4.1. Uncoupled innebär att ingen återkoppling sker från kalmanfiltret till TNS- och GPS-beräkningar. I ett loosely coupled-filter används skillnaden mellan TNS- och GPS-beräkningar för att skatta fel i tillståndsskattningarna samt bias. I ett tightly coupled-filter kan dessutom fel i GPSen skattas. Vi återkommer till modellstrukturen i avsnitt 4.3. INS GPS f, ˆp Kalmanfilter Position Hastighet Attityd INS GPS f, ˆ ˆ f, ˆp Kalmanfilter Position Hastighet Attityd (a) Uncoupled ˆx INS GPS f, ˆ ˆ f,, f D Kalmanfilter (c) Tightly Coupled Position Hastighet Attityd (b) Loosely Coupled Figur 4.1. Olika modellstrukturer Introduktion Kalmanfiltret härleddes på 1960-talet av Rudolf E. Kalman och Richard S. Bucy och har sedan dess fått mycket stor betydelse inom signalbehandling av olika

38 26 Integration av GPS och IMU slag. Filtret är härlett som det bästa linjära filtret för skattning av ett tillstånd. Med bästa menas här att skattningsfelets kovarians, P, minimeras. Filtret tillåter även flervariabla system med tillståndsvektorer av godtycklig storlek. För detta examensarbete är kalmanfiltret av yttersta intresse av den anledningen att flera brusiga mätsignaler kan viktas ihop på ett optimalt sätt, så kallad sensorfusion. Vi behöver inte begränsa oss till att bara använda mätsignaler från GPS och IMU, utan kan med fördel även ta in information från andra sensorer i bilen. Exempelvis kan mätningar av hastighet och rattutslag ge värdefull information som kan viktas samman med GPS- och IMU-data för att få en bättre tillståndsskattning. Det bör påpekas att även om kalmanfiltret är det bästa linjära filtret är det inget som säger att det inte finns ett olinjärt filter som skulle ge en mindre kovarians. Det går dock att visa (se till exempel Gustafsson m.fl. (2001)) att om process- och mätbrus har en gaussisk fördelning, är kalmanfiltret det absolut bästa filtret, det vill säga det finns inte ens något olinjärt filter som kan generera en skattning med lägre kovarians. (Gustafsson m.fl., 2001) Filtermodell En linjär, tidskontinuerlig systemmodell kan skrivas på tillståndsform som ẋ(t) = Ax(t)+Bu(t)+v 1 (t) (4.6a) y(t) = Cx(t)+v 2 (t) (4.6b) Ekvationerna (4.6) anger relationen mellan mätsignalerna y, styrsignalerna u och tillstånden x. Signalerna v 1 och v 2 representerar osäkerheten i systemmodellen respektive mätningarna, vilken ofta modelleras som normalfördelat vitt brus med E{v 1 (t)v 1 (t + τ) T } = E{v 2 (t)v 2 (t + τ) T } = { Q, om τ =0 0 annars { R, om τ =0 0 annars (4.7a) (4.7b) Kovariansmatriserna Q och R är oftast okända och används då som trimningsparametrar för att kalmanfiltret ska få ett bra uppförande. (Gustafsson m.fl., 2001) Om Q och R väljs i kontinuerlig tid, kan det vara svårt att tolka kovarianserna när de överförts till diskret tid. Istället väljs kovarianserna i tidsdiskret tid och vi släpper därför brusmodellen tillfälligt och återupptar den istället i avsnitt efter att en tidsdiskret modell presenterats. Uppgiften är att, i varje tidpunkt, ta fram en så bra skattning av tillståndsvektorn x(t) som möjligt. Som redan nämnts ger kalmanfiltret den bästa linjära skattningen. Vi betecknar skattningen med ˆx(t τ) där τ är tidpunkten för den senaste mätning som tas med i beräkningarna. Filtret tillåter enligt Gustafsson m.fl. (2001) att skattningen görs på tre olika sätt: Prediktion: Skattningen av tillståndsvektorn i tidpunkten t bygger endast på mätvärden y(t), τ<t