%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                         %
%  Program slouzi k vypoctu parametru linarizovaneho stavoveho modelu HPS.%
% Program vyuziva hodnot vypoctenych pomoci programu linearizace.m        %
%                                                                         %
%                                                                         %
%                                       Datum posledni zmeny: 08.06.2010  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%LINEARIZACE

%Prutok kapaliny na pocatku simulace
QLlin=(aLV*sLV*oLlin+auL*suL)*sqrt(2*rok*pplin);
QRlin=(aRV*sRV*oRlin+auR*suR)*sqrt(2*rok*pplin);

%Pocatecni stav vysek hladin
hLHlin=((QLlin/(alfaLH*sLH*rok))^2)/(2*g);%[m]        Vyska hladiny leve horni nadrze na pocatku
hRHlin=((QRlin/(alfaRH*sRH*rok))^2)/(2*g);%[m]        Vyska hladiny prave horni nadrze na pocatku
hLLlin=((QLlin/(alfaLL*sLL*rok))^2)/(2*g);%[m]        Vyska hladiny leve dolni nadrze na pocatku
hRLlin=((QRlin/(alfaRL*sRL*rok))^2)/(2*g);%[m]        Vyska hladiny prave dolni nadrze na pocatku

%Pocatecni pneum. objem
VHlin=gamaH*V0+SL*(H0-hLHlin)+SR*(H0-hRHlin);%[m3]       Pocatecni pneum. objem horni casti - linearizace tlaku
VLlin=gamaL*V0+SL*(H0-hLLlin)+SR*(H0-hRLlin);%[m3]       Pocatecni pneum. objem dolni casti - linearizace tlaku


%-------------------------------3 ins - 2 outs-----------------------------
%leva horni nadrz
a1=-alfaLH^2*sLH^2*rok*g/(SL*QLlin);
a2=-(((aLV*sLV*oLlin+auL*suL)^2+(alfaLH*sLH)^2)/(SL*QLlin));
a3=alfaLH^2*sLH^2/(QLlin*SL);
a4=aLV*sLV*sqrt(2*rok*pplin)/(rok*SL);
a5=(aLV*sLV*oLlin+auL*suL)^2/(SL*QLlin);


%prava horni nadrz
b1=-alfaRH^2*sRH^2*rok*g/(SR*QRlin);
b2=-(((aRV*sRV*oRlin+auR*suR)^2+(alfaRH*sRH)^2)/(SR*QRlin));
b3=alfaRH^2*sRH^2/(QRlin*SR);
b4=aRV*sRV*sqrt(2*rok*pplin)/(rok*SR);
b5=(aRV*sRV*oRlin+auR*suR)^2/(SR*QRlin);

%horni vzdusnik
c1=-alfaLH^2*sLH^2*rok*g*pA/(QLlin*VHlin);
c2=-alfaRH^2*sRH^2*rok*g*pA/(QRlin*VHlin);
c3=-(pA*((aLV*sLV*oLlin+auL*suL)^2+(alfaLH*sLH)^2)/(VHlin*QLlin)+pA*((aRV*sRV*oRlin+auR*suR)^2+(alfaRH*sRH)^2)/(VHlin*QRlin)+R*T*KH*s0H/VHlin);
c4=(alfaLH^2*sLH^2/QLlin+alfaRH^2*sRH^2/QRlin)*pA/VHlin;
c5=aLV*sLV*sqrt(2*rok*pplin)*pA/(rok*VHlin);
c6=aRV*sRV*sqrt(2*rok*pplin)*pA/(rok*VHlin);
c7=pA/VHlin*((aLV*sLV*oLlin+auL*suL)^2/QLlin+(aRV*sRV*oRlin+auR*suR)^2/QRlin);

%Leva dolni nadrz
d1=alfaLH^2*sLH^2*rok*g/(QLlin*SL);
d2=-alfaLL^2*sLL^2*rok*g/(QLlin*SL);
d3=alfaLH^2*sLH^2/(SL*QLlin);
d4=-alfaLH^2*sLH^2/(SL*QLlin)-alfaLL^2*sLL^2/(SL*QLlin);

%Prava dolni nadrz
e1=alfaRH^2*sRH^2*rok*g/(QRlin*SR);
e2=-alfaRL^2*sRL^2*rok*g/(SR*QRlin);
e3=alfaRH^2*sRH^2/(SR*QRlin);
e4=-alfaRH^2*sRH^2/(SR*QRlin)-alfaRL^2*sRL^2/(SR*QRlin);

%Dolni vzdusnik
f1=pA*alfaLH^2*sLH^2*rok*g/(VLlin*QLlin);
f2=pA*alfaRH^2*sRH^2*rok*g/(VLlin*QRlin);
f3=-pA*alfaLL^2*sLL^2*rok*g/(VLlin*QLlin);
f4=-pA*alfaRL^2*sRL^2*rok*g/(VLlin*QRlin);
f5=pA/VLlin*(alfaLH^2*sLH^2/QLlin+alfaRH^2*sRH^2/QRlin);
f6=pA/VLlin*(-alfaLH^2*sLH^2/QLlin-alfaLL^2*sLL^2/QLlin-alfaRH^2*sRH^2/QRlin-alfaRL^2*sRL^2/QRlin)-R*T/VLlin*KL*s0L;

% c1=f3;
% c2=f4;
% c3=f6;


A=[a1 0 0 0 a2 a3;
   0 b1 0 0 b2 b3;
   d1 0 d2 0 d3 d4;
   0 e1 0 e2 e3 e4;
   c1 c2 0 0 c3 c4;
   f1 f2 f3 f4 f5 f6];

B=[a4 0 a5;
   0 b4 b5;
   0 0 0;
   0 0 0;
   c5 c6 c7;
   0 0 0];

C=[0 0 1 0 0 0;
   0 0 0 1 0 0];

D=zeros(2,3);

sys=ss(A,B,C,D);
sysd=c2d(sys,Ts);

Ad=sysd.a;
Bd=sysd.b;