%% Trimming and Linearization of the HL-20 Airframe
% This is Part 1 of a five-part example series on design and tuning
% of the flight control system for the HL-20 vehicle. This part deals
% with trimming and linearization of the airframe.

% Copyright 1986-2017 The MathWorks, Inc.

%% HL-20 Model
% The HL-20 model is adapted from the model described in 
% <docid:aeroblks_ug.f4-50264 NASA HL-20 Lifting Body Airframe>. This is a 6-DOF model of the vehicle
% during the final descent and landing phase of the flight. No thrust
% is used during this phase and the airframe is gliding to the landing
% strip. 

open_system('csthl20_trim')

%%
% This version of the model includes the equations of motion
% (EOM), the force and moment calculation from the aerodynamic tables, the
% environment model, and the "Controls Selector" block which maps
% aileron, elevator, and rudder demands to deflections of 
% the six control surfaces.

%% Batch Trimming
% Trimming consists of calculating aileron, elevator, and rudder
% deflections that zero out the forces and moments on the airframe, or
% equivalently, keep the body velocities ub,vb,wb and angular rates p,q,r steady. Because 
% thrust is not used during descent, one degree-of-freedom is lost and 
% the trim condition must be relaxed to let ub to vary. The trim values 
% of the deflections da,de,dr depend on the
% airframe orientation relative to the wind. This orientation is characterized
% by the angle-of-attack (AoA) alpha and the sideslip angle (AoS) beta.

%% 
% <<../plane_angles.png>>
% 

%%
% With the |operspec| and |findop| functions, you can efficiently
% compute the trim deflections over a grid of (alpha,beta) values 
% covering the operating range of the vehicle. Here we trim the model
% for 8 values of alpha ranging from -10 to 25 degrees, and 5 values of
% beta ranging from -10 to +10 degrees. The nominal altitude and speed 
% are set to 10,000 feet and Mach 0.6.

d2r = pi/180;            % degrees to radians
m2ft = 3.28084;          % meter to feet
Altitude = 10000/m2ft;   % Nominal altitude
Mach = 0.6;              % Nominal Mach
alpha_vec = -10:5:25;	 % Alpha Range
beta_vec = -10:5:10;     % Beta Range
[alpha,beta] = ndgrid(alpha_vec,beta_vec);   % (Alpha,Beta) grid

%%
% Use |operspec| to create an array of operating point specifications.

opspec = operspec('csthl20_trim',size(alpha));

opspec(1)

%%
% Specify the equilibrium conditions for each orientation
% of the airframe. To do this:
% 
% * Specify the orientation by fixing the outputs alpha and beta
%   to their desired values.
% * Specify the airframe speed by fixing the Mach output to 0.6.
% * Mark the angular rates p,q,r as steady.
% * Mark the velocities vb and wb as steady.

for ct=1:40
   % Specify alpha angle
   opspec(ct).Outputs(3).y = alpha(ct);
   opspec(ct).Outputs(3).Known = true;
   % Specify beta angle
   opspec(ct).Outputs(4).y = beta(ct);
   opspec(ct).Outputs(4).Known = true;
   % Specify Mach speed
   opspec(ct).Outputs(5).y = Mach;
   opspec(ct).Outputs(5).Known = true;
   % Mark p,q,r as steady
   opspec(ct).States(2).SteadyState = true(3,1);
   % Mark vb,wb as steady
   opspec(ct).States(3).SteadyState = [false;true;true];
   % (phi,theta,psi) and (Xe,Ye,Ze) are not steady
   opspec(ct).States(1).SteadyState = false(3,1);
   opspec(ct).States(4).SteadyState = false(3,1);
end


%% 
% To fully characterize the trim condition, also
% 
% * Set p=0 to prevent rolling.
% * Set the roll/pitch/yaw angles (phi,theta,psi) to (0,alpha,beta) to
%   align the wind and earth frames.
% * Specify the airframe position (Xe,Ye,Ze) as (0,0,-Altitude).

for ct=1:40
   % Set (phi,theta,psi) to (0,alpha,beta)
   opspec(ct).States(1).x = [0 ; alpha(ct)*d2r ; beta(ct)*d2r];
   opspec(ct).States(1).Known = true(3,1);
   % Set p=0 (no rolling)
   opspec(ct).States(2).x(1) = 0;
   opspec(ct).States(2).Known(1) = true;
   % Set (Xe,Ye,Ze) to (0,0,-Altitude)
   opspec(ct).States(4).x = [0 ; 0 ; -Altitude];
   opspec(ct).States(4).Known = true(3,1);
end

%%
% Now use |findop| to compute the trim conditions for all 
% 40 (alpha,beta) combinations in one go. This batch mode approach
% involves a single compilation of the model. FINDOP uses
% optimization to solve the nonlinear equations characterizing each
% equilibrium. Here we use the "SQP" algorithm for this task.

% Set options for FINDOP solver
TrimOptions = findopOptions;
TrimOptions.OptimizationOptions.Algorithm = 'sqp';
TrimOptions.DisplayReport = 'off';

% Trim model
[ops,rps] = findop('csthl20_trim',opspec,TrimOptions);

%%
% This returns 8-by-5 arrays OPS (operating conditions) and RPS
% (optimization reports). You can use RPS to verify that each trim
% condition was successfully calculated. Results for the
% first (alpha,beta) pair are shown below.

[alpha(1) beta(1)]

%%

ops(1)

%%

rps(1).TerminationString


%% Batch Linearization
% The gains of the flight control system are typically scheduled as a function 
% of alpha and beta, see Part 2 (<docid:control_ug.mw_daf4f3f8-a395-4036-b44d-b83a06ef368a Angular Rate Control in the HL-20 Autopilot>) for more details.
% To tune these gains, you need linearized models of the HL-20 airframe at
% the 40 trim conditions. Use |linearize| to compute these  models
% from the trim operating conditions |ops|.

% Linearize airframe dynamics at each trim condition
G = linearize('csthl20_trim','csthl20_trim/HL20 Airframe',ops);

size(G)

%%
% The linear equivalent of the "Controls Selector" block depends on the
% amount of elevator deflection and should be computed for qbar_inv=1
% (nominal dynamic pressure at Mach=0.6). For convenience, also linearize
% this block at the 40 trim conditions.

CS = linearize('csthl20_trim','csthl20_trim/Controls Selector',ops);

% Zero out a/b and qbar_inv channels
CS = [CS(:,1:3) zeros(6,2)];


%% Linear Model Simplification
% The linearized airframe models have 12 states:

xG = G.StateName

%%
% Some states are not under the authority of the roll/pitch/yaw
% autopilot  and other states contribute little
% to the design of this autopilot. For control purposes, the
% most important states are the roll angle phi, the body velocities 
% ub,vb,wb, and the angular rates p,q,r. Accordingly, use |modred|
% to obtain a 7th-order model that only retains these states.

G7 = G;
xKeep = {...
   'phi theta psi(1)'
   'ub,vb,wb(1)'
   'ub,vb,wb(2)'
   'ub,vb,wb(3)'
   'p,q,r(1)'
   'p,q,r(2)'
   'p,q,r(3)'};
[~,xElim] = setdiff(xG,xKeep);
for ct=1:40
   G7(:,:,ct) = modred(G(:,:,ct),xElim,'truncate');
end


%%
% With these linearized models in hand, you can move to the task of 
% tuning and scheduling the flight control system gains. See 
% <docid:control_ug.mw_daf4f3f8-a395-4036-b44d-b83a06ef368a Angular Rate Control in the HL-20 Autopilot> for Part 2 of this example.
  
