## Contents

- Initialize Starting Variables
- Build Linearized Model
- Decouple Linearized F-16 Model: Isolate Longitudinal States & Actuators
- Select State & Control Weights & Penalties
- Calculate Longitudinal Short Period LQR Gains
- Build State Space Models of Longitudinal F-16
- Compare Margins, Damping Ratios, and Pole Frequencies
- Citations

    % Determines the gains for a decoupled F-16 Linear Quadratic Regulator
    % for the Stevens & Lewis mathematical model of the F-16 aircraft at a
    % specified trim point. The lateral and longitudinal modes of the F-16 are
    % decoupled. This script builds the longitudinal controller based on
    % specified weights on state error and control effort and trim point.
    %
    % <a href="https://github.com/pheidlauf/AeroBenchVV">AeroBenchVV</a>
    % Copyright: GNU General Public License 2017
    %
    % See also: BUILDLATERALLQRCTRL, CONTROLLEDF16

## Initialize Starting Variables

    close all; clear; clc;
    addpath(genpath('F16_Model')); % Add necessary sub folder to path

    disp('------------------------------------------------------------');
    disp('Longitudinal F-16 Controller for Nz tracking');
    disp('------------------------------------------------------------');
    disp('MANUAL INPUTS:');

    % SET THESE VALUES MANUALLY
    altg = 1000;    % Altitude guess (ft msl)
    Vtg = 502;      % Velocity guess (ft/sec)
    phig = 0;       % Roll angle from horizontal guess (deg)
    thetag = 0;     % Pitch angle guess (deg)
    % Note: If a gain-scheduled controller is desired, the above values would
    % be varied for each desired trim point.

    xguess = [Vtg 0 0 phig thetag 0 0 0 0 0 0 altg 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];
    printOn = true;

    if(printOn)
        disp('trimmerFun Inputs:');
        printmat(inputs,'inputs',[],'Vt h gamma psidot thetadot')
        fprintf('Orientation Used: ');
        switch orient
            case 1
                disp('Wings Level (gamma = 0)');
            case 2
                disp('Wings Level (gamme <> 0)');
            case 3
                disp('Constant Altitude Turn');
            case 4
                disp('Steady Pull Up');
            otherwise
                error('orient invalid');
        end
        printmat(xguess,'State Guess',[],...
            'Vt alpha beta phi theta psi p q r pn pe alt pow');
        printmat(uguess,'Control Guess',[],...
            'throttle elevator aileron rudder');
    end

    ------------------------------------------------------------
    Longitudinal F-16 Controller for Nz tracking
    ------------------------------------------------------------
    MANUAL INPUTS:
    trimmerFun Inputs:

    inputs =
                            Vt            h        gamma       psidot     thetadot
                     502.00000   1000.00000            0            0            0

    Orientation Used: Steady Pull Up

    State Guess =
                            Vt        alpha         beta          phi        theta
                     502.00000            0            0            0            0

                           psi            p            q            r           pn
                             0            0            0            0            0

                            pe          alt          pow
                             0   1000.00000            0

    Control Guess =
                      throttle     elevator      aileron       rudder
                       0.20000            0            0            0

## Build Linearized Model

Get Equilibrium Values

    [xequil,uequil] = trimmerFun(xguess,uguess,orient,inputs,printOn);

    if(printOn)
        printmat(xequil','State Equilibrium (ft, rads, etc.)',[],...
            'Vt alpha beta phi theta psi p q r pn pe alt pow');
        printmat(uequil','Control Equilibrium (% & degs)',[],...
            'throttle elevator aileron rudder');
    end

    % Get Linearized Model
    lin_f16 = getLinF16(xequil,uequil,printOn);

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

    Throttle (percent):            0.139462
    Elevator (deg):                -0.749578
    Ailerons (deg):                0
    Rudder (deg):                  0
    Angle of Attack (deg):         2.22738
    Sideslip Angle (deg):          0
    Pitch Angle (deg):             2.22738
    Bank Angle (deg):              0
    Normal Acceleration (g):       -0.998313
    Lateral Acceleration (g):      0
    Dynamic Pressure (psf):        290.886
    Mach Number:                   0.451119

    Initial Cost Function:         10.5418
    Final Cost Function:           2.87856e-10

    State Equilibrium (ft, rads, etc.) =
                            Vt        alpha         beta          phi        theta
                     502.00000      0.03888            0            0      0.03888

                           psi            p            q            r           pn
                             0            0            0            0            0

                            pe          alt          pow
                             0   1000.00000      9.05667

    Control Equilibrium (% & degs) =
                      throttle     elevator      aileron       rudder
                       0.13946     -0.74958            0            0

    ------------------------------------------------------------
    Running jacobFun.m
    Linearized F-16 SS Model

    lin_f16 =

      A =
                      Vt       alpha        beta         phi       theta
       Vt       -0.01855       7.645  -7.784e-06  -3.123e-06      -32.17
       alpha  -0.0002538     -0.9865   3.277e-06    -1.6e-06  -1.246e-05
       beta            0           0     -0.3127     0.06403           0
       phi             0           0           0           0           0
       theta           0           0           0           0           0
       psi             0           0           0           0           0
       p               0           0      -30.05           0           0
       q      -3.236e-13      0.7987           0           0           0
       r               0           0         8.3           0           0
       pn              1  -8.773e-06           0  -1.887e-06  -8.773e-06
       pe              0           0         502      -19.51           0
       alt             0        -502   7.105e-15   4.874e-06         502
       pow             0           0           0           0           0

                     psi           p           q           r          pn
       Vt              0           0     -0.6169           0           0
       alpha           0           0      0.9076           0           0
       beta            0     0.03841           0     -0.9918           0
       phi             0           1           0     0.03889           0
       theta           0           0           1           0           0
       psi             0           0           0       1.001           0
       p               0      -3.568   0.0002627      0.6547           0
       q               0  -8.795e-06      -1.047   -0.002858           0
       r               0    -0.02643    0.002539     -0.4627           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.95e-05      0.3921
       alpha           0   1.877e-06  -3.038e-05
       beta            0           0           0
       phi             0           0           0
       theta           0           0           0
       psi             0           0           0
       p               0           0           0
       q               0   2.368e-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.1661          0          0
       alpha          0  -0.002089          0          0
       beta           0          0  0.0002866  0.0007824
       phi            0          0          0          0
       theta          0          0          0          0
       psi            0          0          0          0
       p              0          0    -0.7125     0.1275
       q              0    -0.1705          0          0
       r              0          0   -0.03091   -0.06024
       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       15.73  -5.296e-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      -4.876           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.9523   -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.007024           0       0.116           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.926e-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.04707         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.004468    0.0122
       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.

## Decouple Linearized F-16 Model: Isolate Longitudinal States & Actuators

    A_long = lin_f16.a([2 8],[2 8]);        % States:   alpha, q
    B_long = lin_f16.b([2 8],2);            % Inputs:   elevator
    C_long = lin_f16.c([3 2 1],[2 8]);      % Outputs:  alpha; q; Nz
    D_long = lin_f16.d([3 2 1],2);          % Inputs:   elevator

    Atilde = [A_long zeros(2,1);            % States:   alpha; q;
        C_long(3,:) 0];                     %           Nz
    Btilde = [B_long;
        D_long(3,:)];                   % Inputs:   elevator

## Select State & Control Weights & Penalties

Set LQR weights Q: Penalty on State Error

    q_alpha = 1000;
    q_q = 0;
    q_Nz = 1500;

    % R: Penalty on Control Effort
    r_elevator = 1;

## Calculate Longitudinal Short Period LQR Gains

    K_long = lqr(Atilde,Btilde,diag([q_alpha q_q q_Nz]), r_elevator);
    printmat(K_long,'LQR Gains','elevator',...
        'alpha q int_e_Nz');

    LQR Gains =
                         alpha            q     int_e_Nz
         elevator   -156.88015    -31.03701    -38.72983

## Build State Space Models of Longitudinal F-16

My ss_asp (with elevator commanded as an output state)

    ss_plant=ss(A_long,B_long,...
        [C_long; 0 0],[D_long; deg2rad(1)]);       % Uncontrolled SS model
    ss_plant.StateName={'alpha','q'};
    ss_plant.u='elevator';                      % Inputs:   elevator
    ss_plant.y={'alpha','q','Nz','ele_out'};    % Outputs:  alpha, q, Nz, ele_out

    ss_Kx=ss(0,[0 0],0,K_long(1:2));     % Pass through gains for alpha, q
    ss_Kx.u={'alpha','q'};          % Inputs:   alpha, q
    ss_Kx.y='Kx';                   % Outputs:  Elevator control from alpha, q

    de_cmd=sumblk('elevator=-Kx-Kint_e_Nz');

    % Integral control on Nz error
    ss_Kint_Nz=ss(0,1,K_long(3),0);
    ss_Kint_Nz.u='e_Nz';                 % Inputs:   Nz error
    ss_Kint_Nz.y={'Kint_e_Nz'};          % Outputs:  K_i*int_e_Nz
    ss_Kint_Nz.StateName = 'int_e_Nz';

    error=sumblk('e_Nz=-Nz_cmd+Nz');

    sys_cl_long=connect(ss_plant,ss_Kx,de_cmd,ss_Kint_Nz,error,...
        'Nz_cmd',{'alpha','q','Nz','e_Nz','ele_out'});

    OLTF = connect(ss_plant,ss_Kx,de_cmd,-ss_Kint_Nz,...
        'e_Nz',{'alpha','q','Nz','ele_out'});
    sys_cl_long

    sys_cl_long =

      A =
                    alpha         q  int_e_Nz
       alpha       -1.314    0.8428  -0.08089
       q           -25.95    -6.338    -6.602
       int_e_Nz     8.345   -0.5087    -1.823

      B =
                 Nz_cmd
       alpha          0
       q              0
       int_e_Nz      -1

      C =
                   alpha         q  int_e_Nz
       alpha           1         0         0
       q               0         1         0
       Nz          8.345   -0.5087    -1.823
       e_Nz        8.345   -0.5087    -1.823
       ele_out     2.738    0.5417     0.676

      D =
                Nz_cmd
       alpha         0
       q             0
       Nz            0
       e_Nz         -1
       ele_out       0

    Continuous-time state-space model.

## Compare Margins, Damping Ratios, and Pole Frequencies

    figure(1);
    margin(OLTF(3,1))

    % View Damping Ratio & Frequency of Poles
    damp(eig(sys_cl_long.a))
    % [Wn,zeta] = damp(eig(sys_cl_long.a))
    % Note: Wn is the natural frequency (aim for 3 rad/s)
    %       zeta is the damping ratio   (aim for 0.707)

    figure(2);
    % step(sys_cl_sp)
    opt = stepDataOptions('StepAmplitude',1);
    step(sys_cl_long,3,opt)

    save('longitudinalCtrlData.mat','K_long','sys_cl_long');
    printmat(K_long,'LQR Gains','elevator',...
        'alpha q int_e_Nz');

             Pole              Damping       Frequency       Time Constant
                                           (rad/TimeUnit)     (TimeUnit)

     -2.09e+00 + 3.87e+00i     4.75e-01       4.40e+00          4.79e-01
     -2.09e+00 - 3.87e+00i     4.75e-01       4.40e+00          4.79e-01
     -5.30e+00                 1.00e+00       5.30e+00          1.89e-01

    LQR Gains =
                         alpha            q     int_e_Nz
         elevator   -156.88015    -31.03701    -38.72983

## Citations

Stevens, Brian L., Frank L. Lewis, and Eric N. Johnson. Aircraft control and simulation: dynamics, controls design, and autonomous systems. John Wiley & Sons, 2015.

Liebst, Bradley S., AFIT