Contents
function [u_ref,t_maneuver] = getAutopilotCommands(t, x_f16, xequil,... uequil, flightLimits, ctrlLimits, autopilot, resetTrue)
% Given the time, f16 state, and trim condition, returns desired Nz, ps, % Ny+r, and throttle settings. % % Function Call: % [u_ref] = getAutopilotCommands(t, x_f16, xequil, uequil, ... % flightLimits, ctrlLimits, autopilot, resetTrue) % % Inputs: % t - time of step (sec) % x_f16 - State vector (f16 state) (16x1) % xequil - trim conditions (13x1) % uequil - equilibrium control (4x1) % flightLimits - struct of flight limits % ctrlLimits - struct of control limits % autopilot - struct of autopilot settings: % simpleGCAS - "Roll & Pull" to level flight % basicSpeedControl - Proportional control on airspeed % steadyLevelFlightHold - PD control on pitch & roll % levelTurnControl - Pulls g's to stay level when banked % turnToHeading - Bank, G, unbank to heading % timeTriggeredControl - Program your own maneuvers f(t) % resetTrue - OPTIONAL: If 1, reset persistent variables % % Outputs: % u_ref(1) = Down force, Nz_ref (g's) % u_ref(2) = Stability roll rate, ps_ref (rad/sec) % u_ref(3) = Side force, Ny_r_ref (g's) % u_ref(4) = Throttle Setting (ft/sec) % t_maneuver(1) = Maneuver start time (sec) % t_maneuver(2) = Maneuver end time (sec) % t_maneuver(3:n) = Checkpoint time (sec) % % x_f16 States: % x_f16(1) = air speed, VT (ft/s) % x_f16(2) = angle of attack, alpha (rad) % x_f16(3) = angle of sideslip, beta (rad) % x_f16(4) = roll angle, phi (rad) % x_f16(5) = pitch angle, theta (rad) % x_f16(6) = yaw angle, psi (rad) % x_f16(7) = roll rate, P (rad/s) % x_f16(8) = pitch rate, Q (rad/s) % x_f16(9) = yaw rate, R (rad/s) % x_f16(10) = northward horizontal displacement, pn (ft) % x_f16(11) = eastward horizontal displacement, pe (ft) % x_f16(12) = altitude, h (ft) % x_f16(13) = engine thrust dynamics lag state, pow (lbs) % ---------------------------------------------------------- % x_f16(14) = Integral of Nz error, Nz_e_i (g's) % x_f16(15) = Integral of Ps_error, Ps_e_i (rad/sec) % x_f16(16) = Integral of Ny_error, Ny_e_i (g's) % % Notes: % This function is where you program the autopilot. Commanded % Nz, ps, Ny+r, and throttle settings can be hard coded, time % triggered, or state triggered. % % Persistent variables are used to track any states that are not % included in the F-16 state vector. These are needed for % integration, tracking time flags, maneuver states, etc. If you % modify this function to add your own, make sure to declare the % persistent variables and included them in the "reset" block. % % <a href="https://github.com/pheidlauf/AeroBenchVV">AeroBenchVV</a> % Copyright: GNU General Public License 2017 % % See also: GETDEFAULTSETTINGS, CONTROLLEDF16, PERSISTENT
Initialize Flags, Controls, Vectors
Run top level function if called without arguments
if(nargin==0) Main; return; end % If "resetTrue" not designated, set to false if(nargin < 8) resetTrue = 0; end % Declare persistent flags and variables persistent man_start man_complete man_check_1 man_check_2; persistent turnDirection; % Reset persistent flags if (re)starting simulation if(isempty(man_start) || t <= 0 || resetTrue) % Reset maneuver time flags if first func call or needed. man_start = -1; man_check_1 = -1; man_check_2 = -1; man_complete = -1; turnDirection = 0; end % Zero default commands Nz = 0; ps = 0; Ny_r = 0; throttle = 0; % Initialize t_maneuver vector to zeros t_maneuver = zeros(1,2);
flightLimits = altitudeMin: 0 altitudeMax: 10000 maneuverTime: 15 NzMax: 9 NzMin: -2 psMaxAccelDeg: 500 vMin: 300 vMax: 900 alphaMinDeg: -10 alphaMaxDeg: 45 betaMaxDeg: 30 ctrlLimits = ThrottleMax: 1 ThrottleMin: 0 ElevatorMaxDeg: 25 ElevatorMinDeg: -25 AileronMaxDeg: 21.500000000000000 AileronMinDeg: -21.500000000000000 RudderMaxDeg: 30 RudderMinDeg: -30 MaxBankDeg: 60 NzMax: 6 NzMin: -1 autopilot = title: 'Default Simulation' basicSpeedControl: 1 steadyLevelFlightHold: 0 levelTurnControl: 0 simpleGCAS: 0 turnToHeading: 0 timeTriggeredControl: 0 ------------------------------------------------------------ F-16 Decoupled LQR Controller for Nz, P_s, and Ny+r tracking ------------------------------------------------------------ Operator Inputs = Vt h gamma psidot thetadot 540.00000 3500.00000 0 0 0 Trim Orientation Selected: Steady Pull Up Inititial Conditions = Vt alpha beta phi theta 540.00000 0.03703 0 0.78540 -1.25664 psi p q r pn -0.78540 0 0 0 0 pe alt pow 0 3500.00000 9.00000 Control Guess = throttle elevator aileron rudder 0.20000 0 0 0 ------------------------------------------------------------ Equilibrium / Trim Conditions State Equilibrium = Vt alpha beta phi theta 540.00000 0.03441 0 0 0.03441 psi p q r pn 0 0 0 0 0 pe alt pow 0 3500.00000 10.35403 Control Equilibrium = throttle elevator aileron rudder 0.15944 -0.77048 0 0 ------------------------------------------------------------ Running jacobFun.m Linearized F-16 SS Model lin_f16 = A = Vt alpha beta phi theta Vt -0.01658 10.6 -8.377e-06 -2.766e-06 -32.17 alpha -0.0002195 -0.9855 3.055e-06 -1.488e-06 -1.025e-05 beta 0 0 -0.3124 0.05954 0 phi 0 0 0 0 0 theta 0 0 0 0 0 psi 0 0 0 0 0 p 0 0 -31.6 0 0 q 6.662e-13 0.858 0 0 0 r 0 0 8.904 0 0 pn 1 -6.607e-06 0 -1.592e-06 -6.607e-06 pe 0 0 540 -18.58 0 alt -6.579e-16 -540 0 4.64e-06 540 pow 0 0 0 0 0 psi p q r pn Vt 0 0 -0.4842 0 0 alpha 0 0 0.9146 0 0 beta 0 0.03387 0 -0.9925 0 phi 0 1 0 0.03443 0 theta 0 0 1 0 0 psi 0 0 0 1.001 0 p 0 -3.574 0.0002627 0.6331 0 q 0 -8.795e-06 -1.045 -0.002858 0 r 0 -0.02249 0.002539 -0.4621 0 pn 0 0 0 0 0 pe 540 0 0 0 0 alt 0 0 0 0 0 pow 0 0 0 0 0 pe alt pow Vt 0 3.81e-05 0.3717 alpha 0 1.774e-06 -2.37e-05 beta 0 0 0 phi 0 0 0 theta 0 0 0 psi 0 0 0 p 0 0 0 q 0 -5.338e-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.1848 0 0 alpha 0 -0.002085 0 0 beta 0 0 0.0002863 0.0007816 phi 0 0 0 0 theta 0 0 0 0 psi 0 0 0 0 p 0 0 -0.765 0.1376 q 0 -0.1833 0 0 r 0 0 -0.03332 -0.06472 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.003698 16.9 -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 -5.24 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.1e-08 0.9454 -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.008953 0 0.1154 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.979e-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.05062 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.004801 0.01311 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. Decoupled LQR Controller Gains = alpha q Nz_i beta p elevator -156.88015 -31.03701 -38.72983 0 0 aileron 0 0 0 38.02751 -5.65497 rudder 0 0 0 17.56400 1.58391 r ps_i Ny_r_i elevator 0 0 0 aileron -14.08804 -34.06416 -9.95406 rudder -41.43509 6.29550 -53.86016 ------------------------------------------------------------ Running Nonlinear Simulation in ODE45
Implementation of a Simple Ground Collision Avoidance System
Concept: Roll until wings level (in the shortest direction) When abs(roll rate) < threshold, pull X g's until pitch angle > X deg
if(autopilot.simpleGCAS) % Choose threshold values: eps_phi = deg2rad(5); % Max roll angle magnitude before pulling g's eps_p = deg2rad(1); % Max roll rate magnitude before pulling g's path_goal = deg2rad(0); % Final desired path angle Nz_des = 5; % Desired maneuver g's % Pull out important variables for ease of use phi = x_f16(4); % Roll angle (rad) p = x_f16(7); % Roll rate (rad/sec) theta = x_f16(5); % Pitch angle (rad) alpha = x_f16(2); % AoA (rad) % Note: pathAngle = theta - alpha % Set Proportional-Derivative Control Gains K_prop = 4; K_der = K_prop*0.3; % Maneuver starts at t == 0 (boring) man_start = 2; if(t<man_start) % Do nothing elseif(man_check_1 < 0) % Determine which angle is "level" (0, 180, 360, 720, etc) radsFromWingsLevel = round(phi/pi); % Until wings are "level" & roll rate is small if(abs(phi-pi*radsFromWingsLevel) < eps_phi && abs(p) < eps_p) man_check_1 = t; else % PD Control until phi == pi*radsFromWingsLevel ps = -(phi - pi*radsFromWingsLevel)*K_prop - p*K_der; end elseif(man_complete < 0) radsFromNoseLevel = round((theta-alpha)/(2*pi)); if((theta-alpha) - 2*pi*radsFromNoseLevel > path_goal) man_complete = t; else Nz = Nz_des; end else % What to do once recovery is complete Nz = 0; ps = 0; autopilot.steadyLevelFlightHold = true; end % Set t_maneuver states t_maneuver(1) = man_start; t_maneuver(2) = man_complete; t_maneuver(3) = man_check_1; end
Implementation of a large-scale yaw to heading maneuver
Note: This is not a heading tracker.
if(autopilot.turnToHeading) % Enable speed control autopilot.basicSpeedControl = true; % Choose threshold values: eps_psi = deg2rad(5); % Max yaw angle at completion eps_r = deg2rad(0.1); % Max yaw rate magnitude at completion eps_phi = deg2rad(1); % Max roll angle at completion eps_p = deg2rad(0.1); % Max roll rate at completion psi_goal = deg2rad(0); % Final desired heading % Pull out important variables for ease of use alpha = x_f16(2); % AoA (rad) phi = x_f16(4); % Roll angle (rad) p = x_f16(7); % Roll rate (rad/sec) psi = x_f16(6); % Heading angle (rad) zero is north r = x_f16(9); % Heading rate (rad/s) % Maneuver starts at t == 0 (boring) man_start = 2; maxBankAngle = deg2rad(ctrlLimits.MaxBankDeg); autopilot.levelTurnControl = true; % Set a flag to determine the direction of the turn if(turnDirection==0) turnDirection = sign(psi_goal - psi); % Negative = end if(t<man_start) % Do nothing autopilot.steadyLevelFlightHold = true; elseif(man_check_1 < 0) autopilot.steadyLevelFlightHold = false; % phi_goal has not been passed yet if(sign(psi_goal - psi) == turnDirection ) % Set desired turn bank angle phi_goal = maxBankAngle*turnDirection; % Set Proportional-Derivative Control Gains K_prop = 4; K_der = K_prop*0.5; % PD control for roll rate ps = (phi_goal-phi)*K_prop - p*K_der; else man_check_1 = t; end elseif(man_complete < 0) % Wings are not level yet if(abs(phi) > eps_phi || abs(p) > eps_p) K_prop = 4; K_der = K_prop*0.5; % PD control for roll rate ps = -phi*K_prop - p*K_der; else man_complete = t; end else % What to do once recovery is complete autopilot.steadyLevelFlightHold = true; end t_maneuver(1) = man_start; t_maneuver(2) = man_complete; t_maneuver(3) = man_check_1; end
Time-Based Commands
Note: This is just a sample of time-scheduled commands
if(autopilot.timeTriggeredControl) maxBank = 60; % degrees if(t<5) ps = deg2rad(maxBank/5); elseif(t<10) ps = deg2rad(-maxBank/5); end end
Basic "Smart" Controls
% Proportional control on airspeed if(autopilot.basicSpeedControl) K_vt = 0.25; throttle = -K_vt*(x_f16(1) - xequil(1)); end % Parallel PD control on pitch and roll modes to stay level if(autopilot.steadyLevelFlightHold) % NOTE: This is not a recovery maneuver % Pull out important variables for ease of use phi = x_f16(4); % Roll angle (rad) p = x_f16(7); % Roll rate (rad/sec) theta = x_f16(5); % Pitch angle (rad) q = x_f16(8); % Pitch rate (rad/sec) alpha = x_f16(2); % AoA (rad) % Set Proportional-Derivative control gains for roll K_prop = 1; K_der = K_prop*0.3; % Determine which angle is "level" (0, 180, 360, 720, etc) radsFromWingsLevel = round(phi/pi); % PD Control on phi using roll rate ps = -(phi-pi*radsFromWingsLevel)*K_prop - p*K_der; % Set Proportional-Derivative control gains for pitch K_prop2 = 2; K_der2 = K_prop*0.3; % Determine "which" angle is level (0, 360, 720, etc) radsFromNoseLevel = round((theta-alpha)/pi); % PD Control on theta using Nz Nz = -(theta - alpha - pi*radsFromNoseLevel)*K_prop2 - p*K_der2; % Note: Could implement PD control on psi using Ny_r (for small errors) % Set Proportional-Derivative control gains for pitch % K_prop3 = 5; % K_der3 = K_prop*0.3; % PD Control on psi using Ny_r % Ny_r = (psi_goal-psi)*K_prop3 - r*K_der3; end % Pull g's to maintain altitude during bank based on geometry/trig if(autopilot.levelTurnControl) % Calculate theta phi = x_f16(4); % Note: This calculation only works for {-pi/2 < phi < pi/2} if(phi <= pi/2 && phi > -pi/2) Nz = 1/cos(phi) - 1; % Keeps plane at altitude else Nz = ctrlLimits.NzMax; fprintf('At time = %d, phi = %d deg\n',t,rad2deg(phi)); warning('Singularity at pi/2 < theta < -pi/2. Nz_ref = 0'); end end
Emforce G-limits and set maneuver states as needed.
if(Nz > ctrlLimits.NzMax) fprintf('Time = %d\n',t); fprintf('Nz = %d g''s\n',Nz); fprintf('phi = %d deg\n',rad2deg(x_f16(4))); Nz = ctrlLimits.NzMax; warning('Nz_ref exceeds NzMax. Constraining...'); fprintf('Nz_trimmed = %d g''s\n',Nz); disp('Note: ODE45 may re-calculate this time step'); end if(Nz < ctrlLimits.NzMin) fprintf('Time = %d\n',t); fprintf('Nz = %d g''s\n',Nz); fprintf('phi = %d deg\n',rad2deg(x_f16(4))); Nz = ctrlLimits.NzMin; warning('Nz_ref below NzMin. Constraining...'); fprintf('Nz_trimmed = %d g''s\n',Nz); disp('Note: ODE45 may re-calculate this time step'); end % Set t_maneuver states t_maneuver(1) = man_start; t_maneuver(2) = man_complete; % Create reference vector u_ref = [Nz; ps; Ny_r; throttle];
end
Simulation Complete Time to simulate (MM:SS.ms): 00:35.807 Begin back-calculation of controls Back-Calculation of Controls Complete Time to calculate (MM:SS.ms): 01:23.684 Begin struct generation & pass/fail analysis Pass/Fail Analysis Complete Time to calculate (MM:SS.ms): 00:00.037 ---------------------------------------------------------- RESULTS: Min & Max Values: Min Altitude: 96.178 ft Altitude Lost: 3403.822 ft Min Airspeed: 540.000 ft/sec Max Airspeed: 639.104 ft/sec Max Down Force: 5.222 g's Min Down Force: -0.426 g's Max Side Force: 0.061 g's Max Roll Accel: 233.522 deg/sec^2 Maneuver Event Times: Maneuver Start: 2.000 sec Checkpoint 1: 3.395 sec Maneuver Complete: 8.000 sec Final Time: 15.000 sec (if Checkpoint 2 < Checkpoint 1, it is due to ODE45 stepping backwards in time) F-16 Attitude at Start: Roll (phi): 45.000 deg Pitch (theta): -72.000 deg Yaw (psi): -45.000 deg F-16 Attitude at End: Roll (phi): -0.000 deg Pitch (theta): 2.249 deg Yaw (psi): -33.514 deg Pass Fail Conditions: stable: 1 airspeed: 1 alpha: 1 beta: 1 Nz: 1 psMaxAccelDeg: 1 altitude: 1 maneuverTime: 1 ---------------------------------------------------------- Begin plotting Plot Generation Complete Time to calculate (MM:SS.ms): 00:01.872