Update DK iterations

1 files changed, 168 insertions, 51 deletions

diff --git a/uav.m b/uav.m
index 3a0da71..4810943 100644
--- a/uav.m
+++ b/uav.m
@@ -70,8 +70,8 @@ if do_hinf
 
   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);
+  [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, ...
@@ -88,7 +88,7 @@ if do_hinf
     'index', index ...
   );
 
-  if gamma >= 1
+  if gamma_hinf >= 1
     error('Failed to syntesize controller (closed loop is unstable).')
   end
 
@@ -139,12 +139,11 @@ if do_hinf
 
   writematrix([simout.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
 end
-
 %%  ------------------------------------------------------------------------
 % Perform mu-Analysis & DK iteration
+drawnow;
 
 if do_musyn
-  drawnow;
 
   fprintf('Performing mu-synthesis controller design...\n')
 
@@ -158,13 +157,13 @@ if do_musyn
   % Options for H-infinity
   nmeas = model.uncertain.Ny;
   nctrl = model.uncertain.Nu;
-  hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
+  hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
     'AutoScale', 'on', 'RelTol', 1e-2);
 
   % Frequency raster resolution to fit D scales
-  nsamples = 300;
   omega_max = 3;
-  omega_min = -3;
+  omega_min = -2;
+  nsamples = (omega_max - omega_min) * 100;
 
   omega = logspace(omega_min, omega_max, nsamples);
   omega_range = {10^omega_min, 10^omega_max};
@@ -178,32 +177,68 @@ if do_musyn
   fprintf(' - Will do (max) %d iterations.\n', niters);
 
   % Maximum degree of D-scales and error
-  d_scales_max_degree = 10;
-  d_scales_max_err_p = .2; % in percentage
-  d_scales_improvement_p = .1; % in percentage, avoid diminishing returns
+  d_scales_max_degree = 4;
+  d_scales_max_err = 15;
+  d_scales_max_err_p = .1; % in percentage
+  d_scales_improvement_p = .5; % 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;
+
+  %% Run D-K iteration
 
   % hand tuned degrees, inf means will pick automatically best fit
   % according to parameters given above
   d_scales_degrees = {
-      0,   0, inf, inf, inf; % alpha
-      1, inf, inf, inf, inf; % omega
-      2, inf, inf, inf, inf; % state
-      0,   0, inf, inf, inf;
+      0,   0,   0, inf, inf; % alpha
+      0, inf, inf, inf, inf; % omega
+      0, inf, inf, inf, inf; % state
+      0,   0,   0, inf, inf; % perf
   };
 
   if size(d_scales_degrees, 2) < niters
-    error('');
+    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);
 
-  scaled_plant_reduce_ord = 100;
-  scaled_plant_reduce_maxerr = 1e-3;
+    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};
 
-  % for plotting later
-  mu_plot_legend = {};
+    D_left = D_left_prev{warmit};
+    D_right = D_right_prev{warmit};
+
+    s = warmit;
+  else
+    s = 1;
+  end
 
-  % Start DK-iteration
   dkstart = tic;
-  for it = 1:niters
+  for it = s:niters
     fprintf(' * Running D-K iteration %d / %d...\n', it, niters);
     itstart = tic();
 
@@ -216,7 +251,8 @@ if do_musyn
     [P_scaled, ~] = prescale(P_scaled, omega_range);
     n = size(P_scaled.A, 1);
     
-    if n > scaled_plant_reduce_ord
+    % disabled
+    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
@@ -225,15 +261,76 @@ if do_musyn
         view(R);
         drawnow;
       end
-      P_scaled = getrom(R, MaxError=scaled_plant_reduce_maxerr);
+      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))
     end
 
+    % Check scaled plant meets H-infinity assumptions
+    A = P_scaled(idx.OutputUncertain, idx.InputUncertain);
+    Bu = P_scaled(idx.OutputUncertain, idx.InputNominal);
+    Cy = P_scaled(idx.OutputNominal, idx.InputUncertain);
+
+    [~, ~, ~, nustab, ~, nudetb] = pbhtest(A);
+    if nustab > 0 || nudetb > 0
+      fprintf('   Scaled plant has unstabilizable or undetectable A.\n');
+    end
+
+    [~, ~, ~, nustab, ~, nudetb] = pbhtest(Bu);
+    if nustab > 0 || nudetb > 0
+      fprintf('   Scaled plant has unstabilizable or undetectable Bu.\n');
+    end
+
+    [~, ~, ~, nustab, ~, nudetb] = pbhtest(Cy);
+    if nustab > 0 || nudetb > 0
+      fprintf('   Scaled plant has unstabilizable or undetectable Cy.\n');
+    end
+    
+    Deu = P_scaled(idx.OutputError, idx.InputNominal);
+    Dyw = P_scaled(idx.OutputNominal, idx.InputExogenous);
+
+    if rank(ctrb(Deu)) < length(Deu.A) || rank(obsv(Deu)) < lenth(Deu.A) 
+      fprintf('   Scaled plant has Deu that is not full rank.\n');
+    end
+
+    if rank(ctrb(Dyw)) < length(Dyw.A) || rank(obsv(Dyw)) < lenth(Dyw.A) 
+      fprintf('   Scaled plant has Dyw that is not full rank.\n');
+    end
+
+    % Scaled pant
+    eigvals = eig(P_scaled.A);
+    stable_modes = 0;
+    for i = 1:n
+      if real(eigvals(i)) < 0
+        stable_modes = stable_modes +1;
+      end
+    end
+
+    % For higher number of states we get numerical problems
+    if n < 100
+      ctrb_modes = rank(ctrb(P_scaled));
+      obsv_modes = rank(obsv(P_scaled));
+
+      fprintf('   Scaled plant has %d modes: %d ctrb %d obsv %d stable %d unstable.\n', ...
+        n, ctrb_modes, obsv_modes, stable_modes, n - stable_modes);
+    else
+      fprintf('   Scaled plant has %d modes: %d stable %d unstable.\n', ...
+        n, stable_modes, n - stable_modes);
+    end
+
     % Find controller using H-infinity
-    [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt);
+    if it > 1 && it ~= warmit
+      K_prev{it} = K;
+      gamma_prev{it} = gamma;
+    end
+
+    gamma_max=inf;
+    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 = 2 * gamma;
     fprintf('   H-infinity synthesis gamma: %g\n', gamma);
     if gamma == inf
       error('Failed to synethesize H-infinity controller');
@@ -249,34 +346,44 @@ if do_musyn
     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');
+    % 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);
 
-    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
-
     % Are we done yet?
     if mu_rp < 1
       fprintf(' - Found robust controller that meets performance.\n');
@@ -320,7 +427,6 @@ if do_musyn
 
     % for each block
     for j = 1:4
-      % for each in left and right
       fprintf('      %s', D_names{j});
       
       % tuned by hand?
@@ -356,14 +462,20 @@ if do_musyn
             % 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.
-            if abs(best_fit_err - fit_err) / best_fit_err > d_scales_improvement_p || best_fit_err == inf
+            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)
+          if fit_err / D_max_sv{j} < d_scales_max_err_p ...
+              || fit_err < d_scales_max_err
             break;
           end
           fprintf('.');
@@ -374,6 +486,9 @@ if do_musyn
     end
 
     % Construct full matrices
+    D_left_prev{it} = D_left;
+    D_right_prev{it} = D_right;
+
     D_left = blkdiag(D_fitted{1} * eye(4), ...
                      D_fitted{2} * eye(1), ...
                      D_fitted{3} * eye(12), ...
@@ -393,10 +508,12 @@ if do_musyn
 
     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 * abs(max_sv_left_frd - max_sv_left) / max_sv_left_frd);
+      100 * d_fit_err / max_sv_left_frd);
 
     % Plot fitted D-scales
     if do_plots