Add more checks, update DK iteration

1 files changed, 171 insertions, 43 deletions

diff --git a/uav.m b/uav.m
index 059c944..3a0da71 100644
--- a/uav.m
+++ b/uav.m
@@ -9,7 +9,7 @@
 clear; clc; close all; s = tf('s');
 
 do_plots = true; % runs faster without
-do_hinf = true; % midterm
+do_hinf = false; % midterm
 do_musyn = true; % endterm
 
 fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n')
@@ -37,7 +37,8 @@ params = uav_params();
 % Define performance requirements
 
 fprintf('Generating performance requirements...\n')
-perf = uav_performance_musyn(params, do_plots);
+perf_hinf = uav_performance_hinf(params, do_plots);
+perf_musyn = uav_performance_musyn(params, do_plots);
 
 %%  ------------------------------------------------------------------------
 % Define stability requirements
@@ -49,7 +50,7 @@ uncert = uav_uncertainty(params, do_plots);
 % Create UAV model
 
 fprintf('Generating system model...\n');
-model = uav_model(params, perf, uncert);
+model = uav_model(params, perf_musyn, uncert);
 
 %% ------------------------------------------------------------------------
 % Perform H-infinity design
@@ -71,7 +72,21 @@ if do_hinf
     '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);
+
+  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 >= 1
     error('Failed to syntesize controller (closed loop is unstable).')
@@ -80,11 +95,39 @@ if do_hinf
 %%  ------------------------------------------------------------------------
 % Measure Performance of H-infinity design
 
-  fprintf(' - Simulating closed loop...\n');
+  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 = 500;
-  do_noise = true;
+  T = 40;
+  nsamples = 800;
+  do_noise = false;
   simout = uav_sim_step(params, model, ctrl.hinf, nsamples, T, do_plots, do_noise);
 
   fprintf(' - Writing simulation results...\n');
@@ -116,10 +159,10 @@ if do_musyn
   nmeas = model.uncertain.Ny;
   nctrl = model.uncertain.Nu;
   hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
-    'AutoScale', 'on', 'RelTol', 1e-3);
+    'AutoScale', 'on', 'RelTol', 1e-2);
 
   % Frequency raster resolution to fit D scales
-  nsamples = 600;
+  nsamples = 300;
   omega_max = 3;
   omega_min = -3;
 
@@ -132,12 +175,28 @@ if do_musyn
 
   % Maximum number of D-K iterations
   niters = 5;
-  fprintf(' - Will do at most %d iterations.\n', niters);
+  fprintf(' - Will do (max) %d iterations.\n', niters);
 
   % 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
+  d_scales_max_degree = 10;
+  d_scales_max_err_p = .2; % in percentage
+  d_scales_improvement_p = .1; % in percentage, avoid diminishing returns
+
+  % 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;
+  };
+
+  if size(d_scales_degrees, 2) < niters
+    error('');
+  end
+
+  scaled_plant_reduce_ord = 100;
+  scaled_plant_reduce_maxerr = 1e-3;
 
   % for plotting later
   mu_plot_legend = {};
@@ -148,9 +207,31 @@ if do_musyn
     fprintf(' * Running D-K iteration %d / %d...\n', it, niters);
     itstart = tic();
 
-    % Find controller using H-infinity
+    % 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);
+    
+    if 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);
+
+      fprintf('   Scaled plant was reduced from order %d to %d\n', ...
+        n, size(P_scaled.A, 1))
+    end
+
+    % Find controller using H-infinity
     [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt);
 
     fprintf('   H-infinity synthesis gamma: %g\n', gamma);
@@ -241,36 +322,55 @@ if do_musyn
     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 = 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
+      
+      % 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);
-
-        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 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;
+        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.
+            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_p)
+            break;
+          end
+          fprintf('.');
         end
-
-        if (fit_err / D_max_sv{j} < d_scales_max_err_p)
-          break;
-        end
-        fprintf('.');
+        fprintf(' degree %d, error %g (%g %%)\n', ...
+            best_fit_deg, best_fit_err, 100 * best_fit_err / D_max_sv{j});
       end
-      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
@@ -340,7 +440,7 @@ if do_musyn
   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.\n');
+    error(' - Failed to synthesize robust controller that meets the desired performance.');
   end
 
   %% Fit worst-case perturbation
@@ -364,11 +464,39 @@ if do_musyn
 %%  ------------------------------------------------------------------------
 % Measure Performance of mu synthesis design
 
+  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;
+  T = 40;
   nsamples = 5000;
-  do_noise = true;
+  do_noise = false;
 
   simout = uav_sim_step(params, model, ctrl.musyn, nsamples, T, do_plots, do_noise);