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