% Cart and Pendulum
% Lecture %20
% Separation Principle

X = [-1,0,0,0]';
Ref = 1;
dt = 0.01;
t = 0;
Kx = [-4.898  -104.29  -10.20  -30.20];
Kr = -4.898;
% Full-Order Observer
Ae = [0,0,1,0;0,0,0,1;0,-4.9,0,0;0,14.7,0,0];
Be = [0;0;0.5;-0.5];
Ce = [1,0,0,0];
H = ppl(Ae', Ce', [-3,-4,-5,-6])';
Xe = X;

n = 0;
y = [];
while(t < 9.9)
 Ref = sign(sin(0.638*t));
 U = Kr*Ref - Kx*Xe;
 dX = CartDynamics(X, U);
 dXe = Ae*Xe + Be*U + H*(X(1) - Ce*Xe);

 X = X + dX * dt;
 Xe = Xe + dXe * dt;
 
 t = t + dt;
 n = mod(n+1, 5);
 if(n == 0)
    CartDisplay(X, Xe, Ref);
 end
 y = [y ; X(1), Xe(1), Ref];
end

hold off;
t = [1:length(y)]' * dt;
plot(t,y);