% 20-stage RC Filter
% Lecture #28: LQG Servo Compensataors

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];

A5 = [A4, 0*B4 ; C4, 0];
B5 = [B4; 0];
B5r = [0*B4; -1];
Q = diag([10,20,40,80,160]);
R = 1;
K5 = lqr(A5, B5, Q, R);
Kx = K5(1:4);
Kz = K5(5);
Z = 0;

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

while(t < 20)

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

   n = mod(n+1, 10);
   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)