% 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 = [1 0;
   0 0];
r = 1;

% compute the LQR feedback gain matrix

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

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

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

xe = [zeros(1,101); xeue(1,:) ];
up = xeue(2,:)+k*xe;

% prepare for plotting step disturbance response

aa = a-b*k;
bb = [w b];
I = ones(101,1);
c = eye(2);
du = zeros(2,3);
dd = [d ;up];
sys1 = ss(aa,bb,c,du);
[xc ,t] = lsim(sys1,dd,t);
plot(t,xc)
title('Anderson Algorithm MPC State Response')
ylabel('Normalized Composition and Tempreature')
xlabel('Time (min)')
text(60,.06,'Concentration')
text(50,0.1,'Temperature')
pause

% compare to the unmeasured PI controller

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

% Dynamic Predictor

ad = [a(1,1) a(1,2);
   a(2,1)-b(2)*k(1,1) a(2,2)-b(2)*k(1,2)];
bd = [.01*t .01*t .01*I .01*I];
wd = [w(1,1) 0 0 0;(a(2,2)-b(2)*k(1,2))*w(1,1)/a(1,2) 0 -w(1,1)/a(1,2) 0];
wnd = zeros(2,4);
sys = ss(ad,wd,c,wnd);
[xdfb,t] = lsim(sys,bd,t);
plot(t,xdfb)
title('Dynamic Predictor Control')
xlabel('Time (min)')
ylabel('State and Predicted State Variables')
text(20,.01,'Temp')
text(40,0.05,'Actual and Predicted Conc')
pause
plot(t,xdfb(:,1),t,xpi(:,1),t,xc(:,1))
title('Temperature Response')
xlabel('Time')
ylabel('Temperature Response')
text(20,.004,'PI')
text(40,.03,'Anderson')
text(60,.001,'Dynamic Predictor')
pause
