air3m.m

library of aircraft models to be used with Simulink

function [X0,U0]=air3m(name,V,H,G)

% [X0,U0]=air3m(name,V,H,G) finds aircraft trim condition
%
% For each entry in the matrices V,H,G (speed,altitude,gamma)
% the function finds the corresponding trim point condition on 
% state and input, (i.e. alpha, theta, thrust, and elevators).
% 
% If the fourth argument, gamma = theta - alpha = asin(Hdot/V),
% (also called "flight path angle"), is zero (radians), then the 
% trim will corresponds to a straight and level flight condition,
% otherwise the trim is not really an equilibrium since the
% airplane is constantly changing its altitude.
% 
% Example 1: find the straight and level flight condition for 
% the scheme airtrim.mdl corresponding to V=260 m/s and H=100 m :
% [x0,u0]=air3m('airtrim',260,100,0);
% 
% Example 2: find the two trim conditions for the points:
% 1) V=200 m/s, H=10e3 m, descending with an angle of 20 degrees
% 2) V=150 m/s, H=50 m, raising with an angle of 45 degrees
% [X0,U0]=air3m('airtrim',[200 150],[10e3 50],[-20 45]*pi/180);
% squeeze(X0),squeeze(U0), % to see the results more clearly
% 
% Example 3: find the trim conditions for the extreme points
% in the envelope 60 m/s < V < 200 m/s, 60 m < H < 16000 m
% and 0 < G < pi/6, for the scheme airtrim.mdl :
% [V,H,G]=ndgrid([60 200],[60 8000 16000],[0 pi/6]);
% [X0,U0]=air3m('airtrim',V,H,G);
% the trim point corresponding to 60 m/s, 16000 m, G=pi/6, 
% (that is V(1,3,2), H(1,3,2), G(1,3,2)) is X0(:,:,1,3,2)
% the full matrices of alpha and elevators corresponding
% to the straight and level flight condition G=0 are:
% squeeze(X0(2,:,:,:,1)) and squeeze(U0(10,:,:,:,1))
% 
% Note that the outputs matrices are ready to be used with the 
% "interpolate matrix" block from the Aerospace Blockset library.
% The function could be easily extended to give as outputs also
% the controller matrices for each trim condition, those matrices 
% too would be ready to be used with the "3D controller" block, 
% still from the GNC sublibrary of the Aerospace Blockset.

% Giampy, May 2003
% the function heavily relies on the function jj_trim from
% the great "trimmod" utility, (thanks to J. J. Buchholz).

% max number of steps and final cost function value
opt=[10e3 1e-13];

% indices of trim variables: alpha, theta, (=> idX=[2 8]')
% thrust, and elevator (=> idU=[1 7]') , and and indices 
% for the requirements on the time derivatives of x:
% vdot=0,alphadot=0,qdot=0,Hdot=V*sin(G) (=> idR=[1 2 5 12]')
idX=[2 8]';idU=[1 7]';idR=[1 2 5 12]';

% NOTE: if using stabilators for the trim, idU=[1 10]';
% remember that an airlib-like structure is assumed
% regarding the number and the order of inputs and states

% check inputs
if nargin ~= 4, 
    error('must supply 4 arguments'); 
end

if exist(name) ~= 4, 
    error(['The model ' name ' hasn''t been found']);
end

SZS=[size(V,1) size(H,1) size(G,1); ...
     size(V,2) size(H,2) size(G,2); ...
     size(V,3) size(H,3) size(G,3)];

if any(SZS<1) | any(SZS>3),
    error(['V H and G must be non empty 2D or 3D matrices']);
end

if any(SZS(:,1)~=SZS(:,2)) | any(SZS(:,2)~=SZS(:,3)), 
    error(['V H and G must have the same size']); 
end

if max(max(max(abs(G)))) > pi/2, 
    error('Gamma must be in radians and between -pi/2 and pi/2');
end

% check model
[msizes,x0,str,ts]=feval(name,0,0,0,0);
if any(msizes([1 4])~=[12;10]),
    error('model should be airlib-like');
else
    % complete initial conditions 
    % (x0 and u0 are used for the max step size computation) 
    u0=zeros(10,1);u0(1)=0.2;
    dx0=zeros(size(x0));
    y0=zeros(msizes(3),1);
end

% initialize outputs
X0=zeros(12,1,size(V,1),size(V,2),size(V,3));
U0=zeros(10,1,size(V,1),size(V,2),size(V,3));

% default linearization step sizes
dxL=1e-5*ones(size(x0));duL=1e-5*ones(size(u0));

% default maximum step sizes
dxM=1e42*ones(size(x0));duM=1e42*ones(size(u0));

% adjust maximum step size for the 4 trim variables
dxM(idX)=0.001+abs(x0(idX)/10);duM(idU)=0.001+abs(u0(idU)/10);
            
% names required by jj_3m
nx=length(x0);xnm=cell(nx,1);for i=1:nx,xnm{i}=['x_' num2str(i) ' ']; end
nu=length(u0);unm=cell(nu,1);for i=1:nu,unm{i}=['u_' num2str(i) ' ']; end
ny=length(y0);ynm=cell(ny,1);for i=1:ny,ynm{i}=['y_' num2str(i) ' ']; end
nd=length(dx0);dnm=cell(nd,1);for i=1:nd,dnm{i}=['d_' num2str(i) ' ']; end

for i = 1:size(V,1),
    for j = 1:size(V,2),
        for k = 1:size(V,3),
            
            % reset initial point
            x0=zeros(12,1);u0=zeros(10,1);u0(1)=0.2;
            
            % set new position in the flight envelope
            x0([1 12])=[V(i,j,k) H(i,j,k)];
            
            % requirement for Hdot
            dx0(12)=V(i,j,k)*sin(G(i,j,k));
            
            % finds the trim value
            [x0,u0,dx0,y0]=jj_3m(name,x0(:),u0(:),dx0(:),y0(:),idX,idU,idR,[],xnm,unm,dnm,ynm,dxM(:),duM(:),dxL(:),duL(:),opt);
            
            % write the trim value into X0 and U0
            X0(:,1,i,j,k)=x0(:);U0(:,1,i,j,k)=u0(:);
            
        end
    end
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%% helper functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% jj_3m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function ...
  [x_tr, u_tr, d_tr, y_tr] = ...
  jj_3m (...
  sys, ...
  x, u, d, y, ...
  i_x, i_u, i_d, i_y, ...
  x_nam, u_nam, d_nam, y_nam, ...
  del_x_max, del_u_max, ...
  del_x_lin, del_u_lin, ...
  options)

%JJ_TRIM   Trim point determination of a nonlinear ordinary differential equation system
%
%   [X_TR, U_TR, D_TR, Y_TR] = JJ_TRIM (SYS, X, U, D, Y, I_X, I_U, I_D, I_Y, X_NAM, U_NAM, D_NAM, Y_NAM)
%   trims the system SYS towards an operating point defined by the elements of
%   X (state vector), U (input vector), D (derivative of the state vector),
%   and Y (output vector).
%   I_X and I_U define the indices of the so-called trim variables, which
%   are those states and inputs the trim algorithm has to find
%   the appropriate values for. Specified values of the trim variables
%   are taken as initial starting guesses for the iteration.
%   I_D and I_Y are the indices of the so-called trim requirements, which
%   the trim algorithm has to satisfy. The values of the other D(i) and Y(i)
%   do not matter.
%   X_NAM, U_NAM, D_NAM, and Y_NAM are cell arrays of the form
%   X_NAM = {'state_1'; 'state_2'; ...}
%   containing the names of the states, inputs, derivatives, and outputs.
%   The names can be chosen arbitrarily. They are used only to identify
%   linear dependent trim variables or trim requirements.
%
%   IMPORTANT: o There have to be as many trim variables as there are
%                trim requirements.
%
%              o All vectors (and cell arrays) have to be column vectors.
%
%   To see more help, enter TYPE JJ_TRIM.

%   [X_TR, ...] = JJ_TRIM (..., Y_NAM, DEL_X_MAX, DEL_U_MAX)
%   allows the additional specification of maximum alterations
%   of state and input during one trim step.
%   The lengths of DEL_X_MAX and DEL_U_MAX are equal to those of
%   X and U respectively.
%   The default values of DEL_X_MAX and DEL_U_MAX are 1e42.
%
%   [X_TR, ...] = JJ_TRIM (..., DEL_U_MAX, DEL_X_LIN, DEL_U_LIN)
%   allows the additional specification of the state and input step size
%   to be used for the calculation of the Jacobian-matrix (sensitivity matrix)
%   in the linearization procedure.
%   The default values of DEL_X_LIN and DEL_U_LIN are
%   1e-6*(1 + abs(x)) and 1e-6*(1 + abs(u)) respectively.
%
%   [X_TR, ...] = JJ_TRIM (..., DEL_U_LIN, OPTIONS)
%   allows the additional specification of
%   the maximum number of iterations "OPTIONS(1)" (default: 42) and
%   the cost value "OPTIONS(2)" (default: 1e-9) to be gained.

%   Copyright 2000-2004, J. J. Buchholz, Hochschule Bremen, [e-mail]

%   Version 1.2     26.05.2000
%
%   The names of inputs, ... outputs for the error messages
%   are now transferred via the parameter list.
%
%   The precompilation and the release of the system is now done in JJ_TRIM.


% Feed through all initial vectors, 
% usefull in case of an emergency exit
x_tr = x;
u_tr = u;
d_tr = d;
y_tr = y;

% Determine lengths of basic vectors
n_x  = length (x);
n_u  = length (u); 
n_d  = length (d);
n_y  = length (y);

n_i_x = length (i_x);
n_i_u = length (i_u);
n_i_y = length (i_y);
n_i_d = length (i_d);

% Assemble generalized input vector and generalized output vector
x_u = [x; u];
d_y = [d; y];

% Determine length of generalized input vector and generalized output vector
n_x_u = length (x_u);
n_d_y = length (d_y);

% Assemble trim variable index vector and trim requirement index vector
i_t_v = [i_x; i_u + n_x]; 
i_t_r = [i_d; i_y + n_d];

% Determine number of trim variables and trim requirements
n_t_v = n_i_x + n_i_u;
n_t_r = n_i_y + n_i_d;

% There have to be as many trim variables as there are trim requirements
if n_t_r ~= n_t_v
  
  l1 = ['The number of trim variables: ', int2str(n_t_v)];
  l2 = 'does not equal';
  l3 = ['the number of trim requirements: ', int2str(n_t_r)];
  l4 = ' ';
  l5 = 'The returned trim point is not valid.';
  h1 = 'Error';
  %  errordlg ({l1; l2; l3; l4; l5}, h1); 
  disp([h1,': ',l1,' ',l2,' ',l3,' ',l4,' ',l5]);
  
  % Game over
  return
  
end

% There should be at least one trim variable and one trim requirement
if ~n_t_r
  
  l1 = 'There should be at least';
  l2 = 'one trim variable and';
  l3 = 'one trim requirement.';
  h1 = 'Nothing to trim';
  
  disp([h1,': ',l1,' ',l2,' ',l3]);
  % warndlg ({l1; l2; l3}, h1);
  
end

% If no maximum step sizes have been defined,
if nargin < 14
  
  % set defaults
  del_max = 1.e42*ones (n_x_u, 1); 
  
  % otherwise assemble generalized maximum step vector
else
  
  del_max = [del_x_max; del_u_max];
  
end

% If no step sizes for the linearization have been defined,
if nargin < 16
  
  % set defaults
  del_lin = 1e-6*(1 + abs(x_u)); 
  
  % otherwise assemble generalized linearization step vector
else
  
  del_lin = [del_x_lin; del_u_lin];
  
end

% If no options have been defined, 
if nargin < 18
  
  % set defaults (maximum number of iterations and cost value to be gained)
  options(1) = 42;
  options(2) = 1e-9;
  
end

% Save and rename the options 
n_iter   = options(1);
cost_tbg = options(2);

% Set time to zero explicitely (assume a time invariant system)
t = 0;

% Set old cost value to infinity, in oder to definitely have 
% an improvement with the first try
cost_old = inf;

% Precompile system.
% Unfortunately, this is necessary because only precompiled systems can be evaluated.
% If the trim algorithm is aborted without the corresponding "Release system" command
% the next precompilation attempt will lead to an error and the simulation cannot
% be started.