servo_model % From Lab 1
clf

P = ss(A,B,eye(4),0);  % All states measureable
[y,t] = impulse(P,3);
for k = 1:4
	subplot(2,2,k)
	plot(t,y(:,k),'Linew',2);
	grid
	hold on
	xlabel('Time')
	title(['x_' num2str(k)])
end

% First attempt, penalize (y^2 + u^2)
Q1 = C2'*C2;
Q2 = 1;
L = lqr(A,B,Q1,Q2);
Gcl = feedback(P,L);

[y,t] = impulse(Gcl,3);
%subplot(3,3,[1 2])
subplot(2,1,1);
plot(t,y(:,3),'Linew',2);
title('y_2')
grid

[u,t] = impulse(feedback(-L*P,1,1),3);
subplot(3,3,[4 5])
subplot(2,1,2);
plot(t,u,'Linew',2);
title('u')
xlabel('Time')
grid

% Second attempt, penalize (y^2 + 100u^2)
Q1 = C2'*C2;
Q2 = 100;
L = lqr(A,B,Q1,Q2);
Gcl = feedback(P,L);
[y,t] = impulse(Gcl,3);
subplot(2,1,1);
plot(t,y(:,3),'Linew',2);
title('y_2')
grid

[u,t] = impulse(feedback(-L*P,1,1),3);
subplot(2,1,2);
plot(t,u,'Linew',2);
title('u')
xlabel('Time')
grid

% Third attempt, penalize (y^2 + 0.1 \dot y^2 + 100u^2)
Q1 = C2'*C2 + 0.1*(C2*A)'*(C2*A);
Q2 = 100;
L = lqr(A,B,Q1,Q2);
Gcl = feedback(P,L);
[y,t] = impulse(Gcl,3);
subplot(2,1,1);
plot(t,y(:,3),'Linew',2);
title('y_2')
grid

[u,t] = impulse(feedback(-L*P,1,1),3);
subplot(2,1,2);
plot(t,u,'Linew',2);
title('u')
xlabel('Time')
grid

return

