%
% Chapter 3, Example 3.13, pages 112-114
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
function min_pow_tf

alphavec = linspace(0.01,0.99,501);
ind = 1;
for alpha = alphavec,

    % floating oscillator benchmark body model
    parS = struct('A',[0 0 1 0;0 0 0 1;-1 1 0 0;1 -1 0 0],'B',[0;0;1;0],'xf',[1;1;0;0],'x0',[0;0;0;0],'C',[1 0 0 0],'D',[0],'R',1,'alpha',alpha);

    xf = parS.xf;
    A = parS.A;
    B = parS.B;
    C = parS.C;
    D = parS.D;
    R = parS.R;
    sys = ss(A,B,C,D);

    x0 = [3];
    options = optimset('Display','Iter');
    xopt = fminunc(@cost,x0,options,parS)
    Tf(ind) = xopt(1);
    ind = ind+1;
end;

tf = Tf(round(length(Tf)/2));
Q = B*inv(R)*B';
% Solve the equation P_dot = -AP - PA' + BB'
Pss = difflyap(-A',Q,tf);
Lam0 = -inv(Pss)*expm(-A*tf)*xf;
tvec = linspace(0,tf,1001);
for ind = 1:length(tvec)
    u(ind) = -B'*expm(-A'*tvec(ind))*Lam0;
end;
[yy,tt,xx] = lsim(sys,u,tvec);
figure(1)
subplot(211), plot(tt,xx(:,1:2)); xlabel('Time'); ylabel('Displacement'); set(gca,'FontSize',12)
subplot(212), plot(tt,xx(:,3:4)); xlabel('Time'); ylabel('Velocity'); set(gca,'FontSize',12)
figure(2)
plot(tvec,u); xlabel('Time'); ylabel('Control'); set(gca,'FontSize',12)

figure(3)
plot(alphavec,Tf);
xlabel('\alpha ')
ylabel('Maneuver Time');

ind = 1;
for tf = Tf,
    Pss = difflyap(-A',Q,tf);
    Lam0 = -inv(Pss)*expm(-A*tf)*xf;
    Pow(ind) = 1/2*Lam0'*inv(Pss)*Lam0;
    ind = ind+1;
end;
figure(4)
semilogy(alphavec,Pow);
xlabel('\alpha ')
ylabel('Power');


function [f] = cost(x,parS);
xf = parS.xf;
A = parS.A;
B = parS.B;
C = parS.C;
D = parS.D;
R = parS.R;
alpha = parS.alpha;
Tf = x(1);

Q = B*inv(R)*B';
% Solve the equation P_dot = -AP - PA' + BB'
Pss = difflyap(-A',Q,Tf);

Lam0 = -inv(Pss)*expm(-A*Tf)*xf;

P = difflyap(-A',Q,Tf);
f = (1-alpha)*Tf + 1/2*alpha*Lam0'*P*Lam0;
