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 = false; % endterm

if do_hinf & do_musyn
  error('Cannot do both H-infinity and mu synthesis.')
end

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

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

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

% Note: for hinf it is needed to call uav_model, but hinf will not actually
% make use of this struct
if do_hinf | 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 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')

  % 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', 'on', 'Method', 'RIC', ...
    'AutoScale', 'on', 'RelTol', 1e-1);

  % Frequency raster resolution to fit D scales
  nsamples = 800;
  omega = logspace(-1, 3, nsamples);

  % Initial values for D-K iteration
  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));

  % degrees for approximations for D-scales, tuned by hand
  fit_degrees = [
    1, 1, 1, 1, 1, 1;  % alpha
    1, 1, 1, 1, 1, 1;  % omega
    1, 1, 1, 1, 1, 1;  % state
    6, 1, 4, 2, 2, 1;  % perf
  ];

  % Number of D-K iterations
  niters = size(fit_degrees, 2);

  % Maximum degree of D-scales
  d_scales_max_err = .01;% in percentage
  d_scales_max_degree = 8;

  % 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
    [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl, hinfopt);
    fprintf('   H-infinity synthesis gamma: %g\n', gamma);
    if gamma == inf
      fprintf('   Failed to synethesize H-infinity controller\n');
      break;
    end

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

    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 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), '-.');
      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

    % Fit D-scales
    % There are three complex, square, full block uncertainties and
    % a non-square full complex block for performance
    [D_left_samples, D_right_samples] = mussvunwrap(mu_info_rp);

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

    i_alpha = 1;
    i_omega = model.uncertain.BlockStructure(1, 1) + 1; % after first block
    i_state = model.uncertain.BlockStructure(2, 1) + 1; % after second block
    i_perf  = model.uncertain.BlockStructurePerf(3, 1) + 1; % after third block

    D_samples = {
      D_left_samples(i_alpha, i_alpha);
      D_left_samples(i_omega, i_omega);
      D_left_samples(i_state, i_state);
      D_left_samples(i_perf, i_perf);
    };

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

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

    for j = 1:4
      fprintf('      %s', D_names{j});

      best_fit_deg = inf;
      best_fit_err = inf;

      for deg = fit_degrees(j, it):d_scales_max_degree
        % Fit D-scale
        % D_fit = fitmagfrd(D_samples{j}, deg);
        D_fit = fitfrd(genphase(D_samples{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
          best_fit_deg = deg;
          best_fit_err = fit_err;
          D_fitted{j} = D_fit;
        end

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

    % Construct full matrices
    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));

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

    % Compute peak of singular values for to estimate how good is the
    % approximation of the D-scales
    sv_left_samples = sigma(D_left_samples);
    max_sv_left_samples = max(max(sv_left_samples));

    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_samples, max_sv_left);
    fprintf('   D scales fit abs. error: %g\n', abs(max_sv_left_samples - max_sv_left));

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

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

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

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

      bodemag(D_samples{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
    fprintf(' - Failed to synthesize robust controller that meets the desired performance.\n');
  else
    ctrl.musyn = struct('K', K, 'mu', mu_rp);
  end
end