Contents

% 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