% input the a and b matrices
a = [-0.05156 -0.05877;
   0.01503 -0.005887];
b = [0 -0.0338]'
% input the w matrix
w = [0.0125 0;
   0      0.0125];

% define the Johnson transformation

aa = [a eye(2);
   zeros(2) zeros(2)];
bb = [0;
   0;
   b];
% define the weighting matrices

q = [1 0;
   0 0];

r = 1;

% Johnson transformation weighting matrices

qq = [q    zeros(2);
   zeros(2) zeros(2)];

% calculate the feedback gain using recursive method of section 5.2

z = [aa   -bb*inv(r)*bb';
   -qq  -aa'   ];

% chose the integration time interval dt and calculate transition matrix

dt = 1;

theta = expm(z);

% partition the transition matrix

theta11 = theta(1:4,1:4);
theta12 = theta(1:4,5:8);
theta21 = theta(5:8,1:4);
theta22 = theta(5:8,5:8);

% define the final Ricatti matrix p(T)

pt = zeros(4);

% define t max

tmax = 70;

% loop for calculating feedback gain matrix k

for t = tmax:-1:1,
   pt1 = inv(theta22 - pt*theta12)*(pt*theta11 - theta21);
   kt1 = inv(r)*bb'*pt1;
   % this loop is to transform the matrix into a vector
   
   for col = 1:4
      k(t,col) = kt1(1,col);
   end
   
   pt = pt1;
end

% plot k vs time

t = 0:(tmax-1);
plot (t,k)
title ('Gain Matrix vs Time'),ylabel('Gain Components'),xlabel('Time (min)');
print pigainmat;
pause

% compute proportional and integral feedback matrices

k1 = kt1(1,1:2);
k2 = kt1(1,3:4);
kp = k2
ki = k1-k2*a

% preparing system for step change response

api = [a-b*kp  -b*ki;
   eye(2)  zeros(2)];
t = [0:1:60];
wi = .2*[ w ;
   zeros(2)];
c = eye(4);
d = zeros(4);
xpi = step(api,wi,c,d,1,t);
x = xpi(1:61,1:2);
plot(t,x)
title('Johnson Algorithm State Response')
ylabel('Normalized composition and temperature')
xlabel('Time (min)')
print xpi;