air3m.m

library of aircraft models to be used with Simulink

% The system then has to be released manually (maybe more than once!) with:
% model_name ([], [], [], 'term')
feval (sys, [], [], [], 'compile');
    
% Loop over maximum n_iter iterations
for i_iter = 0 : n_iter
  
  % Calculate outputs and derivatives at the current trim point.
  % Important: We have to calculate the outputs first!
  %            The derivatives would be wrong otherwise.
  % Unbelievable but true: Mathworks argues that this is not a bug but a feature!
  % And furthermore: Sometimes it is even necessary to do the output calculation twice
  % before you get the correct derivatives!
  % Mathworks says, they will take care of that problem in one of the next releases...
  y_tr = feval (sys, t, x_u(1:n_x), x_u(n_x+1:end), 3); 
  y_tr = feval (sys, t, x_u(1:n_x), x_u(n_x+1:end), 3); 
  d_tr = feval (sys, t, x_u(1:n_x), x_u(n_x+1:end), 1);
  d_y_tr = [d_tr; y_tr];
  
  % Calculate differences between required and current generalized output vectors
  del_d_y = d_y - d_y_tr;
  
  % Pick trim requirements only
  del_t_r = del_d_y(i_t_r);
  
  % Cost value is the maximum element of the trim requirement error vector
  cost = max (abs (del_t_r));
  
  % Cost is an empty matrix if there are no trim variables and trim requirements
  if isempty (cost)
    cost = 0;
  end
  
  % If current cost value has become smaller 
  % than the cost value to be gained
  if cost < cost_tbg
    
    % Output cost value and number of iterations needed
    l1 = ['A cost value of ',num2str(cost)];
    l2 = ['has been gained after ', int2str(i_iter), ' iteration(s).'];   
    h1 = 'Success';
    disp([h1,': ',l1,' ',l2]);
    % msgbox ({l1, l2}, h1);
    
    % Release system
    feval (sys, [], [], [], 'term');
    
    % Game over
    return
    
  end
  
  % If an improvement has been obtained
  % with respect to the last point,
  if cost < cost_old
    
    % accept and save this new point.
    % Important for a possible step size bisection later on
    x_u_old = x_u;
    
    % Save the cost value of this new point for a comparison later on
    cost_old = cost;    
    
    % Reset step size bisection counter
    i_bisec = 0;
    
    % Linearize relevant subsystem at current operating point
    jaco = jj_lin (sys, x_u, n_x, i_t_v, i_t_r, del_lin);
    
    % Singular Value Decomposition of the sensitivity matrix
    [u, s, v] = svd (jaco);
    
    % A singular value is assumed to be "zero", if it is 1e12 times smaller 
    % than the maximum singular value. Such a singular value indicates a rank deficiency.
    sv_min = s(1,1)*1e-12;
    
    % Find the indices of those "zero-singular-values"
    i_sv_zero = find (abs (diag (s)) <= sv_min);
    
    % If there are any zero-singular-values,
    if ~isempty (i_sv_zero) 
      
      % the jacobian matrix is singular.
      h1 = 'Singular Jacobian-Matrix';
      
      % Assemble cell arrays containing the names of all trim variables and trim requirements
      trim_variables = [x_nam; u_nam];
      trim_requirements = [d_nam; y_nam];
      
      % Loop over all zero-singular-values
      for i_sv = i_sv_zero'
        
        % Find those elements of the corresponding singular vectors that are not "zero"
        u_sing = find (abs (u(:,i_sv)) > sv_min);
        v_sing = find (abs (v(:,i_sv)) > sv_min);   
        
	      % Separating empty line
    	  l0 = {' '};

        % If there is only one zero element in the left singular vector,
        if length (u_sing) == 1
          
          % prepare the corresponding error message
          l1 = {'The trim requirement'};
          l2 = trim_requirements(i_t_r(u_sing));
          l3 = {'could not be affected by any trim variable.'};       
          
        % If there are more than one zero element in the left singular vector
        else
          
          % prepare the corresponding error message
          l1 = {'The trim requirements'};
          l2 = trim_requirements(i_t_r(u_sing));
          l3 = {'linearly depend on each other.'};       
          
        end 
        
        % Separating empty line
        l4 = {' '};
        
        % If there is only one zero element in the right singular vector,
        if length (v_sing) == 1
          
          % prepare the corresponding error message
          l5 = {'The trim variable'};
          l6 = trim_variables(i_t_v(v_sing));
          l7 = {'does not affect any trim requirement.'};       
          
        % If there are more than one zero element in the right singular vector
        else
          
          % prepare the corresponding error message
          l5 = {'The trim variables'};
          l6 = trim_variables(i_t_v(v_sing));
          l7 = {'linearly depend on each other.'};       
          
        end 
        
        l8 = {'Chose different trim variables and/or trim requirements.'};
        l9 = {'Or try different initial values.'};
        
        % Separating empty line
        l10 = {' '};
        
        l11 = {'The returned trim point is not valid.'};
        l12 = {'You can use the Untrim menu entry to return to the pre-trim state.'};
        
        % Output error message
        % errordlg ([l0; l1; l2; l3; l4; l5; l6; l7; l4; l8; l9; l10; l11; l12], h1);
        disp([h1,': ',l0{:},' ',l1{:},' ',l2{:},' ',l3{:},' ',l4{:},' ',l5{:},' ',l6{:},' ',l7{:},' ',l8{:},' ',l9{:}]);
        
      end
      
      % Release system
      feval (sys, [], [], [], 'term');
    
      % Game over
      return
      
    end
    
    % Assuming a linear system, the alteration of the trim variables 
    % necessary to compensate the trim requirements error can directly 
    % be calculated by the inversion of the linear subsystem model
    % (differential equations and output equations)
    del_t_v = jaco\del_t_r;
    
    % Calculate maximum ratio between allowed and necessary trim step size
    ratio_t_v = del_t_v ./ del_max(i_t_v);
    max_rat = max (abs (ratio_t_v));
    
    % If allowed step size has been exceeded,
    if max_rat > 1
      
      % scale all state and input step sizes, 
      % in order to exploit most of the allowed step size
      del_t_v = del_t_v/max_rat;
      
    end
    
    % If no improvement has been obtained
    % with respect to the last point,
  else
    
    % and if step size has not been bisected 200 times before,
    if i_bisec < 200
      
      % bisect step size and change sign
      del_t_v = -del_t_v/2;
      
      % and increment bisection counter
      i_bisec = i_bisec + 1;
      
      % If step size has already been bisected 200 times before,
    else
      
      % output error message and stop program
      l1 = 'Step size has been bisected 200 times.';
      l2 = ['Program was aborted after ', int2str(i_iter), ' iteration(s)'];   
      l3 = ['with a cost value of ', num2str(cost)];
      l4 = 'Try different initial values.';
      h1 = 'Program aborted';
      % errordlg ({l1; l2; l3; l4}, h1);
      disp([h1,': ',l1,' ',l2,' ',l3,' ',l4]);
      
      % Release system
      feval (sys, [], [], [], 'term');
    
      % Game over
      return
      
    end
    
  end
  
  % Calculate new trim point.
  % Always use old value *before* first bisection
  x_u(i_t_v) = x_u_old(i_t_v) + del_t_v;
  
  % Disassemble the generalized input vector
  x_tr = x_u(1:n_x);
  u_tr = x_u(n_x+1:end);
  
end

% If maximum number of iterations has been exceeded,
% output error message and abort program
l1 = ['Maximum number of iterations exceeded: ', int2str(i_iter)];   
l2 = 'Program was aborted';   
l3 = ['with a cost value of: ', num2str(cost)];
h1 = 'Program aborted';
% errordlg ({l1; l2; l3}, h1);
disp([h1,': ',l1,', ',l2,' ',l3]);

% Release system
feval (sys, [], [], [], 'term');

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

function ...
  jaco = ...
  jj_lin (...
  sys, ...
  x_u, n_x, ...
  i_x_u, i_d_y, ...
  del_x_u)

%JJ_LIN   Subsystem linearisation of a nonlinear ordinary differential equation system 
% 
%   JACO = JJ_LIN (SYS, X_U, N_X, I_X_U, I_D_Y)  
%   linearizes the system with the name SYS at the operating point,
%   that is defined by the generalized input vector X_U.
%   N_X is the length of the original state vector X. 
%   It is needed for the dissassembling of the X_U vector 
%   in the parameter list of the system calls.
%   The matrix JACO only contains the subsystem defined by the 
%   index vectors I_X_U and I_D_Y.
%
%   JACO = JJ_LIN (SYS, X_U, N_X, I_X_U, I_D_Y, DEL_X_U)
%   additionally allows the specification of the perturbation levels 
%   DEL_X_U to be used for the gradient calculations.

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

%   Version 1.2     26.05.2000


% If the user did not define the perturbation levels
if nargin < 6
  
  % Use default perturbation levels
  del_x_u = 1e-6*(1 + abs(x_u)); 
  
end

% Determine vector lengths
n_i_x_u = length (i_x_u);
n_i_d_y = length (i_d_y);

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

% Initialize matrices (will be constructed columnwise later on) 
jaco = [];

% Loop over all generalized inputs to be linearized.
% IMPORTANT: Vector has to be a row vector!
for i = i_x_u'
  
  % Save whole generalized input vector in x_u_left and x_u_right,
  % because we only want to waggle some specific generalized inputs 
  x_u_left  = x_u;
  x_u_right = x_u;
  
  % Waggle one generalized input
  x_u_left(i)  = x_u(i) - del_x_u(i);
  x_u_right(i) = x_u(i) + del_x_u(i);
  
  % Calculate outputs and derivatives at the current trim point.
  % Important: We have to calculate the outputs first!
  %            The derivatives would be wrong otherwise.
  % Unbelievable but true: Mathworks argues that this is not a bug but a feature!
  % And furthermore: Sometimes it is even necessary to do the output calculation twice
  % before you get the correct derivatives!
  % Mathworks says, they will take care of that problem in one of the next releases...
  y_left = feval (sys, t, x_u_left(1:n_x), x_u_left(n_x+1:end), 3);
  y_left = feval (sys, t, x_u_left(1:n_x), x_u_left(n_x+1:end), 3);
  d_left = feval (sys, t, x_u_left(1:n_x), x_u_left(n_x+1:end), 1);
  
  y_right = feval (sys, t, x_u_right(1:n_x), x_u_right(n_x+1:end), 3);
  y_right = feval (sys, t, x_u_right(1:n_x), x_u_right(n_x+1:end), 3);
  d_right = feval (sys, t, x_u_right(1:n_x), x_u_right(n_x+1:end), 1);
  
  % Assemble generalized output vectors
  d_y_left = [d_left; y_left];
  d_y_right = [d_right; y_right];
  
  % Generate one column of the jacobi-matrix for every generalized input to be linearized
  % and build up the matrix columnwise
  jaco_column = (d_y_right(i_d_y) - d_y_left(i_d_y))/(2*del_x_u(i));   
  jaco = [jaco, jaco_column]; 
  
end