% Lecture #33
% Pink Noise
% Gantry plus full-order observer

X = [0;0;0;0];
dX = zeros(4,1);
Ref = 0;
dt = 0.025;
U = 0;
t = 0;
C = [1,0,0,0;0,1,0,0];

% Wind Disturbance
Aw = [-5,0; 1, -5];
Bw = [1;0];
Cw = [100,-450];
Xw = zeros(2,1);

%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 ; 0,1,0,0];
Xe = X;
H =  lqr(Ae', Ce', diag([1,1,1,1]), 0.01*diag([1,1]))';
y = [];
n = 0;

% Feedback
Kx = [3.1623   -3.8670    5.6097   -1.3269];
Kr = 0;

while(t < 11.9)
 U = -Kx*X;
 Ref = 0;
 Wind = Cw*Xw;
 
 dX = GantryDynamics(X, U + Wind);
 dXe = Ae*Xe + Be*U + H*(C*X - Ce*Xe);
 dXw = Aw*Xw + Bw*randn;

 X = X + dX * dt;
 Xe = Xe + dXe * dt;
 Xw = Xw + dXw * dt;
 
 t = t + dt;
 
 GantryDisplay3(X, Xe, Ref);
 plot([Ref, Ref],[-0.1,0.1],'b');
 
 y = [y ; X(1), Xe(1) ];
 
end

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