X = zeros(4,1);
dX = zeros(4,1);
Ref = 1;
dt = 0.01;
U = 0;
t = 0;

% Full State Feedback
Kx = [4.8980  -40.6041   10.2041    0.4082];
Kr = [4.8980];

y = [];

% Full Order Observer
A = [0,0,1,0;0,0,0,1;0,9.8,0,0;0,-9.8,0,0];
B = [0;0;1;-0.5];
C = [1,0,0,0];
Xe = rand(4,1) ;
H = [    9.2000 ;   -4.2808 ;   21.8400 ;  -19.0384 ];

while(t < 20)
 Ref = sin(t);
 U = Kr*Ref - Kx*X;

% Plant 
 dX = GantryDynamics(X, U);
% Full-Order Observer
 dXe = A*Xe + B*U + H*(X(1) - Xe(1));
 
 X = X + dX * dt;
 Xe = Xe + dXe * dt;
 
 t = t + dt;
 
 GantryDisplay(X, Xe, Ref);
 plot([Ref, Ref],[-0.1,0.1],'b');
 
 y = [y ; Ref, X(1) ];
 
end

hold off
plot(y);