% 20-stage RC Filter
% Lecture #20: Separation Princinple

V = zeros(20,1);
dV = 0*V;

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;

% Full-Order Observer
Ae = [-2,1,0,0;1,-2,1,0;0,1,-2,1;0,0,1,-1];
Be = [1;0;0;0];
Ce = [0,0,0,1];
H = [61, 70, 38, 11]';
Xe = zeros(4,1);

% Full-State Feedback
%Kx = [3, 5, 7, 8];
%Kr = 24;
% Servo Compensator
Kx = [ 2.4000    6.8900   13.0900   13.5100];
Kz = 13.8600;
Z = 0;

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

while(t < 20)
   
   Ref = 0.5 + 0.5*sign(sin(0.314*t));
%   V0 = Kr*Ref - Kx*Xe;
   V0 = -Kz*Z - Kx*Xe;
   
   dV = A*V + B*V0;
   dXe = Ae*Xe + Be*V0 + H*(V(20) - Ce*Xe);
   dZ = Ce*Xe - Ref;
   
   V = V + dV * dt;
   Xe = Xe + dXe * dt;
   Z = Z + dZ * dt;
   t = t + dt;

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

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