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

X = [0;0;0;0];
dX = zeros(4,1);
Ref = 0;
dt = 0.01;
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;

% Observer Plus Wind Model
A6e = [Ae, Be*Cw ; zeros(2,4), Aw];
B6e = [Be ; 0*Bw];
C6e = [Ce, zeros(2,2)];
X6e = zeros(6,1);
H6 = lqr(A6e', C6e', diag([1,1,1,1,1000,1000]), 0.01*eye(2,2))';

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

while(t < 11.9)
 Ref = 0;
 Wind = Cw*Xw;
 Winde = Cw*X6e(5:6);
 
 U = -Kx*X - Winde;

 dX = GantryDynamics(X, U + Wind);
 dX6e = A6e*X6e + B6e*U + H6*(C*X - C6e*X6e);
 dXw = Aw*Xw + Bw*randn;

 X = X + dX * dt;
 X6e = X6e + dX6e * dt;
 Xw = Xw + dXw * dt;
 
 t = t + dt;
 
 n = mod(n+1, 5);
 if(n == 0)
    GantryDisplay3(X, X6e, Ref);
    plot([Ref, Ref],[-0.1,0.1],'b');
 end
 y = [y ; X(1), X6e(1) ];
 
end

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