% Dette script bruges til at generere de matricer der anvendes i
% state-space modellen.
% skal køres for at generere variabler brugt i RegulatorSimulering.mdl

clear,clc, clf;

m_tp_time = 100000;     %Step i stoker
m_luft_time = 900000;   %Step i blæser
Fan_step = 100;         %Step i belastning

stoker_stop = 12;
blower_stop = 69.24;

%% Anvendte konstanter og variable

% Vægtningen af masseflow væk fra muldvarpen
pct_str = 0.4;        %andelen der skyldes størrelse, 40 % indtastes som 0.4
pct_flow = 1-pct_str; %andel der skyldes luftflow

% Procent at træpillerne der indgår i massebalancen
K_m = 0.7; % sættes til 0.8 hvis 80% procent af træpillerne indgår i 
           % massebalancen og 20% går direkte til energibalancen

% Start værdier i steady state
M_tp = 0.300;       %anslået størrelse på muldvarp er 300g
T_frem = 57.64;     %ud fra SS
T_rg = 106.13;      %ud fra SS
T_fm = 200;         %valgt som start værdi
T_ff = 400;         %valgt som start værdi
lambda_rg = 0.084;  %ud fra SS

% steady-state ydelser i procent
stoker_start = 9;       % 90 % indtastes som 90, SS ydelse på stoker
blower_start = 56.36;   % 60 % indtastes som 60, SS ydelse på blæser

% Konstanter
K_VM = 1.105;   %omregning mellem volumen-% og vægt-%
K_prop = 2.456; %forhold mellem ilt og træpiller ved forbrænding, udregnet 1,5

%Masser
M_vand = 89;                     %massen af vand i kedlen
M_rgff = 23.284*10^-3;           %massen af røggas fyrrum, forrest
M_rgfm = 16.466*10^-3;           % massen af røggas i fyrrum, midten
M_rgfb = 15.812*10^-3;           %massen af røggas i fyrum, bagerst
M_rgf = M_rgff + M_rgfm + M_rgfb;%samlede masse af røggas i fyrrum

%Temperaturer
T_omg = 20;                 %anslået temperatur ved SS målingen
T_retur_gain_20 = 0.8288;   %bestemt ud fra målinger, kalorifer 20 Hz
T_retur_gain_30 = 0.8673;   %bestemt ud fra målinger, kalorifer 30 Hz

T_retur = T_frem*T_retur_gain_30;

%flow
mtp = 95*10^-6;         %hældningen på stokerens indfødning
mluft = 0.259*10^-3;    %hældning på blæserens indfødning
m_vand = 0.4336;        %bestemt ud fra at samlede energibalace passede


%opslags konstanter
h_tp = 17.8*10^6;   %brændværdi af træpiller
c_rg = 1012;        %valgt at anvende værdi for luft ved 100 C
c_luft = 1007;      %værdi for luft ved 20 C
c_vand = 4.18*10^3; %værdi for vand ved 50-60 C

%Arealer til overførsel af effekt fra fyrrum til kedlen
A_fb = 235.32*10^-3; %m^2 Beregnet ud fra skitste med dimensioner
A_fm = 594.54*10^-3; %m^2
A_ff = 701.13*10^-3; %m^2 

%Variabler anvendt til udregning af konstanter
m_tp=stoker_start*mtp;     %udregning af masseflow i kg/s, træpiller
m_luft=blower_start*mluft; %udregning af masseflow i kg/s, luft
m_rg=m_luft+0.85*m_tp;     %udregning af masseflow i kg/s, røggas

%Udregnede konstanter, se Appendiks F for detaljer
K_str = (K_m*m_tp*pct_str)/M_tp;
K_flow = (K_m*m_tp*pct_flow)/m_luft;

alpha_ff=(h_tp*m_tp*(1-K_m) +(M_tp*K_str + m_luft*K_flow)*h_tp...
    +c_luft*m_luft*T_omg-c_rg*m_rg*T_ff)...
    /(A_ff*(T_ff-T_frem));
K_ff = alpha_ff/m_rg^0.8;

alpha_fm = (c_rg*m_rg*(T_ff-T_fm))/(A_fm*(T_fm-T_frem));
K_fm = alpha_fm/m_rg^0.8;

alpha_fb = (c_rg*m_rg*(T_fm-T_rg))/(A_fb*(T_rg-T_frem));
K_fb = alpha_fb/m_rg^0.8;

%% A - system matricen, en 6x6 matrice

A = zeros(6,6);

A(1,1) = -K_str;
A(2,1) = (h_tp*K_str)/(M_rgff*c_rg);
A(2,2) = (-c_rg*m_rg - A_ff*K_ff*m_rg^0.8)/(M_rgff*c_rg);
A(2,5) = (A_ff*K_ff*m_rg^0.8)/(M_rgff*c_rg);
A(3,2) = (c_rg*m_rg)/(M_rgfm*c_rg);
A(3,3) = (-c_rg*m_rg - A_fm*K_fm*m_rg^0.8)/(M_rgfm*c_rg);
A(3,5) = (A_fm*K_fm*m_rg^0.8)/(M_rgfm*c_rg);
A(4,3) = (c_rg*m_rg)/(M_rgfb*c_rg);
A(4,4) = (-c_rg*m_rg - A_fb*K_fb*m_rg^0.8)/(M_rgfb*c_rg);
A(4,5) = (A_fb*K_fb*m_rg^0.8)/(M_rgfb*c_rg);
A(5,2) = (A_ff*K_ff*m_rg^0.8)/(M_vand*c_vand);
A(5,3) = (A_fm*K_fm*m_rg^0.8)/(M_vand*c_vand);
A(5,4) = (A_fb*K_fb*m_rg^0.8)/(M_vand*c_vand);
A(5,5) = (-A_ff*K_ff - A_fm*K_fm - A_fb*K_fb)*(m_rg^0.8/(M_vand*c_vand))...
    -m_vand/M_vand;
A(6,1) = (-K_str*K_prop)/M_rgf;
A(6,6) = (-m_rg)/M_rgf;

%% B - input matricen, en 7x2 matrice

B = zeros(6,2);

B(1,1) = -K_flow;
B(1,2) = K_m;
B(2,1) = (h_tp*K_flow + c_luft*T_omg - c_rg*T_ff)/(M_rgff*c_rg)...
    - (0.8*A_ff*K_ff*(T_ff-T_frem))/(M_rgff*c_rg*(m_rg)^0.2);
B(2,2) = (h_tp*(1-K_m) - 0.85*c_rg*T_ff)/(M_rgff*c_rg)...
    - (0.68*A_ff*K_ff*(T_ff-T_frem))/(M_rgff*c_rg*(m_rg)^0.2);
B(3,1) = (c_rg*(T_ff-T_fm))/(M_rgfm*c_rg)...
    - (0.8*A_fm*K_fm*(T_fm-T_frem))/(M_rgfm*c_rg*(m_rg)^0.2);
B(3,2) = 0.85*c_rg*(T_ff-T_fm) - (0.68*A_fm*K_fm*(T_fm-T_frem))...
    /((m_rg)^0.2);
B(4,1) = (c_rg*(T_fm-T_rg))/(M_rgfb*c_rg)...
    - (0.8*A_fb*K_fb*(T_rg-T_frem))/(M_rgfb*c_rg*(m_rg)^0.2);
B(4,2) = (-0.85*c_rg*T_fm)/(M_rgfb*c_rg) ...
    -(0.68*A_fb*K_fb*(T_rg-T_frem))/(M_rgfb*c_rg*(m_rg)^0.2);
B(5,1) = (0.8/(M_vand*c_vand*m_rg^0.2))*(A_ff*K_ff*(T_ff-T_frem)...
    + A_fm*K_fm*(T_fm-T_frem) + A_fb*K_fb*(T_rg*T_frem));
B(5,2) = (0.68/(M_vand*c_vand*m_rg^0.2))*(A_ff*K_ff*(T_ff-T_frem)...
    + A_fm*K_fm*(T_fm-T_frem) + A_fb*K_fb*(T_rg*T_frem));
B(6,1) = (0.21*K_VM - K_flow*K_prop - lambda_rg)/M_rgf;
B(6,2) = ((1-K_m)*K_prop - 0.85*lambda_rg)/M_rgf;

%% Bw matrice - input matricen for forsyrrelsen T_retur, en 1x6 matrice

Bw = zeros(6,1);

Bw(5,1) = m_vand/M_vand;

%% C - output matricen, en 2x6 matrice

C = zeros(2,6);

C(1,5) = 1;
C(2,6) = 1;

%% Diskretisering af matricer
D = zeros(2,2);
Ts = 1/100;
[Ad,Bd,Cd,Dd] = c2dm(A, B, C, D, Ts, 'zoh');

%% L - estimator gain matricen, en 7x2 matrice

%kontroller om systemet er observerbart, vha observerbarhedsmatricen O.
disp('Kontrollerer om systemet er observerbart...')
[AbarO,BbarO,CbarO,TO,kO] = obsvf(Ad,Bd,Cd);

if (sum(kO)~=6)
    disp('Systemet er ikke observerbart.')
else
    disp('Systemet er observerbart.')
end

Qe = zeros(6,6); %matrice til indikation af processstøj
Qe(1,1) = 0.01^2;% M_tp
Qe(2,2) = 2^2;   % T_ff
Qe(3,3) = 2^2;   % T_fm
Qe(4,4) = 2^2;   % T_rg
% Qe(5,5) = 0; % T_frem
% Qe(6,6) = 0; % lambda_rg

Re = zeros(2,2); %matrice til indikation af sensorstøj
Re(1,1) = 1^2;   %sensorstøj på fremløbsvand, T_frem
Re(2,2) = .1^2;  %Sensorstøj på iltniveau, lambda_rg

G = eye(6);

L = dlqe(Ad,G,Cd,Qe,Re);  %L = estimator gain matrice

%% F - tilbagekoblings matricen, en 2x8 matrice
% % F er en sammensat matrice F = [F_E(2x6) F_I(2x2)]

K_int=0.05;             %Konstant til at sløve intralvirkningen i regulator
Ai = [Ad zeros(6,2); 
      Cd eye(2,2)];
Bi = [Bd; zeros(2,2)];
Bi = [Bd; eye(2,2)];
Ci = [Cd zeros(2,2)];
CI = [Ci;
    zeros(2,6) K_int*eye(2,2)];

% %kontroller om systemet er kontrollerbart, vha kontrollerbarhedsmatricen Co.
disp('Kontrollerer om systemet er kontrollerbart...')

[AbarC,BbarC,CbarC,TC,kC] = ctrbf(Ai,Bi,CI);

if (sum(kC)~=8)
    disp('Systemet er ikke kontrollerbart.')
else
    disp('Systemet er kontrollerbart.')
end

% % Qx og R vægtes ud fra Bryson's rule
Qx = zeros(4,4);    % variationer i output
Qx(1,1) = 1/3^2;    %T_frem
Qx(2,2) = 1/1^2;    %lambda_rg
Qx(3,3) = 1/3^2;    %integreret fejl T_frem
Qx(4,4) = 1/1^2;    %integreret fejl lambda_rg

Rr = zeros(2,2);     %variationer i input
Rr(1,1) = 1/40^2;   %variation i blæser
Rr(2,2) = 1/9^2;    %variation i stoker
 
F = dlqr(Ai, Bi, CI'*Qx*CI,Rr);

F_E = zeros(2,6);
F_E = [F(1,1) F(1,2) F(1,3) F(1,4) F(1,5) F(1,6);
    F(2,1) F(2,2) F(2,3) F(2,4) F(2,5) F(2,6)];

F_I = zeros(2,2);
F_I = [F(1,7) F(1,8); F(2,7) F(2,8)];


EstPole = eig(Ad-L*Cd) %print estimatorpoler i command window
CLpole = eig(Ai-Bi*F)  %print lukketsløjfepoler i command window