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

[x01,y_stasj,data]=initialize(z,mG_in,mL_in,P1_stasj,h1_stasj,P2_stasj,P0);
data.mF_G=mG_in/mTot_in;
data.mF_L=mL_in/mTot_in;

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


%   Output selection:
y_sel=[1 2];
yI=2;           %Integral action in measurement point #

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]);
dss=[ds]*0;            %ingen forstyrrelse estimering
x00=[x0(1);x0(2);x0(3)]*0.02; 
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]*0.0002;
ww=diag([x00;dsss]);
[kest,l_dest,p]=kalman(sys,ww,v,0);   %Linear Kalman filter computation
l_dest

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

%Integral action in LQG

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)




