% Ball & Beam System
% Lecture #21
% Observers & Disturbances
% m = 1kg
% J = 0.2 kg m^2

X = [0.8,0,0,0]';
dt = 0.01;
t = 0;
n = 0;
y = [];

% Plant
A = [0,0,1,0;0,0,0,1;0,-7,0,0;-8.167,0,0,0];
B = [0;0;0;0.833];
C = [1,0,0,0];

% Control Law
Kx = ppl(A, B, [-2, -2.5, -3, -3.5]);
DC = -C*inv(A-B*Kx)*B;
Kr = 1/DC;

% Full-Order Observer
%Ae = A;
%Be = B;
%Ce = C;
%H = ppl(A', C', [-3, -4, -5, -6])';
%Xe = X;

% Full-Order Observer with Input disturbance
Ae = [A, B ; zeros(1,4), 0];
Be = [B ; 0];
Ce = [C, 0];
Xe = [X;0];
H =  ppl(Ae', Ce', [-5, -5.1, -5.2, -5.3, -5.4])';



while(t < 20)
 Ref = 1 + 0.2*sign(sin(0.314*t));
 if(t < 2)
    U = Kr*Ref - Kx*X(1:4);
 else
     U = Kr*Ref - Kx*Xe(1:4);
 end
 Y = X(1);
 dX = BeamDynamics(X, U);
 dXe = Ae*Xe + Be*U + H*(Y - Xe(1));
 
 X = X + dX * dt;
 Xe = Xe + dXe * dt;
 t = t + dt;
 
 y = [y ; Ref, X(1)];
 n = mod(n+1,5);
 if(n == 0)
    BeamDisplay3(X, Xe, Ref);
 end
 end
 
t = [1:length(y)]' * dt;


plot(t,y(:,1),'r',t,y(:,2),'b');
xlabel('Time (seconds)');
ylabel('Ball Position');