%
% Chapter 3, Example 3.3, pages 79-81
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
omega = 1;
Pi = pi;
Lam0 = [-2*omega^5*(cos(Pi*omega)+1)/(cos(Pi*omega)*omega*Pi-3*sin(Pi*omega)+2*Pi*omega)
    -2*omega^4*sin(Pi*omega)/(cos(Pi*omega)*omega*Pi-3*sin(Pi*omega)+2*Pi*omega)
    1/2*(-sin(Pi*omega)+Pi*omega)*omega^2/(cos(Pi*omega)*omega*Pi-3*sin(Pi*omega)+2*Pi*omega)];
lam1 = Lam0(1); lam2 = Lam0(2); lam3 = 0;

tau = linspace(0,pi,1001);
u = -1/2*((-cos(omega*tau)+1)*lam1/omega^2-sin(omega*tau)*lam2/omega+lam3);

A = [0 1 0;-omega^2 0 1;0 0 0]; B = [0;0;1];
C = [1 0 0]; D = [0];

sys = ss(A,B,C,D);
[y,t,x] = lsim(sys,u,tau,[0;0;Lam0(3)]);

subplot(211), plot(t,x(:,1:2)); xlabel('Time');ylabel('States')
subplot(212), plot(t,x(:,3)); xlabel('Time');ylabel('Control')