% 20-stage RC Filter
% Lecture #25: LQG Control

V = zeros(20,1);
dt = 0.01;
t = 0;
V0 = 0;

A = zeros(20,20);
for i=1:19
    A(i,i) = -50;
    A(i,i+1) = 25;
    A(i+1,i) = 25;
end
A(20,20) = -25;
B = zeros(20,1);
B(1) = 25;
C = zeros(1,20);
C(20) = 1;
D = 0;

A4 = [-2,1,0,0;1,-2,1,0;0,1,-2,1;0,0,1,-1];
B4 = [1;0;0;0];
C4 = [0,0,0,1];
Q = 1e4 * C4'*C4  +  1e4*(C4*A4)'*(C4*A4);
R = 1;
Kx = lqr(A4, B4, Q, R);
DC = -C4*inv(A4-B4*Kx)*B4;
Kr = 1/DC;

Ref = 1;
n=0;
DATA = [];

while(t < 20)

   Ref = 1 + 0.25*sign(sin(0.628*t));
   V0 = Kr*Ref - Kx*V([5,10,15,20]);
   dV = A*V + B*V0;
   V = V + dV * dt;
   t = t + dt;

   n = mod(n+1, 5);
   if(n == 0)
      plot([0:20], [V0;V], 'b.-', t, 0, 'b+',20,Ref,'b+');
      ylim([0,2]);
      xlim([0,20]);
      pause(0.01);
      DATA = [DATA ; V(20), Ref];
   end
end

t = [1:length(DATA)]' * dt*5;
plot(t,DATA)