function [ lin_f16 ] = getLinF16( xequil,uequil,printOn )
%Given equilibrium trim and controls, returns a linearized state space
% model of the F-16.
%
%   Function Call:
%       lin_f16 = getLinF16( xequil,uequil)
%       lin_f16 = getLinF16( xequil,uequil,printOn )
%
%   Inputs:
%       xequil  -   Equilibrium states (13x1)
%       uequil  -   Equilibrium control (4x1)
%       printOn -   If true, prints intermediate data
%
%   Outputs:
%       lin_f16 -   labeled state space model of f16
%                   (13 state, 4 control, 10 output)
%
%   x_f16 states:
%       x_f16(1)  = air speed, VT                           (ft/s)
%       x_f16(2)  = angle of attack, alpha                  (rad)
%       x_f16(3)  = angle of sideslip, beta                 (rad)
%       x_f16(4)  = roll angle, phi                         (rad)
%       x_f16(5)  = pitch angle, theta                      (rad)
%       x_f16(6)  = yaw angle, psi                          (rad)
%       x_f16(7)  = roll rate, P                            (rad/s)
%       x_f16(8)  = pitch rate, Q                           (rad/s)
%       x_f16(9)  = yaw rate, R                             (rad/s)
%       x_f16(10) = northward horizontal displacement, pn   (ft)
%       x_f16(11) = eastward horizontal displacement, pe    (ft)
%       x_f16(12) = altitude, h                             (ft)
%       x_f16(13) = engine thrust dynamics lag state, pow   (lbs)
%
%   x_f16 controls:
%       u(1) = throttle                                     (0 to 1)
%       u(2) = elevator                                     (rad?)
%       u(3) = aileron                                      (rad?)
%       u(4) = rudder                                       (rad?)
%
% <a href="https://github.com/pheidlauf/AeroBenchVV">AeroBenchVV</a>
% Copyright: GNU General Public License 2017
%
% See also: TRIMMERFUN, JACOBFUN

if(nargin==0)
    printOn = true;
    % SET THESE VALUES MANUALLY
    hg = 0;         % Altitude guess (ft msl)
    Vtg = 502;      % Velocity guess (ft/sec)
    phig = 0;       % Roll angle from horizontal guess (deg)
    thetag = 0;     % Pitch angle guess (deg)
    xguess = [Vtg 0 0 phig thetag 0 0 0 0 0 0 hg 0];

    % u = [throttle elevator aileron rudder]
    uguess = [.2 0 0 0];

    % Orientation for Linearization
    % 1:    Wings Level (gamma = 0)
    % 2:    Wings Level (gamma <> 0)
    % 3:    Constant Altitude Turn
    % 4:    Steady Pull Up
    orient = 4;
    inputs = [xguess(1), xguess(12), 0, 0, 0];

    [xequil,uequil] = trimmerFun(xguess,uguess,orient,inputs,printOn);
end
if(nargin==2)
    printOn=true;
end;

[A, B, C, D] = jacobFun(xequil, uequil, printOn);

% y = [ Az q alpha theta Vt Ay p r beta phi ]T
C([2:4 7:10],:) = deg2rad(C([2:4 7:10],:));
D([2:4 7:10],:) = deg2rad(D([2:4 7:10],:));

% Build Default Linear SS Model
lin_f16 = ss(A,B,C,D);
lin_f16.stateName = {'Vt', 'alpha', 'beta', ...
    'phi','theta','psi', ...
    'p', 'q', 'r',....
    'pn','pe','alt',...
    'pow'};
lin_f16.stateUnit = {'ft/s', 'rad', 'rad',...
    'rad','rad','rad',...
    'rad','rad','rad',...
    'ft', 'ft', 'ft','lbs'};
lin_f16.inputName = {'Throttle','Elevator','Aileron','Rudder'};
lin_f16.inputUnit = {'percent','rad','rad','rad'};
lin_f16.outputName = {'Az', 'q', 'alpha', 'theta', 'Vt', 'Ay',...
    'p', 'r', 'beta', 'phi'};
lin_f16.outputUnit = {'g''s', 'rad/s', 'rad', 'rad', 'ft/s',...
    'g''s', 'rad/s', 'rad/s', 'rad', 'rad'};
lin_f16.name = 'Linearized F-16 SS Model';

if(printOn)
    disp('Linearized F-16 SS Model')
    lin_f16
end

end
------------------------------------------------------------
Running trimmerFun.m

Throttle (percent):            0.13855
Elevator (deg):                -0.758238
Ailerons (deg):                0
Rudder (deg):                  0
Angle of Attack (deg):         2.12147
Sideslip Angle (deg):          0
Pitch Angle (deg):             2.12147
Bank Angle (deg):              0
Normal Acceleration (g):       -0.998384
Lateral Acceleration (g):      0
Dynamic Pressure (psf):        299.507
Mach Number:                   0.449531

Initial Cost Function:         11.2105
Final Cost Function:           2.7691e-10
------------------------------------------------------------
Running jacobFun.m
Linearized F-16 SS Model

lin_f16 =
 
  A = 
                  Vt       alpha        beta         phi       theta
   Vt       -0.01932       8.816  -8.025e-06  -2.975e-06      -32.17
   alpha  -0.0002538      -1.016    3.28e-06    -1.6e-06  -1.186e-05
   beta            0           0      -0.322     0.06404           0
   phi             0           0           0           0           0
   theta           0           0           0           0           0
   psi             0           0           0           0           0
   p               0           0      -30.67           0           0
   q       3.468e-13      0.8223           0           0           0
   r               0           0        8.54           0           0
   pn              1  -9.211e-06           0  -1.717e-06  -7.676e-06
   pe              0           0         502      -18.58           0
   alt    -7.077e-16        -502           0    4.64e-06         502
   pow             0           0           0           0           0
 
                 psi           p           q           r          pn
   Vt              0           0     -0.5785           0           0
   alpha           0           0      0.9051           0           0
   beta            0      0.0365           0     -0.9917           0
   phi             0           1           0     0.03704           0
   theta           0           0           1           0           0
   psi             0           0           0       1.001           0
   p               0      -3.678   0.0002627      0.6652           0
   q               0  -8.795e-06      -1.077   -0.002858           0
   r               0    -0.02554    0.002539     -0.4764           0
   pn              0           0           0           0           0
   pe            502           0           0           0           0
   alt             0           0           0           0           0
   pow             0           0           0           0           0
 
                  pe         alt         pow
   Vt              0   1.906e-05      0.4024
   alpha           0   1.864e-06   -2.97e-05
   beta            0           0           0
   phi             0           0           0
   theta           0           0           0
   psi             0           0           0
   p               0           0           0
   q               0  -2.521e-15           0
   r               0           0           0
   pn              0           0           0
   pe              0           0           0
   alt             0           0           0
   pow             0           0          -1
 
  B = 
           Throttle   Elevator    Aileron     Rudder
   Vt             0     0.1735          0          0
   alpha          0   -0.00215          0          0
   beta           0          0  0.0002951  0.0008056
   phi            0          0          0          0
   theta          0          0          0          0
   psi            0          0          0          0
   p              0          0    -0.7333     0.1315
   q              0    -0.1755          0          0
   r              0          0   -0.03186   -0.06202
   pn             0          0          0          0
   pe             0          0          0          0
   alt            0          0          0          0
   pow        64.94          0          0          0
 
  C = 
                  Vt       alpha        beta         phi       theta
   Az       0.003978        16.2  -4.855e-08           0           0
   q               0           0           0           0           0
   alpha           0           1           0           0           0
   theta           0           0           0           0           1
   Vt              1           0           0           0           0
   Ay              0           0       -5.02           0           0
   p               0           0           0           0           0
   r               0           0           0           0           0
   beta            0           0           1           0           0
   phi             0           0           0           1           0
 
                 psi           p           q           r          pn
   Az              0  -4.096e-08       0.978   -0.001336           0
   q               0           0           1           0           0
   alpha           0           0           0           0           0
   theta           0           0           0           0           0
   Vt              0           0           0           0           0
   Ay              0   -0.008059           0      0.1192           0
   p               0           1           0           0           0
   r               0           0           0           1           0
   beta            0           0           0           0           0
   phi             0           0           0           0           0
 
                  pe         alt         pow
   Az              0  -2.906e-05           0
   q               0           0           0
   alpha           0           0           0
   theta           0           0           0
   Vt              0           0           0
   Ay              0           0           0
   p               0           0           0
   r               0           0           0
   beta            0           0           0
   phi             0           0           0
 
  D = 
          Throttle  Elevator   Aileron    Rudder
   Az            0  -0.04848         0         0
   q             0         0         0         0
   alpha         0         0         0         0
   theta         0         0         0         0
   Vt            0         0         0         0
   Ay            0         0    0.0046   0.01256
   p             0         0         0         0
   r             0         0         0         0
   beta          0         0         0         0
   phi           0         0         0         0
 
Name: Linearized F-16 SS Model
Continuous-time state-space model.


ans =
 
  A = 
                  Vt       alpha        beta         phi       theta
   Vt       -0.01932       8.816  -8.025e-06  -2.975e-06      -32.17
   alpha  -0.0002538      -1.016    3.28e-06    -1.6e-06  -1.186e-05
   beta            0           0      -0.322     0.06404           0
   phi             0           0           0           0           0
   theta           0           0           0           0           0
   psi             0           0           0           0           0
   p               0           0      -30.67           0           0
   q       3.468e-13      0.8223           0           0           0
   r               0           0        8.54           0           0
   pn              1  -9.211e-06           0  -1.717e-06  -7.676e-06
   pe              0           0         502      -18.58           0
   alt    -7.077e-16        -502           0    4.64e-06         502
   pow             0           0           0           0           0
 
                 psi           p           q           r          pn
   Vt              0           0     -0.5785           0           0
   alpha           0           0      0.9051           0           0
   beta            0      0.0365           0     -0.9917           0
   phi             0           1           0     0.03704           0
   theta           0           0           1           0           0
   psi             0           0           0       1.001           0
   p               0      -3.678   0.0002627      0.6652           0
   q               0  -8.795e-06      -1.077   -0.002858           0
   r               0    -0.02554    0.002539     -0.4764           0
   pn              0           0           0           0           0
   pe            502           0           0           0           0
   alt             0           0           0           0           0
   pow             0           0           0           0           0
 
                  pe         alt         pow
   Vt              0   1.906e-05      0.4024
   alpha           0   1.864e-06   -2.97e-05
   beta            0           0           0
   phi             0           0           0
   theta           0           0           0
   psi             0           0           0
   p               0           0           0
   q               0  -2.521e-15           0
   r               0           0           0
   pn              0           0           0
   pe              0           0           0
   alt             0           0           0
   pow             0           0          -1
 
  B = 
           Throttle   Elevator    Aileron     Rudder
   Vt             0     0.1735          0          0
   alpha          0   -0.00215          0          0
   beta           0          0  0.0002951  0.0008056
   phi            0          0          0          0
   theta          0          0          0          0
   psi            0          0          0          0
   p              0          0    -0.7333     0.1315
   q              0    -0.1755          0          0
   r              0          0   -0.03186   -0.06202
   pn             0          0          0          0
   pe             0          0          0          0
   alt            0          0          0          0
   pow        64.94          0          0          0
 
  C = 
                  Vt       alpha        beta         phi       theta
   Az       0.003978        16.2  -4.855e-08           0           0
   q               0           0           0           0           0
   alpha           0           1           0           0           0
   theta           0           0           0           0           1
   Vt              1           0           0           0           0
   Ay              0           0       -5.02           0           0
   p               0           0           0           0           0
   r               0           0           0           0           0
   beta            0           0           1           0           0
   phi             0           0           0           1           0
 
                 psi           p           q           r          pn
   Az              0  -4.096e-08       0.978   -0.001336           0
   q               0           0           1           0           0
   alpha           0           0           0           0           0
   theta           0           0           0           0           0
   Vt              0           0           0           0           0
   Ay              0   -0.008059           0      0.1192           0
   p               0           1           0           0           0
   r               0           0           0           1           0
   beta            0           0           0           0           0
   phi             0           0           0           0           0
 
                  pe         alt         pow
   Az              0  -2.906e-05           0
   q               0           0           0
   alpha           0           0           0
   theta           0           0           0
   Vt              0           0           0
   Ay              0           0           0
   p               0           0           0
   r               0           0           0
   beta            0           0           0
   phi             0           0           0
 
  D = 
          Throttle  Elevator   Aileron    Rudder
   Az            0  -0.04848         0         0
   q             0         0         0         0
   alpha         0         0         0         0
   theta         0         0         0         0
   Vt            0         0         0         0
   Ay            0         0    0.0046   0.01256
   p             0         0         0         0
   r             0         0         0         0
   beta          0         0         0         0
   phi           0         0         0         0
 
Name: Linearized F-16 SS Model
Continuous-time state-space model.