## Contents

- Initialize Starting Variables
- Build Linearized Model
- Decouple Linearized F-16 Model: Isolate Lateral States & Actuators
- Select State & Control Weights & Penalties
- Calculate Lateral Short Period LQR Gains
- Build State Space Models of Lateral 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 lateral 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('Lateral F-16 Controller for stability p 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 = 2;
    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

    ------------------------------------------------------------
    Lateral F-16 Controller for stability p tracking
    ------------------------------------------------------------
    MANUAL INPUTS:
    trimmerFun Inputs:

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

    Orientation Used: Wings Level (gamme <> 0)

    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',[],...
            'Vt alpha beta phi theta psi p q r pn pe alt pow');
        printmat(uequil','Control Equilibrium',[],...
            '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 =
                            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 =
                      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 Lateral States & Actuators

States: beta, phi, p, r

    A_lat = lin_f16.a([3 7 9], [3 7 9]);

    % Inputs:   aileron, rudder
    B_lat = lin_f16.b([3 7 9], [3 4]);

    % Outputs:  beta, phi, p, r, ps, Ny+r
    C_lat = [lin_f16.c([9 7 8],[3 7 9]);
        (lin_f16.c(7,[3 7 9]) + lin_f16.c(8,[3 7 9])*xequil(2));
        lin_f16.c(6,[3 7 9]) + [0 0 1]];
    D_lat = [zeros(4,2);
        lin_f16.d(5,[3 4])];

    Atilde = [A_lat zeros(3,2);           % beta; phi; p; r;
            [C_lat(4:5,:) zeros(2)]];     % ps; Ny+r
    Btilde = [B_lat; D_lat(4:5,:)];

## Select State & Control Weights & Penalties

Set LQR weights Q: Penalty on State Error

    q_beta = 0;
    q_p = 0;
    q_r = 0;
    q_ps_i = 1200;
    q_Ny_r_i = 3000;
    Q = diag([q_beta q_p q_r q_ps_i q_Ny_r_i]);

    % R: Penalty on Control Effort
    r_aileron = 1;
    r_rudder = 1;
    R = diag([r_aileron r_rudder]);

## Calculate Lateral Short Period LQR Gains

    N = 0;
    K_lat = lqr(Atilde,Btilde,Q,R,N);

    % K:                beta   phi    p    r    ps_i    Ny
    %       aileron:
    %        rudder:
    printmat(K_lat,'LQR Gains','aileron rudder',...
            'beta p r int_e_ps int_e_Ny_r');

    LQR Gains =
                          beta            p            r     int_e_ps   int_e_Ny_r
          aileron     38.02751     -5.65497    -14.08804    -34.06416     -9.95406
           rudder     17.56400      1.58391    -41.43509      6.29550    -53.86016

## Build State Space Models of Lateral F-16

Uncontrolled SS model of lateral directional dynamics

    ss_lat=ss(A_lat,B_lat,...
        [C_lat; zeros(2,3)],[D_lat; eye(2)]);
    ss_lat.u={'aileron','rudder'};              % Controls: aileron, rudder
    ss_lat.StateName = {'beta','p','r'};
    ss_lat.y={'beta','p','r','ps','Ny_r','ail_out',...
        'rud_out'};   % Outputs: beta, phi, p, r, ps, ail_cmd, rud_cmd

    % Proportional Compensator (using beta, phi, r)
    ss_Kpx=ss(zeros(2,2),zeros(2,3), ...
        zeros(2,2), K_lat(:,1:3));          % Pass through proportional gains
    ss_Kpx.u={'beta', 'p', 'r'}; % Inputs:   beta, phi, r (bpr)
    ss_Kpx.y='Kp_x';                    % Outputs:  Controls for ail, rud

    controls=sumblk('%u = -Kp_x - Ki_x',{'aileron','rudder'});

    % Integral Compensator (using ps)
    ss_Kint_e=ss(zeros(2),eye(2),K_lat(:,end-1:end),0);   % Integral control on eps, beta
    ss_Kint_e.u={'e_ps','e_Ny_r'};         % Inputs:   2x1 p beta
    ss_Kint_e.StateName = {'int_e_ps','int_e_Ny_r'};
    ss_Kint_e.y='Ki_x';         % Outputs:  Integral controls

    % Negative error between p_cmd and stability p
    error=sumblk('%error = -%cmd + %x',...
        {'e_ps','e_Ny_r'},...
        {'ps_cmd','Ny_r_cmd'},...
        {'ps','Ny_r'});

    sys_cl_lat=connect(ss_lat,ss_Kpx,controls,ss_Kint_e,error,...
        {'ps_cmd','Ny_r_cmd'},...
        {'beta','p','r','ps','Ny_r','e_ps','e_Ny_r','ail_out',...
        'rud_out'});
    sys_cl_lat

    OLTF=connect(ss_lat,ss_Kpx,controls,-ss_Kint_e,...
        {'e_ps','e_Ny_r'},{'beta','p','r','ps','Ny_r','ail_out',...
        'rud_out'});

    sys_cl_lat =

      A =
                         beta           p           r    int_e_ps  int_e_Ny_r
       beta           -0.3374      0.0388     -0.9553    0.004836     0.04499
       p               -5.194        -7.8      -4.098      -25.07     -0.2225
       r                10.53     -0.1058      -3.394     -0.6736      -3.552
       int_e_ps             0           1     0.03888           0           0
       int_e_Ny_r      -4.876   -0.007024       1.116           0           0

      B =
                     ps_cmd  Ny_r_cmd
       beta               0         0
       p                  0         0
       r                  0         0
       int_e_ps          -1         0
       int_e_Ny_r         0        -1

      C =
                      beta           p           r    int_e_ps  int_e_Ny_r
       beta              1           0           0           0           0
       p                 0           1           0           0           0
       r                 0           0           1           0           0
       ps                0           1     0.03888           0           0
       Ny_r         -4.876   -0.007024       1.116           0           0
       e_ps              0           1     0.03888           0           0
       e_Ny_r       -4.876   -0.007024       1.116           0           0
       ail_out      -38.03       5.655       14.09       34.06       9.954
       rud_out      -17.57      -1.584       41.44      -6.295       53.86

      D =
                  ps_cmd  Ny_r_cmd
       beta            0         0
       p               0         0
       r               0         0
       ps              0         0
       Ny_r            0         0
       e_ps           -1         0
       e_Ny_r          0        -1
       ail_out         0         0
       rud_out         0         0

    Continuous-time state-space model.

## Compare Margins, Damping Ratios, and Pole Frequencies

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

    figure(2);
    margin(OLTF(5,2))

    % Output results to .mat file
    save('lateralCtrlData.mat','K_lat','sys_cl_lat');

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

    opt = stepDataOptions('StepAmplitude',1);
    figure(3);
    step(sys_cl_lat(:,:),10,opt)
    printmat(K_lat,'LQR Gains','aileron rudder',...
            'beta p r int_e_ps int_e_Ny_r');

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

     -1.08e+00 + 3.34e+00i     3.08e-01       3.51e+00          9.25e-01
     -1.08e+00 - 3.34e+00i     3.08e-01       3.51e+00          9.25e-01
     -3.96e+00 + 3.10e+00i     7.88e-01       5.03e+00          2.53e-01
     -3.96e+00 - 3.10e+00i     7.88e-01       5.03e+00          2.53e-01
     -1.45e+00                 1.00e+00       1.45e+00          6.88e-01

    LQR Gains =
                          beta            p            r     int_e_ps   int_e_Ny_r
          aileron     38.02751     -5.65497    -14.08804    -34.06416     -9.95406
           rudder     17.56400      1.58391    -41.43509      6.29550    -53.86016

## 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