%--------------------------------------------------------------------------
%
% PURPOSE: Analyze the steering dynamics of Altex for small angles,
%          in the horizontal plane.
% AUTHOR:  Rob McEwen
% DATE:    99/6/16:
%
%--------------------------------------------------------------------------
%
% Assume that: 1) The vehicle is in straight and level flight, so this
%                 analysis is in the horizontal plane.
%              2) All angles remain less than 6 degrees.
%              3) The ring and struts have the lift and drag of 4 
%                 Odyssey fins.
%
% WARNING:  This routine is designed to be called from run.m with LinFlg == 1.
%           LinFlg == 0 should be standalone if the constants below are 
%           correct.
%
% NOTES: 1) The effect of the tailcone body deflection on the hydrodynamic 
%           derivatives is not modeled here.
%        2) The signs of Nv and Yr are flipped from Knut's eqns due to
%           a suspected reference frame flip in Landweber et. al.
%
  if ( exist('LinFlg') == 0 )  LinFlg = 0; end
  if LinFlg == 0
%
% Units and Fundamental Constants:
  in2m = .0254;             % Inches to meters.
  d2r  = pi/180;            % Degrees to radians
  r2d  = 180/pi;            % Radians to degrees
  rho  = 1028;              % Density of sea water at 0 Deg C; kg/m^3
  s2kg = 14.59390;          % Slugs to kg
  ft2m = .3048;             % Feet to meters
  sft2kgm2 = 1.355818;      % Slug-ft^2 to kg-m^2
  lbf2n    = 4.448;         % Pounds force to Newtons
  kph2mps  = 1852/3600;     % Knots per hour to meters per sec
  ftlbf2nm = 1.3558179;     % Foot-pounds force to Newton-meters.
  oi2nm    = .0070615516;   % Ounce-inches to Newton-meters.
  m2c= 200*12/in2m;         % Meters-to-counts
  c2m= 1/m2c;               % Counts-to-meters
%
  caseName = '-1Deg';       % Plot name suffix
  caseName = 'ClosedLoop';  % Plot name suffix
%
% Altex/Dorado Specific Constants:
  vel = 3.0*kph2mps;        % speed in m/s.  
  Tp  = 52;                 % Thrust in Newtons
  mass= 795;                % Mass; kg
  len = 4.19;               % Length; meters
  dia = 21*in2m;            % Diameter; meters
  xG  = 0;                  % Longitudinal CG offset, meters
  Ts = .1;                  % Control System sampling rate, sec.    
  xPcp= -3*in2m;            % Distance from pivot to ring CP in tailcone coords
  StrutArea  = 0.0299;      % Odyssey fin area; m^2
  LinCtrlArea = StrutArea*4;% Equivalent rudder/elevator area (m^2). 
  TotalArea = .47*pi*5*in2m + StrutArea*4;  % Ring + 4 struts;m^2. 
                                            % For drag calc in the Surge Eqn.
  Ar  = 4.26;               % Aspect ratio
  Cdc = 0.723;              % Crossflow Cd
  Cd0 = 0.0065;             % Cd from NACA 0015 section; PNA p. 503
  Ef  = 0.90;               % "Oswald" efficiency factor; PNA p. 503
  dCl = 3.392;              % Lift coefficient slope 
%
% Control System:
  Kph  = .7;                % Unitless
  Krh  = 2;                 % Sec; w/o integrator
  Kvh  = 0;                 % rad/(meter/sec)
  MaxRate = .2 * Krh;       % Rad/sec
  Kwp = .0005;              % Rad/sec/meter; should be > 0; for Prop. Nav.
  Kwp = .05;                % Rad/meter; should be > 0.
  Kwp = 0;                  % Rad/meter; should be > 0.
  Kiwp= .003;               % Rad/sec/meter; should be > 0. For integrator ver.
  Kiwp= 0;                  % Rad/sec/meter; should be > 0. For integrator ver.
%
% Open loop rudder commands:
  deltaR = 1*d2r;
  deltaR = 10*d2r;
%
% Sim Parameters:
  tStop = 2*60;              % Stop time, sec
  Npts  = tStop/Ts;               % Number of points; should be tStop/Ts
% Set up time vector:
  tsim = [0:Npts]'*tStop/Npts;
%
% Computed Constants: Vehicle Dynamics.
  xR  = -len*.45;           % Distance from CG to rudder; m
  xP  = xR - xPcp;          % Distance from CG to pivot point.
  Izz = mass/5*((len/2)^2+(dia/2)^2); %Ellipsoid; Greenwood p501. kg-m^2
  Ixx = 2*mass/5*(dia/2)^2; % Greenwood p501. Assume homogenous dist. kg-m^2
  Nr  = -.045*mass*len;     % Deriv of N wrto r; kg-m (*m/sec^2=Nm), L&J
  Nv  = -.795*mass;         % Deriv of N wrto v; kg (*m^2/sec^2=Nm), L&J
  Yr  = .064*mass;          % Deriv of Y wrto r; kg (*m/sec^2=N), L&J
  Yv  = -.5*mass/len;       % Deriv of Y wrto v; kg/m   (*m^2/sec^2=N), L&J
  Yvdot = -.8*mass;         % Deriv of Y wrto v-dot; (Added Mass) kg; WAG
  Nrdot = -.8*Izz;          % Deriv of N wrto r-dot; (Added Iner) kg-m^2; WAG
  Yrdot = 0;                % Deriv of Y wrto r-dot; (Coupled) kg-m; WAG
  Nvdot = 0;                % Deriv of N wrto v-dot; (Coupled) kg-m; WAG
  Xudot = -.1*mass;         % Deriv of X wrto u-dot (Added Mass); kg; WAG
  Xudot = -1.583e-3*rho/2*len^3;% Deriv of X wrto u-dot; kg; DSRV
  Xrv   = .003358*rho/2*len^3;  % Deriv of X wrto rv; kg; DSRV
  Xrr   = -1.81e-4*rho/2*len^4; % Deriv of X wrto rr; N-sec^2; DSRV
  Xuu   = -5.03e-3*rho/2*len^2; % Deriv of X wrto uu; kg/m; DSRV
  Xuu   = -.5*rho*pi*(dia/2)^2*.15;  % Knut's computation from Brooks et. al.
  Xuu   = .5*rho*LinCtrlArea*Cd0 - Tp/vel^2;  % See Notes, Eqn. 101.
  Xvv   = .0098*rho/2*len^2;    % Deriv of X wrto vv; kg/m; ODYSSEY
  Yvabsr = 0;               % Knut
  Yvabsv = -2.94*mass/len;  % Deriv of Y wrto v|v|; kg/m; Knut
  Nrabsv = 0;               % Knut
  Nvabsv = .344*mass;       % Deriv of N wrto v|v|; kg; Knut
  Nrabsr = -.0705*mass*len^2;% Deriv of N wrto r|r|; kg-m^2; Knut
%
  end %if LinFlg == 0.  
%
%%%%%%%%%%%%%%% Build the matrices%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Form the mass matrices:
  MSway  = [  mass-Yvdot,     mass*xG-Yrdot;
              mass*xG-Nvdot,        Izz-Nrdot];
  MSurge =    mass-Xudot;
%
% Form the AA matrices:
  YrudFixed = .5*rho*LinCtrlArea*vel*(Cd0+dCl);
  AASway  = [  Yv*vel-YrudFixed,          Yr*vel-mass*vel-xR*YrudFixed;
               Nv*vel-xR*YrudFixed,   Nr*vel-mass*xG*vel-xR^2*YrudFixed];
%
% AASwayIdeal has no rudder, just a pure turning moment
  AASwayIdeal = [  Yv*vel,      Yr*vel-mass*vel;
                   Nv*vel,   Nr*vel-mass*xG*vel];
  AASurge = (2*Xuu-rho*LinCtrlArea*Cd0)*vel;
%
% Form the control matrices.  The first term in Ydelta is from the rudder,
% and the second is the contribution from the vectored thrust.
  Ydelta = .5*rho*LinCtrlArea*vel^2*dCl;
  BBSway  = [    Ydelta + Tp;
              xR*Ydelta + xP*Tp];
  BBSurge = 1;
%
% The ideal system has no rudder, just a pure turning moment of xR*Yideal Nm.
  Yideal  = 75;
  BBSwayIdeal  = [  Yideal;
                  xR*Yideal];
%
% If Ideal System;
%  AASway = AASwayIdeal;
%  BBSway = BBSwayIdeal;
%
% First, compute the steady-state values.  Accelerations are zero.
  ySwaySS  = -AASway\BBSway*deltaR;
  betaCGSS = ySwaySS(1,1)/vel;
  Rturn    = abs(vel/ySwaySS(2,1));
  Ncontrol = ( xR*Ydelta + xP*Tp )*deltaR;
  NcontrolP= xPcp*Ydelta*deltaR;
  NrudderSS= Ncontrol - xR*YrudFixed*(ySwaySS(1,1)+ySwaySS(2,1)*xR);
  NpivotSS = NcontrolP-xPcp*YrudFixed*(ySwaySS(1,1)+ySwaySS(2,1)*xR);
  disp(' ')
  disp(' =================== OPEN LOOP =================')
  disp(' Steady-State Values of the OPEN LOOP Linear Model:')
  disp([' The rudder angle is ', num2str(deltaR*r2d), ' Deg.'])
  disp([' The CG drift angle is ', num2str(betaCGSS*r2d),' Deg.'])
  disp([' The turn rate is ', num2str(ySwaySS(2,1)*r2d),' Deg/Sec'])
  disp([' The turn diameter is ', num2str(2*Rturn),' Meters',...
        ' or ', num2str(2*Rturn/len), ' vehicle lengths.'])
  disp([' The control-torque component about the CG is ',...
        num2str(Ncontrol),' N-m'])
  disp([' The control-torque component about the pivot point is ',...
        num2str(NcontrolP),' N-m'])
  disp([' The total steady-state torque about the CG is ',...
        num2str(NrudderSS),' N-m'])
  disp([' The total steady-state torque about the pivot point is ',...
        num2str(NpivotSS),' N-m'])
%
% Form state-space representations:
  ASway = MSway\AASway;
  BSway = MSway\BBSway;
  CSway = eye(2);
  DSway = [0;0];
  SwaySys  = ss(ASway, BSway, CSway, DSway);
  SurgeSys = ss(MSurge\AASurge,  MSurge\BBSurge, 1, 0);
%
% Check the eigenvalues:
  disp(' The OPEN-LOOP eigenvalues are: ')
  evals = eig(ssdata(SwaySys))
  disp(' The natural frequency, damping ratio, and time constant are:')
  disp ' f_n , damping ratio, time constant '
  [abs(evals)/2/pi -real(evals)./abs(evals) -1./real(evals)]
%
% OK, lets put this baby in a loop and see where the eigenvalues go as
% a function of control surface area:
%
  for i=1:Npts,
    Area(i) = LinCtrlArea*i/Npts*2;
    YrudFixed1  = .5*rho*Area(i)*vel*(Cd0+dCl);
    AASway1  = [  Yv*vel-YrudFixed1,          Yr*vel-mass*vel-xR*YrudFixed1;
                  Nv*vel-xR*YrudFixed1,   Nr*vel-mass*xG*vel-xR^2*YrudFixed1];
    poles(i,:) = eig(MSway\AASway1)';
%
%   Form the control matrices.  The first term in Ydelta is from the rudder,
%   and the second is the contribution from the vectored thrust.
%
    Ydelta1 = .5*rho*LinCtrlArea*vel^2*dCl;
    BBSway1 = [    Ydelta1 + Tp;
                   xR*Ydelta1 + xP*Tp];
%
    ySwaySS1     = -AASway1\BBSway1;
    betaCGSS1    = ySwaySS1(1,1)/vel;
    Rturn1       = abs(vel/ySwaySS1(2,1));
    Ncontrol1    = ( xR*Ydelta1 + xP*Tp );
    NrudderSS1(i)= Ncontrol1 - xR*YrudFixed1*(ySwaySS1(1,1)+ySwaySS1(2,1)*xR);
  end
  Area = Area';
% NrudderSS1 is the deltaR to Nm about the CG ss value.
%
% First, form the full open loop Sway system including the linearized 
% kinematics.  The second column of BSwayS is the cross current disturbance.
  ASwayS =  [ASway    0*eye(2);
             eye(2) [0   vel;
                     0    0]];
  BSwayS =  [BSway  [0 0]';
               0      1   ;
               0      0   ];
  CSwayS =  eye(4);
  DSwayS =  [ 0 0;
              0 0;
              0 0;
              0 0];
if 0
%
% Remove the cross current input
  BSwayS =  [BSway;
               0  ; 
               0   ];
  DSwayS =  [ 0;
              0;
              0;
              0];
end

  SwaySysOl = ss(ASwayS, BSwayS, CSwayS, DSwayS);

  disp(' The full order (w/ kinematics) open-loop sway system e-values are:')
  evals = eig(ssdata(SwaySysOl))
%
% Now, discretize the system:
  ssold = c2d(SwaySysOl,Ts);
  disp(' The eigenvalues of the discretized system are:')
  zrootinfo(ssdata(ssold),Ts)
%
% Append the accumulator state.  See notes, p23, Eqn 130.
  [Phi Gamma Cd Dd ] = ssdata(ssold);
  PhiA   = [    Phi       [0 0 0 0]' ;
              0 0 1 0         1     ];
  GammaA = [ Gamma(:,1)'  0 ]';         %This is the rudder input
  CdA    = eye(5);
  DdA    = [0 0 0 0 0]';
  ssoldi = ss( PhiA, GammaA, CdA, DdA, Ts );
  disp(' ')
  disp(' =================== CLOSED LOOP =================')
  disp(' ')
%
% Form the closed loop system.  The minus sign in the feedback equation
% below is due to the SNAME definition of positive deltaR.  See notes, 
% Eqn. 129.
%
  K = [-Kvh  -Krh  -Kph*(Kwp+Kiwp*Ts)  -Kph -Kph*Kiwp*Ts];
  PhiACl = PhiA - GammaA*K;                               %SNAME; see Eqn. 129
  sscl   = ss( PhiACl, -GammaA*Kph, [CdA;-K], [DdA;0],Ts);
  ssloop = series(ssoldi,ss(0,0*K,0,K,Ts));
  zrootinfo(PhiACl, Ts)
%  [Gm Pm] = margin(ssloop);
%  disp(sprintf(' Gain  Margin = %-15f dB', Gm))
%  disp(sprintf(' Phase Margin = %-15f Degrees', Pm))

%
% Open Loop Debugging:
%
  if 0        % ---------- 1 => Open Loop
%
% Simulate an open loop turn, given the input deltaR;
% [ySway, t] = step(SwaySys,tsim);
%  ySway     = dstep(Phi,    Gamma,  Cd,  Dd,    1, Npts+1);
%  ySway     = dstep(PhiA,   GammaA, CdA, DdA,   1, Npts+1);
   ySway     = dstep(PhiA,   GammaA, [CdA;-K], [DdA;0],   1, Npts+1);
%
% Scale for rudder deflection.  The input to the open loop system ssoldi
% is the rudder angle.
  ySway = ySway*deltaR;
  deltaRlin = ySway(:,6) - Kph*Heading;
  deltaR1 = deltaR;        %Sets up the beta and AofA computations
%
  else        % ----------  % Closed Loop Analysis
%
  disp(' Generating closed loop linear step response.')
%
% Simulate  closed loop response to a heading command.  After applying the
% feedback, the input to the closed loop system is the heading angle.  The
% 6th output appended below is deltaR.
%
% ySway = dstep(PhiACl, -Kph*GammaA*Heading, [CdA;-K], [DdA;0], 1, Npts+1);
% deltaRlin = ySway(:,6) - Kph*Heading;
%
% OK.  If we assume that the Sway/Yaw system is linearized about the WAYPOINT
% frame, then the bearing is NEVER an input to the linear system.  The 
% negative of the bearing is an initial condition, and the total bearing 
% is the sum of the linearized version plus the command.
%
  ySway = initial(sscl,[0;0;0;-Heading;0]',tsim);
  ySway(:,4) = ySway(:,4) + Heading; % Add it back to get heading wrto N frame.
  deltaRlin = ySway(:,6);
  deltaR1 = deltaRlin;     %Sets up the beta and AofA computations
%
  end         % ---------- Open v.s. Closed loop debugging
%
%
% Compute drift angles at CG and stern from the linear model.  
% See notes, Eqn.'s 8 & 9. 
%
  beta  = ySway(:,1)/vel;
  betaR = (ySway(:,1)+xR*ySway(:,2))/vel;
  AofA  = (deltaR1-betaR);
%
% Compute turning moment due to rudder.  See notes, Eqn. 32.  Also
% compute the torque about the tailcone pivot point.
%
  FYR      = Ydelta*deltaR1 - YrudFixed*(ySway(:,1)+ySway(:,2)*xR);
  Nrudder  = xR*FYR;
  Nturn    = Nrudder + xP*Tp*deltaR1;
  Npivot   = xPcp*(Ydelta*deltaR1 - YrudFixed*(ySway(:,1)+ySway(:,2)*xR));
