Båtsimulering med diskret Kalmanfilter TTK4115 Lineær systemteori

Transkript

1 NTNU Norges teknisk-naturvitenskapelige universitet Fakultet for informasjonsteknologi, matematikk og elektroteknikk Institutt for teknisk kybernetikk Båtsimulering med diskret Kalmanfilter TTK4115 Lineær systemteori Prosjektrapport Av: Gruppe 14 Rune Haugom & Frode Efteland

2 Innholdsregister 4.1 Identifikasjon av båt parametere... 3 a) Transferfunksjon fra δ til ψ :... 3 b) Parametrene K og T: Identifikasjon av modell for bølgespekter... 5 a) Poer spectral density - PSD... 6 b) Modell av bølgerespons... 6 c) Resonansfrekvens... 6 d) Dempefaktor λ Regulator design... 8 a) Autopilot PD-regulator design... 8 b.c.d) Simulering med og uten forstyrrelser Observerbarhet a) Systemmatriser A,B,C,E b) Observerbarhet uten forstyrrelse (men med måleforstyrrelse) c) Observerbarhet med strømforstyrrelse... 1 d) Observerbarhet med bølgeforstyrrelse... 1 e) Observerbarhet med strøm- og bølgeforstyrrelse Diskret Kalman filter a) Eksakt diskretisering b) Variansestimat av målestøy c) Kalmanfilteret d) Foroverkobling fra estimert bias e) Bølgefiltrert ψ Konklusjon:... Figurliste Figur 1: Plott av systemet med Sin(.5t) på inngangen. Simuleringstid 4s Figur : Plott av systemet med Sin(.5t) på inngangen. Simuleringstid 1s Figur 3: Plott av 1º step på inngangen... 5 Figur 4: Plott av funksjonene Sψ ( ) og P ( ) ψ for bestemmelse av modellparametere... 7 Figur 5: fasemargin φm = 5 o gir en fase φ = 13 o... 8 Figur 6: Blokkskjema i Simulink... 9 Figur 7: Settpunktsammenlikning med og uten forstyrrelser... 9 Figur 8: Plott over rorvinkel med forstyrrelser Figur 9: Simulinkmodellen for Kalmanfilteret Figur 1: Foroverkobling fra estimert bias Figur 11: Plott av y med Kalmanfilter Figur 1: Plott av kompassmåling med Kalmanfilter Figur 13: Plott av bias med Kalmanfilter Figur 14: Plott av rudder med Kalmanfilter

3 4.1 Identifikasjon av båt parametere a) Transferfunksjon fra δ til ψ : ψ& = r 1 K && ψ = r& = ψ& + ( δ b), b= T T 1 K sψ() s = ψs() s + δ() s T T 1 K s + s ψ() s = δ() s T T K ψ K K H() s = () s = T = = δ 1 Ts + s s( Ts + 1) s + s T (1.1) b) Parametrene K og T: Figur 1: Plott av systemet med Sin(.5t) på inngangen. Simuleringstid 4s Avlest amplitude vedω 1 =.5 : H( jω 1 ) = = 3 (1.) - 3 -

4 Figur : Plott av systemet med Sin(.5t) på inngangen. Simuleringstid 1s Avlest amplitude vedω =.5 : H( jω ) = =.77 (1.3) Dette gir oss to likninger med to ukjente. Vi kan dermed finne K og T: K K Ι : A = H( jω ) = = + ( Tω1 ) + ( ω1) ( ω ) 1 1 T j 1 + jω Tω 1 1 j 1 K = = ω T K 1 1 1ω1 ω1 1 ω + 1 K = A T + (1.4) K ΙΙ : A = H( jω) = K = A ω T ω + 1 T j + jω ( ω ) ω ( 1) ( 1) K = Aω T ω + = A ω T ω + = A ω T + A ω 4 K A ω T = (1.5) Aω Innsatt for ω1 =.5, ω =.5, A1 = 3, A =.77 : K = A T + = (1.6) 1ω1 ω K A ω T = = 88.7 (1.7) Aω - 4 -

5 Modellen blir nå: ψ K.175 H() s = () s = = δ Ts + s 88.7s + s (1.8) 1º step på inngangen gir: Figur 3: Plott av 1º step på inngangen. Kommentar: Modellen følger virkelig skip relativt bra innenfor en viss tid. Avviket øker ettersom tiden går

6 4. Identifikasjon av modell for bølgespekter a) Poer spectral density - PSD Fila ave.mat inneholder data om hvordan bølgene virker inn på kompassmålingen. Bruker matlab for å finne et estimat av PSD for bølgestøyen ψ. Det er benyttet et hanning-vindu som glattevindu. [ps,f]=psd(psi_(,:)*pi/18,56,1,hanning(56)) (1.9) b) Modell av bølgerespons Finner transferfunksjonen fra til ψ. Tar utgangspunkt i modelligningene under og laplacetransformerer: Laplace & ξ = ψ sξ = ψ Laplace = + K s = + K ψ& ωξψ ψ ωξψ ψ K s Gs () = = s s + λω + ω (1.1) er hvit støy. Finner uttrykk for PSD funksjonen til ψ, P ( ) ψ Pψ ( ω) = G( jω) G( jω) P ψ P ψ K jω K jω ( ω) = ( jω) jωλω ω ( jω) jωλω ω K jω K jω ( ω) = ( ω + λω ω + ω )( ω λω ω + ω ) j j ( K ω) ( K ω) P ψ ( ω) = = ω ω ω 4λ ω ω ω ω ω ω ω ( 1 λ ) (1.11) c) Resonansfrekvens Plotter funksjonen Sψ ( ) som ble estimert i Figur 4, og leser av resonansfrekvensen til å være ω =.75 rad/s (1.1) - 6 -

7 d) Dempefaktor λ Setter K = λωσ der σ =.716 inn i uttrykket for Pψ ( ). I Figur 4 er også P ( ) ψ tegnet inn. Her er λ =.7, og kurvene passer godt overens. (1.13) Figur 4: Plott av funksjonene Sψ ( ) og P ( ) ψ for bestemmelse av modellparametere - 7 -

8 4.3 Regulator design a) Autopilot PD-regulator design.1 rad o =, φm = 5, s Td = T Gitt: ωc ( ) Hsystem () s = H pd () s Hship () s 1+ Ts d K = K pd 1+ Ts sts+ 1 f ( ) Velger Td = T for å kansellere tidskonstanten: 1+ Ts d K 1 K Hsystem () s = Kpd = Kpd 1+ Ts sts Ts s f ( ) f (1.14) c gir oss de ukjente parametrene i (1.14) ved å tilpasse verdiene for K pd og T f. K pd =.75 T = 8.5 (1.15) Bodeplott med fasemargin på 5º og kryssfrekvens ω =.1 ( rad / s) f o Figur 5: fasemargin φ = 5 gir en fase φ = 13 m o - 8 -

9 b.c.d) Simulering med og uten forstyrrelser Figur 6: Blokkskjema i Simulink Figur 7: Settpunktsammenlikning med og uten forstyrrelser. Figur 8: Plott over rorvinkel med forstyrrelser

10 Kommentar: Simulering uten forstyrrelse gir et godt resultat. Båten klarer fint å regulere seg inn til 9º. Simulering med strømforstyrrelse fører til at ønsket kurs ikke oppnås. Strømforstyrrelsene har svært lave frekvenser som systemet ikke greier å undertrykke. Resultatet blir et vedvarende statisk avvik. Simulering med bølgeforstyrrelse fører også til at ønsket kurs oppnåes, selv om båten svinger en del rundt settpunktet. I Figur 8 ser vi at pådraget fra roret gir svært hurtige justeringer for biasen. Dette kan føre til raskere slitasje på ror og båt

11 4.4 Observerbarhet a) Systemmatriser A,B,C,E x& = Ax + Bu + E y = Cx+ v Tilstandsvektorer: x ξω ψ ω = ψ = x 3 r x4 b x 5 x1 x bølgefrekvens kompasskurs rudder(ror) bias x& 1 ω x& = ω x λω x + K ω x& x& 1 K = x + T T x x& = ω ( δ ) = & ξ = x = x b y = x + x + v 3 ω ω 1 x 1 ω λω x K ω 1 ωω = K K ω b x 4 x& x u T T T x5 1 y = x+ v [ 1 1 ] (1.16) b) Observerbarhet uten forstyrrelse (men med måleforstyrrelse) ξ = x = ψ = x = ω = x = ω 1 ω b 5 Vi får en ny A- og C-matrise: 1 A = 1 T C = [ 1 ] (1.17)

12 C 1 Obs = rank rank full rang CA = = = 1 observerbart (1.18) c) Observerbarhet med strømforstyrrelse ξ = x = ψ = x = ω 1 ω 1 1 K A = T T C = [ 1 ] (1.19) C 1 Obs = rank CA = rank 1 = 3 = full rang CA 1 K T T observerbart (1.) d) Observerbarhet med bølgeforstyrrelse ω b = x 5 = 1 ω λω A = 1 1 T C = [ 1 1 ] (1.1) C CA Obs = rank = rank CA CA = 4 = full rang observerbart (1.) - 1 -

13 e) Observerbarhet med strøm- og bølgeforstyrrelse A og C matrisen er som i (1.16) C CA Obs = rank CA = rank CA CA = 5 = full rang observerbart (1.3) Systemet er observerbart i alle tilfeller. Dette er en forutsetning ved bruk av Kalmanfilter. Dvs vi kan bruke filteret med og uten strøm og bølgeforstyrrelser

14 4.5 Diskret Kalman filter a) Eksakt diskretisering Benytter Matlab for å diskretisere det kontinuerlige systemet: =.75; lambda =.7; sigma =.716; T=88.7; K=.175; A=[ 1 ;-^ -*lambda* ; ; -1/T -K/T; ]; B=[ K/T ]'; E=[ ; *lambda**sigma; ; ; ; 1;]; C=[ 1 1 ]; D=1; [Ad,Bd] = cd(a,b,.1) [Ad,Ed] = cd(a,e,.1); Ed Cd=C Dd=D A d = B d = E d =...1 (1.4) (1.5) (1.6) C d =[ 1 1 ] (1.7) D d =[1] (1.8)

15 b) Variansestimat av målestøy Sampler måleverdier på utgangen av systemet med kun målestøy og ingen inngangssignal. Finner variansen til disse målte verdiene. Estimatet av variansen blir: Var( ˆv ) = 6.8E-7 (1.9) c) Kalmanfilteret Gitt følgende: T 3 =, E[ ] = Q = 6 b 1, 1.13 P = π, x ˆ = Lar kompassmålingen og ror (rudder) være inngangssignaler for s-funksjonen. Kobler til regulatoren laget i oppgave 4.3. Figur 9 viser systemet der det også er tatt med en feed forard fra estimert bias for å eliminere biasen for strømmen. Simulinkmodellen slik den er tegnet blir benyttet i 4.5e. Tilsvarende modell brukes i 4.5d, men da kobles målingen y tilbake til regulatoren. ψ og b er utgangene fra s-funksjonen. Figur 9: Simulinkmodellen for Kalmanfilteret

16 Implementasjon av Kalmanfilteret: Velger implementasjonsmetode : function [sys,x,str,ts] = DiscKal(t,x,u,flag,data) sitch flag, %%%%%%%%%%%%%%%%%% % Initialization % %%%%%%%%%%%%%%%%%% case, [sys,x,str,ts]=mdlinitializesizes(data); %%%%%%%%%%%%% % Outputs % %%%%%%%%%%%%% case 3, sys=mdloutputs(t,x,u,data); %%%%%%%%%%%%% % Terminate % %%%%%%%%%%%%% case, sys=mdlupdate(t,x,u, data); case {1,4,} sys=[]; case 9, sys=mdlterminate(t,x,u); %%%%%%%%%%%%%%%%%%%% % Unexpected flags % %%%%%%%%%%%%%%%%%%%% otherise error(['unhandled flag = ',numstr(flag)]); end function [sys,x,str,ts]=mdlinitializesizes(data); % This is called only at the start of the simulation. sizes = simsizes; % do not modify sizes.numcontstates = ; % Number of continuous states in the system, do not modify sizes.numdiscstates = 35; % Tilstandsvektoren har 35 tilstander sizes.numoutputs = ; % utganger aposteriori-estimat av psi og b sizes.numinputs = ; % innganger, rudder og måling sizes.dirfeedthrough = 1; % 1 if the input is needed directly in the update part sizes.numsampletimes = 1; % Do not modify sys = simsizes(sizes); % Do not modify %Setter initialverdien for tilstandsvektoren x = [data.x_h_',, matvec(data.p_)']'; str = []; % Do not modify ts = [-1 ]; % Sample time. Arver. function sys=mdlupdate(t,x,u, data) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% %Oppdaterer filteret %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% P_= vecmat(x(11:35));%henter Apriori P av tilstandsvektoren K = P_*data.Cd'*inv(data.Cd*P_*data.Cd'+data.R); %Beregner ny kalmanforsterkning xh_= x(1:5);%henter ut Apriori-estimatene fra tilstandsvektoren xh = xh_ + K*(u()-data.Cd*xh_);%Beregner Aposteriori-estimatene P=(eye(5,5)-K*data.Cd)*P_;%Beregner Aposteriori feil-kovarians xh_=data.ad*xh + data.bd*u(1);%beregner aprioriestimat for tilstandene P_= data.ad*p*data.ad' + data.ed*data.q*data.ed';%beregner aprioriestimat for feil-kovarians %x = [xh_', xh', matvec(p_)']'; %Oppdaterer tilstandsvektoren

17 sys=[xh_', xh', matvec(p_)']'; function sys=mdloutputs(t,x,u,data) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% % Returnerer aposterioriestimatene for psi og b. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% sys=[x(8) x(1)]; function sys=mdlterminate(t,x,u) sys = [];

18 d) Foroverkobling fra estimert bias Figur 1: Foroverkobling fra estimert bias Har laget en feed forard fra utgangen y tilbake til regulatoren. Dette eliminerer biasen som skyldes strømmen, og fra Figur 1 ser man at stasjonært vil utgangen følge referansen (9 ). Slik var det ikke før feed forard koblingen ble gjort. Se Figur 8 e) Bølgefiltrert ψ Bruker den bølgefiltrerte ψ istedenfor målt kurs i regulatoren. (Se Figur 9) Figurene på de neste sidene viser målingen y, estimert ψ, estimert bias og rudder. Hvis man sammenligner med Figur 8 ser man at det er mye mindre støy på rudder og slitasje på aktuatorsystemet blir mindre. Figur 11: Plott av y med Kalmanfilter

19 Figur 1: Plott av kompassmåling med Kalmanfilter Figur 13: Plott av bias med Kalmanfilter

20 Figur 14: Plott av rudder med Kalmanfilter Konklusjon: Som vi så fra systemet med PD-regulering og tilbakekobling fra kompassmålingen, så ga pådraget fra regulatoren kontinuerlige rorbevegelser pga de høyfrekvente bølgene (se Figur 8). Når vi så tok tilbakekoblingen fra Kalmanfilteret i PD-regulatoren, førte det til bedre regulering fordi en ikke kompenserer for bølgene. Dermed blir rorbevegelsen mindre og man minker slitasjen på ror og aktuatorsystem (se Figur 14). Som forventet ga Kalmanfilteret det beste resultatet. - -