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

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

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

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

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