% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich
%
% Controller design for a ducted fan VTOL micro-UAV.

%  ------------------------------------------------------------------------
% Clear environment and generate parameters

clear; clc; close all; s = tf('s');
params = uav_params();

do_plots = false;

% ------------------------------------------------------------------------
%% Define performance requirements

% Mechanically, flaps are constrained to a max of 20~25 degrees,
% and they have a maximal angular speed
alpha_max = params.actuators.ServoAbsMaxAngle;
alpha_dot_max = params.actuators.ServoNominalAngularVelocity;

W_Palpha = (s + 100 * alpha_dot_max) / (s + alpha_dot_max);
W_Palpha = alpha_max * W_Palpha / dcgain(W_Palpha); % adjust gain

% Mechanically we have a maximal angular velocity for the propeller in the
% thruster, also there are a lot of unmodelled dynamics in the thruster
omega_max = params.actuators.TurbineMaxSpeed;
W_Pomega = (s + 50 * omega_max) / (s + omega_max);

% We want a nice and smooth movements
v_xy_max = params.performance.MaxHorizontalSpeed;
v_z_max = params.performance.MaxVerticalSpeed;

W_Pxy = 1 / (s + 1 / v_xy_max);
W_Pz = 1 / (s + 1 / v_z_max);

if do_plots
  % Bode plots of performance requirements
  figure; hold on;

  bodemag(1/W_Palpha);
  bodemag(1/W_Pomega);
  bodemag(1/W_Pxy);
  bodemag(1/W_Pz);

  grid on;
  legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ...
    '$W_{P,xy}$', '$W_{P,z}$', ...
    'interpreter', 'latex', 'fontSize', 8);
  title('Performance requirements');


  % Step response of position requirements
  figure; hold on;
  step(W_Pxy); step(W_Pz);
  grid on;
  legend('$W_{P,xy}$', '$W_{P,z}$', 'interpreter', 'latex', 'fontSize', 8);
  title('Step responses of position performance requirements');
end

% Construct performance for position vector by combining xy and z
W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
W_PPdot = tf(1,1) * eye(3);
W_PTheta = tf(1,1) * eye(3);
W_POmega = tf(1,1) * eye(3);

perf = struct(...
  'FlapAngle', W_Palpha * eye(4), ...
  'Thrust', W_Pomega, ...
  'Position', W_PP, ...
  'Velocity', W_PPdot, ...
  'Angle', W_PTheta, ...
  'AngularVelocity', W_POmega); 

%  ------------------------------------------------------------------------
%% Define stability requirements

W_malpha = tf(1,1);
W_momega = tf(1,1);
W_mState = tf(1,1);

if do_plots
  figure; hold on;

  bodemag(W_malpha);
  bodemag(W_momega);
  bodemag(W_mState);

  grid on;
  legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ...
    '$W_{m,\Theta}$', '$W_{m,\Omega}$', ...
    'interpreter', 'latex', 'fontSize', 8);
  title('Uncertainties')
end

uncert = struct(...
  'FlapAngle', W_malpha * eye(4), ...
  'Thrust', W_momega, ...
  'StateLinApprox', W_mState * eye(12));
  
% ------------------------------------------------------------------------
% Create UAV model

model = uav_model(params, perf, uncert);

% ------------------------------------------------------------------------
%% Perform H-infinity design

idx = model.uncertain.index;
idx_ey = [idx.OutputError; idx.OutputNominal];
idx_wu = [idx.InputDisturbance; idx.InputReference; idx.InputNominal];

nmeas = max(size(idx.OutputNominal)); % size of y
nctrl = max(size(idx.InputNominal)); % size of u

% Get nominal system without uncertainty (lower LFT)
G = minreal(model.uncertain.StateSpace(idx_ey, idx_wu));

hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', 'RelTol', 0.01);
[K_inf, N_inf, gamma, info] = hinfsyn(G, nmeas, nctrl, hinfopt);

%  ------------------------------------------------------------------------
% Verify performance satisfaction

%  ------------------------------------------------------------------------
% Perform mu-Analysis & DK iteration

% vim: ts=2 sw=2 et: