För att analysera de olika filtrens egenskaper görs en analys av det tidskontinu- erliga fallet för de linjäriserade systemen. Analysen går ut på att ett linjärt och stationärt Kalmanfilter tas fram för en viss arbetspunkt och analyseras med avse- ende på polplacering och Bodediagram. Vid val av arbetspunkt väljs i först hand den arbetspunkt där helikoptern är stillastående utan att vara tiltad.
Enklast att analysera är girvinkelfiltret eftersom detta filter bara har två till- stånd. Eftersom systemet har två tillstånd kommer filtret ha två poler. Båda po- lerna ligger placerade på den reella axeln, en snabb pol och en långsam. I fallet som visas i Bodediagrammet i Figur 5.2 erhålls polerna −0.01 och −3.16. I Bo- dediagrammet framträder polernas placering i form av gränsvinkelfrekvenser. Den snabba polen dyker upp som gränsvinkelfrekvens i överföringsfunktionen från mät- ning av girvinkeln till skattningen av girvinkeln i Figur 5.2. Den långsamma polen dyker istället upp i överföringsfunktionen till skattad bias. Överföringen från vin- kelhastighet till skattad girvinkel har bandpasskaraktär där gränsvinkelfrekvenser- na ges av de två polernas placeringar. I överföringsfunktionen till skattad gyrobias blir karaktären den omvända. Överföringen från vinkelhastighet till skattad bias har lågpasskaraktär med den långsamma polen som brytfrekvens och överföringen från uppmätt girvinkel till skattad bias har bandpasskaraktär med de två pol- placeringarna som brytfrekvenser. Eftersom skattningen av bias domineras av en lågpassfiltrering av vinkelhastigheten med känd gränsvinkelfrekvens kan denna transformeras till en tidskonstant som avgör hur snabbt skattningen av biasen tillåts variera. Det framgår också av magnituderna att störst vikt läggs vid girvin- kelmätningen vid skattning av girvinkel och vinkelhastighetsmätning vid skattning av bias. Observera att vinkelhastigheterna i y-led och dess bias som båda är in- signaler i filtret inte får något genomslag i lägen där både tipp- och rollvinklar är 0◦ och ett väldigt litet genomslag då vinkelutslagen är små. En frekvensanalys av
filtret visar att i skattningen av girvinkeln läggs en lågpassfiltrering av mätningen av girvinkeln ihop med en högpassfiltrerad integrering av vinkelhastigheten. Något förenklat skattas girvinkeln på följande vis:
ˆ
ψ = Gψ(s)yψ+ Gωz(s)yωz (5.9)
Gωz(s) måste innehålla en integrering så för att se att det är högpassdelen av
5.5 Linjär analys 63
komponenter som tas från de olika mätningarna framgår i Figur 5.2. Som väntat
10−4 10−2 100 102 −150 −100 −50 0 50 Magnitud [dB] Bodediagram 10−4 10−2 100 102 −100 0 100 200 Frekvens [rad/s] Fas [deg] Girvinkel Vinkelhastighet
Figur 5.2: Bodediagram för girvinkel för Kalmanfilter med delad tillståndsvektor framgår att lågpassdelen från girvinkelmätningen kombineras med högpassdelen av gyromätningen.
Attitydfiltret har fyra tillstånd och därför också fyra poler. Eftersom detta filter är olinjärt analyseras det linjäriserade systemet. Linjäriseringen går till på sam- ma sätt som i det EKF som används. Det är så att systemet i den stillastående arbetspunkten där helikoptern inte är tiltad kommer att vara två stycken system som är likadana som girvinkelsystemet och samma analys kan därför göras. Den stora skillnaden är att mätningarna av tipp- och rollvinklar är mycket brusigare än mätningen av girvinkeln och att polerna därför får en helt annan placering i detta fall. En typisk polplacering är −0.01 och −0.316. Som sagt finns fyra poler och båda angivna polplaceringarna utgör dubbelpoler. Biasen är här lika långsam som i girfiltret men den snabba polen är en faktor 10 långsammare vilket innebär att större vikt läggs vid dödräknad vinkelhastighet här.
För positionsfiltret fås 9 tillstånd och 9 poler. Det är noterbart att de 9 polerna kommer i par av 3 eftersom tillstånden består av tre uppsättningar av positioner, hastigheter och biaser. Av den anledningen kan analysen göras på ett reducerat system med en position, hastighet och bias. Vid skattningen av positionen fås en summa av en lågpassfiltrering av positionsmätningen, en högpassfiltrerad integre- ring av hastigheten och en högpassfiltrerad dubbelintegrering av accelerationen. Vid skattningen av hastigheten fås en lågpassfiltrering av hastigheten, en hög- passfiltrerad integrering av accelerationen och en lågpassfiltrerad derivering av positionen. Skattningen av positionen kan skrivas på följande sätt:
ˆ
p = G1(s)yp+ G2(s)yv+ G3(s)ya (5.10)
1
s och G3(s) innehålla en dubbelintegrering 1
s2. För att se vilken del av frekvens-
spektrat som tas från hastighetsmätningen måste därför G2(s) multipliceras med
s. På motsvarande sätt måste G3(s) multipliceras med s2. Resultatet i form av en
bodeplot finns att se i Figur 5.3. I fallet att hastigheten skattas så innehåller över-
10−4 10−2 100 102 −200 −100 0 Magnitud [dB] Bodediagram 10−4 10−2 100 102 −100 0 100 200 Frekvens [rad/s] Fas [deg] Position Hastighet Acceleration
Figur 5.3: Bodediagram för positionsskattning för Kalmanfilter med delad till- ståndsvektor
föringen från position till hastighet en derivering och denna måste multipliceras med 1
s för att se vilken del av frekvensspektret som används medan överföringen
från accelerationen innehåller en integrering och måste multipliceras med s. Resul- tatet presenteras i Figur 5.3 och 5.4. Polerna som svarar mot Figur 5.3 och 5.4 är −0.01, −0.3178 och −3.1462. -0.01 utgör tidskonstanten för biasen, -0.3178 bryt- ningen mellan dödräknad hastighet och positionsmätning och -3.1462 brytpunkten för dödräknad acceleration.
5.5 Linjär analys 65 10−4 10−2 100 102 −200 −150 −100 −50 0 50 Magnitud [dB] Bodediagram Position Hastighet Acceleration 10−4 10−2 100 102 −100 0 100 Frekvens [rad/s] Fas [deg]
Figur 5.4: Bodediagram för hastighetsskattning för Kalmanfilter med delad till- ståndsvektor
Kapitel 6
Resultat
Resultaten från simuleringarna av de implementerade filtren presenteras i detta kapitel. De resultat som har tagits fram kommer först visas och kommer avslutas med att de tre framtagna filtren analyseras. Den mätdata som har använts vid simuleringarna är tagen från flygprov med helikoptern som genomfördes i slutet av examensarbetet. Filtren har trimmats in individuellt för att de inte skall diver- gera. Bedömningen blir mer likvärdig då enkelheten i trimning av filtret vägs in i helhetsbedömningen.
6.1
Sensorfusion med delad tillståndsvektor
I kapitlet kommer filtret med en delad tillståndsvektor att analyseras. Varje del- filter kommer att utvärderas för sig .
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 Tid [s] Tippvinkel [ o] Skattning AHRS (b) Tippvinkeldynamik
Figur 6.1: Skattning av attityd.
6.1.1
Attitydfilter
Det skapas fiktiva mätningar av roll- och tippvinkel från accelerometern. För att stötta attitydskattningarna med GPS-mätningar måste accelerationen och dess bias återkopplas i denna filterstruktur. Den fiktiva mätningen kommer att vara väldigt känslig för felskattning, speciellt i accelerometerbias. Figur 6.1 visar att vinklarna dynamiskt följer varandra.
Skillnaden mellan AHRS och skattningen av tipp- och rollvinkel illustreras i Figur 6.2. Skillnaden mellan de två skattningarna ligger ungefär inom 1.5◦.
670 675 680 685 690 695 700 −0.7 −0.6 −0.5 −0.4 −0.3 −0.2 −0.1 0 0.1 0.2 0.3 Tid [s] Rollvinkelskillnad [ o]
(a) Skillnaden mellan AHRS och skattning av rollvinkel 670 675 680 685 690 695 700 −1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 Tid [s] Tippvinkelskillnad [ o]
(b) Skillnaden mellan AHRS och skattning av tippvinkel
Figur 6.2: Skillnaden mellan AHRS och skattning av attityd.
6.1.2
Girvinkelfilter
En fiktiv mätning skapas av girvinkel från magnetometern och skattningarna av attityden som gjordes i attitydfiltret. Hur den skattade girvinkeln dynamiskt följer AHRS samt skillnaden mellan dem visas i Figur 6.3.
670 675 680 685 690 695 700 −70 −68 −66 −64 −62 −60 −58 −56 −54 Tid [s] Girvinkel [ o] Skattning AHRS (a) Girvinkeldynamik 670 675 680 685 690 695 700 −1.5 −1 −0.5 0 0.5 1 1.5 2 2.5 3 3.5 Tid [s] Girvinkelskillnad [ o]
(b) Skillnaden mellan AHRS och skattning av girvinkel
6.1 Sensorfusion med delad tillståndsvektor 69
6.1.3
Navigationsfilter
I navigationsfiltret krävs ingen fiktiv mätning. Den största utmaningen för filtret är att släta ut mätningarna av position och hastighet med hjälp av accelerome- tern. Accelerometrarna har högre sampelfrekvens än mätningarna av position och hastighet och kan därför användas till att ge utslätade skattningar med samma höga sampelfrekvens som accelerometrarna. Figur 6.4 hur skattningar av position och hastighet typiskt ser ut.
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
Figur 6.4: Skattning av position och hastighet i nordlig riktning
6.1.4
Totala filtret
Varje delfilter för sig har analyserats ovanför och fungerar för sig. Enligt observer- barhetsanalysen var detta väntat, men hur filtren fungerar tillsammans efter att ha kopplats ihop är betydligt mer komplext. Då ingen observerbarhetsanalys är gjord för hela denna filterstruktur är det inte helt klart vad som är observerbart vid ihopkoppling. Eftersom mätekvationen för accelerometrarna innehåller både verklig acceleration och tyngdaccelerationen samt accelerometerbias och det ba- ra är tyngdaccelerationsvektorn som ger information om tipp- och rollvinklar så måste detta måste detta hanteras när mätningarna av tipp- och rollvinklar skapas. Det första alternativet är att strunta i verklig acceleration, det andra att återkopp- la den skattade hastigheten genom en lågpassfiltrerad derivering och kompensera bort den verkliga accelerationen samtidigt som den skattade accelerometerbiasen kompenseras bort.
Filtret lider av ett problem vilket är hos kopplingen mellan orienteringen och ac- celerometerbiasen. Antag att attitydfiltret skattar sina vinklar lite fel. När dessa vinklar används i navigeringsfiltret kommer det bli en konflikt mellan acceleromet- rarna och GPS:en. Filtret kommer svara med att konflikten beror på en accelero- meterbias. Den felaktiga accelerometerbiasen kommer in i attitydfiltret och filtret svarar med felaktiga vinklar som riskerar att förvärra problemet. Filtret kommer aldrig få chans att göra sanna skattningar då felaktig skattning på accelerometer- biasen görs.
Om ingen återkoppling av accelerometerbias görs så kommer skattningen av till- stånden fungera bättre. Nackdelen är att det då blir svårt att skatta accelerometer- bias eftersom en bias i en accelerometer då istället kommer in som en felskattning i vinklarna.
Även om biasen inte återkopplas är det inget hinder för att återkoppla acceleratio- nen. I en helikopter finns det en naturlig sammanblandning mellan verklig accelera- tion och vridningen av tyngdaccelerationsvektorn eftersom helikoptern börjar röra på sig om den tippas eller rollas så att separera dessa tillför mycket information.
Den största fördelen med denna filterstruktur är enkelheten i tuningen av filt- ret. Skulle det vara så att skattningen av ett tillstånd inte har blivit tillräckligt bra är det inte några större svårigheter att ändra designvariablerna så att huvudsakli- gen just det tillstånd påverkas. Nackdelarna är svårigheten att skatta accelerome- terbias och den onaturliga kopplingen mellan orientering och GPS-mätningar.