Implement DK iteration

1 files changed, 44 insertions, 0 deletions

diff --git a/uav.m b/uav.m
index ec84682..62f6543 100644
--- a/uav.m
+++ b/uav.m
@@ -116,7 +116,51 @@ end
 %% Perform mu-Analysis & DK iteration
 
 if do_musyn
+  fprintf('Performing mu-synthesis controller design...\n')
 
+  idx = model.uncertain.index;
+  P = model.uncertain.StateSpace([idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ...
+                                 [idx.InputUncertain; idx.InputExogenous; idx.InputNominal]);
+
+  % Initial values for D-K iteration
+  D_left = eye(model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny);
+  D_right = eye(model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu);
+
+  % Options for H-infinity
+  nmeas = model.uncertain.Ny;
+  nctrl = model.uncertain.Nu;
+  hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
+    'AutoScale', 'off', 'RelTol', 1e-3);
+
+  % Number of D-K iterations
+  niters = 4;
+
+  % Frequency raster resolution to fit D scales
+  nsamples = 51;
+  omega = logspace(-2, 2, nsamples);
+
+  % Start DK-iteration
+  for it = 1:niters
+    fprintf(' - Running D-K iteration %d ...\n', it);
+
+    % Find controller using H-infinity
+    [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl);
+    fprintf('   H-infinity synthesis gamma: %g\n', gamma);
+
+    % Calculate frequency response of closed loop
+    N = minreal(lft(P, K), [], false); % slient
+    N_frd = frd(N, omega);
+
+    % Calculate upper bound D scaling
+    [mu_bounds, mu_info] = mussv(N_frd, model.uncertain.BlockStructurePerf, 'sU');
+    mu_rp = norm(mu_bounds(1,1), inf, 1e-6);
+    fprintf('   Mu value for RP: %g\n', mu_rp)
+
+    % Fit D-scales
+    [dsysl, dsysr] = mussvunwrap(mu_info);
+    D_left = fitfrd(genphase(dsysl(1,1)), 1);
+    D_right = fitfrd(genphase(dsysr(1,1)), 1);
+  end
 end
 
 %  ------------------------------------------------------------------------