%
% Chapter 3, Example 3.9, pages 98-100
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
% Comparison of minimum power and robust minimum power control
%
A = [0 1;-1 0]; B = [0;1]; C = eye(2);D = [0;0];
sys = ss(A,B,C,D);
t = linspace(0,10,501);
% Minimum Power control
a = -inv([1/4*(20-sin(20)) 1/4*(cos(20)-1);1/4*(cos(20)-1) 1/4*(20+sin(20))])*[1;0]
u1 = -a(1)*sin(t)+a(2)*cos(t);

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

% Robust Minimum Power control

AMf = [4.771763687, -.1479794846, -2.838925343, 11.39581330;
       -.1479794846, 5.228236313, -13.53019696, .4530434990;
       -2.838925343, -13.53019696, 49.24508067, -7.696326190;
       11.39581330, .4530434990, -7.696326190, 35.73423710]

lamda = -inv(AMf)*[1;0;0;0]

 u2 = -lamda(1)*sin(t)+lamda(2)*cos(t)+lamda(3)*(-0.5*t.*cos(t)+0.5*sin(t))-lamda(4)*0.5*t.*sin(t);

figure(2)
[yy,tt] = lsim(sys,u2,t,x0);
subplot(211), plot(tt,yy);
xlabel('Time (sec)')
ylabel('System States')
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,
    A = [0 1;-k 0]; B = [0;1]; C = eye(2);D = [0;0];
    sys = ss(A,B,C,D);
    [yy1,tt1] = lsim(sys,u1,t,x0);
    [yy2,tt2] = lsim(sys,u2,t,x0);
    Res1(ind) = sqrt(0.5*(yy1(end,2)^2 + k*yy1(end,1)^2));
    Res2(ind) = sqrt(0.5*(yy2(end,2)^2 + k*yy2(end,1)^2));
    Cost1(ind) = trapz(tt1(:),u1(:).*u1(:));
    Cost2(ind) = trapz(tt2(:),u2(:).*u2(:));
    ind = ind+1;
end;
figure(3)
plot(kvec,[Res1(:) Res2(:)]);
xlabel('Variation in Spring Stiffness');
ylabel('Square root of Residual Energy');

figure(4)
plot(kvec,[Cost1(:) Cost2(:)]);
xlabel('Variation in Spring Stiffness');
ylabel('Cost Function');

[Cost1(1) Cost2(1) (Cost2(1)-Cost1(1))/Cost1(1)*100]
