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;
do_musyn = true;

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_hinf = uav_performance_hinf(params, do_plots);
perf_musyn = uav_performance_musyn(params, do_plots);

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

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

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

if do_hinf
  fprintf('Generating system model for H-infinity design...\n');
  model = uav_model(params, perf_hinf, uncert);

  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_hinf, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt);
  fprintf(' - H-infinity synthesis gamma: %g\n', gamma_hinf);

  index = struct( ...
    'Ix', 1, 'Iy', 2, 'Iz', 3, ...
    'IPdot', (4:6)', ...
    'Iroll', 7, 'Ipitch', 8, 'Iyaw', 9, ...
    'ITheta', (10:12)', ...
    'Ialpha', (1:4)', ...
    'Iomega', 5 ...
  );

  ctrl.hinf = struct( ...
    'Name', '$\mathcal{H}_{\infty}$', ...
    'K', K_inf, ...
    'index', index ...
  );

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

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

  if do_plots
    fprintf(' - Plotting resulting controller...\n');

    % Plot transfer functions
    figure; hold on;
    bode(ctrl.hinf.K(index.Ialpha(1), index.Ix));
    bode(ctrl.hinf.K(index.Ialpha(2), index.Ix));

    bode(ctrl.hinf.K(index.Ialpha(1), index.Iy));
    bode(ctrl.hinf.K(index.Ialpha(2), index.Iy));

    bode(ctrl.hinf.K(index.Iomega, index.Ix));
    bode(ctrl.hinf.K(index.Iomega, index.Iy));
    bode(ctrl.hinf.K(index.Iomega, index.Iz));

    title(sprintf('\\bfseries %s Controller', ctrl.hinf.Name), ...
      'interpreter', 'latex');
    legend('$x \rightarrow \alpha_1$', ...
      '$x \rightarrow \alpha_2$', ...
      '$y \rightarrow \alpha_1$', ...
      '$y \rightarrow \alpha_2$', ...
      '$x \rightarrow \omega$', ...
      '$y \rightarrow \omega$', ...
      '$z \rightarrow \omega$', ...
      'interpreter', 'latex');
    grid on;
  end

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

  T = 60;
  nsamples = 5000;
  do_noise = false;
  simout = uav_sim_step(params, model, ctrl.hinf, uncert, 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.PlotAlpha) * 180 / pi, ...
      simout.StepX(:, simout.index.EulerAngles) * 180 / pi];

  writematrix([simout.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
end
%%  ------------------------------------------------------------------------
% Perform mu-Analysis & DK iteration
drawnow;

if do_musyn
  fprintf('Generating system model for mu-synthesis design...\n');
  model = uav_model(params, perf_musyn, uncert);

  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
  omega_max = 2;
  omega_min = -3;
  nsamples = (omega_max - omega_min) * 100;

  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 degree of D-scales and error
  d_scales_max_degree = 5;
  d_scales_max_err = 15;
  d_scales_max_err_p = .05; % in percentage
  d_scales_improvement_p = .25; % in percentage, avoid diminishing returns

  % Limit order of scaled plant
  scaled_plant_reduce_ord = 100;
  scaled_plant_reduce_maxerr = 1e-8;

  % for plotting later
  mu_plot_legend = {};

  % For warm start
  K_prev = {}; gamma_prev = {};
  K_prev{1} = 0; gamma_prev{1} = inf;

  mu_bounds_rp_prev = {}; mu_bounds_rs_prev = {};
  D_left_prev = {}; D_right_prev = {};

  % Start DK-iteration
  warmit = 0;

  % Do we have a lower bound for gamma?
  gamma_max = 20;
  gamma_min = .8;

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

  %% Run D-K iteration

  % hand tuned degrees, inf means will pick automatically best fit
  % according to parameters given above
  d_scales_degrees = {
      0,   0,   0,   0,   0, inf, inf; % alpha
    inf, inf, inf, inf, inf, inf, inf; % omega
      0,   0,   0,   0,   0, inf, inf; % state
      0,   0,   0,   0,   0, inf, inf; % perf
  };

  if size(d_scales_degrees, 2) < niters
    error('Number of columns of d_scales_degrees must match niter');
  end

  % if iterations fails set warmit and re-run this code section
  if warmit > 0
    fprintf("\n");
    fprintf(" - Warm start, resuming from iteration %d.\n", warmit);

    K = K_prev{warmit};
    gamma_max = gamma_prev{warmit};

    mu_bounds_rp = mu_bounds_rp_prev{warmit};
    mu_bounds_rs = mu_bounds_rs_prev{warmit};

    D_left = D_left_prev{warmit};
    D_right = D_right_prev{warmit};

    s = warmit;
  else
    s = 1;
  end

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

    % Scale plant and reduce order, fitting the D-scales adds a lot of near
    % zero modes that cause the plant to become very numerically ill
    % conditioned. Since the D-scales are an approximation anyways (i.e.
    % there is not mathematical proof for the fitting step), we limit the
    % order of the scaled system to prevent poor scaling issues.
    P_scaled = minreal(D_left * P * inv(D_right), [], false);
    [P_scaled, ~] = prescale(P_scaled, omega_range);
    n = size(P_scaled.A, 1);

    % disabled, seems to work without
    if false && n > scaled_plant_reduce_ord
      R = reducespec(P_scaled, 'balanced'); % or ncf
      R.Options.FreqIntervals = [omega_range{1}, omega_range{2}];
      R.Options.Goal = 'absolute'; % better hinf norm
      if do_plots
        figure(102);
        view(R);
        drawnow;
      end
      P_scaled = getrom(R, MaxError=scaled_plant_reduce_maxerr, Method='truncate');

      fprintf('   Scaled plant was reduced from order %d to %d\n', ...
        n, size(P_scaled.A, 1))

      % update n
      n = size(P_scaled.A, 1);
    end

    % For higher number of states we run into numerical problems
    if n < 70
      % Scaled plant (whole)
      [nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(P_scaled);

      fprintf('   Scaled plant has %d modes:\n', nx);
      fprintf('       %d stable, %d unstable\n', nsta, nx - nsta);
      fprintf('       %d controllable, %d unstabilizable\n', nctrb, nustab);
      fprintf('       %d observable, %d undetectable\n', nobsv, nudetb);

      % Check scaled plant parts meet H-infinity assumptions
      A = P_scaled(idx.OutputUncertain, idx.InputUncertain);
      Bu = P_scaled(idx.OutputUncertain, idx.InputNominal);
      Cy = P_scaled(idx.OutputNominal, idx.InputUncertain);

      [~, ~, ~, A_nustab, ~, A_nudetb] = pbhtest(A);
      [~, ~, ~, Bu_nustab, ~, Bu_nudetb] = pbhtest(Bu);
      [~, ~, ~, Cy_nustab, ~, Cy_nudetb] = pbhtest(Cy);

      Deu = P_scaled(idx.OutputError, idx.InputNominal);
      Dyw = P_scaled(idx.OutputNominal, idx.InputExogenous);

      Deu_fullrank = rank(ctrb(Deu)) < length(Deu.A) || rank(obsv(Deu)) < lenth(Deu.A);
      Dyw_fullrank = rank(ctrb(Dyw)) < length(Dyw.A) || rank(obsv(Dyw)) < lenth(Dyw.A);

      if A_nustab > 0 || A_nudetb > 0
        fprintf('       A has %d unstabilizable modes and %d undetectable modes!\n', ...
          A_nustab, A_nudetb);
      end

      if Bu_nustab > 0 || Bu_nudetb > 0
        fprintf('       Bu has %d unstabilizable modes and %d undetectable modes!\n', ...
          Bu_nustab, Bu_nudetb);
      end

      if Cy_nustab > 0 || Cy_nudetb > 0
        fprintf('       Cy has %d unstabilizable modes and %d undetectable modes!\n', ...
          Cy_nustab, Cy_nudetb);
      end

      if ~ Deu_fullrank
        fprintf('       Deu is not full rank!\n');
      end

      if ~ Dyw_fullrank
        fprintf('       Dyw is not full rank!\n');
      end
    else
      eigvals = eig(P_scaled.A);
      stable_modes = 0;
      for i = 1:n
        if real(eigvals(i)) < 0
          stable_modes = stable_modes +1;
        end
      end
      fprintf('   Scaled plant has %d modes: %d stable %d unstable.\n', ...
        n, stable_modes, n - stable_modes);
    end

    % Find controller using H-infinity
    if it > 1 && it ~= warmit
      K_prev{it} = K;
      gamma_prev{it} = gamma;
    end

    [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt);

    % fprintf('   Running H-infinity with gamma in [%g, %g]\n', gamma_min, gamma_max);
    % [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, [gamma_min, gamma_max], hinfopt);
    % gamma_max = 1.2 * gamma;

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

    % if warm start we do not need to recompute this
    % as we are fiddling with D-scales
    if it == warmit
      fprintf("   Warm start, using SSVs from iteration %d\n", warmit);
    else
      % 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, 'fU');

      mu_bounds_rp_prev{it} = mu_bounds_rp;
      mu_bounds_rs_prev{it} = mu_bounds_rs;

      % Plot SSVs
      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
    end

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

    % 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

    % No need to find scales for last iteration
    if it == niters
      break;
    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
      fprintf('      %s', D_names{j});

      % tuned by hand?
      if d_scales_degrees{j, it} < inf
        % D_fit = fitmagfrd(D_frd{j}, d_scales_degrees{j, it});
        D_fit = fitfrd(genphase(D_frd{j}), d_scales_degrees{j, it});

        max_sv = max(max(sigma(D_fit, omega)));
        fit_err = abs(D_max_sv{j} - max_sv);
        D_fitted{j} = D_fit;

        fprintf(' tuned degree %d, error %g (%g %%)\n', ...
                d_scales_degrees{j, it}, fit_err, ...
                100 * fit_err / D_max_sv{j});

      else
        % find best degree
        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 prevent 
            % adding too many states to the controller as it depends on
            % the order of the D-scales.
            improvement = abs(best_fit_err - fit_err);
            improvement_p = improvement / best_fit_err;

            if improvement_p > 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 ...
              || fit_err < d_scales_max_err
            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
    end

    % Construct full matrices
    D_left_prev{it} = D_left;
    D_right_prev{it} = D_right;

    D_left = blkdiag(D_fitted{1} * eye(4), ... % alpha uncert
                     D_fitted{2} * eye(1), ... % omega uncert
                     D_fitted{3} * eye(9), ... % state uncert
                     D_fitted{4} * eye(14), ...
                     eye(12));

    D_right = blkdiag(D_fitted{1} * eye(4), ... % alpha uncert
                      D_fitted{2} * eye(1), ... % omega uncert
                      D_fitted{3} * eye(9), ... % state uncert
                      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));
    d_fit_err = abs(max_sv_left_frd - max_sv_left);

    fprintf('   Max SVD of D: %g, Dhat: %g\n', max_sv_left_frd, max_sv_left);
    fprintf('   D scales fit abs. error: %g\n', d_fit_err)
    fprintf('   D scales fit rel. error: %g %%\n', ...
      100 * d_fit_err / 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.');
  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

  index = struct( ...
    'Ix', 1, 'Iy', 2, 'Iz', 3, ...
    'IPdot', (4:6)', ...
    'Iroll', 7, 'Ipitch', 8, 'Iyaw', 9, ...
    'ITheta', (10:12)', ...
    'Ialpha', (1:4)', ...
    'Iomega', 5 ...
  );

  if do_plots
    fprintf(' - Plotting resulting controller...\n');

    % Plot transfer functions
    figure; hold on;
    bode(ctrl.musyn.K(index.Ialpha(1), index.Ix));
    bode(ctrl.musyn.K(index.Ialpha(2), index.Ix));

    bode(ctrl.musyn.K(index.Ialpha(1), index.Iy));
    bode(ctrl.musyn.K(index.Ialpha(2), index.Iy));

    bode(ctrl.musyn.K(index.Iomega, index.Ix));
    bode(ctrl.musyn.K(index.Iomega, index.Iy));
    bode(ctrl.musyn.K(index.Iomega, index.Iz));

    title(sprintf('\\bfseries %s Controller', ctrl.musyn.Name), ...
      'interpreter', 'latex');
    legend('$x \rightarrow \alpha_1$', ...
      '$x \rightarrow \alpha_2$', ...
      '$y \rightarrow \alpha_1$', ...
      '$y \rightarrow \alpha_2$', ...
      '$x \rightarrow \omega$', ...
      '$y \rightarrow \omega$', ...
      '$z \rightarrow \omega$', ...
      'interpreter', 'latex');
    grid on;
  end

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

  T = 60;
  nsamples = 5000;
  do_noise = false;

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

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

end