% Generate transfer functions for loop shaping stability (uncertainty)
% requirements from parameters specified in uav_params.m
%
% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
% This work is distributed under a permissive license, see LICENSE.txt
%
% Arguments:
%   PARAMS  Struct of design parameters and constants generated by uav_params
%   PLOT    When set to 'true' it plots the inverse magnitude of the
%           performance transfer function
%
% Return value:
%   UNCERT  Struct of uncertainty transfer functions


function [uncert] = uav_performance(params, do_plots)

s = tf('s');

% relative errors
eps_T = params.aerodynamics.ThrustOmegaPropUncertainty;
eps_r = params.mechanical.GyroscopicInertiaZUncertainty;
eps_S = params.aerodynamics.FlapAreaUncertainty;
eps_l = params.aerodynamics.LiftCoefficientUncertainty;
eps_d = params.aerodynamics.DragCoefficientsUncertainties(1);
% eps_0 = params.aerodynamics.DragCoefficients(2);

eps_omega = max(.5 * eps_T, eps_r);
eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega);

b = 12;
G = make_weight(b, 2, .2);

W_malpha = .1 * eps_alpha * G;
W_momega = .1 * eps_omega * G;
W_mState = blkdiag( ...
  .05 * tf(1) * eye(3), ...
  .01 * tf(1) * eye(3), ...
  .001 * tf(1) * eye(3), ...
  .05 * tf(1) * eye(3) ...
);

uncert = struct(...
  'FlapAngleEpsilon', eps_alpha, ...
  'ThrustEpsilon', eps_omega, ...
  'FlapAngle', W_malpha * eye(4), ...
  'Thrust', W_momega, ...
  'StateLinApprox', W_mState);

if do_plots
  % Bode plots of performance requirements
  figure; hold on;
  bodemag(W_malpha);
  bodemag(W_momega);

  bodemag(W_mState(1,1));
  bodemag(W_mState(4,4));
  bodemag(W_mState(7,7));
  bodemag(W_mState(11,11));

  grid on;
  legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ...
         '$W_{m,\mathbf{P}}$', '$W_{m,\mathbf{\dot{P}}}$', ...
         '$W_{m,\mathbf{\Theta}}$', '$W_{m,\mathbf{\Omega}}$', ...
         'interpreter', 'latex')
  title('\bfseries Stability Requirement (only for $\mu$-Synthesis)', ...
        'interpreter', 'latex')
end

end

% Make a n-order performance weight function
%
% Arguments:
%   OMEGA  Cutting frequency (-3dB)
%   A      Magnitude at DC, i.e. |Wp(0)|
%   M      Magnitude at infinity, i.e. |Wp(inf)|
%   ORD    Order
function [Wp] = make_weight(omega, A, M, ord)

if nargin > 3
  n = ord;
else
  n = 1;
end

s = tf('s');
Wp = (s / (M^(1/n)) + omega)^n / (s + omega * A^(1/n))^n;

end
% vim: ts=2 sw=2 et: