Update DK-iteration and clean up

1 files changed, 133 insertions, 96 deletions

diff --git a/uav.m b/uav.m
index d9198c4..059c944 100644
--- a/uav.m
+++ b/uav.m
@@ -10,11 +10,7 @@ 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
+do_musyn = true; % endterm
 
 fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n')
 fprintf('Will do:\n')
@@ -40,24 +36,14 @@ 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
+fprintf('Generating performance requirements...\n')
+perf = uav_performance_musyn(params, do_plots);
 
 %%  ------------------------------------------------------------------------
 % 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
+fprintf('Generating stability requirements...\n')
+uncert = uav_uncertainty(params, do_plots);
 
 %% ------------------------------------------------------------------------
 % Create UAV model
@@ -81,38 +67,42 @@ if do_hinf
   nmeas = model.uncertain.Ny;
   nctrl = model.uncertain.Nu;
 
-  hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
+  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
-    fprintf('Failed to syntesize controller (closed loop is unstable).\n')
+    error('Failed to syntesize controller (closed loop is unstable).')
   end
 
 %%  ------------------------------------------------------------------------
 % Measure Performance of H-infinity design
 
-  fprintf('Simulating closed loop...\n');
+  fprintf(' - Simulating closed loop...\n');
 
+  T = 60;
   nsamples = 500;
   do_noise = true;
-  simout = uav_sim_step_hinf(params, model, ctrl.hinf, nsamples, do_plots, do_noise);
+  simout = uav_sim_step(params, model, ctrl.hinf, nsamples, T, do_plots, do_noise);
 
-  fprintf('Writing simulation results...\n');
+  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')
+  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)
@@ -125,31 +115,29 @@ if do_musyn
   % Options for H-infinity
   nmeas = model.uncertain.Ny;
   nctrl = model.uncertain.Nu;
-  hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
-    'AutoScale', 'on', 'RelTol', 1e-1);
+  hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
+    'AutoScale', 'on', 'RelTol', 1e-3);
 
   % Frequency raster resolution to fit D scales
-  nsamples = 800;
-  omega = logspace(-1, 3, nsamples);
+  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
+  % 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));
 
-  % 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
-  ];
+  % Maximum number of D-K iterations
+  niters = 5;
+  fprintf(' - Will do at most %d iterations.\n', niters);
 
-  % 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;
+  % 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 = {};
@@ -157,21 +145,26 @@ if do_musyn
   % Start DK-iteration
   dkstart = tic;
   for it = 1:niters
-    fprintf(' - Running D-K iteration %d / %d...\n', it, 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);
+    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
-      fprintf('   Failed to synethesize H-infinity controller\n');
-      break;
+      error('Failed to synethesize H-infinity controller');
     end
 
     % Calculate frequency response of closed loop
-    N = minreal(lft(P, K), [], false); % slient
+    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);
 
@@ -187,13 +180,13 @@ if do_musyn
     fprintf('   SSV for Performance: %g, for Stability: %g\n', mu_rp, mu_rs);
 
     if do_plots
-      fprintf('   Plotting mu\n');
+      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), '-.');
+      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', ...
@@ -206,109 +199,123 @@ if do_musyn
     % Are we done yet?
     if mu_rp < 1
       fprintf(' - Found robust controller that meets performance.\n');
-      break
+      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
-    % 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);
+    [D_left_frd, D_right_frd] = 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);
+    % 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_samples{1})));
-      max(max(sigma(D_samples{2})));
-      max(max(sigma(D_samples{3})));
-      max(max(sigma(D_samples{4})));
+      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 = fit_degrees(j, it):d_scales_max_degree
+      for deg = 0:d_scales_max_degree
         % Fit D-scale
-        % D_fit = fitmagfrd(D_samples{j}, deg);
-        D_fit = fitfrd(genphase(D_samples{j}), deg);
+        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
-          best_fit_deg = deg;
-          best_fit_err = fit_err;
-          D_fitted{j} = D_fit;
+          % 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
+        if (fit_err / D_max_sv{j} < d_scales_max_err_p)
           break;
         end
         fprintf('.');
       end
-      fprintf(' degree %d, error %g\n', best_fit_deg, best_fit_err);
+      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_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));
 
+    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_samples = sigma(D_left_samples);
-    max_sv_left_samples = max(max(sv_left_samples));
+    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_samples, max_sv_left);
-    fprintf('   D scales fit abs. error: %g\n', abs(max_sv_left_samples - 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_samples{1}, omega, 'r-');
+      bodemag(D_frd{1}, omega, 'r-');
       bodemag(D_fitted{1}, omega, 'b');
       fprintf('.');
 
-      bodemag(D_samples{2}, omega, 'r--');
+      bodemag(D_frd{2}, omega, 'r--');
       bodemag(D_fitted{2}, omega, 'b--');
       fprintf('.');
 
-      bodemag(D_samples{3}, omega, 'c-');
+      bodemag(D_frd{3}, omega, 'c-');
       bodemag(D_fitted{3}, omega, 'm-');
       fprintf('.');
 
-      bodemag(D_samples{4}, omega, 'c--');
+      bodemag(D_frd{4}, omega, 'c--');
       bodemag(D_fitted{4}, omega, 'm--');
       fprintf('.');
 
@@ -332,9 +339,39 @@ if do_musyn
   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);
+  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