%
% Chapter 3, Example 3.10, pages 103-104
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
% Comparison of minimum power and robust minimum power control
%
M = eye(2); K = [1 -1;-1 1];
A = [zeros(2) eye(2); -inv(M)*K zeros(2)]; B = [0;0;inv(M)*[1;0]]; C = [eye(2) zeros(2)];D = [0;0];
sys = ss(A,B,C,D);
t = linspace(0,10,501);
% Minimum Power control
parvec = sqrt(2)*[.8906381301e-1 -.1781276260e-1 .1156978535e-1*j .1162741519e-1];

u1 = parvec(1) + parvec(2)*t + parvec(3)*j*sin(sqrt(2)*t) + parvec(4)*cos(sqrt(2)*t);

x0 = [1;1;0;0];
[yy,tt] = lsim(sys,u1,t,x0);
figure(1)
subplot(211), plot(tt,yy(:,1),tt,yy(:,2),'r--');
xlabel('Time (sec)')
ylabel('System States')
legend('Mass 1','Mass 2');
subplot(212), plot(t,u1);
xlabel('Time (sec)')
ylabel('Control')
