%   LQGdesign.m 
clear all
global data 
%   Initializing
mG_in=0.362;
mL_in=8.64;
z=0.13;
P1_stasj=70.95e5;
h1_stasj=0.11;
P2_stasj=53.4e5;
P0=50e5;

[x0,y_stasj,data]=initialize(z,mG_in,mL_in,P1_stasj,h1_stasj,P2_stasj,P0);

%   Operating point:
z_op=0.30;
load Stationary;
I=find(abs(z-z_op)==min(abs(z-z_op)));
z_star=z(I);
u=[z_star;P0];
x0=[xs(1,I);xs(2,I);xs(3,I)];       %tilstander
ds=[mG_in;mL_in];                %forstyrrelser
[A,B,C,D]=linearize(@slug_3D_lin,[],[x0;ds],[],u);
A(4,4)=-1e-5;
A(5,5)=-1e-5;

%   No disturbance estimation

%   Output selection:
y_sel=[1 2]; %1=P1[bar], 2=P2[bar], 3=rho_T[kg/m^3], 4=w[kg/s] og 5=Q[m^3/s]
n=length(A);
B=B(:,1);C=C(y_sel,:);D=D(y_sel,1);
G=eye(n);                       %Process noise on state derivatives
H=zeros(length(y_sel),n);       %Process noise on measurements

%Kalman filter uten forstyrrelses estimering
sys=ss(A,[B G],C,[D H]);
x00=[x0(1); x0(2); x0(3)]*0.02;
dss=[1*ds(1); 1*ds(2)]*0;       %ingen forstyrrelses estimering
w=diag([x00;dss]);              %Process noice variance 
v=diag([0.1 0.1]);              %Measurement noise variance
[kest,l,p]=kalman(sys,w,v,0);   %Linear Kalman filter computation
l

%Kalman filter med forstyrrelses estimering
dsss=[ds(1); ds(2)]*0.02;
ww=diag([x00;dsss])*0.02;
[kest,lest,p]=kalman(sys,ww,v,0);   %Linear Kalman filter computation
lest

%Regulator uten I-virkning
Q=1*eye(n);         
R=80e8;                 %Weighting on input usage
LQG=lqr(A,B,Q,R)

%Regulator med I-virkning
yI=2;           %Integral action in measurement point #
CI=C(yI,:);
DI=D(yI,:);


p=size(CI,1);         % # measurements with integral action
[n,m]=size(B);        % m= # inputs (u)

nn=zeros(n,n);
pn=zeros(p,n);
np=zeros(n,p);
pp=zeros(p,p);

A=[A np; -CI pp];
B=[B;-DI];
Q=[nn np;pn eye(p,p)];          %Weighting on states (only weighting the measurement with integral action)
R=80e6*eye(m);                   %Weighting on input usage
LQGI=lqr(A,B,Q,R)




