%--------------------------------------------------------------------------
%
% 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:  You must first execute "setupAltex.m" and "compute.m" to 
%           generate the required constants.
%
% 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.
%
%
%%%%%%%%%%%%%%% Build the matrices%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% A few constants specific to linearization ONLY:
  phiBar = 0;                %Steady state roll angle; for 12th order model
%
% Form the generalized mass matrices:
%
  MSway  = [  mass-Yvdot,     mass*xG-Yrdot;
              mass*xG-Nvdot,        Izz-Nrdot];
%
  MSurge =    mass-Xudot;
%
  MHeave = [GenMass(3,3) GenMass(3,5);
            GenMass(5,3) GenMass(5,5)];
%
% Compute some intermediate terms:
%
  YrudFixed = .5*rho*LinCtrlArea*(Cd0+dCl);
  Ydelta    = .5*rho*LinCtrlArea*dCl;
  Ydrag     = .5*rho*LinCtrlArea*Cd0;
%
% Form the AA matrices:
%
  AASway  =   [ (Yv-YrudFixed)*vel          (Yr-mass-xR*YrudFixed)*vel;
                (Nv-xR*YrudFixed)*vel, (Nr-mass*xG-xR^2*YrudFixed)*vel];

  AAHeave =   [ (Yv-YrudFixed)*vel         -(Yr-mass-xR*YrudFixed)*vel;
               -(Nv-xR*YrudFixed)*vel, (Nr-mass*xG-xR^2*YrudFixed)*vel];
  AAHeave12 = [         0                             0;
                        0                    weight*(zB - zG)         ];
%
% 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;
%
  BBSway  = [    Ydelta*vel^2 + Tp;
              xR*Ydelta*vel^2 + xP*Tp];
%
  BBSurge = 1;
%
  BBHeave = [  -(Ydelta*vel^2 + Tp);
              xR*Ydelta*vel^2 + xP*Tp];
%
% 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*vel^2 + xP*Tp )*deltaR;
  NcontrolP= xPcp*Ydelta*vel^2*deltaR;
  NrudderSS= Ncontrol - xR*YrudFixed*vel*(ySwaySS(1,1)+ySwaySS(2,1)*xR);
  NpivotSS = NcontrolP-xPcp*YrudFixed*vel*(ySwaySS(1,1)+ySwaySS(2,1)*xR);
  disp(' ')
  disp(' =================== OPEN LOOP =================')
  disp(' Steady-State Values of the OPEN LOOP Sway/Yaw 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 Sway/Yaw eigenvalues:
%
  disp(' The Sway/Yaw 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);
%
% Now build the heave system:
%
  AHeave = [MHeave\[ AAHeave  AAHeave12 ];
            eye(2)            [0   -vel;
                               0     0 ]];
%
  BHeave = [MHeave\BBHeave;
                  0;
                  0       ];
%
  HeaveSysOl = ss(AHeave, BHeave, eye(4), [0 0 0 0]');
%
  disp(' The full order (w/ kinematics) open-loop sway and heave system e-values are:')
  [ eig(ssdata(SwaySysOl))   eig(ssdata(HeaveSysOl)) ]
%
% 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(' =================== SWAY/YAW 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*vel^2*deltaR1 - YrudFixed*vel*(ySway(:,1)+ySway(:,2)*xR);
  Nrudder  = xR*FYR;
  Nturn    = Nrudder + xP*Tp*deltaR1;
  Npivot   = xPcp*(Ydelta*vel^2*deltaR1 - ...
                   YrudFixed*vel*(ySway(:,1)+ySway(:,2)*xR));
   

  return





%
% Build the linearized 12th order model.  See Franz Hover's reports, and
% my notes, p. 24.
%
% AA11 is the "A" matrix, not including kinematics, before premultiplication
% by the inverse of the generalized mass matrix.  It doesn't include the 
% spring restoring terms, which are functions of the states, not the 
% generalized speeds.  These terms form the AA12 matrix, which relate the 
% dependance of the generalized speed derivatives on the generalized states.
% The spring restoring term affects pitch only, assuming that yG = yB (= 0), 
% and so AA12 is all zeros except for these elements.
%
  AA11(1,1) = 2*Xuu*vel;
  AA11(2,2) = (Yv - YrudFixed)*vel;
  AA11(2,6) = (Yr - mass - xR*YrudFixed)*vel;
  AA11(3,3) = (Yv - YrudFixed)*vel;
  AA11(3,5) = (-Yr + mass + xR*YrudFixed)*vel;
  AA11(4,5) = mass*yG*vel;
  AA11(4,6) = mass*zG*vel;
  AA11(5,3) = (-Nv + xR*YrudFixed )*vel;
  AA11(5,5) = ( Nr - mass*xG - xR^2*YrudFixed )*vel;
  AA11(6,2) = ( Nv - xR*YrudFixed )*vel;
  AA11(6,6) = ( Nr - mass*xG - xR^2*YrudFixed )*vel;
%
  AA12(4,4) = (zB - zG)*weight;             %FRANZ! What happened?
  AA12(5,5) = (zB - zG)*weight;
  AA12(6,4) = (xG - xB)*weight;             %FRANZ! What happened?
  AA12(6,5) = (yG - yB)*weight;
  AA12(6,6) = 0;                            %Correctly dimension it.
%
% The A21 and A22 matrices are the linearized kinematics. A21 is the identity
% when the steady-state roll angle is zero. A22 is all zeros except for the 
% coupling of yaw to inertial y position, and pitch to inertial z position.
%
  TPR = [ 1         0              0;       %D.C from the Roll frame to
          0    cos(phiBar) -sin(phiBar);    %  the Pitch frame.
          0    sin(phiBar)  cos(phiBar)];
%
  A21(1:3,1:3) = TPR;
  A21(4:6,4:6) = TPR;
%
  A22(2,6) = vel;            
  A22(3,5) = -vel;            
  A22(6,6) = 0;                             %Correctly dimension it.
%
% Form the control matrices.  The first term in Ydelta is from the rudder,
% and the second is the contribution from the vectored thrust.  
%
  BB = [ -2*Ydrag*vel           -2*Ydrag*vel;
        Ydelta*vel^2+Tp                0;
              0                 -Ydelta*vel^2-Tp;
              0                        0;
              0               xR*Ydelta*vel^2+xP*Tp;
        xR*Ydelta*vel^2+xP*Tp          0          ];
%
% Form the open-loop continuous-time state space system:
%
  Adyn = [GenMass\AA11 GenMass\AA12;
              A21           A22    ];
  Bdyn = [GenMass\BB;
             0*BB   ];
  Cdyn = eye(12);
  Ddyn = 0*Bdyn;
%
  dynSysOl = ss(Adyn, Bdyn, Cdyn, Ddyn);

  disp(' ')
  disp(' =================== Entire System Open Loop  =================')
  disp(' ')
%
% Check the eigenvalues:
  disp(' The Entire System OPEN-LOOP eigenvalues are: ')
  evals = eig(ssdata(dynSysOl))
  disp(' The natural frequency, damping ratio, and time constant are:')
  disp ' f_n , damping ratio '
  sz = size(evals);
  j=1;
  for i=1:sz(1,1)
   if abs(evals(i,1)) > 1/3600          %Faster than tau = 1 hrwhen zeta = 1
     evals1(j,1) = evals(i,1);
     j = j+1;
   end      
  end
  [abs(evals1)/2/pi -real(evals1)./abs(evals1) ]
  disp([' There are ' num2str(i-j+1) ' evalues at zero.'])
%
% Find the time constants. 
%
if 1
  jj=1;
  for i=1:j-1
   if real(evals1(i,1)) < -1/3600        %Always Faster than tau = 1 hr
     evals2(jj,1) = evals1(i,1);
     jj = jj+1;
   end      
  end
  jj
  disp(' Time constants are:')
  [-1./real(evals2)]
end


