Fix DK iteration scales, add realistic values

also separate performance requirements from hinf and musyn

1 files changed, 136 insertions, 51 deletions

diff --git a/uav.m b/uav.m
index 62f6543..ca33346 100644
--- a/uav.m
+++ b/uav.m
@@ -9,11 +9,15 @@
 clear; clc; close all; s = tf('s');
 
 % Flags to speed up running for debugging
-do_plots = true;
+do_plots = true; % runs faster without
 do_lqr = false; % unused
-do_hinf = true; % midterm
+do_hinf = false; % midterm
 do_musyn = true; % 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
@@ -41,15 +45,21 @@ params = uav_params();
 % ------------------------------------------------------------------------
 %% Define performance requirements
 
-if do_hinf | do_musyn
+if do_hinf
   fprintf('Generating performance requirements...\n')
-  perf = uav_performance(params, do_plots);
+  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
 
-if do_musyn
+% Note: for hinf it is needed to call uav_mode, 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
@@ -119,33 +129,48 @@ if do_musyn
   fprintf('Performing mu-synthesis controller design...\n')
 
   idx = model.uncertain.index;
-  P = model.uncertain.StateSpace([idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ...
-                                 [idx.InputUncertain; idx.InputExogenous; idx.InputNominal]);
-
-  % Initial values for D-K iteration
-  D_left = eye(model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny);
-  D_right = eye(model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu);
+  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', 'off', 'RelTol', 1e-3);
+  hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
+    'AutoScale', 'on', 'RelTol', 1e-2);
 
   % Number of D-K iterations
-  niters = 4;
+  niters = 8;
 
   % Frequency raster resolution to fit D scales
-  nsamples = 51;
-  omega = logspace(-2, 2, nsamples);
+  nsamples = 61;
+  omega = logspace(-2, 3, nsamples);
+
+  % Initial values for D-K iteration
+  nleft = model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny;
+  nleft_clp = model.uncertain.Nz + model.uncertain.Ne;
+
+  nright = model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu;
+  nright_clp = model.uncertain.Nv + model.uncertain.Nw;
+
+  D_left = tf(eye(nleft));
+  D_right = tf(eye(nright));
+
+  last_mu_rp = inf;
+  mu_plot_legend = {};
 
   % Start DK-iteration
   for it = 1:niters
     fprintf(' - Running D-K iteration %d ...\n', it);
 
     % Find controller using H-infinity
-    [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl);
+    [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
@@ -156,43 +181,103 @@ if do_musyn
     mu_rp = norm(mu_bounds(1,1), inf, 1e-6);
     fprintf('   Mu value for RP: %g\n', mu_rp)
 
+    if do_plots
+      fprintf('   Plotting mu\n');
+      figure(100); hold on;
+      bodemag(mu_bounds(1,1));
+      mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{%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
-    [dsysl, dsysr] = mussvunwrap(mu_info);
-    D_left = fitfrd(genphase(dsysl(1,1)), 1);
-    D_right = fitfrd(genphase(dsysr(1,1)), 1);
+    % 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);
+
+    % D scale for alpha uncertainty (first block)
+    i = 1;
+    D_left_samples_alpha = D_left_samples(i, i);
+    D_alpha = fitmagfrd(D_left_samples_alpha, 2);
+
+    % D scale for omega uncertainty (second block)
+    i = model.uncertain.BlockStructure(1, 1) + 1; % after first block
+    D_left_samples_omega = frd(D_left_samples(i, i));
+    D_omega = fitmagfrd(D_left_samples_omega, 3);
+
+    % D scale for state uncertainty (third block)
+    i = model.uncertain.BlockStructure(2, 1) + 1; % after second block
+    D_left_samples_state = D_left_samples(i, i);
+    D_state = fitmagfrd(D_left_samples_state, 5);
+
+    % D scale for performance (non-square)
+    i = model.uncertain.BlockStructurePerf(3, 1); % after third block
+    D_left_samples_perf = D_left_samples(i, i);
+    D_perf = fitmagfrd(D_left_samples_perf, 2);
+
+    % Construct full matrices
+    D_right = blkdiag(D_alpha * eye(4), ...
+                      D_omega * eye(1), ...
+                      D_state * eye(12), ...
+                      D_perf * eye(10), ...
+                      eye(5));
+
+    D_left = blkdiag(D_alpha * eye(4), ...
+                     D_omega * eye(1), ...
+                     D_state * eye(12), ...
+                     D_perf * eye(14), ...
+                     eye(12));
+
+    % Plot fitted D-scales
+    if do_plots
+      fprintf('   Plotting D-scales ');
+      f = figure(101); clf(f); hold on;
+
+      bodemag(D_left_samples_alpha, omega);
+      bodemag(D_alpha, omega);
+      fprintf('.');
+
+      bodemag(D_left_samples_omega, omega);
+      bodemag(D_omega, omega);
+      fprintf('.');
+
+      bodemag(D_left_samples_state, omega);
+      bodemag(D_state, omega);
+      fprintf('.');
+
+      bodemag(D_left_samples_perf, omega);
+      bodemag(D_perf, omega);
+      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
+  end
+
+  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
 
 %  ------------------------------------------------------------------------
 %% 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
-
-% vim: ts=2 sw=2 et: