Add worst case

1 files changed, 64 insertions, 9 deletions

diff --git a/uav.m b/uav.m
index 7676148..4155e93 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;
+do_hinf = false;
 do_musyn = true;
 
 fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n')
@@ -134,7 +134,7 @@ if do_hinf
       simout.StepX(:, simout.index.PlotAlpha) * 180 / pi, ...
       simout.StepX(:, simout.index.EulerAngles) * 180 / pi];
 
-  writematrix([simout.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
+  writematrix([simout.Time', cols], 'fig/stepsim_hinf.dat', 'Delimiter', 'tab')
 end
 %%  ------------------------------------------------------------------------
 % Perform mu-Analysis & DK iteration
@@ -208,7 +208,7 @@ if do_musyn
   % according to parameters given above
   d_scales_degrees = {
       0,   0,   0,   0,   0, inf, inf; % alpha
-    inf, inf, inf, inf, inf, inf, inf; % omega
+      5,   1, inf, inf, inf, inf, inf; % omega
       0,   0,   0,   0,   0, inf, inf; % state
       0,   0,   0,   0,   0, inf, inf; % perf
   };
@@ -346,6 +346,10 @@ if do_musyn
       error('Failed to synethesize H-infinity controller');
     end
 
+     if it == 1
+       K_hinf = K;
+     end
+
     % Calculate frequency response of closed loop
     N = minreal(lft(P, K), [], false);
     M = minreal(N(idx.OutputUncertain, idx.InputUncertain), [], false);
@@ -370,6 +374,12 @@ if do_musyn
       mu_bounds_rp_prev{it} = mu_bounds_rp;
       mu_bounds_rs_prev{it} = mu_bounds_rs;
 
+      % Save for worst case perturbation analysis
+      if it == 1
+        mu_bounds_rp_first = mu_bounds_rp;
+        mu_info_rp_first = mu_info_rp;
+      end
+
       % Plot SSVs
       if do_plots
         fprintf('   Plotting SSV mu\n');
@@ -578,13 +588,43 @@ if do_musyn
   %% Fit worst-case perturbation
   fprintf(' - Computing worst case perturbation.\n')
 
-  % Find peak of mu
-  [mu_upper_bound_rp, ~] = frdata(mu_bounds_rp(1,1));
-  max_mu_rp_idx = find(mu_rp == mu_upper_bound_rp, 1);
-  Delta = mussvunwrap(mu_info_rp);
-  % TODO: finish here
+  % Find worst case perturbation on first controller
+  max_mus = max(frdata(mu_bounds_rp_first));
+  max_idx = find(max_mus == max(max_mus));
+  max_idx = max_idx(1);
+
+  Delta_frd = mussvunwrap(mu_info_rp_first);
+  Delta_frd = frdata(Delta_frd);
+  Delta_frd = Delta_frd(:, :, max_idx);
+
+  % Fit a plant
+  Delta = ss(zeros(model.uncertain.Nv, model.uncertain.Nz));
+  s = tf('s');
+
+  for r = 1:model.uncertain.Nv
+    for c = 1:model.uncertain.Nz
+      d = Delta_frd(r, c);
+      g = abs(d);
+
+      if g < 1e-6
+        continue
+      end
+
+      if imag(d) > 0
+        d = -1 * d;
+        g = -1 * g;
+      end
+      x = real(d) / abs(g);
+      tau = 2 * omega(max_idx) * (sqrt((1 + x) / (1 - x)));
+      Delta(r,c) = g * (-s + tau / 2) / (s + tau / 2);
+    end
+  end
+
+  Delta = Delta / norm(Delta, inf);
+
+  % Save controllers
+  ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_hinf);
 
-  % Save controller
   ctrl.musyn = struct('Name', '$\mu$-Synthesis', ...
                       'K', K, 'Delta', Delta, ...
                       'mu_rp', mu_rp, 'mu_rs', mu_rs);
@@ -640,7 +680,22 @@ if do_musyn
   do_noise = false;
 
   simout = uav_sim_step(params, model, ctrl.musyn, uncert, 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.PlotAlpha) * 180 / pi, ...
+  %     simout.StepX(:, simout.index.EulerAngles) * 180 / pi];
+  % 
+  % writematrix([simout.Time', cols], 'fig/stepsim_musyn.dat', 'Delimiter', 'tab')
+
+  %% Worst case analysis
   fprintf('Simulating worst-case closed loop...\n');
 
+  uncert.Delta = ctrl.musyn.Delta;
+  simout = uav_sim_step(params, model, ctrl.musyn, uncert, nsamples, T, do_plots, do_noise);
+  simout = uav_sim_step(params, model, ctrl.hinf, uncert, nsamples, 5, do_plots, do_noise);
+  uncert = rmfield(uncert, 'Delta');
 end