Update DK iteration and model structure

1 files changed, 110 insertions, 87 deletions

diff --git a/uav.m b/uav.m
index 4810943..2ee58da 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;
+do_musyn = true;
 
 fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n')
 fprintf('Will do:\n')
@@ -47,15 +47,12 @@ fprintf('Generating stability requirements...\n')
 uncert = uav_uncertainty(params, do_plots);
 
 %% ------------------------------------------------------------------------
-% Create UAV model
-
-fprintf('Generating system model...\n');
-model = uav_model(params, perf_musyn, uncert);
-
-%% ------------------------------------------------------------------------
 % Perform H-infinity design
 
 if do_hinf
+  fprintf('Generating system model for H-infinity design...\n');
+  model = uav_model(params, perf_hinf, uncert);
+
   fprintf('Performing H-infinty controller design...\n')
 
   idx = model.uncertain.index;
@@ -125,17 +122,17 @@ if do_hinf
 
   fprintf('Simulating closed loop...\n');
 
-  T = 40;
-  nsamples = 800;
+  T = 60;
+  nsamples = 5000;
   do_noise = false;
-  simout = uav_sim_step(params, model, ctrl.hinf, nsamples, T, do_plots, do_noise);
+  simout = uav_sim_step(params, model, ctrl.hinf, uncert, nsamples, T, do_plots, do_noise);
 
   fprintf(' - Writing simulation results...\n');
   cols = [
       simout.StepX(:, simout.index.Position), ...
       simout.StepX(:, simout.index.Velocity), ...
-      simout.StepX(:, simout.index.FlapAngles) * 180 / pi, ...
-      simout.StepX(:, simout.index.Angles) * 180 / pi];
+      simout.StepX(:, simout.index.PlotAlpha) * 180 / pi, ...
+      simout.StepX(:, simout.index.EulerAngles) * 180 / pi];
 
   writematrix([simout.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
 end
@@ -144,6 +141,8 @@ end
 drawnow;
 
 if do_musyn
+  fprintf('Generating system model for mu-synthesis design...\n');
+  model = uav_model(params, perf_musyn, uncert);
 
   fprintf('Performing mu-synthesis controller design...\n')
 
@@ -157,12 +156,12 @@ if do_musyn
   % Options for H-infinity
   nmeas = model.uncertain.Ny;
   nctrl = model.uncertain.Nu;
-  hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
+  hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
     'AutoScale', 'on', 'RelTol', 1e-2);
 
   % Frequency raster resolution to fit D scales
-  omega_max = 3;
-  omega_min = -2;
+  omega_max = 2.5;
+  omega_min = -3.5;
   nsamples = (omega_max - omega_min) * 100;
 
   omega = logspace(omega_min, omega_max, nsamples);
@@ -172,10 +171,6 @@ if do_musyn
   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));
 
-  % Maximum number of D-K iterations
-  niters = 5;
-  fprintf(' - Will do (max) %d iterations.\n', niters);
-
   % Maximum degree of D-scales and error
   d_scales_max_degree = 4;
   d_scales_max_err = 15;
@@ -203,21 +198,25 @@ if do_musyn
   gamma_max = 20;
   gamma_min = .8;
 
+  % Maximum number of D-K iterations
+  niters = 4;
+  fprintf(' - Will do (max) %d iterations.\n', niters);
+
   %% Run D-K iteration
 
   % hand tuned degrees, inf means will pick automatically best fit
   % according to parameters given above
   d_scales_degrees = {
-      0,   0,   0, inf, inf; % alpha
-      0, inf, inf, inf, inf; % omega
-      0, inf, inf, inf, inf; % state
-      0,   0,   0, inf, inf; % perf
+      0,   0,   0,   0,   0, inf, inf; % alpha
+      1,   1,   3,   3,   0, inf, inf; % omega
+      0,   1,   0,   0,   0, inf, inf; % state
+      0,   0,   0,   0,   0, inf, inf; % perf
   };
 
   if size(d_scales_degrees, 2) < niters
     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");
@@ -225,7 +224,7 @@ if do_musyn
 
     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};
 
@@ -239,6 +238,7 @@ if do_musyn
 
   dkstart = tic;
   for it = s:niters
+    fprintf('\n');
     fprintf(' * Running D-K iteration %d / %d...\n', it, niters);
     itstart = tic();
 
@@ -250,8 +250,7 @@ if do_musyn
     P_scaled = minreal(D_left * P * inv(D_right), [], false);
     [P_scaled, ~] = prescale(P_scaled, omega_range);
     n = size(P_scaled.A, 1);
-    
-    % disabled
+
     if false && n > scaled_plant_reduce_ord
       R = reducespec(P_scaled, 'balanced'); % or ncf
       R.Options.FreqIntervals = [omega_range{1}, omega_range{2}];
@@ -265,56 +264,66 @@ if do_musyn
 
       fprintf('   Scaled plant was reduced from order %d to %d\n', ...
         n, size(P_scaled.A, 1))
+
+      % update 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);
+    % For higher number of states we run into numerical problems
+    if n < 70
+      % Scaled plant (whole)
+      [nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(P_scaled);
 
-    [~, ~, ~, nustab, ~, nudetb] = pbhtest(A);
-    if nustab > 0 || nudetb > 0
-      fprintf('   Scaled plant has unstabilizable or undetectable A.\n');
-    end
+      fprintf('   Scaled plant has %d modes:\n', nx);
+      fprintf('       %d stable, %d unstable\n', nsta, nx - nsta);
+      fprintf('       %d controllable, %d unstabilizable\n', nctrb, nustab);
+      fprintf('       %d observable, %d undetectable\n', nobsv, nudetb);
 
-    [~, ~, ~, nustab, ~, nudetb] = pbhtest(Bu);
-    if nustab > 0 || nudetb > 0
-      fprintf('   Scaled plant has unstabilizable or undetectable Bu.\n');
-    end
+      % Check scaled plant parts meet 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(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);
+      [~, ~, ~, A_nustab, ~, A_nudetb] = pbhtest(A);
+      [~, ~, ~, Bu_nustab, ~, Bu_nudetb] = pbhtest(Bu);
+      [~, ~, ~, Cy_nustab, ~, Cy_nudetb] = pbhtest(Cy);
 
-    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
+      Deu = P_scaled(idx.OutputError, idx.InputNominal);
+      Dyw = P_scaled(idx.OutputNominal, idx.InputExogenous);
 
-    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
+      Deu_fullrank = rank(ctrb(Deu)) < length(Deu.A) || rank(obsv(Deu)) < lenth(Deu.A);
+      Dyw_fullrank = rank(ctrb(Dyw)) < length(Dyw.A) || rank(obsv(Dyw)) < lenth(Dyw.A);
 
-    % Scaled pant
-    eigvals = eig(P_scaled.A);
-    stable_modes = 0;
-    for i = 1:n
-      if real(eigvals(i)) < 0
-        stable_modes = stable_modes +1;
+      if A_nustab > 0 || A_nudetb > 0
+        fprintf('       A has %d unstabilizable modes and %d undetectable modes!\n', ...
+          A_nustab, A_nudetb);
+      end
+
+      if Bu_nustab > 0 || Bu_nudetb > 0
+        fprintf('       Bu has %d unstabilizable modes and %d undetectable modes!\n', ...
+          Bu_nustab, Bu_nudetb);
       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));
+      if Cy_nustab > 0 || Cy_nudetb > 0
+        fprintf('       Cy has %d unstabilizable modes and %d undetectable modes!\n', ...
+          Cy_nustab, Cy_nudetb);
+      end
 
-      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);
+      if ~ Deu_fullrank
+        fprintf('       Deu is not full rank!\n');
+      end
+
+      if ~ Dyw_fullrank
+        fprintf('       Dyw is not full rank!\n');
+      end
     else
+      eigvals = eig(P_scaled.A);
+      stable_modes = 0;
+      for i = 1:n
+        if real(eigvals(i)) < 0
+          stable_modes = stable_modes +1;
+        end
+      end
       fprintf('   Scaled plant has %d modes: %d stable %d unstable.\n', ...
         n, stable_modes, n - stable_modes);
     end
@@ -325,12 +334,12 @@ if do_musyn
       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, hinfopt);
+
+    % 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 = 1.2 * gamma;
 
-    [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');
@@ -364,13 +373,13 @@ if do_musyn
       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');
@@ -396,6 +405,11 @@ if do_musyn
                           'mu_rp', mu_rp, 'mu_rs', mu_rs);
     end
 
+    % No need to find scales for last iteration
+    if it == niters
+      break;
+    end
+
     % Fit D-scales
     [D_left_frd, D_right_frd] = mussvunwrap(mu_info_rp);
 
@@ -428,7 +442,7 @@ if do_musyn
     % for each block
     for j = 1:4
       fprintf('      %s', D_names{j});
-      
+
       % tuned by hand?
       if d_scales_degrees{j, it} < inf
         % D_fit = fitmagfrd(D_frd{j}, d_scales_degrees{j, it});
@@ -451,11 +465,11 @@ if do_musyn
           % 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
@@ -464,7 +478,7 @@ if do_musyn
             % the order of the D-scales.
             improvement = abs(best_fit_err - fit_err);
             improvement_p = improvement / best_fit_err;
-            
+
             if improvement_p > d_scales_improvement_p ...
               || best_fit_err == inf
 
@@ -473,7 +487,7 @@ if do_musyn
                 D_fitted{j} = D_fit;
             end
           end
-  
+
           if fit_err / D_max_sv{j} < d_scales_max_err_p ...
               || fit_err < d_scales_max_err
             break;
@@ -489,15 +503,15 @@ if do_musyn
     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), ...
+    D_left = blkdiag(D_fitted{1} * eye(4), ... % alpha uncert
+                     D_fitted{2} * eye(1), ... % omega uncert
+                     D_fitted{3} * eye(9), ... % state uncert
                      D_fitted{4} * eye(14), ...
                      eye(12));
 
-    D_right = blkdiag(D_fitted{1} * eye(4), ...
-                      D_fitted{2} * eye(1), ...
-                      D_fitted{3} * eye(12), ...
+    D_right = blkdiag(D_fitted{1} * eye(4), ... % alpha uncert
+                      D_fitted{2} * eye(1), ... % omega uncert
+                      D_fitted{3} * eye(9), ... % state uncert
                       D_fitted{4} * eye(10), ...
                       eye(5));
 
@@ -581,6 +595,15 @@ if do_musyn
 %%  ------------------------------------------------------------------------
 % Measure Performance of mu synthesis design
 
+  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 ...
+  );
+
   if do_plots
     fprintf(' - Plotting resulting controller...\n');
 
@@ -611,11 +634,11 @@ if do_musyn
 
   fprintf('Simulating nominal closed loop...\n');
 
-  T = 40;
+  T = 60;
   nsamples = 5000;
   do_noise = false;
 
-  simout = uav_sim_step(params, model, ctrl.musyn, nsamples, T, do_plots, do_noise);
+  simout = uav_sim_step(params, model, ctrl.musyn, uncert, nsamples, T, do_plots, do_noise);
 
   fprintf('Simulating worst-case closed loop...\n');