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