Clean up comments

4 files changed, 83 insertions, 75 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
diff --git a/uav_performance_hinf.m b/uav_performance_hinf.m
index 6ece761..0a12b72 100644
--- a/uav_performance_hinf.m
+++ b/uav_performance_hinf.m
@@ -83,7 +83,8 @@ if do_plots
     '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
     '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ...
     'interpreter', 'latex', 'fontSize', 8);
-  title('Performance Requirements');
+  title('\bfseries Performance Requirements ($\mathcal{H}_\infty$ Weights)', ...
+        'interpreter', 'latex');
 
   % Step response of position requirements
   figure; hold on;
@@ -96,7 +97,8 @@ if do_plots
     '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
     '$W_{P,\alpha}$', '$W_{P,\omega}$', ...
     'interpreter', 'latex', 'fontSize', 8);
-  title('Step responses of performance requirements');
+  title('\bfseries Step responses of $\mathcal{H}_\infty$ Weights', ...
+        'interpreter', 'latex');
 end
 
 end
diff --git a/uav_performance_musyn.m b/uav_performance_musyn.m
index 6ece761..180be8a 100644
--- a/uav_performance_musyn.m
+++ b/uav_performance_musyn.m
@@ -13,44 +13,27 @@
 %   PERF    Struct performance transfer functions
 
 
-function [perf] = uav_performance(params, do_plots)
+function [perf] = uav_performance_musyn(params, do_plots)
 
 % Laplace variable
 s = tf('s');
 
-alpha_max = params.actuators.ServoAbsMaxAngle;
-alpha_max_omega = params.actuators.ServoNominalAngularVelocity;
+% TODO: set proper constraints
+G = 1e2 / (s + 1e2);
 
-T_xy = params.performance.HorizontalSettleTime;
-T_z = params.performance.VerticalSettleTime;
+W_Palpha = G; 
+W_Pomega = G;
 
-omega_nxy = 5 / T_xy;
-omega_nz = 10 / T_z;
+W_Pxy = G;
+W_Pz = G;
 
-% W_Palpha = 1 / (s^2 + 2 * alpha_max_omega * s + alpha_max_omega^2);
-% W_Palpha = (1 - W_Palpha / dcgain(W_Palpha)) * .8;
-% W_Pomega = (1 - 1 / (T_z / 5 * s + 1)) * .1;
-
-W_Palpha = make_weight(alpha_max_omega, 15, 1.1, 3);
-W_Pomega = make_weight(omega_nz, 50, 10);
-
-% zeta = 1; % Almost critically damped
-% W_Pxy = 1 / (s^2  + 2 * zeta * omega_nxy * s + omega_nxy^2);
-% W_Pxy = 1 * W_Pxy / dcgain(W_Pxy);
-% W_Pz  = 1 / (s^2 + 2 * zeta * omega_nz * s + omega_nz^2);
-% W_Pz  = 1 * W_Pz / dcgain(W_Pz);
-
-W_Pxy = make_weight(omega_nxy, 2, 5);
-W_Pz = make_weight(omega_nz, 1, 10);
-
-% Set a speed limit
-W_Pxydot = .2 * tf(1, 1);
-W_Pzdot = .2 * tf(1, 1);
+W_Pxydot = tf(0);
+W_Pzdot = tf(0);
  
-W_Pphitheta = .01 * tf(1, [.1, 1]);
-W_Ppsi = .01 * tf(1, 1); % don't care
+W_Pphitheta = tf(0);
+W_Ppsi = tf(0);
 
-W_PTheta = tf(1, [.1, 1]) * eye(3);
+W_PTheta = tf(0);
 
 % Construct performance vector by combining xy and z
 W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
diff --git a/uav_uncertainty.m b/uav_uncertainty.m
index 6f28d8d..aff30ca 100644
--- a/uav_uncertainty.m
+++ b/uav_uncertainty.m
@@ -28,13 +28,18 @@ eps_d = params.aerodynamics.DragCoefficientsUncertainties(1);
 eps_omega = max(.5 * eps_T, eps_r);
 eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega);
 
+% TODO: set proper constraints
 % band pass parameters for W_malpha
-wh = 20; % high freq
-wl = .1; % low freq
+% wh = 20; % high freq
+% wl = .1; % low freq
 
-W_malpha = eps_alpha * (s / wl) / ((s / wh + 1) * (s / wl + 1));
-W_momega = eps_omega * 10 / (s + 10);
-W_mState = .01 * 10 / (s + 10);
+% W_malpha = eps_alpha * (s / wl) / ((s / wh + 1) * (s / wl + 1));
+% W_momega = eps_omega * 10 / (s + 10);
+% W_mState = .01 * 10 / (s + 10);
+
+W_malpha = eps_alpha * tf(1);
+W_momega = tf(0);
+W_mState = .01 * tf(1);
 
 uncert = struct(...
   'FlapAngle', W_malpha * eye(4), ...
@@ -50,8 +55,9 @@ if do_plots
 
   grid on;
   legend('$W_{m,\alpha}$', '$W_{m,\omega}$', '$W_{m,\mathbf{P}}$', ...
-    'interpreter', 'latex')
-  title('Stability (Uncertainty) Requirement')
+         'interpreter', 'latex')
+  title('\bfseries Stability Requirement (only for $\mu$-Synthesis)', ...
+        'interpreter', 'latex')
 end
 
 end