path: root/uav.m

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

do_plots = true; % runs faster without
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_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

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

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

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

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

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

%% ------------------------------------------------------------------------
% 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', 'off', 'Method', 'RIC', ...
    'AutoScale', 'off', 'RelTol', 1e-3);
  [K_inf, ~, gamma, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt);
  fprintf(' - H-infinity synthesis gamma: %g\n', gamma);
  ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_inf);

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

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

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

  T = 60;
  nsamples = 500;
  do_noise = true;
  simout = uav_sim_step(params, model, ctrl.hinf, nsamples, T, 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.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
end

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

if do_musyn
  drawnow;

  fprintf('Performing mu-synthesis controller design...\n')

  % Get complete system (without debugging outputs for plots)
  idx = model.uncertain.index;
  P = minreal(model.uncertain.StateSpace(...
        [idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ...
        [idx.InputUncertain; idx.InputExogenous; idx.InputNominal]), ...
        [], false);

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

  % Frequency raster resolution to fit D scales
  nsamples = 600;
  omega_max = 3;
  omega_min = -3;

  omega = logspace(omega_min, omega_max, nsamples);
  omega_range = {10^omega_min, 10^omega_max};

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

  % Maximum number of D-K iterations
  niters = 5;
  fprintf(' - Will do at most %d iterations.\n', niters);

  % Maximum degree of D-scales and error
  d_scales_max_degree = 3;
  d_scales_max_err_p = .4; % in percentage
  d_scales_improvement_p = .15; % in percentage

  % for plotting later
  mu_plot_legend = {};

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

    % Find controller using H-infinity
    P_scaled = minreal(D_left * P * inv(D_right), [], false);
    [P_scaled, ~] = prescale(P_scaled, omega_range);
    [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt);

    fprintf('   H-infinity synthesis gamma: %g\n', gamma);
    if gamma == inf
      error('Failed to synethesize H-infinity controller');
    end

    % Calculate frequency response of closed loop
    N = minreal(lft(P, K), [], false);
    M = minreal(N(idx.OutputUncertain, idx.InputUncertain), [], false);

    [N, ~] = prescale(N, omega_range);
    [M, ~] = prescale(M, omega_range);

    N_frd = frd(N, omega);
    M_frd = frd(M, omega);

    % Calculate upper bound D scaling
    fprintf('   Computing Performance SSV... ')
    [mu_bounds_rp, mu_info_rp] = mussv(N_frd, model.uncertain.BlockStructurePerf, 'U');
    fprintf('   Computing Stability SSV... ')
    [mu_bounds_rs, mu_info_rs] = mussv(M_frd, model.uncertain.BlockStructure, 'U');

    mu_rp = norm(mu_bounds_rp(1,1), inf, 1e-6);
    mu_rs = norm(mu_bounds_rs(1,1), inf, 1e-6);

    fprintf('   SSV for Performance: %g, for Stability: %g\n', mu_rp, mu_rs);

    if do_plots
      fprintf('   Plotting SSV mu\n');
      figure(100); hold on;

      bodemag(mu_bounds_rp(1,1));
      mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{P,%d}$', it)};

      bodemag(mu_bounds_rs(1,1), 'k:');
      mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{S,%d}$', it)};

      title('\bfseries $\mu_\Delta(\omega)$ for both Stability and Performance', ...
            'interpreter', 'latex');
      legend(mu_plot_legend, 'interpreter', 'latex');
      grid on;
      drawnow;
    end

    % Are we done yet?
    if mu_rp < 1
      fprintf(' - Found robust controller that meets performance.\n');
      break;
    end

    if mu_rs < 1
      fprintf('   Found robust controller that is stable.\n')
      ctrl.musyn = struct('Name', '$\mu$-Synthesis', 'K', K, ...
                          'mu_rp', mu_rp, 'mu_rs', mu_rs);
    end

    % Fit D-scales
    [D_left_frd, D_right_frd] = mussvunwrap(mu_info_rp);

    fprintf('   Fitting D-scales\n');

    % There are three complex, square, full block uncertainties and
    % a non-square full complex block for performance
    i_alpha = [1, 1];
    i_omega = model.uncertain.BlockStructure(1, :) + 1; % after first block
    i_state = sum(model.uncertain.BlockStructure(1:2, :)) + 1; % after second block
    i_perf  = sum(model.uncertain.BlockStructurePerf(1:3, :)) + 1; % after third block

    D_frd = {
      D_left_frd(i_alpha(1), i_alpha(1));
      D_left_frd(i_omega(1), i_omega(1));
      D_left_frd(i_state(1), i_state(1));
      D_left_frd(i_perf(1), i_perf(1));
    };

    D_max_sv = {
      max(max(sigma(D_frd{1, 1})));
      max(max(sigma(D_frd{2, 1})));
      max(max(sigma(D_frd{3, 1})));
      max(max(sigma(D_frd{4, 1})));
    };

    D_names = {'alpha', 'omega', 'state', 'perf'};
    D_fitted = {};

    % for each block
    for j = 1:4
      % for each in left and right
      fprintf('      %s', D_names{j});
      best_fit_deg = inf;
      best_fit_err = inf;
      for deg = 0:d_scales_max_degree
        % Fit D-scale
        D_fit = fitmagfrd(D_frd{j}, deg);
        % D_fit = fitfrd(genphase(D_frd{j}), deg);

        % Check if it is a good fit
        max_sv = max(max(sigma(D_fit, omega)));
        fit_err = abs(D_max_sv{j} - max_sv);

        if fit_err < best_fit_err
          % Choose higher degree only if we improve by at least a specified
          % percentage over the previous best fit (or we are at the first
          % iteration). This is a heuristic to avoid adding too many states
          % to the controller as it depends on the order of the D-scales.
          if abs(best_fit_err - fit_err) / best_fit_err > d_scales_improvement_p || best_fit_err == inf
              best_fit_deg = deg;
              best_fit_err = fit_err;
              D_fitted{j} = D_fit;
          end
        end

        if (fit_err / D_max_sv{j} < d_scales_max_err_p)
          break;
        end
        fprintf('.');
      end
      fprintf(' degree %d, error %g (%g %%)\n', ...
          best_fit_deg, best_fit_err, 100 * best_fit_err / D_max_sv{j});
    end

    % Construct full matrices
    D_left = blkdiag(D_fitted{1} * eye(4), ...
                     D_fitted{2} * eye(1), ...
                     D_fitted{3} * eye(12), ...
                     D_fitted{4} * eye(14), ...
                     eye(12));

    D_right = blkdiag(D_fitted{1} * eye(4), ...
                      D_fitted{2} * eye(1), ...
                      D_fitted{3} * eye(12), ...
                      D_fitted{4} * eye(10), ...
                      eye(5));

    % Compute peak of singular values for to estimate how good is the
    % approximation of the D-scales
    sv_left_frd = sigma(D_left_frd);
    max_sv_left_frd = max(max(sv_left_frd));

    sv_left = sigma(D_left, omega);
    max_sv_left = max(max(sv_left));

    fprintf('   Max SVD of D: %g, Dhat: %g\n', max_sv_left_frd, max_sv_left);
    fprintf('   D scales fit rel. error: %g %%\n', ...
      100 * abs(max_sv_left_frd - max_sv_left) / max_sv_left_frd);

    % Plot fitted D-scales
    if do_plots
      fprintf('   Plotting D-scales');
      f = figure(101); clf(f); hold on;

      bodemag(D_frd{1}, omega, 'r-');
      bodemag(D_fitted{1}, omega, 'b');
      fprintf('.');

      bodemag(D_frd{2}, omega, 'r--');
      bodemag(D_fitted{2}, omega, 'b--');
      fprintf('.');

      bodemag(D_frd{3}, omega, 'c-');
      bodemag(D_fitted{3}, omega, 'm-');
      fprintf('.');

      bodemag(D_frd{4}, omega, 'c--');
      bodemag(D_fitted{4}, omega, 'm--');
      fprintf('.');

      fprintf('\n');
      title(sprintf('\\bfseries $D(\\omega)$ Scales Approximations at Iteration %d', it), ...
            'interpreter', 'latex')
      legend(...
        '$D_{\alpha}$', '$\hat{D}_{\alpha}$', ...
        '$D_{\omega}$', '$\hat{D}_{\omega}$', ...
        '$D_{\mathbf{x}}$', '$\hat{D}_{\mathbf{x}}$', ...
        '$D_{\Delta}$', '$\hat{D}_{\Delta}$', ...
        'interpreter', 'latex' ...
      );
      grid on;
      drawnow;
    end

    itend = toc(itstart);
    fprintf('   Iteration took %.1f seconds\n', itend);
  end
  dkend = toc(dkstart);
  fprintf(' - D-K iteration took %.1f seconds\n', dkend);

  if mu_rp > 1 && mu_rs > 1
    error(' - Failed to synthesize robust controller that meets the desired performance.\n');
  end

  %% Fit worst-case perturbation
  fprintf(' - Computing worst case perturbation.\n')

  % Find peak of mu
  [mu_upper_bound_rp, ~] = frdata(mu_bounds_rp(1,1));
  max_mu_rp_idx = find(mu_rp == mu_upper_bound_rp, 1);
  Delta = mussvunwrap(mu_info_rp);
  % TODO: finish here

  % Save controller
  ctrl.musyn = struct('Name', '$\mu$-Synthesis', ...
                      'K', K, 'Delta', Delta, ...
                      'mu_rp', mu_rp, 'mu_rs', mu_rs);

  if mu_rp >= 1
    fprintf(' - Synthetized robust stable controller does not meet the desired performance.\n');
  end

%%  ------------------------------------------------------------------------
% Measure Performance of mu synthesis design

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

  T = 60;
  nsamples = 5000;
  do_noise = true;

  simout = uav_sim_step(params, model, ctrl.musyn, nsamples, T, do_plots, do_noise);

  fprintf('Simulating worst-case closed loop...\n');

end