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