% askhsh_b thewrias elegxou 2013
% gh, 5-4-2013


% Arithmitiko montello (grammikopoihmeno)
%
  r0=1; % aktina
  w0=1; % gwniakh taxythta
  k=(r0^3)*(w0^2); % stathera pediou varhthtas
  np=1000; % time samples
  t=linspace(0,2*pi,np); % dianysma xronou
%   
%--------------------------------------------------------------------------
% Ekthetikos pinakas exp(At) apo askhsh_a.m:
%
  exp_at= zeros(4,4,np);
  for j=1:np
  exp_at(:,:,j) = [4-3*cos(t(j)*w0),                  sin(t(j)*w0)/w0,                 0, (4*r0*sin((t(j)*w0)/2)^2)/w0
                   3*w0*sin(t(j)*w0),                 cos(t(j)*w0),                    0,  2*r0*sin(t(j)*w0)
                   (3*(2*sin(t(j)*w0)-2*t(j)*w0))/r0, -(4*sin((t(j)*w0)/2)^2)/(r0*w0), 1,  (4*sin(t(j)*w0))/w0 - 3*t(j)
                  -(12*w0*sin((t(j)*w0)/2)^2)/r0,     -(2*sin(t(j)*w0))/r0,            0,  4*cos(t(j)*w0) - 3];
  end
%  
%--------------------------------------------------------------------------
% Grammikopoihmeno systhma:
%
% Pinakas A:
 a=[0 1 0 0;
     3*w0^2 0 0 2*w0*r0;
     0 0 0 1;
     0 -2*w0/r0 0 0];
%
% Pinakas B:
  b = [0 0;
       1 0;
       0 0;
       0 1/r0];
%  
intensity=0.05; % impulse intensity
%
%   sys=ss(a,b,eye(4),zeros(4,2)); % state-space system all states
%   y=intensity*impulse(sys,t); % impulse response
%   y1=y(:,:,1); % from input 1 (radial thrust)
%   y2=y(:,:,2); % from input 2 (tangential thrust)
% 
%   y1a=zeros(np,4);
%   y2a=zeros(np,4);
% 
  for j=1:np
      y1a(j,:)=intensity*(exp_at(:,:,j)*b(:,1))'; % from input 1 (radial thrust)
      y2a(j,:)=intensity*(exp_at(:,:,j)*b(:,2))'; % from input 2 (tangential thrust)
  end
%  
%   figure(1)
%   plot(t',y1(:,1),'r',t',y1(:,2),'b',t',y1(:,3),'g',t',y1(:,4),'m');
%   xlabel('time sec');
%   ylabel('Linearised responses - radial input');
%   title('impulse function')
%   grid
  
  figure(1)
  plot(t',y1a(:,1),'r',t',y1a(:,2),'b',t',y1a(:,3),'g',t',y1a(:,4),'m');
  xlabel('time sec');
  ylabel('Linearised responses - radial input');
  title('expm function')
  grid
  
%   figure(2)
%   plot(t',y2(:,1),'r',t',y2(:,2),'b',t',y2(:,3),'g',t',y2(:,4),'m');
%   xlabel('time sec');
%   ylabel('Linearised responses - tangential input');
%   title('impulse function')
%   grid
  
  figure(3)
  plot(t',y2a(:,1),'r',t',y2a(:,2),'b',t',y2a(:,3),'g',t',y2a(:,4),'m');
  xlabel('time sec');
  ylabel('Linearised responses - tangential input');
  title('expm function')
  grid
%
%--------------------- end askhsh_b ---------------------------------------
  