%Heidi Sivertsen 06-05-06
%Script for a analysere ulike regulatorstrukturer
slug_3D_linearize;
clear
gen_data;
Exp_data;
%global Y data

warning('off')
OPz=0.20;  %Ventilåpning man vil linearisere rundt
load Stationary
load slugdata
index=find(abs(z-OPz)==min(abs(z-OPz))); %finner indeks hvor z er tilnaermet lik operasjonell pkt.
u= z(index);
d= [mG_in; mL_in];
[x0,y_stasj,data]=initialize(z(index),mG_in,mL_in,P1_stasj,h1_stasj,P2_stasj);

%% Linearizing
%global data
[A,B,C,D,E,F,ys]=linearize(@slug_3D_lin,[],xs(1:3, index),[],u,d);
disp('-----------------------------------------------------------')
disp('Poles')
disp(eig(A)')
%transfer funksjon for:
%  Y     output( P1,P2,p.TA,W, Q, alpha)
% ---    
%  z    ventilaapning

G=ss(A,B,C,D);


%transfer funksjon for:
%  Y     output( P1,P2,p.T,W, Q, alpha)
% ---    
%  d     forstyrrelse (mG_in, mL_in)
Gd=ss(A,E,C,F);
%Skalering:
De=(diag([0.1 0.05 100 0.01 1e-5 0.1]));
Dd=(diag([1e-5 1e-2]));
Du=1-OPz;

G=De^-1*G*Du;
G_tf=tf(G);
Gd=De^-1*Gd*Dd;
Gd_tf=tf(Gd);

%transfer funksjon for ulike målinger y/z:
GP1=G_tf(1,1);GP1=zpk(GP1); GdP1=minreal(Gd_tf(1,1)+Gd_tf(1,2));
GP2=G_tf(2,1);GP2=zpk(GP2); GdP2=minreal(Gd_tf(2,1)+Gd_tf(2,2));
Grho=G_tf(3,1);Grho=zpk(Grho);Gdrho=minreal(Gd_tf(3,1)+Gd_tf(3,2));
GW=G_tf(4,1);GW=zpk(GW); GdW=minreal(Gd_tf(4,1)+Gd_tf(4,2));
GQ=G_tf(5,1); GQ=zpk(GQ); GdQ=minreal(Gd_tf(5,1)+Gd_tf(5,2));
Ga=G_tf(6,1);Ga=zpk(Ga); Gda=minreal(Gd_tf(6,1)+Gd_tf(6,2));
%Gz=G_tf(7,1);Gz=zpk(Gz); Gdz=minreal(Gd_tf(7,1)+Gd_tf(7,2));

%%%%%%%%%%%%%%%%SENSITIVITY PEAKS
% s=TF('s');
% G=(s-2)/(s-3);  %testfunksjoner
% Gd=(s-5)/(s-1);

[G0,STmin,SGmin,KSmin,SGdmin,KSGdmin]=sens_peaks(GP2,GdP2);
disp('Zeros')
disp(zero(GQ)')

disp('limits:G0    S/Tmin       SGmin    KSmin       SGdmin  KSGdmin')
disp( [     G0 ;    STmin   ;  SGmin;    KSmin;  SGdmin;   KSGdmin]')


%MIMO:
%y1=[P2 rho] y2=[z P2]
% 
% G=[ GP2 Grho];
% GdP2=[Gd_tf(2,1) Gd_tf(2,2)];
% Gdrho=[Gd_tf(3,1) Gd_tf(3,2)];
% Gd=[GdP2 ;Gdrho];
% 
% 
% G0=evalfr(G,0);
% Gd0=evalfr(Gd,0);
% 
% [Ud,Sd,Vd]=svd(G0)
% maxS=Sd(1,1);








%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%PI-regulator:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Kc=-2;  %bara(-1)
% tau=25;  %s
% K=Kc*tf([tau 1],[tau 0]);
% LP1=K*GP1;
% SP1=1/(1+LP1);
% TP1=LP1/(1+LP1);
% figure(1)
% pzmap(GP1);
% figure(2)
% pzmap(SP1);
% ltiview('bode',SP1,TP1)
% %Simulering:
% 
% figure(1)
% subplot(3,1,1)
% plot(time,60*W, time, W_sp,':')
% title('W')
% subplot(3,1,2)
% plot(time,z)
% title('z')
% subplot(3,1,3)
% plot(time,P1,time, P1_sp,':')
% title('P1')


