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