% Cart and Pendulum
% Lecture %32
% LQG/LTR with a servo compensator

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];

Ref = 1;
dt = 0.01;
t = 0;

%Reference Model
Gm = zpk([],[-0.5,-1,-1.2,-1.4],1);

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

A7 = [ A, zeros(4,1), zeros(4,n) ;
       C, 0, -Cm;
       zeros(n,4), zeros(n,1), Am];
B7 = [B; 0; zeros(n,1)];
B7r = [zeros(4,1); 0; Bm];

C7 = [0*C, 1, 0*Cm];

Q = C7' * C7;
R = 1;

K7 = lqr(A7, B7, Q*1e2, 1);

Kx = K7(1:4);
Kz = K7(5);
Km = K7(6:5+n);

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

Z = 0;
n = 0;
y = [];
while(t < 29)
 Ref = 1*(sin(0.2*t) > 0);
 U = -Km*Xm - Kx*X - Kz*Z;
 
 dX = CartDynamics(X, U);
 dXm = Am*Xm + Bm*Ref;
 dZ = X(1) - Cm*Xm;

 X = X + dX * dt;
 Xm = Xm + dXm * dt;
 Z = Z + dZ*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);