Institutionen för systemteknik

Transkript

1 Institutionen för systemteknik Department of Electrical Engineering Examensarbete Gyrobiasskattning för små autonoma flygplan Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Haider Shareef LITH-ISY-EX--7/3862--SE Linköping 27 Department of Electrical Engineering Linköpings universitet SE Linköping, Sweden Linköpings tekniska högskola Linköpings universitet Linköping

2

3 Gyrobiasskattning för små autonoma flygplan Examensarbete utfört i Reglerteknik vid Tekniska högskolan i Linköping av Haider Shareef LITH-ISY-EX--7/3862--SE Handledare: Examinator: David Törnqvist isy, Linköpings universitet Staffan Kjerrström Aerodyn Anders Helmersson isy, Linköpings universitet Linköping, 26 januari, 27

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--7/3862--SE Serietitel och serienummer Title of series, numbering ISSN URL för elektronisk version Titel Title Gyrobiasskattning för små autonoma flygplan Gyro bias estimation for MAV banking control Författare Author Haider Shareef Sammanfattning Abstract A nonlinear model that describes the dynamics of the airplane has been produced and simulated. The model has been linearized around a trajectory and discretized to examine the observability of the system. Depending on if the system is observable or not decides whether it is possible or not to apply the extended Kalman filter to estimate the systems states. The Kalman filter is used because we don t have access to measurements to all the states and therefore we need to estimate them. Different tests have been made to examine the performance of the Kalman filter. Even though the Kalman filter is an optimal filter, one can not guarantee satisfying results because the predictions depends on the number of available measurable states. Nyckelord Keywords EKF, flygmekanik, linjärisering, observerbarhet, tidsberoende

6

7 Abstract A nonlinear model that describes the dynamics of the airplane has been produced and simulated. The model has been linearized around a trajectory and discretized to examine the observability of the system. Depending on if the system is observable or not decides whether it is possible or not to apply the extended Kalman filter to estimate the systems states. The Kalman filter is used because we don t have access to measurements to all the states and therefore we need to estimate them. Different tests have been made to examine the performance of the Kalman filter. Even though the Kalman filter is an optimal filter, one can not guarantee satisfying results because the predictions depends on the number of available measurable states. Sammanfattning En patenterad metod [11] ligger till grund för utförandet av det här examensarbetet där målet var att beskriva metoden i reglertekniska termer. En lösning för att kompensera för gyrodrifter var också önskvärd. En olinjär modell som beskriver flygplanets dynamik har tagits fram och simulerats. Modellen har linjäriserats kring en trajektoria och diskretiserats för att undersöka observerbarheten för systemet. Huruvida ett system är observerbart eller inte avgör om det är möjligt eller inte att applicera ett extended kalmanfilter (EKF) på för att skatta systemets tillstånd. Kalmanfiltret används för att man inte har tillgång till mätningar på systemets alla tillstånd och måste därför skatta dem. Olika tester har gjorts för att undersöka kalmanfiltrets prestanda. Trots att kalmanfiltret är ett optimalt filter kan man inte garantera tillfredsställande resultat eftersom antalet tillgängliga mätbara tillstånd påverkar hur bra skattningar man får. v

8

9 Tack Jag vill tacka Aerodyn och min handledare Staffan Kjerrström för att jag har fått göra mitt examensarbete hos de och för ett gott samarbete. Jag vill även rikta ett stort tack till min handledare David Törnqvist som har varit till stor hjälp med alla hans råd och idéer. Sedan vill jag tacka mina vänner som jag har lärt känna under min tid här i Linköping, tack för alla underbara stunder som vi har tillbringat tillsammans. Sist men inte minst vill jag tacka min kära familj för deras ovillkorliga stöd och kärlek som har gjort att jag står där jag står idag. vii

10

11 Notation Indexering på symboler b e Kroppsfast Jordfast Signaler r u y x TE Referenssignal Styrsignal Utsignal Tillståndsvektor Total energi Matriser A, B, C Systemmatriser för den kontinuerliga tillståndsbeskrivningen F, G, H Systemmatriser för den diskreta tillståndsbeskrivningen A lin, B lin De linjära motsvarigheterna till A och B Q kov Kovariansmatris för processtörningen R kov Kovariansmatris för mätstörningen J Jacobian Γ b Tröghetsmatris P ( k k) Kovariansmatris för skattningsfelet Symboler z e x e, y e u v w V T v Höjd relativt jorden Lägen i x- resp. y-led Hastighetskomponent i x-riktning Hastighetkomponent i y-riktning Hastighetkomponent i z-riktning Luftflödets hastighet Hastighet ix

12 x φ θ ψ w x w y w z ω q -q 3 δ a,δ e b S c q ρ I x,i y,i z T eb F M H F aero M aero X Y Z L M N g m C x,c y,c z C l,c m,c n ω k v k λ,σ Rollvinkel Tippvinkel Girvinkel Rollvinkelhastighet Tippvinkelhastighet Girvinkelhastighet Vinkelhastighet för en roterande kropp Eulerparametrar som utgör kvaternionen Skevroder- och höjdroderutslag Vingens spännvidd Vingens area Aerodynamisk medelkorda Aerodynamisk tryck Luftflödets densitet Tröghetsmoment kring x, y, z - axeln Transformationsmatris Kraft Moment Rörelsemängdsmoment Totala aerodynamiska krafter Totala aerodynamiska moment Kraft i x-led Kraft i y-led Kraft i z-led Moment kring x-axeln Moment kring y-axeln Moment kring z-axeln Gravitationskonstant Flygplanets massa Dimensionslösa kraftkoefficienter Dimensionslösa momentkoefficienter Systemstörning eller processtörning Mätstörning Egenvärden och singulära värden

13 Figurer 2.1 Blockschema som visar hur de ingående delsystemen interagerar med varandra Krafter och moment verkande på flygplanet Spännvidd, area och aerodynamiska medelkordan på en vinge Vinkelhastigheterna i roll, gir- och tippled Rotation med Eulers vinklar Rotation med kvaternioner Simulering av flygplansmodellen utan brus, δ a = 1.3 och δ e = Eulervinklarna i grader. Roll, tipp respektive girvinkel Skattning av Eulervinklarna, sanna värden är streckade och de skattade är heldragna. Q kov =.7 och R kov = Projecering av tillståndsvariablerna Kvaternionernas längd efter korrigering Tillståndsskattning med tre mätningar, skattning (heldragna) och sanna tillstånd (streckade). Q kov =.7 och R kov = Tillståndsskattning med tre mätningar, skattning (heldragna) och sanna tillstånd (streckade). Q kov =.7 och R kov = Skattning av Eulervinklarna med tre mätningar, skattning (heldragna) och sanna tillstånd (streckade). Q kov =.7 och R kov = Skattning av Eulervinklarna med tre mätningar, skattning (heldragna) och sanna tillstånd (streckade). Q kov =.7 och R kov = Tillståndsskattning med fyra mätningar, skattning (heldragna) och sanna tillstånd (streckade). Q kov =.7 och R kov = Tillståndsskattning med fyra mätningar, skattning (heldragna) och sanna tillstånd (streckade). Q kov =.7 och R kov = Skattning av Eulervinklarna med fyra mätningar, skattning (heldragna) och sanna tillstånd (streckade). Q kov =.7 och R kov = Skattning av Eulervinklarna, sanna värden är streckade och de skattade är heldragna. Q kov =.7 och R kov = Exempel på hur kovariansmatrisen P k k för estimeringsfelet konvergerar mot sitt stationära värde när fyra mätningar används, Q kov =.7 och R kov = Exempel på hur förstärkningen K k för estimeringsfelet konvergerar mot sitt stationära värde när fyra mätningar används, Q kov =.7 och R kov = Projecering av tillståndsvariablerna med sex mätningar Tillståndsskattning med sex mätningar, skattning (heldragna) och sanna tillstånd (streckade). Q kov =.7 och R kov = Exempel på hur kovariansmatrisen P k k för estimeringsfelet konvergerar mot sitt stationära värde när sex mätningar används, Q kov =.7 och R kov =

14 xii C.1 Översiktsbild över simuleringsmodellen

15 Innehåll 1 Inledning Uppgift och syfte Metod Avgränsningar Läsanvisningar Teori Systembeskrivning Modellering av gyrodrift Aerodynamik Flygmekanik Definition av flygplansvariabler Eulers vinklar Kvaternioner Relationen mellan Eulervinklarna och kvaternionerna Stelkroppsrörelse Beskrivning av flygplansmodellen Linjärisering Diskretisering Observerbarhet Singulärvärdesfaktorisering - SVD Signalbehandling Kalmanfilter Extended Kalmanfilter - EKF Designaspekter - val av Q kov och R kov Resultat Sammanställning av ekvationer Flygplansmodellen Simulering av flygplansmodellen Den linjära modellen Observerbarhet Kalmanfiltreringen Verifiering av kraven på kvaternionerna xiii

16 3.6.2 Val av Q kov och R kov med tre mätningar Val av olika Q kov och R kov med fyra mätningar Kovariansmatrisen för skattningsfelet Verifiering av kalmanfiltret - ytterligare mätningar Slutsatser och diskussion 43 Litteraturförteckning 45 A Definition av parametrar 47 A.1 Stabilitetskoefficienter A.2 Stabilitetsderivator B Exempel-körning 5 B.1 Den linjära modellen C Källkod 52

17 Kapitel 1 Inledning Detta examensarbete är utfört på Aerodyn. Aerodyn bygger små modellsegelplan och utvecklar elektronik för automatisk flygning. Deras system användas både för vanlig modellflygning såväl som för övervakningssystem. Luftmotståndet hos ett flygplan orsakar energiförluster, vilka kan mätas med flyginstrumenten. Aerodyn har gjort flygprov som visar att om flygplanets lutning (bankning) styrs så att energiförlusten minimeras kommer lutningen automatiskt att anpassas till ett givet höjdroderutslag. Resultatet blir att flygplanet flyger i en stationär sväng (eller rakt fram). Det är alltså möjligt att automatiskt styra flygplanet utan tillgång till en uppmätt eller beräknad lodlinje [12]. Förr i tiden användes mekaniska gyron för att mäta flygplanets orientering, baserat på principen om rörelsemängsmomentets bevarande inom fysiken. Dessa bestod av ett roterande hjul och en axel som hänger fritt i luften så att den kan röra sig obehindrat. När hjulet roterar tenderar axeln att motverka ändringar av sin orientering så att den hela tiden pekar åt samma håll. När flygplanet lutar kommer gyrot alltså inte att följa med och bibehåller sin orientering, vilket gör att man hela tiden vet vilken riktning norr och söder är [16]. Dagens flygplan använder en gyroplattform eller vinkelhastighetsgyron för att kunna blindflyga. Dessa är oftast elektriska och används för att mäta vinkelhastigheten hos flygplanet relativt en fast referenspunkt (t ex jorden). De flesta gyron är känsliga kring en axel, t ex om axeln är monterad vertikalt kommer gyrot att bli känslig i girled, men okänslig i roll- och tippled [1]. 1.1 Uppgift och syfte En patenterad metod [11] ligger till grund för utförandet av det här examensarbetet där man vill beskriva metoden i reglertekniska termer. Mekaniska gyron ska kunna ersättas med elektriska halvledargyron (t ex piezogyron) genom att konstruera automatisk styrning av ett flygplan. Man vill minimera flygplanets energiförluster genom att få den att flyga i en stationär sväng, d.v.s. stabilisera flygplanet kring dess giraxel. En lösning för att kompensera för gyrodrifterna är därför önskvärd. 1

18 2 Inledning 1.2 Metod Lösningsgången kommer att gå till på följande sätt, först behövs en modell som beskriver flygplanets dynamik. Vanligtvis är en sådan modell olinjär och den behöver linjäriseras kring en trajektoria som beskriver en sväng. Trajektorian fås genom att simulera den olinjära modellen. Detta gör det möjligt att beskriva modellen på diskret form och undersöka observerbarheten för modellen. Sedan skattas modellens tillstånd med hjälp av extended kalmanfilter. Design och implementering samt simulering och verifiering av modellen och resultaten sker i matlab och simulink. 1.3 Avgränsningar För att fokusera på uppgiften och inte hamna på sidovägar samt att arbetet inte ska bli för omfattande måste vissa avgränsningar göras. Fokus har lagts på att undersöka om girvinkelhastigheten är observerbart, detta för att få insikt i om problemet är möjligt att lösa. Dessutom har vindens påverkan på flygplanet vid simuleringar bortsetts från och masströgheten antas vara konstant samt att flygplanet är symmetrisk kring rollaxeln. Utöver det antas även att flygplanet har en punktformig massa så att flygplanets tyngdpunkt koncentreras till en punkt belägen i flygplanskroppens centrum. 1.4 Läsanvisningar Rapporten består av följande kapitel; teori, resultat, slutsatser och diskussion samt följande appendix; definition av parametrar, exempel-körning och källkod. Kapitel 2 behandlar den teori som arbetet bygger på och ger en bra grund för läsaren att förstå arbetet. Kapitel 3 redovisar resultat med plottar. Kapitel 4 tar upp slutsatser och diskussion kring arbetet. Appendix A definierar de flygplansparametrar som används. Appendix B presenterar exempel på simuleringsresultat. Appendix C innehåller källkoden för arbetet.

19 Kapitel 2 Teori Flygplansmodellen är en matematisk modell som bygger på fysikens kraftlagar och som beskriver flygplanets rörelse i luften. Modelleringsprocessen är en iterativ process där man utgår från vilka data som ska tas hänsyn till och sedan kan modellen förfinas och anpassas till de aktuella datan. Modelleringsprocessen utgörs av två delar, flygplanets aerodynamik och flygplanets mekanik. Dessa beskrivs nedan, men först en kort beskrivning av systemet och modelleringen av gyrot. 2.1 Systembeskrivning Figur 2.1 visar en schematisk beskrivning över hela systemet. Dock kommer endast flygplansdynamiken, beskrivs i 2.4.5, och observatören (EKF), beskrivs i 2.7.2, att studeras i det här arbetet. De olika delsystemen interagerar med varandra med hjälp av in- och utsignaler. Utsignaler från regulatorn, som således blir in-eller styrsignaler till flygplansdynamiken, utgörs av höjdroder- och skevroderutslagen, δ e och δ a. Intressanta utsignaler från flygplansdynamiken är hastighetskomponenterna, u och v, i x- respektive y-led, girvinkelhastigeten w z med bias och höjden z e (e anger att höjden är relativt jorden). Dessa kommer tillsammans med styrsignalerna utgöra insignaler till Kalmanfiltret som returnerar ŵ zref, ett skattat värde på girvinkelhastigheten. Eftersom regulatorn (se figur 2.1) inte har implementerats antas styrsignalerna vara konstanta. 2.2 Modellering av gyrodrift Gyrodrift är ett känt problem och det finns olika sätt att beskriva driften på. Vanligtvis representeras gyrodriften av bias som i sin tur representeras av integrerat, bandbegränsat vitt brus med medelvärdet noll. Biasen kan alltså enkelt uttryckas enligt följande: b = w k dt (2.1) där w k är det bandbegränsade vita bruset. 3

20 4 Teori Figur 2.1. Blockschema som visar hur de ingående delsystemen interagerar med varandra. 2.3 Aerodynamik Aerodynamiken innefattar de krafter och moment (figur 2.2) som påverkar flygplanet när det flyger i luften. Dessa krafter och moment varierar bland annat beroende på vingens spännvidd, vingens area och den så kallade aerodynamiska medelkordan. Dessa variabler betecknas enligt (figur 2.3): b = Vingens spännvidd [m] S = Vingens area [m 2 ] c = Aerodynamiska medelkordan [m] där det aerodynamiska trycket q = 1 2 ρv2 T (2.2) beror på luftflödets densitet och hastighet där ρ = Luftflödets densitet V T = Luftflödets hastighet De aerodynamiska krafterna och momenten beror även på vinkelhastigheterna (w x, w y, w z ), se figur 2.4, och de dimensionslösa aerodynamiska koefficienterna C i. De sistnämnda är svåra att bestämma analytiskt, men de kan uppskattas t ex genom vindtunnelexperiment.

21 2.4 Flygmekanik 5 Figur 2.2. Krafter och moment verkande på flygplanet. 2.4 Flygmekanik Flygmekanik omfattar beskrivning och bestämning av ett flygplans (eller någon annan flygande kropps) beteende och dess flygbanor. För dessa ändamål behöver man studera olika matematiska samband för att få försåelse för hur flygplan beter sig. Vanligtvis är det lättare att beskriva flygplanets orientering om man använder tre koordinatsystem. Då betraktar man ett jordfast koordinatsystem, S e, med origo på jordytan, ett skrovfast, S b, med origo i flygplanets tyngdpunkt och ett vindkoordinatsystem S w. I det här arbetet kommer däremot endast S e och S b att användas Definition av flygplansvariabler Innan kraftekvationerna som beskriver flygplanets rörelse ställs upp behöver man definiera de variabler som kommer att användas. Flygplanet kan betraktas som en stel kropp där dess rörelse beskrivs av dess position, hastighet, orientering och vinkelhastigheter.

22 6 Teori Figur 2.3. Spännvidd, area och aerodynamiska medelkordan på en vinge. Position I det jordfasta koordinatsystemet anger vektorn p positionen p i = (x e y e z e) T (2.3) närmare bestämt läget för flygplanets tyngdpunkt i det jordfasta koordinatsystemet. z e anger alltså flygplanets höjd. Att den är negativ beror på att Z-axeln är riktad nedåt mot jordytan. Hastighet I det kroppsfasta koordinatsystemet anger vektorn V b hastigheten V b = ( u v w) T (2.4) där u, v och w anger hastigheten i longitudinell, lateral- och normalled. Kvaternioner används för att beskriva flygplanets orientering re- Orientering lativt jorden q = ( q q 1 q 2 q 3 ) T (2.5)

23 2.4 Flygmekanik 7 Figur 2.4. Vinkelhastigheterna i roll, gir- och tippled. där elementen q -q 3 kallas Eulerparametrar. I det kroppsfasta koordinatsystemet anger vektorn w b vin- Vinkelhastigheter kelhastigheterna w b = ( w x w y w z ) T (2.6) där w x är rollvinkelhastigheten, w y är tippvinkelhastigheten och w z är girvinkelhastigheten. Sedan har man även tröghetsmatrisen och tyngdkraftens verkan på flygplanet uttryckta i flygplanets respektive jordens koordinatsystem Γ b = I x I xy I xz I yx I y I yz I zx I zy I z (2.7) g e = ( g) T (2.8) samt insignalen till flygplanet som representeras av flygplanets roder u = ( δ a δ e ) T (2.9)

24 8 Teori där δ a = skevroder och δ e = höjdroder. Sist har man utsignalerna y = (u v w z + b z e) T (2.1) som alltså är hastighetskomponenterna i x- och y-led, girvinkelhastigheten med bias och höjden. Alternativa utsignaler Alternativa utsignaler från flygplanet är tidsändringen av den totala energin, TE, och girvinkelhastigheten w z. Den totala energin utgörs av den kinetiska och potentiella energin (E k respektive E p ), d.v.s. E k = mu2 (2.11) 2 E p = mgz e (2.12) och tidsändringen av den totala energin ges av TE = d dt (E k + E p ) = d ( ) mu 2 + mgz e = mu u + mgz dt 2 e (2.13) Den totala energin mäts med en variometer monterad ombord i flygplanet. Dessa signaler är de enda man har tillgång till enligt patentet [11]. Men eftersom TE består av tillståndsvariablerna u och z e är det lättare att anta att man har tillgång till mätningar på u och z e, vilket har gjorts i det här arbetet. Detta har dock inte påverkat patentet eftersom mätningar på u och z e kunde fås med den utrustning som finns på flygplanet Eulers vinklar Det vanligaste och mest förekommande sättet att beskriva flygplanets orientering i luften är med Eulervinklarna [2]. Man utgår från det flygplansfasta koordinatsystemet, som i tidpunkten t = sammanfaller med det jordfasta koordinatsystemet och utför tre successiva rotationer för att få det jordfasta koordinatsystemet att sammanfalla med det flygplansfasta koordinatsystemet vid tidpunkten t. Dessa tre rotationer beskriver flygplanets orientering och ordningen på rotationerna är viktig. Rotationerna görs enligt högerhandsregeln, där första rotationen är ψ grader stor och sker kring z-axel. Sedan sker en rotation med θ grader kring den nya y-axeln och slutligen en rotation med φ grader kring den nya x-axeln (figur 2.5) Kvaternioner Kvaternionerna [2] har blivit alltmer populära att använda för beskrivning av en kropps rotation i flygsammanhang. Metoden är inte lika intuitiv som Eulers vinklar, däremot eliminerar metoden de begränsningar och numeriska problem som Eulers vinklar ger upphov till.

25 2.4 Flygmekanik 9 Figur 2.5. Rotation med Eulers vinklar. Kvaternionerna kan beskrivas på flera olika sätt där den bästa beskrivningen beror på användningsområdet. Den mest intuitiva beskrivningen är att betrakta kvaternionerna som en skalär plus en vektor på följande form: (w,v), v = ix + jy + kz Däremot är följande beskrivning, ur matematisk synvinkel, lättare att manipulera: w + ix + jy + jz För att definiera hur en fast kropps rotation kan beskrivas med kvaternioner bör man betrakta några av de matematiska lagarna som är förknippade med kvaternionerna. De vanliga algebraiska lagarna går att applicera på kvaternionerna, förutom multiplikation som är icke-kommutativ, d.v.s. de imäginära enhetsvektorerna i, j och k har följande egenskaper: i 2 = j 2 = k 2 = ijk = 1 ij = k, jk = i, ki = j = ik Ett vanligt sätt att definiera orientering med användning av kvaternioner är med hjälp av Eulers Teorem som säger att orienteringen av en fast kropp kan beskrivas som en rotation kring en axel v med vinkeln Φ (figur 2.6). Med kravet att vektorn v ska ha enhetslängd får kvaternionerna följande form: cos Φ 2,vsinΦ 2 (2.14) som alltid har enhetslängd sådan att: w 2 + x 2 + y 2 + z 2 = 1

26 1 Teori Figur 2.6. Rotation med kvaternioner. Med algebraisk notation utförs multiplikation av två kvaternioner p och q med hjälp av associationslagen på följande sätt: r = pq = (p + p 1 i + p 2 j + p 3 k) (q + q 1 i + q 2 j + q 3 k) = p q + p q 1 i + p q 2 j + p q 3 k + p 1 q i + p 1 q 1 i 2... som så småningom kan skrivas om på matrisform enligt följande: r p p 1 p 2 p 3 q r 1 r 2 = p 1 p p 3 p 2 q 1 p 2 p 3 p p 1 q 2 (2.15) r 3 p 3 p 2 p 1 p q 3 För att förstå vad multiplikation av två kvaternioner representerar är det viktigt att komma ihåg att en kvaternion består av en vektor och en skalär, där vektorn representerar en axel och skalären representerar rotationsvinkeln kring axeln. För att ändra rotationen som en kvaternion, p, representerar behöver man definiera en ny kvaternion, q, som beskriver hur denna ändring av rotationen ska utföras. Multiplikation av p och q resulterar i en ny kvaternion, r, som har ändrats med den rotation som q beskrev. Detta förfarande är just det man måste ta till med för att beskriva hur två olika koordinatsystem (jordens och flygplanets) förhåller sig till varandra när de rör sig med cirkulär rörelse relativt varandra. Då vill man, med hjälp av kvaternioner, kunna beskriva flygplanets rörelse relativt jorden uttryckt i jordens koordinatsystem. Transformationen blir: X e Y e Z e = q 2 + q1 2 q2 2 q3 2 2(q 1 q 2 q q 3 ) 2(q q 2 + q 1 q 3 ) 2(q 1 q 2 + q q 3 ) q 2 q1 2 + q2 2 q3 2 2(q 2 q 3 q q 1 ) 2(q 1 q 3 q q 2 ) 2(q 2 q 3 + q q 1 ) q 2 q1 2 q2 2 + q3 2 } {{ } T eb X b Y b Z b (2.16)

27 2.4 Flygmekanik 11 där T eb är transformationsmatrisen uttryckt i kvaternioner och indexen e och b visar att det är jordens respektive flygplanets koordinatsystem. Eftersom flygplanets orientering hela tiden ändras måste man kunna uppdatera kvaternionen för att bibehålla samma orientering relativt jorden. Detta görs på följande sätt: q q q 1 q 3 2 = w x w y w z w x w z w y w y w z w x w z w y w x q q 1 q 2 q 3 (2.17) Eftersom man har kravet att kvaternionen ska ha enhetslängd måste man, p.g.a numeriska fel vid integration, korrigera Eulerparametrarna enligt: qj korr q j = q 2 + q1 2 + q2 2 + q2 3 j =...3 (2.18) Man måste ha i åtanke att alltid korrigera kvaternionerna efter varje uppdatering Relationen mellan Eulervinklarna och kvaternionerna I vissa fall önskar man översätta kvaternionen till Eulers vinklar för att få en mer intuitiv bild över flygplanets rörelse. Då gäller följande samband ([14] sida 29 och 32): [ ] φ = atan2 2(q 2 q 3 + q q 1 ),(2q 2 + 2q3 2 1) (2.19) θ = arcsin [ 2(q q 2 q 3 q 1 ) ] (2.2) [ ] ψ = atan2 (2q 1 q 2 + 2q q 3 ),(2q 2 + 2q 2 1 1) (2.21) där atan2 är en matlabfunktion som är ett specialfall av arctangens. I andra fall kan det vara praktiskt att översätta åt det andra hållet, d.v.s. från Eulers vinklar till kvaternioner. Då gäller följande: ( ) ( ) ( ) ( ) ( ) ( ) ψ θ φ ψ θ φ q = cos cos cos + sin sin sin ( ) ( ) ( ) ( ) ( ) ( ) ψ θ φ ψ θ φ q 1 = cos cos sin sin sin cos ( ) ( ) ( ) ( ) ( ) ( ) ψ θ φ ψ θ φ q 2 = cos sin cos + sin cos sin ( ) ( ) ( ) ( ) ( ) ( ) ψ θ φ ψ θ φ q 3 = sin cos cos cos sin sin (2.22) (2.23) (2.24) (2.25)

28 12 Teori men då ska man ha i åtanke att ett byte av tecken på Eulerparametrarna(q -q 3 ) ger för samma orientering en annan uppsättning Eulerparametrar. Detta eftersom ett teckenbyte motsvarar en vridning i motsatt riktning kring en motsatt riktad axel Stelkroppsrörelse Nu kan arbetet med att ta fram en modell för flygplanets dynamik påbörjas. Som tidigare nämnt kan man betrakta flygplanet som en stel kropp, vilket gör att man kan använda Newtons rörelselagar för att ta fram rörelseekvationerna. Flygplanets rörelse kan delas in i två delar, translation och rotation, där Newtons rörelselagar kan appliceras på båda delarna. För translationsrörelsen fås enligt Newtons andra lag att summan av alla externa krafter som påverkar en kropp är lika med tidsändringen av rörelsemängden som kroppen har. Det ger följande ([13] och [8]): d F = (mv) (2.26) dt där F = externa krafter och m = flygplanets massa. För rotationsrörelsen gäller enligt Newtons andra lag att summan av alla externa moment som påverkar en kropp är lika med tidsändringen av rörelsemängdsmomentet. Det kan uttryckas på följande sätt: M = d dt H (2.27) där M = externa moment och H = rörelsemängdsmomentet. Sedan måste även hänsyn tas till att flygplanets koordinatsystem roterar relativt det inertiella, jordens, koordinatsystemet. Antag att flygplanet roterar med vinkelhastigheten ω relativt jorden. Då ger Coriolis teoremet, [14] sida 1, att d dt v = d e dt v + ω v (2.28) b där e och b betecknar de koordinatsystem som derivatan beräknas i, d.v.s. i jordens respektive flygplanets koordinatsystem. Detta gör att ekvationerna för krafter och moment, (2.26)-(2.27), kan skrivas om enligt följande: F = d dt (mv) + ω v (2.29) b M = d dt H + ω H (2.3) b Det är även känt att för rörelsemängdsmoment gäller att: H = Γ b ω b (2.31)

29 2.4 Flygmekanik 13 där tröghetsmatrisen Γ b är konstant, eftersom man betraktar rörelsemängdsmomentet i flygplanets koordinatsystem. Dessutom är flygplan normalt symmetriska i xz-planet vilket gör att tröghetsmatrisen blir Γ b = I x I xz I y I zx I z (2.32) Samlar man alla vektorer och betraktar det flygplansfasta koordinatsystemet fås följande slutgiltiga ekvationer för stelkroppsrörelse: F aero = m( v + ω v) (2.33) M aero = Γ b ω + ω Γ b ω (2.34) De totala externa krafterna utgörs av de aerodynamiska krafterna och gravitationskraften. Gravitationskraften En kraft riktad i jordytans normalled och verkar på flygplanets tyngdpunkt. Således får man F g = = T ebt = mg 2(q 1q 3 q q 2 ) 2(q 2 q 3 + q q 1 ) (2.35) mg mg (q 2 q1 2 q2 2 + q3) 2 e b där transformationsmatrisen T eb, definierad i (2.16), måste transponeras eftersom gravitationskraften beräknas uttryckt i flygplanets koordinatsystem. Enligt figur 2.2 har man följande ae- Aerodynamiska krafter och moment rodynamiska krafter och moment X F aero = Y, Z M aero = L M N Stabilitetskoefficienterna, C x..., beskrivs i appendix A Beskrivning av flygplansmodellen b b, X = qsc x Y = qsc y Z = qsc z (2.36) L = qsbc l M = qscc m N = qsbc n (2.37) För att applicera reglertekniska metoder på ett system behöver man en modell av systemet, vanligtvis uttryckt i matematiska termer. I det här avsnittet beskrivs hur flygplanets dynamik kan modelleras med hjälp av matematiska ekvationer. Det blir en fortsättning på resultaten från föregående avsnitt. Modellen har det generella utseendet b

30 14 Teori ẋ(t) = Ax(t) + Bu(t) (2.38) y = Cx(t) (2.39) och beskriver en tidskontinuerlig tillståndsmodell som kan studeras med hjälp av reglerteori. A är en n n matris, B är en n m matris och C är en p n matris, där n = antal element i tillståndsvektorn, m = antalet insignaler och p = antalet utsignaler. Den färdiga flygplansmodellen beskrivs i avsnitt Linjärisering Linjärisering av de aerodynamiska krafterna och momenten görs med hjälp av den så kallade small disturbance theory ([13] sida 14) som går ut på att man antar att flygplanets rörelse består av små avvikelser kring det trimmade flygläget (flygning rakt fram). Detta beskrivs av följande ekvationer ([13] sida 17) X m = X u(u u ) + X w (w w ) + X δe (δ e δ e ) X u = X u X w = X w X δe = X δ e Y m = Y v(v v ) + Y wx (w x w x ) + Y wz (w z w z ) Y v = Y v Y wx = Y w x Y wz = Y w z Z m = Z u(u u ) + Z w (w w ) + Z wy (w y w y ) + Z δe (δ e δ e ) Z u = Z u Z w = Z w Z wy = Z w y Z δe = Z δ e L I x = L v (v v ) + L wx (w x w x ) + L wz (w z w z ) + L δa (δ a δ a ) L v = L v L wx = L w x L wz = L w z L δa = L δ a M I y = M u (u u ) + M w (w w ) + M wy (w y w y ) + M δe (δ e δ e ) M u = M u M w = M w M wy = M w y M δe = M δ e N I z = N v (v v ) + N wx (w x w x ) + N wz (w z w z ) + N δa (δ a δ a ) N v = N v N wx = N w x N wz = N w z N δa = N δ a

31 2.5 Observerbarhet 15 där man linjäriserar kring en arbetspunkt (x, u ) som ger: ẋ = f(x,u) f(x,u u ) + F x (x,u )(x x ) + F u (x,u )(u u ) (2.4) som tillsammans med den olinjära modell som kommer tas fram i avsnitt 3.2 och derivering med avseende på x enligt Jacobianmatrisen: J = f 1 f 1 x n x f n x 1 f n x n (2.41) ger den linjära modell som kommer att presenteras i avsnitt 3.4. I appendix A.2 och A.1 definieras de dimensionslösa stabilitetsderivatorna och stabilitetskoefficienterna som används i linjäriseringen ovan Diskretisering Målet med diskretiseringen är att möjliggöra uppdateringen av A och C matriserna efter varje tidssteg för att så småningom kunna undersöka observerbarhet för flygplansmodellen. Detta för att observerbarhetsmatrisen, som det kommer att visas i nästa avsnitt, byggs upp allteftersom efter varje tidssteg och då behöver man tillgång till A och C matriserna för varje tidssteg. Man antar att man samplar med samplingsintervallet T och att insignalen u(t) är styckvis konstanst, d.v.s. u(t) = u(kt), kt t < (k + 1)T. (2.42) Då fås, enligt [4] sida 7, att ett system som beskrivs av ekvation (2.38)-(2.39) kan samplas och beskrivas enligt följande där x(kt + T) = Fx(kT) + Gu(kT) (2.43) y(kt) = Hx(kT) (2.44) F = e AT, G = T e At B dt, H = C (2.45) Byter man ut A och B mot A lin respektive B lin i ekvation (2.45) får man en tidsdiskret flygplansmodell som man kan simulera och undersöka dess observerbarhet. Det görs i avsnitt Observerbarhet Observerbarhet används i reglertekniska sammanhang som ett mått på hur väl man kan dra slutsats om systemets interna tillstånd baserat på systemets utsignaler. Ett system sägs vara observerbart om man på ändlig tid kan bestämma det

32 16 Teori nuvarande tillståndet baserat på utsignalerna. Med andra ord betyder detta att man kan betrakta utsignalerna och utifrån dem veta vad som pågår i systemet eller beräkna de tillstånd som ledde till den betraktade utsignalen. För system som inte är observerbara går det inte att bestämma de nuvarande tillstånden baserat på utsignalerna. Definition på observerbarhet enligt [6] Definition 2.1 (Observerbarhet) Tillståndet x sägs vara icke observerbart om, då u(t) =, t och x() = x utsignalen y(t) =, t. Systemet säges vara observerbart om det saknar icke observerbara tillstånd. För linjära tidskontinuerliga system är det enkelt att bestämma observerbarhet. Om ett system har n tillstånd är det observerbart om observerbarhetsmatrisen C CA CA 2 (2.46). CA n 1 har full rang = n, d.v.s. antalet linjärt oberoende ekvationer ska vara n stycken. Däremot är bestämning av observerbarhet för ett tidsvariabelt och olinjärt system generellt ett svårt problem. För att förenkla problemet approximeras det olinjära systemet med ett tidsvariabelt och tidsdiskret linjärt system och sedan beräknas observerbarhetsmatrisen för detta system. Observerbarhetsmatrisen blir då [9] H t H t+1 F t+1 H t+2 F t+2 F t+1. H t+n F t+n F t+1 (2.47) Då ser man att de ingående elementen i observerbarhetsmatrisen, matriserna F och H, varierar med tiden och som bekant är de det tidsdiskreta systemets motsvarigheter till A och C i det tidskontinuerliga fallet. 2.6 Singulärvärdesfaktorisering - SVD Observerbarhetsmatrisen har en tendens att växa i storlek beroende på tidshorisonten som den beräknas över och antalet mätningar som är tillgängliga från flygplanet. Detta gör att man behöver en kraftfull och pålitlig metod som hanterar stora och singulära matriser. SVD är en sådan metod som man med hjälp av kan analysera ett givet matematiskt problem som representeras av matriser. Dessutom är SVD den mest pålitliga metoden för att numeriskt beräkna rangen på en matris. Innan arbetet med att definiera vad beräkningen av SVD representerar påbörjas behöver begreppen egenvärden, egenvektorer och singulära värden definieras [5].

33 2.7 Signalbehandling 17 Definition 2.2 (Egenvärde och egenvektor) Egenvärdet, λ, för en matris A definieras som ett tal som för någon vektor, s.k. egenvektor, f ger att följande likhet gäller Av = λv Definition 2.3 (Singulära värden) Singulära värden, σ i, för en matris A definieras som σ i = λ i där λ i är egenvärdena till A T A. Enligt [6] representeras en n m matris A som en faktorisering av matriser, singulärvärdesfaktorisering: A = UΣV T (2.48) där U är en n n unitär matris, d.v.s. UU T = I, Σ är en n m matris som har A:s singulära värden utmed diagonalen och nollor för övrigt, medan V är en m m unitär matris. Beräkningen av SVD utgörs i princip av att hitta egenvärden och egenvektorer av A T A och AA T. V :s kolumner utgörs av egenvektorerna till A T A och egenvektorerna till AA T utgör U:s kolumner. Vidare utgörs Σ:s singulära värden som roten av egenvärdena till AA T eller A T A. Som tidigare nämnt ska observerbarhetsmatrisen ha full rang för att systemet ska vara observerbart. Rangen för en matris utgörs av dess nollskilda singulära värden, d.v.s. rang(a) = σ i, alternativt antalet linjärt oberoende kolonnvektorer i matrisen. Ekvation 2.48 kan då skrivas om enligt följande A = UΣV T = ( U 1 U 2 ) ( Σ )( V T 1 V T 2 ) = U 1 ΣV T 1, (2.49) där U 1 är en n (n σ i ) matris och V1 T är en m σ i matris. Således utgör V1 T värderummet för matrisen A. Värderummet för observerbarhetsmatrisen representerar alla linjära kombinationer av alla tillståndsvariabler som är observerbara. Ett enkelt sätt att undersöka exakt vilka tillståndsvariabler som är observerbara är genom projicering enligt följande x proj = V 1 V T 1 x (2.5) där vektorn x som bekant är tillståndsvektorn. Om x proj = x, d.v.s. V 1 V T 1 är lika med enhetsmatrisen, innebär det att observerbarhet råder. 2.7 Signalbehandling Som tidigare nämnt är observerbarhetsmatrisens element, matriserna F och H, tidsberoende och antar olika värden vid olika tidpunkter. För att ta hänsyn till tidsberoendet behövs en uppdateringsmekanism som uppdaterar vår tidsdiskreta modell i varje tidssteg. Kalmanfiltret är ett perfekt verktyg för detta ändamål.

34 18 Teori Kalmanfilter Kalmanfiltret är en process som på ett rekursivt sätt skattar ett dynamiskt systems tillstånd utifrån brusiga mätningar. Ett systems tillstånd, som också kan påverkas av störningar, är som bekant de variabler som beskriver systemets beteende. Detta kan beskrivas matematisk enligt följande: Man tänker sig en signalmodell x k+1 = Ax k + w k (2.51) y k = Cx k + v k (2.52) som beskriver relationen mellan mätsignalen y och tillstånden x, där w k och v k är systemstörningen respektive mätstörningen i tidpunkten k. Normalt representeras både w k och v k av vitt brus, d.v.s. stokastiska processer med medelvärden och kovariansfunktioner E[w k ] = E[v k ] = (2.53) E[w k w T k1] = Q kov δ k k1 (2.54) E[v k v T k1] = R kov δ k k1 (2.55) Man vill alltså beräkna skattningen ˆx k k1 av tillståndsvektorn x k så bra man kan utifrån mätvärdena {y,...,y k1 }, där k > k1. Således blir skattningsfelet x k k1 = x k ˆx k k1 (2.56) där man vill minimera dess varians. Det är också av stor vikt att skattningen är tillförlitlig, annars har man inte så stor nytta av den. Därför definieras P k k1 = E[(x k ˆx k k1 )][(x k ˆx k k1 )] T (2.57) som betecknar kovarians-matrisen för skattningsfelet. Kalmanfiltret arbetar i två faser, tidsuppdatering och mätuppdatering ([7], s ). Tidsuppdatering : Beräknar ˆx k+1 k och ˆP k+1 k förutsatt att man känner ˆx k k och ˆP k k. Detta görs enligt ˆx k+1 k = Aˆx k k (2.58) ˆP k+1 k = A ˆP k+1 k A T + Q kov (2.59)

35 2.7 Signalbehandling 19 Hur en skattning predikteras från en tidpunkt till nästa, då ingen ny information erhålls, definieras av ekvation (2.58). Den första termen i (2.59) anger hur felet i skattningen fortplantas till nästa tidpunkt och den andra termen anger hur mycket felet ökar på grund av processbruset w t i ekvation (2.51) ([7] sida 298). Mätuppdatering : Beräknar hur mätningen y k+1 ska viktas i tidsuppdateringen ˆx k k = ˆx k k 1 + L k e k (2.6) e k = y k Cˆx k k 1 (2.61) L k = P k k 1 C T [CP k k 1 C T + R kov ] 1 (2.62) P k k = P k k 1 P k k 1 C T [CP k k 1 C T + R kov ] 1 CP k k 1 (2.63) där e k är prediktionsfelet, som även kallas för innovationen. När man viktar in informationen i den nya mätningen enligt ekvation (2.6) kommer osäkerheten att minska. Hur mycket den osäkerheten minskar anges av den andra termen i (2.63) ([7] sida 31). Ekvationerna (2.58)-(2.63) utgör således Kalmanfiltret, där ekvation (2.57) vid tidpunkten k = ger följande initiering P 1 = E[x ˆx 1 ][x ˆx 1 ] T (2.64) Det är av stor vikt att ha så små prediktionsfel som möjligt och det har visat sig att Extended Kalmanfiltret (EKF) ger mindre prediktionsfel än Kalmanfiltret som har beskrivit ovan. Dessutom är det Kalmanfiltret begränsad till endast linjära system medan EKF är ett icke-linjärt filter avsedd för icke-linjära system Extended Kalmanfilter - EKF Med EKF linjäriserar man kring det senast predikterade tillståndet vilket har visat sig ge mindre prediktionsfel. Man har följande icke-linjära system [15] x k+1 = f(x k,u k ) + g(x k,u k )v k (2.65) y k = h(x k,u k ) + e k (2.66) som man kan linjärisera kring estimatet ˆx k, vilket ger f(x k ) f(ˆx k k,u k ) + F k (x k ˆx k k ), (2.67) h(x k ) h(ˆx k k 1,u k ) + H k (x k ˆx k k 1 ), (2.68) g(x k ) g(ˆx k k,u k ) G v k, (2.69) F k = f(x,u), H k = h(x,u) (2.7) x x (x,u)=(ˆxk k,u k ) (x,u)=(ˆxk k 1,u k )

36 2 Teori och algoritmen för EKF [1] Tidsuppdatering Mätuppdatering ˆx k+1 k = f(ˆx k k,u k ) (2.71) ˆP k+1 k = F k ˆPk k Fk T + G v kq kov G vt k (2.72) K k = P k k 1 Hk T [H k P k k 1 Hk T + R kov ] 1 (2.73) ˆx k k = ˆx k k 1 + K k (y k h(ˆx k k 1,u k )) (2.74) P k k = (I K k H k )P k k 1 (2.75) Med linjäriseringen och algoritmen för EKF kan tillståndsvektorn skattas Designaspekter - val av Q kov och R kov Trots att kalmanfiltret innehar egenskapen av att vara ett optimalt filter [7], d.v.s. det filter som ger de bästa skattningarna, är det nödvändigt att studera hur olika värden på Q kov och R kov kan påverka resultatet. Detta beror på att förstärkningen K k beror på matriserna F k, H k, Q kov och R kov vilket betyder att den kommer att påverkas av process- och mätbrusets egenskaper. Kovariansmatrisen Q kov bestämmer hur mycket tillstånden slumpvandrar, d.v.s. ju större Q kov blir desto mindre nytta har man av all gammal information. Däremot anger R kov, för ett skalärt fall, hur mycket information mätningen tillför skattningen, d.v.s. R kov = innebär att mätningen inte tillför någon nyttig information till skattningen medan R kov = innebär att mätningarna är perfekta. Alltså blir skattningen, d.v.s. återskapandet, av tillstånden olika tillfredsställande beroende på hur man väljer designparametrarna (Q kov och R kov ). Det finns oftast två punkter att tänka på: Om mätbruset är mycket större än processbruset medför det att förstärkningen K k från ekvation 2.73 blir liten, d.v.s. att man inte ger så stor tillit till mätningarna. Man får då ett långsamt och störningsokänsligt filter. Om mätbruset är mycket mindre än processbruset medför det att förstärkningen K k från ekvation 2.73 blir stor, d.v.s. man ger stor tillit till mätningarna. Detta ger ett snabbt och bruskänsligt filter. Man ska också komma ihåg att det bara är storleksrelationen mellan Q kov och R kov som påverkar filtret och inte de absoluta värdena [7].

37 Kapitel 3 Resultat 3.1 Sammanställning av ekvationer Nu kan vi sammanställa ekvationerna som utgör den olinjära flygplansmodellen. Resultatet vi kom fram till i avsnitt 2.4.5, ekvationerna (2.33)-(2.37), ger oss följande ekvationer uttryckta i flygplanets koordinatsystem Krafter X Y Z + T ebt mg = m u v ẇ + m w y w z w x w z w x w y u v w Moment L M N = Γ b w x w y w z + w x w y w z Γ b w x w y w z 3.2 Flygplansmodellen Flygplansmodellen har 16 variabler som utgör tillstånden och insignalerna. Vi har följande tillståndsvektor och insignalvektor: x = ( u v w w x w y w z q q 1 q 2 q 3 x e y e z e b ) T (3.1) u = ( δ a δ e ) T (3.2) 21

38 22 Resultat där de tre näst sista tillstånden i x som tidigare nämnt representerar tyngdpunktens läge i det jordfasta koordinatsystemet. Detta, tillsammans med resultaten från avsnitt 3.1 och ekvation (2.32) ger följande icke-linjära tillståndsmodell för flygplan u v ẇ w x w y w z q q q 1 q 2 3 x e y e z e = = Γ b 1 = 1 2 w y w z w x w z w x w y = T eb L M N Γ b 1 w x w y w z w x w y w z w x w y w z w x w y w z u v w w x w y w z u v w + T ebt Γ b q q 1 q 2 q 3 w x w y w z g + 1 m X Y Z ḃ = w k där w k är vitt brus med medelvärde noll och representerar processbruset. Utsignalerna, d.v.s. de variabler som vi kan mäta från flygplanet ges av följande: y = u v w z + b z e + v k, där v k är vitt brus med medelvärde noll och representerar mätbruset. De tillgängliga mätningar från det fysiska flygplanet som det fanns tillgång till är u, w z +b och z e. Dock kommer det senare visa sig att dessa inte är tillräckliga för att extended kalmanfiltret ska ge tillfredsställande resultat. Därför behövs ytterliggare en mätning, v som är hastighetskomponenten i y-led, för att få bra skattningar från extended kalmanfiltret. 3.3 Simulering av flygplansmodellen Den tidskontinuerliga och olinjära flygplansmodellen kan simuleras kring en trajektoria som representerar flygning rakt fram med utslag på rodren för att initiera en sväng. Eftersom det är lättare att få en visuell bild över flygplanets rörelse om man tänker i Eulervinklar kan man välja dessa så att de motsvarar en önskad flygbana och använda ekvationerna (2.23)-(2.25) för att översätta dessa värden till kvaternioner. Följande värden på Eulervinklarna:

39 3.3 Simulering av flygplansmodellen 23 φ = θ = ψ = representerar trimmad flygning, d.v.s. flygning med konstant hastighet längs en rät linje parallell med jordytan och med alla vinkelhastigheter noll. Det ger följande motsvarande kvaternion q = 1 q 1 = q 2 = q 3 = Hastighetskomponenten i x-led sätts till 1 m/s och höjden till 1. Detta ger följande initialtillståndsvektor: 1 x i = 1 (3.3) 1 Med δ a = 1.3 och δ e =.1 samt vitt brus med ett energispektrum som har en amplitud på.7, fås följande beteende på tillståndsvariablerna (fig 3.1). Man ser t ex att flygplanet stiger i höjd genom att betrakta tillståndsvariabeln z e och y e visar att flygplanet svänger. För att få en klarare bild om flygplanets orientering omvandlar vi kvaternionerna, tillståndsvariablerna q -q 3, till Eulervinklarna φ, θ och ψ. Då ser man i figur 3.2 att flygplanet till en början utför en roll- och gir samt tipprörelse, men rätas snabbt upp igen. Det bör noteras att här har flygplansmodellen simulerats utan processbrus. Skattningen av Eulervinklarna visas i figur 3.3.

40 24 Resultat 1.1 u 2 v w w x w y.2 w z q.1 q q 2.4 q x e z e y e bias Figur 3.1. Simulering av flygplansmodellen utan brus, δ a = 1.3 och δ e =.1.

41 3.3 Simulering av flygplansmodellen 25 phi [deg] theta [deg] psi [deg] Figur 3.2. Eulervinklarna i grader. Roll, tipp respektive girvinkel. 6 phi [deg] theta [deg] psi [deg] Figur 3.3. Skattning av Eulervinklarna, sanna värden är streckade och de skattade är heldragna. Q kov =.7 och R kov = 1.

42 26 Resultat 3.4 Den linjära modellen Linjäriseringen sker kring en arbetspunkt (x,u ) som i vårt fall representeras av de olika värden som tillståndsvariablerna antar under simuleringen. Detta resulterar i följande systemmatris, som har delats upp i tre mindre matriser för att få plats. F 1 = f = x x=x1 där X u w z w y + Z u w z Y v w x X w w y w x Z w w v w u + Z wy q1 2 q2 2 q 2 q3 2 q 32 q 2 q2 2 q 12 q 22 q1 2 v u q3 2 w 2gq 2 2gq 1 2gq x w y w z gq 3 2gq 2gq 1 wx 2 wz w y 2 2 2gq 2gq 3 2gq 2 wy w z 2 2 wx 2 2gq 1 2gq 2 2gq 3 wz 2 wy w x 2 2 x 1 = ( u v w q q 1 q 2 q 3 ) T q 2 T (3.4) där F 2 = f = x x=x2 L v I x N v M w L wx r Ix Iz I y I x N wx w z I z I y w y I z I y I x I x M wy I w x I z x I y I x N wz T (3.5) x 2 = ( w x w y w z ) T F 3 = f = x x=x3

43 3.5 Observerbarhet 27 där q 2 + q1 2 q2 2 q3 2 2(q 1 q 2 + q q 3 ) 2(q 1 q 3 q q 2 ) 2(q 1 q 2 q q 3 ) q 2 q1 2 + q2 2 q3 2 2(q 2 q 3 + q q 1 ) 2(q 1 q 3 + q q 2 ) 2(q 2 q 3 q q 1 ) q 2 q1 2 q2 2 + q3 2 2(q u q 3 v + q 2 w) 2(q 3 u + q v q 1 w) 2( q 2 u + q 1 v + q w) 2(q 1 u + q 2 v + q 3 w) 2(q 2 u q 1 v q w) 2(q 3 u + q v q 1 w) 2( q 2 u + q 1 v + q w) 2(q 1 u + q 2 v + q 3 w) 2( q u + q 3 v q 2 w) 2( q 3 u q v + q 1 w) 2(q u q 3 v + q 2 w) 2(q 1 u + q 2 v + q 3 w) 1 x 3 = ( x e y e z e) T T (3.6) F 1, F 2 och F 3 utgör tillsammans matrisen A lin i modellen i ekvation (2.38)-(2.39). Vidare måste B matrisen linjäriseras som ger B lin T L δa B lin = ( N δa ) F u,δ a = (3.7) samt C matrisen som i vårt fall redan är linjär: C lin = 3.5 Observerbarhet (3.8) Enligt avsnitt 2.5 kan man bestämma de nuvarande tillstånden i ett system baserat på systemets utsignaler endast om systemet är observerbart. Detta blir intressant

44 28 Resultat Projektion Tillstånd [nr] Figur 3.4. Projecering av tillståndsvariablerna. för oss när vi ska skatta systemets tillstånd utifrån de mätningar vi har tillgång till. Detta innebär också att vi inte behöver ha tillgång till mätningar på alla tillståndsvariabler. Systemet är inte helt observerbart. Alla förutom två tillstånd, x e och y e, är observerbara vilket ger att systemets rang = 12. Detta, som vi senare kommer att se, påverkar dock inte skattningen av girvinkelhastigheten eftersom den inte är kopplad till dessa variabler. Om projeceringen enligt ekvation 2.5 används, där y = enhetsmatrisen, fås i figur 3.4 att tillstånd 11 och 12 inte är observerbara då x proj (11) och x proj (12) är lika med noll.

45 3.6 Kalmanfiltreringen Quaternionernas längd Figur 3.5. Kvaternionernas längd efter korrigering. 3.6 Kalmanfiltreringen Verifiering av kraven på kvaternionerna Enligt avsnitt ska kvaternionerna ha längden ett. Detta krav måste vara uppfyllt efter varje tids- och mätuppdatering (ekv och 2.74). Så kommer dock inte vara fallet och man måste justera kvaternionernas längd enligt ekvation Figur 3.5 visar att kvaternionernas längd är ett. I avsnitten nedan presenteras skillnaderna på tillståndsskattningen då olika värden på kovariansmatriserna för processbruset respektive mätbruset används samt då vi har tillgång till tre, fyra respektive sex mätningar Val av Q kov och R kov med tre mätningar När följande utsignalvektor används y = u w z + b z e (3.9) visar figur 3.6 skattningen om vi väljer Q kov =.7 och R kov =.1 och figur 3.7 visar skattningen om vi väljer Q kov =.7 och R kov = 1. Som vi kan se blir skattningen av de flesta tillstånden inte så bra oavsett om vi väljer att förlita oss på mätningarna eller inte. Figur 3.8 och 3.9 visar skattningen av Eulervinklarna.

46 3 Resultat u [m/s] w [m/s] w y [rad/s] q v [m/s] w x [rad/s] w z [rad/s] q q 2 q x e [m] y e [m] z e [m] 1 bias Figur 3.6. Tillståndsskattning med tre mätningar, skattning (heldragna) och sanna tillstånd (streckade). Q kov =.7 och R kov =.1