% 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');

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

fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n')
fprintf('Will do:\n')
if do_plots
  fprintf(' - Produce plots\n')
end
if do_lqr
  fprintf(' - LQR synthesis\n')
end
if do_hinf
  fprintf(' - H-infinity synthesis\n')
end
if do_musyn
  fprintf(' - Mu synthesis\n')
end

% Synthesized controllers will be stored here
ctrl = struct();

% ------------------------------------------------------------------------
%% Define system parameters

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

% ------------------------------------------------------------------------
%% 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]), [], false);

  nmeas = model.uncertain.Ny;
  nctrl = model.uncertain.Nu;

  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
  fprintf('Performing mu-synthesis controller design...\n')

  idx = model.uncertain.index;
  P = model.uncertain.StateSpace([idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ...
                                 [idx.InputUncertain; idx.InputExogenous; idx.InputNominal]);

  % Initial values for D-K iteration
  D_left = eye(model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny);
  D_right = eye(model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu);

  % Options for H-infinity
  nmeas = model.uncertain.Ny;
  nctrl = model.uncertain.Nu;
  hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
    'AutoScale', 'off', 'RelTol', 1e-3);

  % Number of D-K iterations
  niters = 4;

  % Frequency raster resolution to fit D scales
  nsamples = 51;
  omega = logspace(-2, 2, nsamples);

  % Start DK-iteration
  for it = 1:niters
    fprintf(' - Running D-K iteration %d ...\n', it);

    % Find controller using H-infinity
    [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl);
    fprintf('   H-infinity synthesis gamma: %g\n', gamma);

    % Calculate frequency response of closed loop
    N = minreal(lft(P, K), [], false); % slient
    N_frd = frd(N, omega);

    % Calculate upper bound D scaling
    [mu_bounds, mu_info] = mussv(N_frd, model.uncertain.BlockStructurePerf, 'sU');
    mu_rp = norm(mu_bounds(1,1), inf, 1e-6);
    fprintf('   Mu value for RP: %g\n', mu_rp)

    % Fit D-scales
    [dsysl, dsysr] = mussvunwrap(mu_info);
    D_left = fitfrd(genphase(dsysl(1,1)), 1);
    D_right = fitfrd(genphase(dsysr(1,1)), 1);
  end
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: