% input the system matrices

a = [-0.05156 -0.05877;
   0.01503 -0.005887];
b = [0 -0.03384]';
w = .0125*eye(2);

% input the weighting matrices

q = [0 0;
   0 1];
r = 1;

% compute the LQR feedback gain matrix

[k,p,e] = lqr(a,b,q,r)

% compute the equilibrium control and unpecified stat
% when composition is the specified state

d = [.2 .2]';
ass = [a(1,1) 0;a(2,1) b(2)];
xeue = -inv(ass)*w*d
% compute the predicted feedforward control

xe = [ xeue(1) 0]';
up = xeue(2)+k*xe

% prepare for plotting step disturbance response

aa = a-b*k;
bb = w*d+b*up
t = [0:1:100];
c = eye(2);
dd = [0 0]';
xc = step(aa,bb,c,dd,1,t);
plot(t,xc)
title('Anderson Algorithm MPC State Response')
ylabel('Normalized Composition and Tempreature')
xlabel('Time (min)')
text(20,.02,'Concentration')
text(20,0,'Temperature')
pause

% compare to the unmeasured PI controller

aaug = [a zeros(2,1);
   0 1 0];
baug = [b ;0];
qp = [ 0 0; 0 1];
qi = 1;
qaug = [qp zeros(2,1);
   0 0 qi];
[kaug paug eaug] = lqr(aaug,baug,qaug,r)
wid = [w ; zeros(1,2)]*d;
caug = eye(3);
daug = zeros(3,1);
api = aaug -baug*kaug;
xpi = step(api,wid,caug,daug,1,t);
plot(t,xpi(:,2),t,xc(:,2))
title('Comparison of Anderson and PI algorithms')
xlabel ('Time (min)')
ylabel ('Composition State')
text(10,-.003,'Anderson')
text(30, .0005,'PI')
pause

% Dynamic Predictor

ad = [a zeros(2,1);
   zeros(1,2) a(1,1)];
bd = [b;0];
wd = [w;w(1,1) 0];
ap1 = [0;b(2)*k(1,1)-a(2,1)];
adfb = [a-b*k ap1;
   zeros(1,2) a(1,1)];
wn = [w(1,1) 0;0 0;w(1,1) 0];
wnd = wn*d;
xdfb = step(adfb,wnd,caug,daug,1,t);
plot(t,xdfb)
title('Dynamic Predictor Control')
xlabel('Time (min)')
ylabel('State and Predicted State Variables')
text(20,.0025,'Temp')
text(20,0.02,'Actual and Predicted Conc')
pause
plot(t,xdfb(:,2),t,xpi(:,2),t,xc(:,2))
title('Temperature Response')
xlabel('Time')
ylabel('Temperature Response')
text(20,.004,'PI')
text(60,-.004,'Anderson')
text(60,.001,'Dynamic Predictor')
pause
