Update DK iteration and model structure

10 files changed, 579 insertions, 355 deletions

diff --git a/pbhtest.m b/pbhtest.m
index f56aa21..546bbc6 100644
--- a/pbhtest.m
+++ b/pbhtest.m
@@ -1,4 +1,4 @@
-% Do A PBH test on system. Given SYS returns:
+% Do A PBH test on system. Given a state space model SYS returns:
 %
 %  - Nr. of states
 %  - Nr. of stable states
@@ -23,9 +23,9 @@ end
 % Check system controllability / stabilizability
 Wc = ctrb(sys);
 nctrb = rank(Wc);
+nustab = 0;
 if nctrb < nx
   % Is the system at least stabilizable?
-  nustab = 0;
   for i = 1:nx
     if real(eigvals(i)) >= 0
       % PBH test
@@ -40,13 +40,13 @@ end
 % Check system observability / detectability
 Wo = obsv(sys);
 nobsv = rank(Wo);
+nudetb = 0;
 if nobsv < nx
   % is the system at least detectable?
-  nudetb = 0;
   for i = 1:nx
     if real(eigvals(i)) >= 0
       % PBH test
-      W = [sys.C; (sys.A - eigvals(i) * eye(nx))];
+      W = [(sys.A' - eigvals(i) * eye(nx)), sys.C'];
       if rank(W) < nx
         nudetb = nudetb + 1;
       end
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');
 
diff --git a/uav_model.m b/uav_model.m
index 6a7f9da..af0be20 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -221,16 +221,13 @@ C = eye(size(A));
 D = zeros(12, 5);
 
 % ------------------------------------------------------------------------
-% Model actuators
+% Model actuators (normalized to [-1, 1]!)
 
-% TODO: better model?
-w = params.actuators.ServoNominalAngularVelocity;
-zeta = 1;
-G_servo = tf(w^2, [1, 2 * zeta * w, w^2]);
+T = params.actuators.ServoSecondsTo60Deg;
+G_servo = tf(1, [T, 1]);
 
-w = 60;
-zeta = 1;
-G_prop = tf(w^2, [1, 2 * zeta * w, w^2]);
+% TODO: measurements for propeller
+G_prop = tf(1, [.1, 1]);
 
 model.actuators = struct( ...
   'FlapServo', G_servo, ...
@@ -240,28 +237,43 @@ model.actuators = struct( ...
 % ------------------------------------------------------------------------
 % Scale inputs and outputs of linearized model
 
+% All scaling matrices scale UP FROM RANGE (-1, 1), i.e. for outputs they can
+% be used as-is, while for inputs they need to be inverted
+
 S_actuators = blkdiag(...
-  eye(4) * 1 / params.actuators.ServoAbsMaxAngle, ...
-  eye(1) * 1 / params.actuators.PropellerMaxAngularVelocity);
+  eye(4) * params.normalization.FlapAngle, ...
+  eye(1) * params.normalization.ThrustAngularVelocity ...
+);
 
-S_state = blkdiag(...
+S_positions = blkdiag(...
   eye(2) * params.normalization.HPosition, ...
-  eye(1) * params.normalization.VPosition, ...
+  eye(1) * params.normalization.VPosition ...
+);
+
+S_velocities = blkdiag(...
   eye(2) * params.normalization.HSpeed, ...
-  eye(1) * params.normalization.VSpeed, ...
+  eye(1) * params.normalization.VSpeed ...
+);
+
+S_angles = blkdiag(...
   eye(2) * params.normalization.PitchRollAngle, ...
-  eye(1) * params.normalization.YawAngle, ...
-  eye(3) * params.normalization.AngularRate);
+  eye(1) * params.normalization.YawAngle ...
+);
+
+S_angular_velocities = eye(3) * params.normalization.AngularRate;
+
+S_state = blkdiag(S_positions, S_velocities, S_angles, S_angular_velocities);
 
 % Scale system matrices to have inputs and outputs between zero and one
-B = B * inv(S_actuators);
+B = B * S_actuators;
 C = inv(S_state) * C;
-D = D * inv(S_actuators);
+D = D * S_actuators;
 
 % Create state space object
 T = params.measurements.SensorFusionDelay;
 n = params.linearization.PadeApproxOrder;
 sys = pade(ss(A, B, C, D, 'OutputDelay', T), n);
+% sys = ss(A, B, C, D);
 
 % Add actuators
 sys = sys * model.actuators.StateSpace;
@@ -279,7 +291,7 @@ model.linear = struct(...
   'State', xi, 'Inputs', u, ... % state and input variables
   'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized
   'StateSpace', sys, ... % state space objec
-  'InputScalingMatrix', S_actuators, 'OutputScalingMatrix', S_state ...
+  'InputScaling', S_actuators, 'OutputScaling', S_state ...
 );
 
 % ------------------------------------------------------------------------
@@ -293,7 +305,7 @@ fprintf('   %d controllable modes, %d unstabilizable modes.\n', nctrb, nustab);
 fprintf('   %d observable modes, %d undetectable modes.\n', nobsv, nustab);
 
 if nctrb < 12
-  error('Linearized model has less than 12 controllable modes!');
+  fprintf('   Linearized model has less than 12 controllable modes!\n');
 end
 
 if nustab > 0 || nudetb > 0
@@ -301,11 +313,6 @@ if nustab > 0 || nudetb > 0
 end
 
 % ------------------------------------------------------------------------
-% Compute absolute value of error caused by linearization around set point
-
-% TODO
-
-% ------------------------------------------------------------------------
 % Add uncertainties using SIMULINK model
 
 % Load simulink model with uncertainties and pass in parameters
@@ -320,13 +327,13 @@ hws.assignin('uncert', uncert);
 
 % Get uncertain model
 ulmod = linmod('uav_model_uncertain');
-usys = ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d);
+usys = minreal(ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d), [], false);
 
 % Specify uncertainty block structure for mussv command
 blk_stab = [
   4, 4; % alpha uncert, full
   1, 1; % omega uncert, full
-  12, 12; % state uncert, full
+  9, 9; % state uncert, full
 ];
 
 blk_perf = [
@@ -334,29 +341,17 @@ blk_perf = [
   10, 14 % always full
 ];
 
-% ------------------------------------------------------------------------
-% Scale inputs and outputs of uncertain model
-
-% Scaling of reference is same as position
-S_ref = blkdiag(...
-  eye(2) * 1 / params.normalization.HPosition, ...
-  eye(1) * 1 / params.normalization.VPosition);
-
-% TODO: finish
-S_uncert_out = blkdiag(...
-  S_actuators, ...
-  S_state, ...
-  S_actuators, ...
-  S_state(1:9, 1:9));
-
 % Save uncertain model
 model.uncertain = struct(... 
-  ... % 'Simulink', ulmod, ...
+  'Simulink', ulmod, ...
   'BlockStructure', blk_stab, ...
   'BlockStructurePerf', blk_perf, ...
   'StateSpace', usys ...
 );
 
+% ------------------------------------------------------------------------
+% Create indices to take subsystems
+
 % The uncertain system is partitioned into the following matrix
 %
 % [ z ]   [ A    B_w   B_u  ] [ v ]
@@ -369,33 +364,189 @@ model.uncertain = struct(...
 %   - model.uncertain.Simulink.InputName(model.uncertain.index.InputX)
 %   - model.uncertain.Simulink.OutputName(model.uncertain.index.OutputX)
 %
-% Function make_idx(start, size) is defined below.
-model.uncertain.index = struct(...
-  'InputUncertain',    make_idx( 1, 17), ... % 'v' inputs
-  'InputDisturbance',  make_idx(18,  7), ... % 'w' inputs for noise
-  'InputReference',    make_idx(25,  3), ... % 'w' inputs for reference
-  'InputExogenous',    make_idx(18, 10), ... % 'w' inputs (all of them)
-  'InputNominal',      make_idx(28,  5), ... % 'u' inputs
-  'OutputUncertain',   make_idx( 1, 17), ... % 'z' outputs
-  'OutputError',       make_idx(18, 14), ... % 'e' outputs
-  'OutputNominal',     make_idx(32, 12), ... % 'y' outputs
-  'OutputPlots',       make_idx(44, 10)  ... % 'p' outputs for plots in closed loop
+
+% Size of signals to generate indices
+input_dims = struct(...
+  'InputUncertainAlpha',    4, ... % 'v' inputs
+  'InputUncertainOmega',    1, ... %  .
+  'InputUncertainState',    9, ... %  .
+  'InputDisturbanceWind',   3, ... % 'w' inputs
+  'InputDisturbanceAlpha',  4, ... %  .
+  'InputReference',         3, ... %  .
+  'InputNominalAlpha',      4, ... % 'u' inputs
+  'InputNominalOmega',      1 ...  %  .
 );
 
-idx = model.uncertain.index;
+output_dims = struct(...
+  'OutputUncertainAlpha',      4, ... % 'z' outputs
+  'OutputUncertainOmega',      1, ... %  .
+  'OutputUncertainState',      9, ... %  .
+  'OutputErrorAlpha',          4, ... % 'e' outputs
+  'OutputErrorOmega',          1, ... %  .
+  'OutputErrorPosition',       3, ... %  .
+  'OutputErrorVelocity',       3, ... %  .
+  'OutputErrorEulerAngles',    3, ... %  .
+  'OutputNominalPosition',     3, ... % 'y' outputs
+  'OutputNominalVelocity',     3, ... %  .
+  'OutputNominalEulerAngles',  3, ... %  .
+  'OutputNominalAngularRates', 3, ... %  .
+  'OutputPlotAlpha',           4, ... %  plot / "debugging" outputs
+  'OutputPlotOmega',           1, ... %  .
+  'OutputPlotReference',       3, ... %  .
+  'OutputPlotPosition',        3 ...  %  .
+);
 
-% Number of inputs
-model.uncertain.Nv = max(size(idx.InputUncertain));
-model.uncertain.Nw = max(size(idx.InputExogenous));
-model.uncertain.Nu = max(size(idx.InputNominal));
+model.uncertain.dims = struct(...
+  'input', input_dims, ...
+  'output', output_dims ...
+);
+
+% model.uncertain.dims.OutputError = sum([
+%   model.uncertain.dims.OutputErrorAlpha;
+%   model.uncertain.dims.OutputErrorOmega;
+%   model.uncertain.dims.OutputErrorPosition;
+%   model.uncertain.dims.OutputErrorVelocity;
+%   model.uncertain.dims.OutputErrorEulerAngles;
+% ]);
+% 
+% model.uncertain.dims.OutputNominal; = sum([
+%   model.uncertain.dims.OutputNominalPosition;
+%   model.uncertain.dims.OutputNominalVelocity;
+%   model.uncertain.dims.OutputNominalEulerAngles;
+%   model.uncertain.dims.OutputNominalAngularRates;
+% ]);
+% 
+% model.uncertain.dims.OutputPlot = sum([
+%   model.uncertain.dims.OutputPlotAlpha;
+%   model.uncertain.dims.OutputPlotOmega;
+%   model.uncertain.dims.OutputPlotReference;
+% ]);
+
+% Create indices
+model.uncertain.index = struct();
+
+% Create indices for inputs
+start = 1; fields = fieldnames(input_dims);
+for f = fields'
+  dim = getfield(input_dims, f{1});
+  i = (start:(start + dim - 1))';
+  model.uncertain.index = setfield(model.uncertain.index, f{1}, i);
+  start = start + dim;
+end
+
+% Create indices for output
+start = 1; fields = fieldnames(output_dims);
+for f = fields'
+  dim = getfield(output_dims, f{1});
+  i = (start:(start + dim - 1))';
+  model.uncertain.index = setfield(model.uncertain.index, f{1}, i);
+  start = start + dim;
+end
+
+% Create groups of inputs
+model.uncertain.index.InputUncertain = [
+  model.uncertain.index.InputUncertainAlpha;
+  model.uncertain.index.InputUncertainOmega;
+  model.uncertain.index.InputUncertainState;
+];
+
+model.uncertain.index.InputDisturbance = [
+  model.uncertain.index.InputDisturbanceWind;
+  model.uncertain.index.InputDisturbanceAlpha;
+];
+
+model.uncertain.index.InputExogenous = [
+  model.uncertain.index.InputDisturbance;
+  model.uncertain.index.InputReference;
+];
+
+model.uncertain.index.InputNominal = [
+  model.uncertain.index.InputNominalAlpha;
+  model.uncertain.index.InputNominalOmega;
+];
 
-model.uncertain.Nr = max(size(idx.InputReference));
+% Create groups of outputs
+model.uncertain.index.OutputUncertain = [
+  model.uncertain.index.OutputUncertainAlpha;
+  model.uncertain.index.OutputUncertainOmega;
+  model.uncertain.index.OutputUncertainState;
+];
+
+model.uncertain.index.OutputError = [
+  model.uncertain.index.OutputErrorAlpha;
+  model.uncertain.index.OutputErrorOmega;
+  model.uncertain.index.OutputErrorPosition;
+  model.uncertain.index.OutputErrorVelocity;
+  model.uncertain.index.OutputErrorEulerAngles;
+];
+
+model.uncertain.index.OutputNominal = [
+  model.uncertain.index.OutputNominalPosition;
+  model.uncertain.index.OutputNominalVelocity;
+  model.uncertain.index.OutputNominalEulerAngles;
+  model.uncertain.index.OutputNominalAngularRates;
+];
+
+model.uncertain.index.OutputPlot = [
+  model.uncertain.index.OutputPlotAlpha;
+  model.uncertain.index.OutputPlotOmega;
+  model.uncertain.index.OutputPlotReference;
+];
+
+% Number of inputs
+model.uncertain.Nv = max(size(model.uncertain.index.InputUncertain));
+model.uncertain.Nw = max(size(model.uncertain.index.InputExogenous));
+model.uncertain.Nu = max(size(model.uncertain.index.InputNominal));
+model.uncertain.Nr = max(size(model.uncertain.index.InputReference));
 % size of noise is (Nw - Nr)
 
 % Number of outputs
-model.uncertain.Nz = max(size(idx.OutputUncertain));
-model.uncertain.Ne = max(size(idx.OutputError));
-model.uncertain.Ny = max(size(idx.OutputNominal));
+model.uncertain.Nz = max(size(model.uncertain.index.OutputUncertain));
+model.uncertain.Ne = max(size(model.uncertain.index.OutputError));
+model.uncertain.Ny = max(size(model.uncertain.index.OutputNominal));
+
+% ------------------------------------------------------------------------
+% Scaling matrices for uncertain model
+
+% Note howerver that the uncertain (SIMULINK) model is entirely described in
+% normalized coordinates, so these matrices are to scale the result e.g. for
+% plotting.
+
+% Input matrices scale DOWN TO range (-1, 1)
+% Output matrices scale UP FROM range (-1, 1)
+
+model.uncertain.scaling = struct();
+
+% TODO Scale for 'v' inputs if necessary
+
+% Scale for 'w' inputs
+model.uncertain.scaling.InputUncertainScaling = blkdiag(...
+  S_actuators, ...
+  S_positions ... % reference use same as position
+);
+
+% Scale for 'u' inputs
+model.uncertain.scaling.InputNominalScaling = S_actuators;
+
+% TODO Scale for 'z' outputs if necessary
+
+% Scale for 'e' outputs
+model.uncertain.scaling.OutputErrorScaling = blkdiag(...
+  S_actuators, ...
+  S_positions, ...
+  S_velocities, ...
+  S_angles ...
+);
+
+% Scale for 'y' outputs
+model.uncertain.scaling.OutputNominalScaling = S_state;
+
+% Scale for 'p' outputs
+model.uncertain.scaling.OutputPlotScaling = blkdiag(...
+  S_actuators, ...
+  S_positions, ... % reference same as position
+  S_positions ...
+);
 
 % ------------------------------------------------------------------------
 % Check properties of uncertain model
@@ -408,7 +559,7 @@ fprintf('   %d controllable modes, %d unstabilizable modes.\n', nctrb, nustab);
 fprintf('   %d observable modes, %d undetectable modes.\n', nobsv, nustab);
 
 if nctrb < 12
-  error('Uncertain model has less than 12 controllable modes!');
+  fprintf('   Uncertain model has less than 12 controllable modes!');
 end
 
 if nustab > 0 || nudetb > 0
@@ -424,8 +575,14 @@ A = model.uncertain.StateSpace(...
 
 [~, ~, ~, nustab, ~, nudetb] = pbhtest(A);
 
-if nustab > 0 || nudetb > 0
-  fprintf('   Uncertain system has undetectable or uncontrollable A.\n');
+if nustab > 0
+  fprintf('   Uncertain system has unstabilizable A! (%d modes)\n', ...
+    nustab);
+end
+
+if nudetb > 0
+  fprintf('   Uncertain system has undetectable A! (%d modes)\n', ...
+    nudetb);
 end
 
 B_u = model.uncertain.StateSpace(...
@@ -435,8 +592,14 @@ B_u = model.uncertain.StateSpace(...
 
 [~, ~, ~, nustab, ~, nudetb] = pbhtest(B_u);
 
-if nustab > 0 || nudetb > 0
-  fprintf('   Uncertain system has undetectable or uncontrollable Bu.\n');
+if nustab > 0
+    fprintf('   Uncertain system has unstabilizable Bu! (%d modes)\n', ...
+      nustab);
+end
+
+if nudetb > 0
+  fprintf('   Uncertain system has undetectable Bu! (%d modes)\n', ...
+    nudetb);
 end
 
 C_y = model.uncertain.StateSpace(...
@@ -446,8 +609,12 @@ C_y = model.uncertain.StateSpace(...
 
 [~, ~, ~, nustab, ~, nudetb] = pbhtest(C_y);
 
-if nustab > 0 || nudetb > 0
-  fprintf('   Uncertain system has undetectable or uncontrollable Cy.\n');
+if nustab > 0
+    fprintf('   Uncertain system has unstabilizable Cy!\n');
+end
+
+if nudetb > 0
+  fprintf('   Uncertain system has undetectable Cy!\n');
 end
 
 
@@ -474,8 +641,4 @@ end
 
 end
 
-function [indices] = make_idx(start, size)
-  indices = (start:(start + size - 1))';
-end
-
 % vim: ts=2 sw=2 et:
diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx
index fd94980..6f1b050 100644
--- a/uav_model_uncertain.slx
+++ b/uav_model_uncertain.slx
diff --git a/uav_model_uncertain_clp.slx b/uav_model_uncertain_clp.slx
index 36be668..10df3a6 100644
--- a/uav_model_uncertain_clp.slx
+++ b/uav_model_uncertain_clp.slx
diff --git a/uav_params.m b/uav_params.m
index fdd519a..2ac187e 100644
--- a/uav_params.m
+++ b/uav_params.m
@@ -35,7 +35,7 @@ J = [
 ] * 1e-9;
 
 % Approximate propeller with a disk
-m_prop = 200e-3; % mass of propeller
+m_prop = 500e-3; % mass of propeller
 r_prop = 85e-3; % radius of propeller
 J_prop = .5 * m_prop * r_prop^2;
 
@@ -46,26 +46,23 @@ params.mechanical = struct(...
   'FlapZDistance', 98e-3, ... % flap distance along z from center of mass
   'InertiaTensor', J, ...
   'GyroscopicInertiaZ', J_prop, ... % assume small angle
-  'GyroscopicInertiaZUncertainty', .01 ... % in %
+  'GyroscopicInertiaZUncertainty', .05 ... % in %
 );
 
 % ------------------------------------------------------------------------
 % Actuator limits and measurements
 
 % Servos usually give a "speed" in seconds / 60 degrees without load
-servo_speed = 0.13; % seconds / 60 deg
-servo_angular_velocity = (60 / servo_speed) * (pi / 180); % rad / s
-
 params.actuators = struct(...
   'PropellerMaxAngularVelocity', 620.7, ... % in rad / s
   'ServoAbsMaxAngle', 20 * pi / 180, ... % in radians
   'ServoMaxTorque', 4.3 * 1e-2, ... % in kg / m
-  'ServoNominalAngularVelocity', servo_angular_velocity ...
+  'ServoSecondsTo60Deg', 0.13 ... % in s / 60 deg
 );
 
 % IMU runs in NDOF mode
 params.measurements = struct(...
-  'SensorFusionDelay', 30e-3, ... % in s
+  'SensorFusionDelay', 10e-3, ... % in s, from 100 Hz sampling rate
   'LIDARAccuracy', 10e-3, ... % in m
   'LIDARMaxDistance', 40, ... % inm
   'LIDARBandwidth', 100, ... % in Hz for z position
@@ -91,13 +88,13 @@ k_T = F_max / omega_max^2;
 % https://scienceworld.wolfram.com/physics/LiftCoefficient.html
 params.aerodynamics = struct(...
   'ThrustOmegaProp', k_T, ... % in s^2 / (rad * N)
-  'ThrustOmegaPropUncertainty', .01, ... % in %
+  'ThrustOmegaPropUncertainty', .05, ... % in %
   'FlapArea', 23e-3 * 10e-3, ... % in m^2
-  'FlapAreaUncertainty', .02, ... % in %
+  'FlapAreaUncertainty', .1, ... % in %
   'DragCoefficients', [0, 0], ... % TODO
   'DragCoefficientsUncertainties', [.1, .1], ... % in %
   'LiftCoefficient', 2 * pi, ... % TODO
-  'LiftCoefficientUncertainty', .05 ...% in %
+  'LiftCoefficientUncertainty', .1 ...% in %
 );
 
 
@@ -126,14 +123,16 @@ params.linearization = struct(...
 % Normalization (maximum values)
 
 params.normalization = struct(...
-  'HPosition', 1, ... % m
-  'VPosition', 1, ...% m
-  'HSpeed', 2, ... % m / s
-  'VSpeed', 2, ... % m / s
+  'FlapAngle', params.actuators.ServoAbsMaxAngle, ...
+  'ThrustAngularVelocity', params.actuators.PropellerMaxAngularVelocity, ...
+  'HPosition', .5, ... % m
+  'VPosition', .5, ...% m
+  'HSpeed', .5, ... % m / s
+  'VSpeed', .5, ... % m / s
   'PitchRollAngle', 10 * pi / 180, ... % rad
   'YawAngle', 5 * pi / 180, ... % rad
-  'AngularRate', 2 * pi / 180, ... % rad / s
-  'WindSpeed', 2 ... % m / s
+  'AngularRate', 1 * pi / 180, ... % rad / s
+  'WindSpeed', .01 ... % m / s
 );
 end
 % vim: ts=2 sw=2 et:
diff --git a/uav_performance_hinf.m b/uav_performance_hinf.m
index ba2fd3b..4a6de8a 100644
--- a/uav_performance_hinf.m
+++ b/uav_performance_hinf.m
@@ -19,41 +19,43 @@ function [perf] = uav_performance_hinf(params, do_plots)
 s = tf('s');
 
 % Bandwitdhs
-bw_alpha = .7 * params.actuators.ServoNominalAngularVelocity;
-bw_omega = 8;
+T_alpha = params.actuators.ServoSecondsTo60Deg;
+T_omega = 0.1;
 
-bw_xy = .1;
-bw_z = .4;
+T_xy = 5;
+T_z = 10;
 
-bw_xydot = .5;
-bw_zdot = .1;
+% Inverse performance functions
+W_Palpha = make_weight(1/T_alpha, 10, 2);
+W_Pomega = make_weight(1/T_omega, 10, 2);
 
-bw_phitheta = bw_xy;
-bw_psi = .08;
+W_Rxy = 1 / ((s * T_xy)^2 + 2 * T_xy * .8 * s + 1);
+W_Rz = 1 / (s * T_z + 1);
 
-% Inverse performance functions
-W_Palpha = .25 / (s / bw_alpha + 1); 
-W_Pomega = .1 / (s / bw_omega + 1);
+W_Pxy = tf(5);
+W_Pz = tf(1);
 
-W_Pxy = 5 * bw_xy^2 / (s^2 + 2 * .9 * bw_xy * s + bw_xy^2);
-W_Pz = bw_z^2 / (s^2  + 2 * 1 * bw_z * s + bw_z^2);
+W_Pxydot = tf(.01);
+W_Pzdot = tf(.01);
 
-W_Pxydot = tf(.1); % .2 / (s / bw_xydot + 1);
-W_Pzdot = tf(.1); % .5 / (s / bw_zdot + 1);
- 
-W_Pphitheta = .01 / (s / bw_phitheta + 1);
-W_Ppsi = tf(.1); % .1 / (s / bw_psi + 1);
+W_Pphitheta = .001 / (s * T_xy + 1);
+W_Ppsi = tf(.1);
 
 % Construct performance vector by combining xy and z
+W_ref = blkdiag(W_Rxy * eye(2), W_Rz);
+
 W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
 W_PPdot = blkdiag(W_Pxydot * eye(2), W_Pzdot);
 W_PTheta = blkdiag(W_Pphitheta * eye(2), W_Ppsi);
+
 perf = struct(...
   'FlapAngle', W_Palpha * eye(4), ...
   'Thrust', W_Pomega, ...
+  'ReferenceFilter', W_ref, ...
   'Position', W_PP, ...
   'Velocity', W_PPdot, ...
-  'Angles', W_PTheta);
+  'Angles', W_PTheta ...
+);
 
 if do_plots
   % Bode plots of performance requirements
@@ -61,6 +63,8 @@ if do_plots
 
   bodemag(W_Palpha);
   bodemag(W_Pomega);
+  bodemag(W_Rxy);
+  bodemag(W_Rz);
   bodemag(W_Pxy);
   bodemag(W_Pz);
   bodemag(W_Pxydot);
@@ -70,6 +74,7 @@ if do_plots
 
   grid on;
   legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ...
+    '$W_{R,xy}$', '$W_{R,z}$', ...
     '$W_{P,xy}$', '$W_{P,z}$', ...
     '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
     '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ...
@@ -80,11 +85,13 @@ if do_plots
   % Step response of position requirements
   figure; hold on;
   step(W_Pxy); step(W_Pz);
+  step(W_Rxy); step(W_Rz);
   step(W_Pxydot); step(W_Pzdot);
   step(W_Palpha);
   step(W_Pomega);
   grid on;
   legend('$W_{P,xy}$', '$W_{P,z}$', ...
+    '$W_{R,xy}$', '$W_{R,z}$', ...
     '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
     '$W_{P,\alpha}$', '$W_{P,\omega}$', ...
     'interpreter', 'latex', 'fontSize', 8);
diff --git a/uav_performance_musyn.m b/uav_performance_musyn.m
index 57237fa..5ad5fa5 100644
--- a/uav_performance_musyn.m
+++ b/uav_performance_musyn.m
@@ -19,32 +19,32 @@ function [perf] = uav_performance_musyn(params, do_plots)
 s = tf('s');
 
 % Bandwitdhs
-bw_alpha = .7 * params.actuators.ServoNominalAngularVelocity;
-bw_omega = 8;
+T_alpha = params.actuators.ServoSecondsTo60Deg;
+T_omega = 0.1;
 
-bw_xy = .1;
-bw_z = .4;
+T_xy = 8;
+T_z = 10;
 
-bw_xydot = .5;
-bw_zdot = .1;
+% Inverse performance functions
 
-bw_phitheta = bw_xy;
-bw_psi = .08;
+W_Palpha = make_weight(1/T_alpha, 10, 4);
+W_Pomega = make_weight(1/T_omega, 10, 2);
 
-% Inverse performance functions
-W_Palpha = .25 / (s / bw_alpha + 1); 
-W_Pomega = .1 / (s / bw_omega + 1);
+W_Rxy = 1 / ((s * T_xy)^2 + 2 * T_xy * .8 * s + 1);
+W_Rz = 1 / (s * T_z + 1);
+
+W_Pxy = tf(5);
+W_Pz = tf(1);
 
-W_Pxy = 2 * bw_xy^2 / (s^2 + 2 * .9 * bw_xy * s + bw_xy^2);
-W_Pz = bw_z^2 / (s^2  + 2 * 1 * bw_z * s + bw_z^2);
+W_Pxydot = tf(.01);
+W_Pzdot = tf(.1);
 
-W_Pxydot = tf(.1); % .2 / (s / bw_xydot + 1);
-W_Pzdot = tf(.1); % .5 / (s / bw_zdot + 1);
- 
-W_Pphitheta = .01 / (s / bw_phitheta + 1);
-W_Ppsi = tf(.1); % .1 / (s / bw_psi + 1);
+W_Pphitheta = .001 / (s * T_xy + 1);
+W_Ppsi = tf(.1);
 
 % Construct performance vector by combining xy and z
+W_ref = blkdiag(W_Rxy * eye(2), W_Rz);
+
 W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
 W_PPdot = blkdiag(W_Pxydot * eye(2), W_Pzdot);
 W_PTheta = blkdiag(W_Pphitheta * eye(2), W_Ppsi);
@@ -52,6 +52,7 @@ W_PTheta = blkdiag(W_Pphitheta * eye(2), W_Ppsi);
 perf = struct(...
   'FlapAngle', W_Palpha * eye(4), ...
   'Thrust', W_Pomega, ...
+  'ReferenceFilter', W_ref, ...
   'Position', W_PP, ...
   'Velocity', W_PPdot, ...
   'Angles', W_PTheta);
@@ -100,4 +101,24 @@ if do_plots
 end
 
 end
+
+% Make a n-order performance weight function
+%
+% Arguments:
+%   OMEGA  Cutting frequency (-3dB)
+%   A      Magnitude at DC, i.e. |Wp(0)|
+%   M      Magnitude at infinity, i.e. |Wp(inf)|
+%   ORD    Order
+function [Wp] = make_weight(omega, A, M, ord)
+
+if nargin > 3
+  n = ord;
+else
+  n = 1;
+end
+
+s = tf('s');
+Wp = (s / (M^(1/n)) + omega)^n / (s + omega * A^(1/n))^n;
+
+end
 % vim: ts=2 sw=2 et:
diff --git a/uav_sim_step.m b/uav_sim_step.m
index 82c05e9..65fb36f 100644
--- a/uav_sim_step.m
+++ b/uav_sim_step.m
@@ -3,127 +3,141 @@
 % Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
 % This work is distributed under a permissive license, see LICENSE.txt
 
-function [simout] = uav_sim_step_musyn(params, model, ctrl, nsamp, T, do_plots, do_noise)
+function [simout] = uav_sim_step(params, model, ctrl, uncert, nsamp, T, do_plots, do_noise)
 
 % Load closed loop model and add controller
 % more or less equivalent to doing usys = lft(Pnom, K)
 h = load_system('uav_model_uncertain_clp');
 hws = get_param('uav_model_uncertain_clp', 'modelworkspace');
-hws.assignin('K', ctrl.K);
 
-ulmod_clp = linmod('uav_model_uncertain_clp');
-usys_clp = ss(ulmod_clp.a, ulmod_clp.b, ulmod_clp.c, ulmod_clp.d);
+if isfield(ctrl, 'K')
+  hws.assignin('K', ctrl.K);
+else
+  error('You need to provide a controller ctrl.K');
+end
 
-% Take dynamics without uncertainty
-% Also nominal ouput to make plots
-P_nom_clp = minreal(usys_clp(...
-  [model.uncertain.index.OutputError; 
-    model.uncertain.index.OutputNominal;
-    model.uncertain.index.OutputPlots], ...
-  model.uncertain.index.InputExogenous), [], false);
+% There is an uncertainty block
+if isfield(uncert, 'Delta')
+  hws.assignin('Delta', uncert.Delta);
+else
+  fprintf(' - No uncertainty error Delta was provided, setting to zero.\n');
+  Delta = tf(0) * zeros(sum(model.uncertain.BlockStructure));
+  hws.assignin('Delta', Delta);
+end
 
-% Check that closed loop is actually stable
-eigvals = eig(P_nom_clp.A);
-nx = size(P_nom_clp.A, 1);
+ulmod_clp = linmod('uav_model_uncertain_clp');
+P_clp = minreal(ss(ulmod_clp.a, ulmod_clp.b, ulmod_clp.c, ulmod_clp.d), [], false);
 
-for i = 1:nx
-  if real(eigvals(i)) >= 0
+% Check that closed loop is actually stable
+nx = size(P_clp.A, 1);
+fprintf(' - Closed loop dynamics have %d states\n', nx);
+if nx < 60
+  [~, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(P_clp);
+  if nx ~= nsta
     error('Closed loop is not stable!');
   end
-end
-
-% Indices for exogenous inputs
-Iwwind = (1:3)';
-Iwalpha = (4:7)';
-Ir = (8:10)';
 
-% Indices for outputs
-Iealpha = (1:4)';
-Ieomega = 5;
-IeP = (6:8)';
-IePdot = (9:11)';
-IeTheta = (12:14)';
-
-% Indices for y outputs (for plots)
-IP = (15:17)';
-IPdot = (18:20)';
-ITheta = (21:23)';
-IOmega = (24:26)';
+  fprintf('      %d stable modes, %d unstable modes\n', nsta, nx - nsta);
+  fprintf('      %d controllable modes, %d unstabilizable\n', nctrb, nustab);
+  fprintf('      %d observable modes, %d undetectable\n', nobsv, nudetb);
+else
+  eigvals = eig(P_clp.A);
+  for i = 1:nx
+    if real(eigvals(i)) >= 0
+      error('Closed loop is not stable!');
+    end
+  end
+end
 
-% Indices for p outputs (for plots)
-Ialpha = (27:30)';
-Iomega = 31;
+% Generate indices for closed loop plant
+o = model.uncertain.dims.output;
+i = model.uncertain.dims.input;
+
+dims = struct(...
+  'ErrorAlpha', o.OutputErrorAlpha, ...
+  'ErrorOmega', o.OutputErrorOmega, ...
+  'ErrorPosition', o.OutputErrorPosition, ...
+  'ErrorVelocity', o.OutputErrorVelocity, ...
+  'ErrorEulerAngles', o.OutputErrorEulerAngles, ...
+  'Position', o.OutputNominalPosition, ...
+  'Velocity', o.OutputNominalVelocity, ...
+  'EulerAngles', o.OutputNominalEulerAngles, ...
+  'AngularRates', o.OutputNominalAngularRates, ...
+  'PlotAlpha', o.OutputPlotAlpha, ...
+  'PlotOmega', o.OutputPlotOmega, ...
+  'PlotReference', o.OutputPlotReference, ...
+  'PlotPosition', o.OutputPlotPosition, ...
+  'InputAlpha', i.InputNominalAlpha, ...
+  'InputOmega', i.InputNominalOmega ...
+);
+
+idx = struct();
+start = 1; fields = fieldnames(dims);
+for f = fields'
+  dim = getfield(dims, f{1});
+  i = (start:(start + dim - 1))';
+  idx = setfield(idx, f{1}, i);
+  start = start + dim;
+end
 
-Iualpha = (32:35)';
-Iuomega = 36;
+% Input indices
+idx.InputDisturbanceWind = (1:3)';
+idx.InputDisturbanceFlaps = (4:7)';
+idx.InputReference = (8:10)';
 
-noise = zeros(7, nsamp); % no noise
+% Create noise
+noise = zeros(7, nsamp);
 if do_noise
-  % Noise in percentage
+  % Noise (normalized)
   noise_alpha_amp = (.5 * (pi / 180)) / params.actuators.ServoAbsMaxAngle;
-  noise_wind_amp = .1;
+  noise_wind_amp = .01;
   noise = [noise_wind_amp * randn(3, nsamp);
     noise_alpha_amp * randn(4, nsamp)];
 end
 
+% Step size
+step_size_h = .5  / params.normalization.HPosition;
+step_size_v = .5  / params.normalization.VPosition;
+
 % Create step inputs (normalized)
-ref_step = .5 * ones(1, nsamp); % 1d step function
+ref_step = ones(1, nsamp); % 1d step function
 
-in_step_x = [ noise; ref_step; zeros(2, nsamp) ];
-in_step_y = [ noise; zeros(1, nsamp); ref_step; zeros(1, nsamp) ];
-in_step_z = [ noise; zeros(2, nsamp); ref_step ];
+in_step_x = [ noise; step_size_h * ref_step; zeros(2, nsamp) ];
+in_step_y = [ noise; zeros(1, nsamp); step_size_h * ref_step; zeros(1, nsamp) ];
+in_step_z = [ noise; zeros(2, nsamp); step_size_v * ref_step ];
 
 % Simulation time
-n_settle_times = 10;
 t = linspace(0, T, nsamp);
 
+% Scale simulation outputs
+S = blkdiag(...
+  model.uncertain.scaling.OutputErrorScaling, ...
+  model.uncertain.scaling.OutputNominalScaling, ...
+  model.uncertain.scaling.OutputPlotScaling, ...
+  model.uncertain.scaling.InputNominalScaling ...
+);
+
 % Simulate step responses
-out_step_x_norm = lsim(P_nom_clp, in_step_x, t);
-out_step_y_norm = lsim(P_nom_clp, in_step_y, t);
-out_step_z_norm = lsim(P_nom_clp, in_step_z, t);
-
-% Scale outputs
-S_actuators = blkdiag(...
-  eye(4) * params.actuators.ServoAbsMaxAngle, ...
-  eye(1) * params.actuators.PropellerMaxAngularVelocity);
-
-S_state = blkdiag(...
-  eye(2) * params.normalization.HPosition, ...
-  eye(1) * params.normalization.VPosition, ...
-  eye(2) * params.normalization.HSpeed, ...
-  eye(1) * params.normalization.VSpeed, ...
-  eye(2) * params.normalization.PitchRollAngle, ...
-  eye(1) * params.normalization.YawAngle, ...
-  eye(3) * params.normalization.AngularRate);
-
-S = blkdiag(S_actuators, S_state(1:9, 1:9), S_state, S_actuators, S_actuators);
-
-out_step_x = out_step_x_norm * S';
-out_step_y = out_step_y_norm * S';
-out_step_z = out_step_z_norm * S';
+out_step_x_norm = lsim(P_clp, in_step_x, t, 'foh');
+out_step_y_norm = lsim(P_clp, in_step_y, t, 'foh');
+out_step_z_norm = lsim(P_clp, in_step_z, t, 'foh');
+
+out_step_x = out_step_x_norm * S;
+out_step_y = out_step_y_norm * S;
+out_step_z = out_step_z_norm * S;
 
 % Return simulation
 simout = struct(...
   'Time', t, ...
-  'StepX', out_step_x_norm, ...
-  'StepY', out_step_y_norm, ...
-  'StepZ', out_step_z_norm, ...
+  'StepX', out_step_x, ...
+  'StepY', out_step_y, ...
+  'StepZ', out_step_z, ...
+  'StepXNorm', out_step_x_norm, ...
+  'StepYNorm', out_step_y_norm, ...
+  'StepZNorm', out_step_z_norm, ...
   'Simulink', ulmod_clp, ...
-  'StateSpace', P_nom_clp);
-
-simout.index = struct(...
-  'ErrorFlapAngles', Iealpha, ...
-  'ErrorThrust', Ieomega , ...
-  'ErrorPos', IeP, ...
-  'ErrorVel', IePdot, ...
-  'ErrorAngles', IeTheta, ...
-  'Position', IP, ...
-  'Velocity', IPdot, ...
-  'Angles', ITheta, ...
-  'FlapAngles', Ialpha, ...
-  'Thruster', Iomega, ...
-  'InputFlapAngles', Iualpha, ...
-  'InputThruster', Iuomega);
+  'StateSpace', P_clp, ...
+  'index', idx);
 
 if do_plots
   % Conversion factors
@@ -137,7 +151,6 @@ if do_plots
     ctrl.Name), 'Interpreter', 'latex');
 
   % Plot limits
-  ref_value     = .5;
   alpha_max_deg = params.actuators.ServoAbsMaxAngle * to_deg;
   euler_lim_deg = 1.5;
   omega_max_rpm = (params.actuators.PropellerMaxAngularVelocity ...
@@ -147,10 +160,10 @@ if do_plots
   % Plot step response from x to alpha
   subplot(2, 3, 1);
   hold on;
-  plot(t, out_step_x(:, Ialpha(1)) * to_deg);
-  plot(t, out_step_x(:, Ialpha(2)) * to_deg);
-  plot(t, out_step_x(:, Ialpha(3)) * to_deg);
-  plot(t, out_step_x(:, Ialpha(4)) * to_deg);
+  plot(t, out_step_x(:, idx.PlotAlpha(1)) * to_deg);
+  plot(t, out_step_x(:, idx.PlotAlpha(2)) * to_deg);
+  plot(t, out_step_x(:, idx.PlotAlpha(3)) * to_deg);
+  plot(t, out_step_x(:, idx.PlotAlpha(4)) * to_deg);
   plot([0, T], [1, 1] * alpha_max_deg, 'r--');
   plot([0, T], [-1, -1] * alpha_max_deg, 'r--');
   grid on;
@@ -164,10 +177,10 @@ if do_plots
 
   % Plot step response from y to alpha
   subplot(2, 3, 2); hold on;
-  plot(t, out_step_y(:, Ialpha(1)) * to_deg);
-  plot(t, out_step_y(:, Ialpha(2)) * to_deg);
-  plot(t, out_step_y(:, Ialpha(3)) * to_deg);
-  plot(t, out_step_y(:, Ialpha(4)) * to_deg);
+  plot(t, out_step_y(:, idx.PlotAlpha(1)) * to_deg);
+  plot(t, out_step_y(:, idx.PlotAlpha(2)) * to_deg);
+  plot(t, out_step_y(:, idx.PlotAlpha(3)) * to_deg);
+  plot(t, out_step_y(:, idx.PlotAlpha(4)) * to_deg);
   plot([0, T], [1, 1] * alpha_max_deg, 'r--');
   plot([0, T], [-1, -1] * alpha_max_deg, 'r--');
   grid on;
@@ -181,10 +194,10 @@ if do_plots
 
   % Plot step response from z to alpha
   subplot(2, 3, 3); hold on;
-  plot(t, out_step_z(:, Ialpha(1)) * to_deg);
-  plot(t, out_step_z(:, Ialpha(2)) * to_deg);
-  plot(t, out_step_z(:, Ialpha(3)) * to_deg);
-  plot(t, out_step_z(:, Ialpha(4)) * to_deg);
+  plot(t, out_step_z(:, idx.PlotAlpha(1)) * to_deg);
+  plot(t, out_step_z(:, idx.PlotAlpha(2)) * to_deg);
+  plot(t, out_step_z(:, idx.PlotAlpha(3)) * to_deg);
+  plot(t, out_step_z(:, idx.PlotAlpha(4)) * to_deg);
   plot([0, T], [1, 1] * alpha_max_deg, 'r--');
   plot([0, T], [-1, -1] * alpha_max_deg, 'r--');
   grid on;
@@ -198,9 +211,9 @@ if do_plots
 
   % Plot step response from x to Theta
   subplot(2, 3, 4); hold on;
-  plot(t, out_step_x(:, ITheta(1)) * to_deg);
-  plot(t, out_step_x(:, ITheta(2)) * to_deg);
-  plot(t, out_step_x(:, ITheta(3)) * to_deg);
+  plot(t, out_step_x(:, idx.EulerAngles(1)) * to_deg);
+  plot(t, out_step_x(:, idx.EulerAngles(2)) * to_deg);
+  plot(t, out_step_x(:, idx.EulerAngles(3)) * to_deg);
   grid on;
   xlim([0, T]);
   ylim([-euler_lim_deg, euler_lim_deg]);
@@ -212,9 +225,9 @@ if do_plots
 
   % Plot step response from y to Theta
   subplot(2, 3, 5); hold on;
-  plot(t, out_step_y(:, ITheta(1)) * to_deg);
-  plot(t, out_step_y(:, ITheta(2)) * to_deg);
-  plot(t, out_step_y(:, ITheta(3)) * to_deg);
+  plot(t, out_step_y(:, idx.EulerAngles(1)) * to_deg);
+  plot(t, out_step_y(:, idx.EulerAngles(2)) * to_deg);
+  plot(t, out_step_y(:, idx.EulerAngles(3)) * to_deg);
   grid on;
   xlim([0, T]);
   ylim([-euler_lim_deg, euler_lim_deg]);
@@ -226,9 +239,9 @@ if do_plots
 
   % Plot step response from z to Theta
   subplot(2, 3, 6); hold on;
-  plot(t, out_step_z(:, ITheta(1)) * to_deg);
-  plot(t, out_step_z(:, ITheta(2)) * to_deg);
-  plot(t, out_step_z(:, ITheta(3)) * to_deg);
+  plot(t, out_step_z(:, idx.EulerAngles(1)) * to_deg);
+  plot(t, out_step_z(:, idx.EulerAngles(2)) * to_deg);
+  plot(t, out_step_z(:, idx.EulerAngles(3)) * to_deg);
   grid on;
   xlim([0, T]);
   ylim([-euler_lim_deg, euler_lim_deg]);
@@ -245,7 +258,7 @@ if do_plots
     ctrl.Name), 'Interpreter', 'latex');
 
   hold on;
-  step(P_nom_clp(Iomega, Ir(3)) * to_rpm, T);
+  step(P_clp(idx.PlotOmega, idx.InputReference(3)) * to_rpm, T);
   % plot([0, T], [1, 1] * omega_min_rpm, 'r--');
   % plot([0, T], [1, 1] * omega_max_rpm, 'r--');
   grid on;
@@ -263,20 +276,19 @@ if do_plots
 
   % Plot step response from horizontal reference to horizontal position
   subplot(2, 2, 1); hold on;
-  plot(t, out_step_x(:, IP(1)));
-  plot(t, out_step_y(:, IP(2)));
-  % plot([0, T], [1, 1] * ref_value, 'r:');
-  % plot(t, out_step_xydes, 'r--');
+  plot(t, out_step_x(:, idx.PlotPosition(1)));
+  plot(t, out_step_y(:, idx.PlotPosition(2)));
+  plot(t, out_step_x(:, idx.PlotReference(1)), '--');
   grid on;
-  title('Horizontal Position Error', 'Interpreter', 'latex');
-  ylabel('Error (meters)', 'Interpreter', 'latex');
+  title('Horizontal Position', 'Interpreter', 'latex');
+  ylabel('Distance (meters)', 'Interpreter', 'latex');
   xlabel('Time (seconds)', 'Interpreter', 'latex');
   legend('$x(t)$', '$y(t)$', 'Interpreter', 'latex');
 
   % Plot step response horizontal reference to horizontal speed
   subplot(2, 2, 2); hold on;
-  plot(t, out_step_x(:, IPdot(1)));
-  plot(t, out_step_y(:, IPdot(2)));
+  plot(t, out_step_x(:, idx.Velocity(1)));
+  plot(t, out_step_y(:, idx.Velocity(2)));
   grid on;
   title('Horizontal Velocity', 'Interpreter', 'latex');
   ylabel('Velocity (m / s)', 'Interpreter', 'latex');
@@ -285,18 +297,16 @@ if do_plots
 
   % Plot step response from vertical reference to vertical position
   subplot(2, 2, 3); hold on;
-  plot(t, out_step_z(:, IP(3)));
-  % plot([0, T], [1, 1] * ref_value, 'r:');
-  % plot(t, out_step_zdes, 'r--');
+  plot(t, out_step_z(:, idx.PlotPosition(3)));
   grid on;
-  title('Vertical Position Error', 'Interpreter', 'latex');
-  ylabel('Error (meters)', 'Interpreter', 'latex');
+  title('Vertical Position', 'Interpreter', 'latex');
+  ylabel('Distance (meters)', 'Interpreter', 'latex');
   xlabel('Time (seconds)', 'Interpreter', 'latex');
   legend('$z(t)$', 'Interpreter', 'latex');
 
   % Plot step response vertical reference to vertical speed
   subplot(2, 2, 4); hold on;
-  plot(t, out_step_z(:, IPdot(3)));
+  plot(t, out_step_z(:, idx.ErrorVelocity(3)));
   grid on;
   title('Vertical Velocity', 'Interpreter', 'latex');
   ylabel('Velocity (m / s)', 'Interpreter', 'latex');
diff --git a/uav_uncertainty.m b/uav_uncertainty.m
index 7cd7e38..03b7b3b 100644
--- a/uav_uncertainty.m
+++ b/uav_uncertainty.m
@@ -26,18 +26,18 @@ eps_d = params.aerodynamics.DragCoefficientsUncertainties(1);
 % eps_0 = params.aerodynamics.DragCoefficients(2);
 
 eps_omega = max(.5 * eps_T, eps_r);
-eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega);
+eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega)
 
 b = 12;
-G = make_weight(b, 2, .2);
-
-W_malpha = .1 * eps_alpha * G;
-W_momega = .1 * eps_omega * G;
-W_mState = blkdiag( ...
-  .05 * tf(1) * eye(3), ...
-  .01 * tf(1) * eye(3), ...
-  .001 * tf(1) * eye(3), ...
-  .05 * tf(1) * eye(3) ...
+T = 1;
+G = make_weight(b, 4, 1);
+
+W_malpha = eps_alpha * tf(1);
+W_momega = eps_omega * tf(1);
+W_mState = (1 - tf(1, [T, 1])) * blkdiag( ...
+  .2 * eye(3), ...
+  .1 * eye(3), ...
+  .2 * eye(3) ...
 );
 
 uncert = struct(...
@@ -56,12 +56,13 @@ if do_plots
   bodemag(W_mState(1,1));
   bodemag(W_mState(4,4));
   bodemag(W_mState(7,7));
-  bodemag(W_mState(11,11));
 
   grid on;
   legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ...
-         '$W_{m,\mathbf{P}}$', '$W_{m,\mathbf{\dot{P}}}$', ...
-         '$W_{m,\mathbf{\Theta}}$', '$W_{m,\mathbf{\Omega}}$', ...
+         ... % '$W_{m,\mathbf{P}}$', ...
+         '$W_{m,\mathbf{\dot{P}}}$', ...
+         '$W_{m,\mathbf{\Theta}}$', ...
+         '$W_{m,\mathbf{\Omega}}$', ...
          'interpreter', 'latex')
   title('\bfseries Stability Requirement (only for $\mu$-Synthesis)', ...
         'interpreter', 'latex')