% Askhsh thewrias elegxou
% gh

% Grammikopoihmeno montello me r0=1
%
  om_0=sym('om_0'); % orizoume symbolikh metablhth om_0
%
% Pinakas a:
%
  a=[0 1 0 0;
     3*om_0^2 0 0 2*om_0;
     0 0 0 1;
     0 -2*om_0 0 0];
%
% Pinakas B
  b = [0 0;
       1 0;
       0 0;
       0 1];
%
%--------------------------------------------------------------------------
% Jordan morfh pinaka:
%
  [v,j]=jordan(a)
% 
% Genikeymena idiodianysmata: 
% 
% v = [       0,            4,            -3/2,            -3/2]
%     [       0,            0,    (3*i*om_0)/2,   -(3*i*om_0)/2]
%     [ (-6)*om_0,          0,             3*i,          (-3)*i]
%     [       0,    (-6)*om_0,          3*om_0,          3*om_0]
% 
% kai pinakas jordan me idiotimes (0,0,-i*om_0,i*om_0):
%  
% j = [ 0, 1,       0,       0]
%     [ 0, 0,       0,       0]
%     [ 0, 0, -i*om_0,       0]
%     [ 0, 0,       0,  i*om_0]
%  
% Epibebaiwsh apotelesmatos:

  v*j*inv(v)-a
%
% ans = [ 0, 0, 0, 0]
%       [ 0, 0, 0, 0]
%       [ 0, 0, 0, 0]
%       [ 0, 0, 0, 0]
%
%--------------------------------------------------------------------------
  t=sym('t'); % symbolikh metablhth
%--------------------------------------------------------------------------
% Ekthetikos pinakas exp(At)
%
  exp_at=expm(a*t);
  exp_at=simplify(exp_at) 
% 
% exp_at = [            4 - 3*cos(t*om_0),              sin(t*om_0)/om_0,   0,   (4*sin((t*om_0)/2)^2)/om_0]
%          [             3*w0*sin(t*om_0),                cos(t*om_0),      0,                2*sin(t*om_0)]
%          [       6*sin(t*om_0)-6*t*om_0,   -(4*sin((t*om_0)/2)^2)/om_0,   1,   (4*sin(t*om_0))/om_0 - 3*t]
%          [ (-12)*om_0*sin((t*om_0)/2)^2,              (-2)*sin(t*om_0),   0,            4*cos(t*om_0) - 3]
%  
%--------------------------------------------------------------------------
% Arithmitiko montello (grammikopoihmeno)
%
  ro=1; % radius
  wo=1; % angular
  
  
  a=[0 1 0 0;
     3*wo^2 0 0 2*wo*ro;
     0 0 0 1;
     0 -2*wo/ro 0 0];
%
% Pinakas B
  b = [0 0;
       1 0;
       0 0;
       0 1/ro];
  
%  
  sys=ss(a,b,eye(4),zeros(4,2)); % state-space system all states
  t1=linspace(0,20,1000); % time vector
  y=0.05*impulse(sys,t1); % impulse response
  %y=0.1*step(sys,t1);
  y1=y(:,:,1); % from input 1 (radial thrust)
  y2=y(:,:,2); % from input 2 (tangential thrust)
  
  dr1=y1(:,1); % delta r 
  dtheta1=y1(:,3); % delta theta
%   xx1=ro*cos(wo*t1')+dr1.*cos(dtheta1); % trajectory 1 x-syntetagmenh
%   yy1=ro*sin(wo*t1')+dr1.*sin(dtheta1); % trajectory 1 y-syntetagmenh
  xx1=(ro+dr1).*cos(wo*t1'+dtheta1); % trajectory 1 x-syntetagmenh
  yy1=(ro+dr1).*sin(wo*t1'+dtheta1); % trajectory 1 y-syntetagmenh
  
  
  
  dr2=y2(:,1); % delta r 
  dtheta2=y2(:,3); % delta theta
%   xx2=ro*cos(wo*t1')+dr2.*cos(dtheta2); % trajectory 2 x-syntetagmenh
%   yy2=ro*sin(wo*t1')+dr2.*sin(dtheta2); % trajectory 2 y-syntetagmenh
  xx2=(ro+dr2).*cos(wo*t1'+dtheta2); % trajectory 2 x-syntetagmenh
  yy2=(ro+dr2).*sin(wo*t1'+dtheta2); % trajectory 2 y-syntetagmenh
  
  r_nl=y_nl(:,1);
  th_nl=y_nl(:,3);
  xx_nl=r_nl.*cos(th_nl);
  yy_nl=r_nl.*sin(th_nl);
  
  
  figure(1)
  plot(t1',ro+y1(:,1),'r',t1',y1(:,2),'b',t1',mod(wo*t1'+y1(:,3),2*pi),'g',t1',y1(:,4),'m')
  xlabel('time sec');
  ylabel('Linearised responses - radial input');
  grid
  
  figure(2)
  %plot(xx1,yy1,'r',ro*cos(wo*t1'),ro*sin(wo*t1'),'b',xx_nl,yy_nl,'g')
  plot(xx1,yy1,'r',xx_nl,yy_nl,'g')
  title('xy trajectory radial impulse thrust')
  grid
   
  figure(3)
  plot(t1',ro+y2(:,1),'r',t1',y2(:,2),'b',t1',mod(wo*t1'+y2(:,3),2*pi),'g',t1',y2(:,4),'m')
  xlabel('time sec');
  ylabel('Linearised responses - tangential input');
  grid
  
  figure(4)
  %plot(xx2,yy2,'r',ro*cos(wo*t1'),ro*sin(wo*t1'),'b')
  plot(xx2,yy2,'r',xx_nl,yy_nl,'g')
  title('xy trajectory tangential impulse thrust')
  grid