% Cart and Pendulum
% Lecture %31
% LQG/LTR

dt = 0.01;
t = 0;
%Plant
A = [0,0,1,0;0,0,0,1;0,-4.9,0,0;0,14.7,0,0];
B = [0;0;0.5;-0.5];
C = [1,0,0,0];

%Reference Model
Gm = zpk([],[-1+j*2,-1-j*2,-4,-5],1);

DC = evalfr(Gm,0);
Gm = Gm / DC;
X = ss(Gm);
Am = X.a;
Bm = X.b;
Cm = X.c;
[n,m] = size(Am);

X = zeros(4,1);
Xm = zeros(n,1);

A6 = [A, zeros(4,n) ; zeros(n,4), Am];
B6 = [B; zeros(n,1)];
C6 = [C, -Cm];

Q = C6' * C6;
R = 1;

K6 = lqr(A6, B6, Q*1e4, 1);

Kx = K6(1:4);
Km = K6(5:4+n);

DC = -[C,0*Cm]*inv(A6-B6*K6)*[0*B;Bm];
Bm = Bm/DC;

n = 0;
y = [];
while(t < 29.9)
 Ref = sign(sin(0.2*t));
 U = -Km*Xm- Kx*X;
 
 dX = CartDynamics(X, U);
 dXm = Am*Xm + Bm*Ref;

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

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