Delete LQR option from main script, start tuning D-scales

1 files changed, 41 insertions, 40 deletions

diff --git a/uav.m b/uav.m
index ca33346..071ea76 100644
--- a/uav.m
+++ b/uav.m
@@ -8,9 +8,7 @@
 
 clear; clc; close all; s = tf('s');
 
-% Flags to speed up running for debugging
 do_plots = true; % runs faster without
-do_lqr = false; % unused
 do_hinf = false; % midterm
 do_musyn = true; % endterm
 
@@ -23,9 +21,6 @@ fprintf('Will do:\n')
 if do_plots
   fprintf(' - Produce plots\n')
 end
-if do_lqr
-  fprintf(' - LQR synthesis\n')
-end
 if do_hinf
   fprintf(' - H-infinity synthesis\n')
 end
@@ -71,14 +66,6 @@ fprintf('Generating system model...\n');
 model = uav_model(params, perf, uncert);
 
 % ------------------------------------------------------------------------
-%% Perform LQR design
-
-if do_lqr
-  fprintf('Performing LQR controller design...\n')
-  ctrl.lqr = uav_ctrl_lqr(params, model);
-end
-
-% ------------------------------------------------------------------------
 %% Perform H-infinity design
 
 if do_hinf
@@ -140,29 +127,34 @@ if do_musyn
   hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
     'AutoScale', 'on', 'RelTol', 1e-2);
 
-  % Number of D-K iterations
-  niters = 8;
-
   % Frequency raster resolution to fit D scales
-  nsamples = 61;
-  omega = logspace(-2, 3, nsamples);
+  nsamples = 501;
+  omega = logspace(-3, 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;
+  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));
 
-  nright = model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu;
-  nright_clp = model.uncertain.Nv + model.uncertain.Nw;
+  % degrees for approximations of D-scales, tuned by hand
+  fit_degrees = [
+    2, 1, 1, 1; % 1, 1; % alpha
+    2, 4, 1, 1; % 1, 1; % omega
+    1, 2, 1, 1; % 1, 1; % state
+    3, 4, 1, 1; % 1, 1; % perf
+  ];
 
-  D_left = tf(eye(nleft));
-  D_right = tf(eye(nright));
+  % Number of D-K iterations
+  niters = size(fit_degrees, 2);
+  % niters = 5;
 
-  last_mu_rp = inf;
+  % for plotting later
   mu_plot_legend = {};
 
   % Start DK-iteration
+  dkstart = tic;
   for it = 1:niters
-    fprintf(' - Running D-K iteration %d ...\n', it);
+    fprintf(' - Running D-K iteration %d...\n', it);
+    itstart = tic();
 
     % Find controller using H-infinity
     [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl, hinfopt);
@@ -206,22 +198,26 @@ if do_musyn
     % 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_alpha = fitmagfrd(D_left_samples_alpha, fit_degrees(1, it));
+    D_alpha = fitfrd(genphase(D_left_samples_alpha), fit_degrees(1, it));
 
     % 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_omega = fitmagfrd(D_left_samples_omega, fit_degrees(2, it));
+    D_omega = fitfrd(genphase(D_left_samples_omega), fit_degrees(2, it));
 
     % 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_state = fitmagfrd(D_left_samples_state, fit_degrees(3, it));
+    D_state = fitfrd(genphase(D_left_samples_state), fit_degrees(3, it));
 
     % 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);
+    % D_perf = fitmagfrd(D_left_samples_perf, fit_degrees(4, it));
+    D_perf = fitfrd(genphase(D_left_samples_perf), fit_degrees(4, it));
 
     % Construct full matrices
     D_right = blkdiag(D_alpha * eye(4), ...
@@ -238,27 +234,27 @@ if do_musyn
 
     % Plot fitted D-scales
     if do_plots
-      fprintf('   Plotting D-scales ');
+      fprintf('   Plotting D-scales');
       f = figure(101); clf(f); hold on;
 
-      bodemag(D_left_samples_alpha, omega);
-      bodemag(D_alpha, omega);
+      bodemag(D_left_samples_alpha, omega, 'r-');
+      bodemag(D_alpha, omega, 'b');
       fprintf('.');
 
-      bodemag(D_left_samples_omega, omega);
-      bodemag(D_omega, omega);
+      bodemag(D_left_samples_omega, omega, 'r--');
+      bodemag(D_omega, omega, 'b--');
       fprintf('.');
 
-      bodemag(D_left_samples_state, omega);
-      bodemag(D_state, omega);
+      bodemag(D_left_samples_state, omega, 'c-');
+      bodemag(D_state, omega, 'm-');
       fprintf('.');
 
-      bodemag(D_left_samples_perf, omega);
-      bodemag(D_perf, omega);
+      bodemag(D_left_samples_perf, omega, 'c--');
+      bodemag(D_perf, omega, 'm--');
       fprintf('.');
 
       fprintf('\n');
-      title(sprintf('\bfseries $D(\\omega)$ Scales Approximations at Iteration %d', it), ...
+      title(sprintf('\\bfseries $D(\\omega)$ Scales Approximations at Iteration %d', it), ...
             'interpreter', 'latex')
       legend(...
         '$D_{\alpha}$', '$\hat{D}_{\alpha}$', ...
@@ -270,7 +266,12 @@ if do_musyn
       grid on;
       drawnow;
     end
+
+    itend = toc(itstart);
+    fprintf('   Iteration took %.1f seconds\n', itend);
   end
+  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');