%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% finn_TF 
% Einar Hauge
%   
% Dette programmet starter med å linearisere systemet rundt
% ønsket ventilåpning. Denne ventilåpningen settes under "Definer
% parametere" (z_setpkt). For å finne x0 som systemet skal lineariseres
% rundt kjøres en simulering i simulink for å finne stasjonære verdier som
% trengs for å finne x0. (Bakgrunnen for at akkurat disse verdiene ble
% funnet er at disse ble benyttet i initialize2 laget av Espen Storkaas.
% Ønsket å benytte denne filen for å regne ut x0. Merk at til forskjell fra
% den opprinnelige initalize2 er her parameterene K1, K2, K3 satt til sine respektive
% verdier, og ikke beregnet i denne filen).
%
% En liten digresjon til Simulink-simuleringen. For hver gang man endrer
% settpunktet bør man sjekke at parameterne i kaskaderegulatoren
% stabiliserer systemet. Parameterne ble i utgangspunktet tunet inn for
% z_setpkt=0.3. 
%
% Etter lineariseringa finnes transferfunksjonene mellom en rekke målinger
% - og pådraget u=z eller forstyrrelsene mG_in, mL_in og P0. Hvilke målinger 
% man ønsker defineres i målevektoren Y i slug_3D_lin.
%
% Til slutt finnes minimum peaks for en del lukkede transferfunksjoner på
% bakgrunn av prosessen G. (mellom diverse målinger og pådraget u=z) Til
% dette benyttes funksjonen beregn_bounds. Denne funksjonen har pr.dags
% dato en del mangler (peaks for KS, S=T fungerer som de skal. For SG er
% ikke peaken tight)
%
% Written by: Einar Hauge
% Date :      27.juli 2006  
% version 1.0
% einarha@stud.ntnu.no
% 
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all
global data 


%=========================================================================\
%Definerer parametere
%=========================================================================/
mG_in=0.362; %l/min denne blir satt også i s_slug som benyttes av simulink!!!!!!! bør kanskje prøve å sette den bare ett sted
mL_in=8.64; %l/min denne blir satt også i s_slug som benyttes av simulink!!!!!!! bør kanskje prøve å sette den bare ett sted
z_setpkt=0.3 %0.175;%  %ventilåpning det skal lineariseres rundt 
z_arbpkt=70;%Arbeidspunkt for pådraget i ytre sløyfe. Se simulinkdiagram stasjonaer.sim


%=========================================================================\
%Simulerer for å finne P1, P2 og h1 stasjonært
%=========================================================================/
simtid=3*3600; %variabelen simtid ligger i simulinkdiagrammet
sim('stasjonaer');
P1_stationary=1e5*P1_simulert.signals.values(length(P1_simulert.signals.values)); %henter ut siste verdi -> viktig å se til at reguleringen er stabil.
P2_stationary=1e5*P2_simulert.signals.values(length(P2_simulert.signals.values)); %henter ut siste verdi
h1_stationary=h1_simulert.signals.values(length(h1_simulert.signals.values)); %henter ut siste verdi


%=========================================================================\
%Finner den stasjonære løsningen x0 rundt z=0.3 (evnt z=0.175)
%=========================================================================/
[x0,y_stasj,data]=initialize2(z_setpkt,mG_in,mL_in,P1_stationary,h1_stationary,P2_stationary); %mer modulært hvis jeg tar inn K1, K2, K3 og n i funksjonskallet. 


%=========================================================================\
%Lineariserer rundt den stasjonære løsningen x0 for z=0.3
%=========================================================================/
u=[z_setpkt;mG_in;mL_in;data.P0]; %u0 = u da u er konstant. z_setpkt
%Linearizing
[A,B,C,D]=linearize(@slug_3D_lin,[],x0(1:3),[],u);
disp('================================================================')
disp('Polene til systemet')
disp(eig(A))


%=========================================================================\
%Finner transferfunksjonen mellom u=z og y=[DP rho_T w Q alpha_L alpha_Lm
%etc....] se slug_3D_lin's Y-vektor.
%=========================================================================/
iu=1; % skal ha transferfunksjonen fra iu`th input - dvs. z.
[tf_teller,tf_nevner]=ss2tf(A,B,C,D,iu); %
disp('================================================================')
disp('Finner transferfunksjoner fra u=Z til Y=[...]')
disp('================================================================')
disp('================================================================')
disp('Transferfunksjon fra u=z -> y=DP')
disp('================================================================')
h1=tf(tf_teller(1,:),tf_nevner) %DP
disp('Nullpunkt')
nullpunkt=roots(tf_teller(1,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=z -> y=rho_T')
disp('================================================================')
h2=tf(tf_teller(2,:),tf_nevner) %rho_T  
disp('Nullpunkt')
nullpunkt=roots(tf_teller(2,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=z -> y=w')
disp('================================================================')
h3=tf(tf_teller(3,:),tf_nevner) %w   
disp('Nullpunkt')
nullpunkt=roots(tf_teller(3,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=z -> y=Q')
disp('================================================================')
h4=tf(tf_teller(4,:),tf_nevner) %Q
disp('Nullpunkt')
nullpunkt=roots(tf_teller(4,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=z -> y=P1')
disp('================================================================')
h12=tf(tf_teller(12,:),tf_nevner)
disp('Nullpunkt')
nullpunkt=roots(tf_teller(12,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

%=========================================================================\
% Finner transferfunksjoner fra u=mG_in -> y=[...]
%=========================================================================/
iu=2; % skal ha transferfunksjonen fra iu`th input - dvs. mG_in.
[tf_teller,tf_nevner]=ss2tf(A,B,C,D,iu); %
disp('================================================================')
disp('Finner transferfunksjoner fra u=mG_in til Y=[...]')
disp('================================================================')
disp('Transferfunksjon fra u=mG_in -> y=DP')
disp('================================================================')
tf_DP_mG_in=tf(tf_teller(1,:),tf_nevner) %DP
disp('Nullpunkt')
nullpunkt=roots(tf_teller(1,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=mG_in -> y=rho_T')
disp('================================================================')
tf_rhoT_mG_in=tf(tf_teller(2,:),tf_nevner) %rho_T  
disp('Nullpunkt')
nullpunkt=roots(tf_teller(2,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=mG_in -> y=w')
disp('================================================================')
tf_w_mG_in=tf(tf_teller(3,:),tf_nevner) %w   
disp('Nullpunkt')
nullpunkt=roots(tf_teller(3,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=mG_in -> y=Q')
disp('================================================================')
tf_Q_mG_in=tf(tf_teller(4,:),tf_nevner) %Q
disp('Nullpunkt')
nullpunkt=roots(tf_teller(4,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=mG_in -> y=P1')
disp('================================================================')
tf_P1_mG_in=tf(tf_teller(12,:),tf_nevner)
disp('Nullpunkt')
nullpunkt=roots(tf_teller(12,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

%=========================================================================\
% Finner transferfunksjoner for u=mL_in -> y=[...]
%=========================================================================/
iu=3; % skal ha transferfunksjonen fra iu`th input - dvs. mL_in.
[tf_teller,tf_nevner]=ss2tf(A,B,C,D,iu); %
disp('================================================================')
disp('Finner transferfunksjoner fra u=mL_in til Y=[...]')
disp('================================================================')
disp('Transferfunksjon fra u=mL_in -> y=DP')
tf_DP_mL_in=tf(tf_teller(1,:),tf_nevner) %DP
disp('Nullpunkt')
nullpunkt=roots(tf_teller(1,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=mL_in -> y=rho_T')
disp('================================================================')
tf_rhoT_mL_in=tf(tf_teller(2,:),tf_nevner) %rho_T  
disp('Nullpunkt')
nullpunkt=roots(tf_teller(2,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=mL_in -> y=w')
disp('================================================================')
tf_w_mL_in=tf(tf_teller(3,:),tf_nevner) %w   
disp('Nullpunkt')
nullpunkt=roots(tf_teller(3,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=mL_in -> y=Q')
disp('================================================================')
tf_Q_mL_in=tf(tf_teller(4,:),tf_nevner) %Q
disp('Nullpunkt')
nullpunkt=roots(tf_teller(4,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=mL_in -> y=P1')
disp('================================================================')
tf_P1_mL_in=tf(tf_teller(12,:),tf_nevner)
disp('Nullpunkt')
nullpunkt=roots(tf_teller(12,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

%=========================================================================\
% Finner transferfunksjoner for u=P0 -> y=[...]
%=========================================================================/
iu=4; % skal ha transferfunksjonen fra iu`th input - dvs. P0.
[tf_teller,tf_nevner]=ss2tf(A,B,C,D,iu); %
disp('================================================================')
disp('Finner transferfunksjoner fra u=P0 til Y=[...]')
disp('================================================================')
disp('Transferfunksjon fra u=P0 -> y=DP')
tf_DP_P0=tf(tf_teller(1,:),tf_nevner) %DP
disp('Nullpunkt')
nullpunkt=roots(tf_teller(1,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=P0 -> y=rho_T')
disp('================================================================')
tf_rhoT_P0=tf(tf_teller(2,:),tf_nevner) %rho_T  
disp('Nullpunkt')
nullpunkt=roots(tf_teller(2,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=P0 -> y=w')
disp('================================================================')
tf_w_P0=tf(tf_teller(3,:),tf_nevner) %w   
disp('Nullpunkt')
nullpunkt=roots(tf_teller(3,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=P0 -> y=Q')
disp('================================================================')
tf_Q_P0=tf(tf_teller(4,:),tf_nevner) %Q
disp('Nullpunkt')
nullpunkt=roots(tf_teller(4,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

disp('================================================================')
disp('Transferfunksjon fra u=P0 -> y=P1')
disp('================================================================')
tf_P1_P0=tf(tf_teller(12,:),tf_nevner)
disp('Nullpunkt')
nullpunkt=roots(tf_teller(12,:))
gain=finn_Gain(nullpunkt,A) %finner gain til TF, når TF skal skrives på tidskonstantform

% %=========================================================================\
% % Måling y=DP/rho
% %=========================================================================/
% disp('Lineærkombinasjon av DP - rho')
% num_h1=tfdata(h1,'v');
% num_h1=num_h1*1/data.lin_rho;
% num_h2=tfdata(h2,'v');
% num_h2=(num_h2*data.lin_DP)/(data.lin_rho^2)
% H_num_dp_minus_rho=num_h1-num_h2;
% H_dp_minus_rho=tf(H_num_dp_minus_rho,tf_nevner)
% disp('Nullpunkt');
% roots([H_num_dp_minus_rho])
% disp('================================================================')
% 
% %=========================================================================\
% % Måling av y=DP*rho
% %=========================================================================/
% disp('Lineærkombinasjon av DP+rho')
% num_h1=tfdata(h1,'v');
% num_h1=num_h1*1/data.lin_rho;
% num_h2=tfdata(h2,'v');
% num_h2=(num_h2*data.lin_DP)/(data.lin_rho^2)
% H_num_dp_pluss_rho=num_h1+num_h2;
% H_dp_pluss_rho=tf(H_num_dp_pluss_rho,tf_nevner)
% disp('Nullpunkt');
% roots([H_num_dp_pluss_rho])
% disp('================================================================')


%=========================================================================\
% Finner peaks for en rekke transferfunksjoner; Husk å skalere!
%=========================================================================/
%Observer : Peaks på S og T er ikke avhengig av skalering. Dette samsvarer
%også med bodes sensitivitetsintegral! :)
disp('================================================================')
disp('Finner peaks for en rekke tranferfunksjoner med u=z') 
disp('================================================================')
disp('Transferfunksjon fra u=z -> y=w')
Du=0.7;
Dy=1;
h3_skalert=inv(Dy)*h3*Du;
[num_h3_skalert, den_h3_skalert]=tfdata(h3_skalert,'v');
[bound_S, bound_T, bound_KS, bound_SG, bound_KSGd, bound_SGd] = beregn_bounds(h3_skalert,roots(num_h3_skalert).',roots(den_h3_skalert).',h3_skalert);
bound_S
bound_KS
G_0=abs(evalfr(h3_skalert,0))

disp('Transferfunksjon fra u=z -> y=Q')
Du=0.7;
Dy=0.002;
h4_skalert=inv(Dy)*h4*Du;
[num_h4_skalert, den_h4_skalert]=tfdata(h4_skalert,'v'); 
[bound_S, bound_T, bound_KS, bound_SG, bound_KSGd, bound_SGd] = beregn_bounds(h4_skalert,roots(num_h4_skalert).',roots(den_h4_skalert).',h4_skalert);
bound_S
bound_KS
G_0=abs(evalfr(h4_skalert,0))

disp('Transferfunksjon fra u=z -> y=rho_T')
Du=0.7;
Dy=50;
h2_skalert=inv(Dy)*h2*Du;
[num_h2_skalert, den_h2_skalert]=tfdata(h2_skalert,'v');
[bound_S, bound_T, bound_KS, bound_SG, bound_KSGd, bound_SGd] = beregn_bounds(h2_skalert,roots(num_h2_skalert).',roots(den_h2_skalert).',h2_skalert);
bound_S
bound_KS
G_0=abs(evalfr(h2_skalert,0))

disp('Transferfunksjon fra u=z -> y=DP')
Du=0.7;
if(z_setpkt==0.175)
    Dy=1;
elseif(z_setpkt==0.3)
    Dy=0.5;
else
    Dy=0.5;
end

h1_skalert=inv(Dy)*h1*Du;
[num_h1_skalert,den_h1_skalert]=tfdata(h1_skalert,'v');
[bound_S, bound_T, bound_KS, bound_SG, bound_KSGd, bound_SGd] = beregn_bounds(h1_skalert,roots(num_h1_skalert).',roots(den_h1_skalert).',h1_skalert);
bound_S
bound_KS
G_0=abs(evalfr(h1_skalert,0))

disp('Transferfunksjon fra u=z-> y=P1')
Du=0.7;
Dy=1;
h12_skalert=inv(Dy)*h12*Du;
[num_h12_skalert,den_h12_skalert]=tfdata(h12_skalert,'v');
[bound_S, bound_T, bound_KS, bound_SG, bound_KSGd, bound_SGd] = beregn_bounds(h12_skalert,roots(num_h12_skalert).',roots(den_h12_skalert).',h12_skalert);
bound_S
bound_KS
G_0=abs(evalfr(h12_skalert,0))






    