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