From 83c1d7cbca88dc82fa873445e20e152ae1ee0507 Mon Sep 17 00:00:00 2001
From: Nao Pross <np@0hm.ch>
Date: Fri, 31 May 2024 14:26:27 +0200
Subject: Add worst case

---
 uav.m                       |  73 ++++++++++++++++++++++++++++++++++++++------
 uav_model_uncertain.slx     | Bin 64772 -> 65416 bytes
 uav_model_uncertain_clp.slx | Bin 66499 -> 77006 bytes
 uav_sim_step.m              |   4 ++-
 4 files changed, 67 insertions(+), 10 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
diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx
index 437c82d..4d6074a 100644
Binary files a/uav_model_uncertain.slx and b/uav_model_uncertain.slx differ
diff --git a/uav_model_uncertain_clp.slx b/uav_model_uncertain_clp.slx
index 10df3a6..46ee9d2 100644
Binary files a/uav_model_uncertain_clp.slx and b/uav_model_uncertain_clp.slx differ
diff --git a/uav_sim_step.m b/uav_sim_step.m
index 2130a3d..7bf0f2b 100644
--- a/uav_sim_step.m
+++ b/uav_sim_step.m
@@ -18,6 +18,7 @@ end
 
 % There is an uncertainty block
 if isfield(uncert, 'Delta')
+  fprintf(' - Running worst case with provided Delta.\n');
   hws.assignin('Delta', uncert.Delta);
 else
   fprintf(' - No uncertainty error Delta was provided, setting to zero.\n');
@@ -44,7 +45,8 @@ else
   eigvals = eig(P_clp.A);
   for i = 1:nx
     if real(eigvals(i)) >= 0
-      error('Closed loop is not stable!');
+      fprintf('   Closed loop is not stable!\n');
+      break;
     end
   end
 end
-- 
cgit v1.2.1