poles = [0.5,0.51,0.52,0.53];
%poles = [0.63,0.73,0.87,0.98];

%Four compartment thermal model with coupling conductivity g and input
%from fixed temperature source next to compartment one.
T = 1;
g = 0.1;
A = [1-2*g*T,g*T,0,0; g*T,1-2*g*T,g*T,0; 0,g*T,1-2*g*T,g*T; 0,0,g*T,1-g*T];
B = [g*T,0,0,0]';
C = [0,0,0,1];
D = 0;

%Natural frequency picking
K = place(A,B,poles);

%Print k
disp("K values");
disp(K)

%calculate Kr:
KrInv = C*((eye(size(A))-(A-B*K))\B);
Kr = 1/KrInv;
disp('Kr');
disp(Kr);
    
%Form closed loop matrix 
Ac = A-B*K; 
Bc = B*Kr*20;  % Scale to 20 degree step.
Cc = eye(size(A));
Cc(length(A)+1,:) = K;
Dc = D;
sys_cl = ss(Ac,Bc,Cc,Dc,T,'OutputName', {'x1', 'x2','x3','x4','-KX'});

% Plot step response of all three states and the -K*x part of Vpwm.
figure;
step(sys_cl);
display(K);
display(Kr);
