% Controller design for a ducted fan VTOL micro-UAV.
%
% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich
% This work is distributed under a permissive license, see LICENSE.txt

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

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

fprintf('Generating system parameters...\n')
params = uav_params();
ctrl = struct();

% Flags to speed up running for debugging
do_plots = true;
do_lqr = false; % unused
do_hinf = false; % midterm
do_musyn = true; % endterm

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

if do_hinf | do_musyn
  fprintf('Generating performance requirements...\n')
  perf = uav_performance(params, do_plots);
end

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

if do_musyn
  fprintf('Generating stability requirements...\n')
  uncert = uav_uncertainty(params, do_plots);
end

% ------------------------------------------------------------------------
%% Create UAV model

fprintf('Generating system model...\n');
model = uav_model(params, perf, uncert);

% ------------------------------------------------------------------------
%% Perform LQR design

if do_lqr
  fprintf('Performing LQR controller design...\n')
  ctrl.lqr = uav_ctrl_lqr(params, model);
end

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

if do_hinf
  fprintf('Performing H-infinty controller design...\n')

  idx = model.uncertain.index;
  P = model.uncertain.StateSpace;

  % Get nominal system without uncertainty (for lower LFT)
  P_nom = minreal(P([idx.OutputError; idx.OutputNominal], ...
                    [idx.InputExogenous; idx.InputNominal]));

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

  hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
    'AutoScale', 'off', 'RelTol', 1e-3);
  [K_inf, ~, gamma, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt);
  ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_inf);

  if gamma >= 1
    fprintf('Failed to syntesize controller (closed loop is unstable).\n')
  end

%  ------------------------------------------------------------------------
%% Measure Performance of H-infinity design

  fprintf('Simulating closed loop...\n');

  nsamples = 500;
  do_noise = true;
  simout = uav_sim_step_hinf(params, model, ctrl.hinf, nsamples, do_plots, do_noise);

  fprintf('Writing simulation results...\n');
  cols = [
      simout.StepX(:, simout.index.Position), ...
      simout.StepX(:, simout.index.Velocity), ...
      simout.StepX(:, simout.index.FlapAngles) * 180 / pi, ...
      simout.StepX(:, simout.index.Angles) * 180 / pi];

  writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
end

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

if do_musyn

end

%  ------------------------------------------------------------------------
%% Verify performance satisfaction via mu-analysis

% omega = logspace(-3, 3, 250);
%
% N_inf = lft(P, K_inf);
% N_inf_frd = frd(N_inf, omega);
%
% % robust stability
% [mu_inf_rs, ~] = mussv(N_inf_frd(idx.OutputUncertain, idx.InputUncertain), ...
%             model.uncertain.BlockStructure);
%
% % robust performance
% blk_perf = [model.uncertain.BlockStructure;
%             model.uncertain.BlockStructurePerf];
%
% [mu_inf_rp, ~] = mussv(N_inf_frd, blk_perf);
%
% % nominal performance
% mu_inf_np = svd(N_inf_frd(idx.OutputError, idx.InputExogenous));
%
% if do_plots
%   figure; hold on;
%   bodemag(mu_inf_rs(1));
%   bodemag(mu_inf_np(1));
%   bodemag(mu_inf_rs(2));
%   bodemag(mu_inf_np(2));
%
%   grid on;
%   title('$\mu_\Delta(N)$', 'Interpreter', 'latex');
% end

% vim: ts=2 sw=2 et: