Improve code for DK-iteration

1 files changed, 102 insertions, 63 deletions

diff --git a/uav.m b/uav.m
index 275481b..d9198c4 100644
--- a/uav.m
+++ b/uav.m
@@ -52,7 +52,7 @@ end
 %%  ------------------------------------------------------------------------
 % Define stability requirements
 
-% Note: for hinf it is needed to call uav_mode, but hinf will not actually 
+% 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')
@@ -125,12 +125,12 @@ if do_musyn
   % Options for H-infinity
   nmeas = model.uncertain.Ny;
   nctrl = model.uncertain.Nu;
-  hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
-    'AutoScale', 'on', 'RelTol', 1e-2);
+  hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
+    'AutoScale', 'on', 'RelTol', 1e-1);
 
   % Frequency raster resolution to fit D scales
-  nsamples = 500;
-  omega = logspace(-5, 5, nsamples);
+  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));
@@ -138,15 +138,19 @@ if do_musyn
 
   % degrees for approximations for D-scales, tuned by hand
   fit_degrees = [
-    1, 1;  % alpha
-    6, 1;  % omega
-    1, 1;  % state
-    7, 1;  % perf
+    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 = {};
 
@@ -166,18 +170,32 @@ if do_musyn
 
     % 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
-    [mu_bounds, mu_info] = mussv(N_frd, model.uncertain.BlockStructurePerf, 'sU');
-    mu_rp = norm(mu_bounds(1,1), inf, 1e-6);
-    fprintf('   Mu value for RP: %g\n', mu_rp)
+    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(1,1));
-      mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{%d}$', it)};
+
+      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');
@@ -194,51 +212,72 @@ if do_musyn
     % 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);
-
-    fprintf('   Fitting D-scales');
-
-    % 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, fit_degrees(1, it));
-    % D_alpha = fitfrd(genphase(D_left_samples_alpha), fit_degrees(1, it));
-    fprintf('.');
-
-    % 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, fit_degrees(2, it));
-    % D_omega = fitfrd(genphase(D_left_samples_omega), fit_degrees(2, it));
-    fprintf('.');
-
-    % 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, fit_degrees(3, it));
-    % D_state = fitfrd(genphase(D_left_samples_state), fit_degrees(3, it));
-    fprintf('.');
-
-    % 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, fit_degrees(4, it));
-    % D_perf = fitfrd(genphase(D_left_samples_perf), fit_degrees(4, it));
-    fprintf('.');
-
-    fprintf('\n');
+    [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_alpha * eye(4), ...
-                      D_omega * eye(1), ...
-                      D_state * eye(12), ...
-                      D_perf * eye(10), ...
+    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_alpha * eye(4), ...
-                     D_omega * eye(1), ...
-                     D_state * eye(12), ...
-                     D_perf * eye(14), ...
+    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
@@ -257,20 +296,20 @@ if do_musyn
       fprintf('   Plotting D-scales');
       f = figure(101); clf(f); hold on;
 
-      bodemag(D_left_samples_alpha, omega, 'r-');
-      bodemag(D_alpha, omega, 'b');
+      bodemag(D_samples{1}, omega, 'r-');
+      bodemag(D_fitted{1}, omega, 'b');
       fprintf('.');
 
-      bodemag(D_left_samples_omega, omega, 'r--');
-      bodemag(D_omega, omega, 'b--');
+      bodemag(D_samples{2}, omega, 'r--');
+      bodemag(D_fitted{2}, omega, 'b--');
       fprintf('.');
 
-      bodemag(D_left_samples_state, omega, 'c-');
-      bodemag(D_state, omega, 'm-');
+      bodemag(D_samples{3}, omega, 'c-');
+      bodemag(D_fitted{3}, omega, 'm-');
       fprintf('.');
 
-      bodemag(D_left_samples_perf, omega, 'c--');
-      bodemag(D_perf, omega, 'm--');
+      bodemag(D_samples{4}, omega, 'c--');
+      bodemag(D_fitted{4}, omega, 'm--');
       fprintf('.');
 
       fprintf('\n');