Clean up comments

1 files changed, 54 insertions, 37 deletions

diff --git a/uav.m b/uav.m
index 071ea76..275481b 100644
--- a/uav.m
+++ b/uav.m
@@ -9,8 +9,8 @@
 clear; clc; close all; s = tf('s');
 
 do_plots = true; % runs faster without
-do_hinf = false; % midterm
-do_musyn = true; % endterm
+do_hinf = true; % midterm
+do_musyn = false; % endterm
 
 if do_hinf & do_musyn
   error('Cannot do both H-infinity and mu synthesis.')
@@ -31,14 +31,14 @@ end
 % Synthesized controllers will be stored here
 ctrl = struct();
 
-% ------------------------------------------------------------------------
-%% Define system parameters
+%% ------------------------------------------------------------------------
+% Define system parameters
 
 fprintf('Generating system parameters...\n')
 params = uav_params();
 
-% ------------------------------------------------------------------------
-%% Define performance requirements
+%% ------------------------------------------------------------------------
+% Define performance requirements
 
 if do_hinf
   fprintf('Generating performance requirements...\n')
@@ -49,8 +49,8 @@ if do_musyn
   perf = uav_performance_musyn(params, do_plots);
 end
 
-%  ------------------------------------------------------------------------
-%% Define stability requirements
+%%  ------------------------------------------------------------------------
+% Define stability requirements
 
 % Note: for hinf it is needed to call uav_mode, but hinf will not actually 
 % make use of this struct
@@ -59,14 +59,14 @@ if do_hinf | do_musyn
   uncert = uav_uncertainty(params, do_plots);
 end
 
-% ------------------------------------------------------------------------
-%% Create UAV model
+%% ------------------------------------------------------------------------
+% Create UAV model
 
 fprintf('Generating system model...\n');
 model = uav_model(params, perf, uncert);
 
-% ------------------------------------------------------------------------
-%% Perform H-infinity design
+%% ------------------------------------------------------------------------
+% Perform H-infinity design
 
 if do_hinf
   fprintf('Performing H-infinty controller design...\n')
@@ -90,8 +90,8 @@ if do_hinf
     fprintf('Failed to syntesize controller (closed loop is unstable).\n')
   end
 
-%  ------------------------------------------------------------------------
-%% Measure Performance of H-infinity design
+%%  ------------------------------------------------------------------------
+% Measure Performance of H-infinity design
 
   fprintf('Simulating closed loop...\n');
 
@@ -109,12 +109,13 @@ if do_hinf
   writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
 end
 
-%  ------------------------------------------------------------------------
-%% Perform mu-Analysis & DK iteration
+%%  ------------------------------------------------------------------------
+% Perform mu-Analysis & DK iteration
 
 if do_musyn
   fprintf('Performing mu-synthesis controller design...\n')
 
+  % Get complete system (without debugging outputs for plots)
   idx = model.uncertain.index;
   P = minreal(model.uncertain.StateSpace(...
         [idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ...
@@ -128,24 +129,23 @@ if do_musyn
     'AutoScale', 'on', 'RelTol', 1e-2);
 
   % Frequency raster resolution to fit D scales
-  nsamples = 501;
-  omega = logspace(-3, 3, nsamples);
+  nsamples = 500;
+  omega = logspace(-5, 5, nsamples);
 
   % Initial values for D-K iteration
   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));
 
-  % degrees for approximations of D-scales, tuned by hand
+  % degrees for approximations for 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
+    1, 1;  % alpha
+    6, 1;  % omega
+    1, 1;  % state
+    7, 1;  % perf
   ];
 
   % Number of D-K iterations
   niters = size(fit_degrees, 2);
-  % niters = 5;
 
   % for plotting later
   mu_plot_legend = {};
@@ -153,7 +153,7 @@ if do_musyn
   % Start DK-iteration
   dkstart = tic;
   for it = 1:niters
-    fprintf(' - Running D-K iteration %d...\n', it);
+    fprintf(' - Running D-K iteration %d / %d...\n', it, niters);
     itstart = tic();
 
     % Find controller using H-infinity
@@ -178,7 +178,8 @@ if do_musyn
       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');
+      title('\bfseries $\mu_\Delta(\omega)$ for both Stability and Performance', ...
+            'interpreter', 'latex');
       legend(mu_plot_legend, 'interpreter', 'latex');
       grid on;
       drawnow;
@@ -195,29 +196,37 @@ if do_musyn
     % 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));
+    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));
+    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));
+    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));
+    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');
 
     % Construct full matrices
     D_right = blkdiag(D_alpha * eye(4), ...
@@ -232,6 +241,17 @@ if do_musyn
                      D_perf * eye(14), ...
                      eye(12));
 
+    % Compute peak of singular values for to estimate how good is the
+    % approximation of the D-scales
+    sv_left_samples = sigma(D_left_samples);
+    max_sv_left_samples = max(max(sv_left_samples));
+
+    sv_left = sigma(D_left, omega);
+    max_sv_left = max(max(sv_left));
+
+    fprintf('   Max SVD of D: %g, Dhat: %g\n', max_sv_left_samples, max_sv_left);
+    fprintf('   D scales fit abs. error: %g\n', abs(max_sv_left_samples - max_sv_left));
+
     % Plot fitted D-scales
     if do_plots
       fprintf('   Plotting D-scales');
@@ -279,6 +299,3 @@ if do_musyn
     ctrl.musyn = struct('K', K, 'mu', mu_rp);
   end
 end
-
-%  ------------------------------------------------------------------------
-%% Verify performance satisfaction via mu-analysis