%
% Chapter 3, Example 3.11, pages 106-108
% 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')


% Robust Minimum Power control

parvec = sqrt(2)*[.1177789267 -.2355578533e-1 .6733294810e-1*j -.3580593088e-1 -.1034729921e-1*j .1029601396e-1];

u2 = parvec(1) + parvec(2)*t + parvec(3)*j*sin(sqrt(2)*t) + parvec(4)*cos(sqrt(2)*t) +...
     parvec(5)*j*t.*sin(sqrt(2)*t) + parvec(6)*t.*cos(sqrt(2)*t) ;


figure(2)
[yy,tt] = lsim(sys,u2,t,x0);
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,u2);
xlabel('Time (sec)')
ylabel('Control')

% Generation of sensitivity plot

kvec = linspace(0.7,1.3,41);

ind = 1;
for k=kvec,
    K = [k -k;-k k];
    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);
    [xx1,tt1,yy1] = lsim(sys,u1,t,x0);
    [xx2,tt2,yy2] = lsim(sys,u2,t,x0);
    Res1(ind) = sqrt(0.5*(yy1(end,1:2)*K*yy1(end,1:2)' + yy1(end,3:4)*M*yy1(end,3:4)'));
    Res2(ind) = sqrt(0.5*(yy2(end,1:2)*K*yy2(end,1:2)' + yy2(end,3:4)*M*yy2(end,3:4)'));
    ind = ind+1;
end;
figure(3)
plot(kvec,Res1(:),kvec, Res2(:),'r--');
xlabel('Variation in Spring Stiffness');
ylabel('Square root of Residual Energy');
legend('Minimum Power','Robust Minimum Power');

Cost1 = trapz(tt1(:),u1(:).*u1(:));
Cost2 = trapz(tt2(:),u2(:).*u2(:));
[Cost1 Cost2 (Cost2-Cost1)/Cost1*100]