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

do_plots = true;

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

fprintf('Generating performance requirements...\n')
perf = uav_requirements(params, do_plots);

%  ------------------------------------------------------------------------
%% 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

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

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

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

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

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

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

nsamples = 500;
do_noise = true;
% uav_sim_step(params, model, ctrl.lqr, nsamples, do_plots);

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

%  ------------------------------------------------------------------------
%% 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

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

% vim: ts=2 sw=2 et: