Position och hastighet

Figur 6.8 visar position- och hastighetsskattningar tillsammans med deras mät- ningar. Det finns ingen kunskap om sanna positioner och hastigheter så den enda validering som kan göras är just att skattningen följer mätningen men är mindre kantig.

6802 685 690 695 700 2.5 3 3.5 4 4.5 5 5.5 Tid [s] Position [m] Skattning GPS (a) Position 680 685 690 695 700 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 Tid [s] Hastighet [m/s] Skattning GPS (b) Hastighet

Figur 6.8: Skattning av position och hastighet i nordlig riktning

6.2.3

Bias

I Figur 6.9 visas bias för accelerationen och vinkelhastigheten som skattas av filtret.

0 200 400 600 800 1000 −0.14 −0.12 −0.1 −0.08 −0.06 −0.04 −0.02 0 0.02 0.04 0.06 Tid [s] Gyrobias [ o/s] b wx b_wy b wz (a) Rategyrobias 0 200 400 600 800 1000 −0.25 −0.2 −0.15 −0.1 −0.05 0 0.05 0.1 Tid [s] Accelerometerbias [m/s 2] b ax b_ay b az (b) Accelerometerbias

Figur 6.9: Skattning av bias för accelerometer och vinkelhastighet. Bias är svårt att bestämma och näst intill omöjligt när helikoptern inte har någon rörelse i alla leder. Om en korrekt skattning av biasen har gjorts är därför omöjligt att säga eftersom ingen vet vad den faktiska biasen är. Det som kan göras för att se om biastrackingen fungerar är att lägga till en konstantförändring och se om filtret klarar av att upptäcka den. Förändringen kan antingen göras på vinkelhastigheterna eller accelerometrarna. De båda biaserna klaras även av att bestämmas när en konstant biasförändring görs, se Figur 6.10. För att skapa Figur 6.10 har för gyrona en offset på 0.1◦_{/s manuellt lagts på ovanpå den vanliga}

mätningen och en offset på 0.1m/s2 _{lagts på ovanpå accelerometermätningarna.}

Om en simulering görs med den vanliga mätningen och en med offseterna pålagda så ska skillnaden mellan de skattade biaserna i de två fallen vara den pålagda offseten om biastrackingen fungerar. Figur 6.10 visar just denna skillnad. Samtliga kurvor i Figur 6.10 ska således svänga in sig mot värdet 0.1.

6.2 Sensorfusion med en tillståndsvektor 73 0 200 400 600 800 1000 1200 −0.02 0 0.02 0.04 0.06 0.08 0.1 0.12 Tid [s] Skillnad i gyrobias [ o/s] b_wx b wy b wz

(a) Skillnad i vinkelhastighetbias

0 200 400 600 800 1000 1200 0 0.02 0.04 0.06 0.08 0.1 0.12 Tid [s] Skillnad i accelerometerbias [m/s 2] b_ax b ay b az (b) Skillnad i accelerometerbias

Figur 6.10: Biasskilland med en konstant bias pålagd.

Det går betydligt sämre att upptäcka förändringen i x- och y-led för accelero- meterbiasen. Dessa skattningar genomgår två stycken tydliga förändringar till det bättre under simuleringen och ligger ganska stilla under resten av simuleringen. Den första vid tid 440 s kan härledas från att helikoptern gör take-off vid den tid- punkten och den andra vid tid 860 s till att helikoptern gör några förhållandevis kraftiga svängar där. Resten av datasetet håller sig helikoptern ganska stilla. Det- ta är tydligt tecken på att helikoptern måste röra på sig för att accelerometerbias ska vara observerbar. För bias i gyrona resulterar det i att de svänger in sig mot den konstanta förändringen medan accelerometrarna får en konstant förskjutning eftersom de inte svänger in sig mot ett värde.

Accelerometerbiasen svänger typiskt inte in sig mot ett värde utan varierar rätt kraftigt under tiden. Om det tyder på rätt biasskattning är svårt att veta, men felskattningar av orienteringen i filtret skulle få stor påverkan eftersom filtret då kommer ha fel riktning på gravitationsvektorn. Denna felriktning har en stor påverkan eftersom gravitationsvektorn är väldigt stor jämfört med accelerometer- biasen. 0 200 400 600 800 1000 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2 Tid [s] Standardavvikelse [ o/s] b wx b_wy b wz

(a) Standardavvikelse i gyrobias

0 200 400 600 800 1000 0 0.02 0.04 0.06 0.08 0.1 0.12 Tid [s] Standardavvikelse [m/s 2] b ax b_ay b az (b) Standardavvikelse i accelerometerbias

I Figur 6.11 visas standardavvikelser för biasskattningarna. P-matrisen anger kovariansen för skattningarna där diagonalelementen är variansen i skattningar- na. Dessa värden illustrerar kvaliteten i skattningarna. För tydlighets skull har kvadratroten dragits ur varianserna, dvs standardavvikelserna visas, för att göra resultatet mer lättolkat. Resultatet visar att kovarianserna för gyrobiaserna och accelerometerbiasen i z-led stabiliserar sig mot ett visst lågt värde, vilket är bra. De övriga accelerometerbiaserna har högre kovarianser och har svårare att konver- gera. Detta är ännu ett tecken på hur svårskattade dessa biaser är.

Värt att notera är sambandet mellan Figur 6.10 och Figur 6.11 där kovariansen, som väntat, är hög där felet är stort.

Intressant är även att simulera ett steg på biasen så att ett stegsvar som visar hur snabbt biasen kan variera fås. Eftersom accelerometerbiasen inte svänger in sig mot ett konstant värde går det inte säga hur lång tid det tar för filtret att svänga in sig. Det blir mer ett uttalande om att utseendet generellt blir förskjutet men accelerometerbiasen i z-led har en tidskonstant på ungefär 50 sekunder. När det läggs till en konstant på vinkelhastigheterna har filtret en tidskonstant på ungefär 100 sekunder.

För filtret är det enklare att bestämma bias för vinkelhastigheterna än bias för acceleromterarna på grund av att gyrona har en enklare mätekvation.

6.2.4

Accelerometer

För att se hur väl accelerometrarna kan understödja GPS-mottagaren är det intres- sant och se, genom en dubbelintegration av accelerationen, om det går att få fram positionen. Det finns inga som helst osäkerheter i att en dubbelintegrering skall ge positionen, men hur länge skulle det hjälpa? Om accelerationen integrerades under hela körningen finns det inget som helst hopp att få se en korrekt posi- tion utan GPS-stöd eftersom accelerometern driver iväg med tiden. Det är snarare intressant att veta hur länge en sådan integrering av accelerometern kan pågå utan GPS-stöttning innan driften blir alltför kraftig. För att testa detta används ett alternativ till att se hur väl accelerationen fungerar. Vid GPS-bortfall kommer positionen att studeras eftersom accelerationens påverkan kommer att framgå tyd- ligast där beroende på den korskoppling som finns mellan dem. Detta illustrerar även hur filtret skulle fungera i svårare miljöer där satellitbortfall kan vara väntat. GPS-bortfallet har simulerats med samma data som tidigare eftersom en uteslu- ten mätuppdatering är samma sak som bortfall av GPS. Bortfallet simuleras till 10 sekunder och som Figur 6.12 visar fungerar det bra i ungefär 4 sekunder men sedan får driften i sensorn en för stor påverkan i resultatet. Figurerna visar att resultatet inte enbart beror på hur länge bortfallet varar. I båda figurerna sker ett bortfall under exakt lika lång tid, men för den ena blir dynamiken mer varierande. Det som skiljer de två figurerna åt är att starttiden för GPS-bortfallet skiljer sig med 2 sekunder.

Det visar sig alltså att den skattade positionen klarar av att följa med i dynami- ken men att den driver iväg med tiden. Den felskattning som fås vid GPS-bortfall beror inte bara på hur länge avbrottet från GPS varar utan hur kraftigt dynamiken varierar. Vid väldigt kraftig dynamik kommer felskattningen att bli större än vid

6.2 Sensorfusion med en tillståndsvektor 75 565 570 575 580 585 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 Tid [s] Position [m] Skattning GPS

(a) GPS-bortfall vid tid 570 s

565 570 575 580 585 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 Tid [s] Position [m] Skattning GPS

(b) GPS-bortfall vid tid 572 s

Figur 6.12: Positionsskattning vid simulerat GPS-bortfall under 10 s färd rakt fram. Det beror dels på att accelerometerbiasen blir svårare att skatta då den kommer att variera mer samtidigt som skattningarna av orienteringen in- te kommer att bli helt korrekt. När orienteringen inte skattas helt rätt kommer gravitationen få mer påverkan i filtret eftersom orienteringen pekar ut en felaktig riktning för gravitationsvektorn.

6.2.5

Observerbarhet

Observerbarheten för systemet har inte analyserats analytiskt. Det linjäriserade observerbarhetskriteriet på det linjäriserade systemet har istället fått användas under körning. Det visar sig att full rang på 15 erhålls i alla tidpunkter (de 15 tillstånden är 3 positioner, 3 hastigheter, 3 vinklar och 6 biaser). Vid en fullständig analys borde också resultatet bli att observerbarhet erhålls vid tillräcklig rörelse.

6.2.6

Slutsats

Filtret ger skattningar i både attityd och kurs som har en dynamik som liknar det AHRS levererar. Skillnaden mellan filtrets skattning och AHRS är inte speciellt stor i attityd. Det är väldigt viktigt att just attityden skattas rätt eftersom den har störst påverkan på regleringen och stabiliteten av helikoptern. Om kursen skattas något fel är det inte något som är speciellt allvarligt.

Filtret klarar av att skatta bias både för vinkelhastigheten och accelerometern. Hur väl biasen skattas går inte direkt att bestämma men med tanke på att filtret klarar av att registrera en konstant biasförändring är den inte alldeles fel. Biasen för vinkelhastigheten fungerar bättre att skatta för filtret än biasen för accelero- metern, vilket beror på att mätekvationen är mindre komplex. Biasskattningen för accelerometern är i någon mening relativt god eftersom vid GPS-bortfall påverkas inte skattningen av positionen speciellt mycket vid långsam rörelsedynamik.

Observerbarheten för filtret har varit svårbestämd. Att bestämma observer- barheten analytiskt skulle bli väldigt resurskrävande på den begränsade tiden. Det skulle inte funnits några garantier för att det skulle ha blivit en korrekt ana-

lys då det är betydligt svårare att göra den i det olinjära fallet. Att istället göra analysen på det linjäriserade systemet under körning av filtret visar på full rang och observerbarhet men detta är en förenkling. Dock har inte en fullständig analys med Liederivator gjorts vilket borde ge att systemet är observerbart.

Den största nackdelen med denna filterstruktur är svårigheten i att tuna filtret. Fördelen är att kopplingen mellan orienteringen och position kommer in på ett tydligt sätt.

6.3

Heuristiskt filter

Som en jämförelse med de framtagna Kalmanfiltren visas här resultat från heuris- tiska filter. Heuristiska filtret används för att visa hur man intuitivt kombinerar mätningarna från sensorerna med dödräkning.

670 675 680 685 690 695 700 −6 −4 −2 0 2 4 6 8 Tid [s] Rollvinkel [ o] Skattning AHRS (a) Rollvinkeldynamik 670 675 680 685 690 695 700 −6 −4 −2 0 2 4 6 8 Tid [s] Tippvinkel [ o] Skattning AHRS (b) Tippvinkeldynamik

Figur 6.13: Skattning av attityd.

6.3.1

Orientering

I denna filterstruktur skapas fiktiva mätningar av orienteringen som lågpassfiltre- ras. Den lågpassfiltrerade mätningen adderas med en högpassfiltrerad dödräkning för att skapa orienteringen. Figur 6.13 visar att den skattade attityden dynamiskt följer AHRS-skattningen som används som referens.

Figur 6.14 visar att skillnaden mellan de skattade vinklarna och AHRS-skattningarna är små.

Det illustreras i Figur 6.15 att den skattade girvinkel och AHRS följer varandra med en konstant offset.

6.3.2

Position och hastighet

I navigationsfiltret krävs ingen fiktiv mätning. Den största utmaningen för filtret är att GPS-mätningarna skall slätas ut med hjälp av accelerometern. Den råa mätningen är fyrkantig och behövs därför släta ut. I Figur 6.16 visas att den hårda signalen från GPS slätas ut.

6.3 Heuristiskt filter 77 670 675 680 685 690 695 700 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 0.4 Tid [s] Rollvinkelskillnad [ o]

(a) Skillnaden mellan AHRS och skattning av rollvinkel 670 675 680 685 690 695 700 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 0.55 Tid [s] Tippvinkelskillnad [ o]

(b) Skillnaden mellan AHRS och skattning av tippvinkel

Figur 6.14: Skillnaden mellan AHRS och skattning av attityd.

670 675 680 685 690 695 700 −70 −68 −66 −64 −62 −60 −58 −56 −54 −52 Tid [s] Girvinkel [ o] Skattning AHRS (a) Girvinkeldynamik 670 675 680 685 690 695 700 2.3 2.4 2.5 2.6 2.7 2.8 2.9 3 3.1 3.2 Tid [s] Girvinkelskillnad [ o]

(b) Skillnaden mellan AHRS och skattning av girvinkel

Figur 6.15: Skattning av girvinkel och referensskattning från AHRS

680 685 690 695 700 1 2 3 4 5 6 7 Tid [s] Position [m] Skattning GPS (a) Position 690 692 694 696 698 700 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 1.2 Tid [s] Hastighet [m/s] Skattning GPS (b) Hastighet