Update DK-iteration and clean up

9 files changed, 341 insertions, 542 deletions

diff --git a/uav.m b/uav.m
index d9198c4..059c944 100644
--- a/uav.m
+++ b/uav.m
@@ -10,11 +10,7 @@ clear; clc; close all; s = tf('s');
 
 do_plots = true; % runs faster without
 do_hinf = true; % midterm
-do_musyn = false; % endterm
-
-if do_hinf & do_musyn
-  error('Cannot do both H-infinity and mu synthesis.')
-end
+do_musyn = true; % endterm
 
 fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n')
 fprintf('Will do:\n')
@@ -40,24 +36,14 @@ params = uav_params();
 %% ------------------------------------------------------------------------
 % Define performance requirements
 
-if do_hinf
-  fprintf('Generating performance requirements...\n')
-  perf = uav_performance_hinf(params, do_plots);
-end
-if do_musyn
-  fprintf('Generating performance requirements...\n')
-  perf = uav_performance_musyn(params, do_plots);
-end
+fprintf('Generating performance requirements...\n')
+perf = uav_performance_musyn(params, do_plots);
 
 %%  ------------------------------------------------------------------------
 % Define stability requirements
 
-% Note: for hinf it is needed to call uav_model, but hinf will not actually
-% make use of this struct
-if do_hinf | do_musyn
-  fprintf('Generating stability requirements...\n')
-  uncert = uav_uncertainty(params, do_plots);
-end
+fprintf('Generating stability requirements...\n')
+uncert = uav_uncertainty(params, do_plots);
 
 %% ------------------------------------------------------------------------
 % Create UAV model
@@ -81,38 +67,42 @@ if do_hinf
   nmeas = model.uncertain.Ny;
   nctrl = model.uncertain.Nu;
 
-  hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
+  hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
     'AutoScale', 'off', 'RelTol', 1e-3);
   [K_inf, ~, gamma, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt);
+  fprintf(' - H-infinity synthesis gamma: %g\n', gamma);
   ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_inf);
 
   if gamma >= 1
-    fprintf('Failed to syntesize controller (closed loop is unstable).\n')
+    error('Failed to syntesize controller (closed loop is unstable).')
   end
 
 %%  ------------------------------------------------------------------------
 % Measure Performance of H-infinity design
 
-  fprintf('Simulating closed loop...\n');
+  fprintf(' - Simulating closed loop...\n');
 
+  T = 60;
   nsamples = 500;
   do_noise = true;
-  simout = uav_sim_step_hinf(params, model, ctrl.hinf, nsamples, do_plots, do_noise);
+  simout = uav_sim_step(params, model, ctrl.hinf, nsamples, T, do_plots, do_noise);
 
-  fprintf('Writing simulation results...\n');
+  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];
 
-  writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
+  writematrix([simout.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
 end
 
 %%  ------------------------------------------------------------------------
 % Perform mu-Analysis & DK iteration
 
 if do_musyn
+  drawnow;
+
   fprintf('Performing mu-synthesis controller design...\n')
 
   % Get complete system (without debugging outputs for plots)
@@ -125,31 +115,29 @@ if do_musyn
   % Options for H-infinity
   nmeas = model.uncertain.Ny;
   nctrl = model.uncertain.Nu;
-  hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
-    'AutoScale', 'on', 'RelTol', 1e-1);
+  hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
+    'AutoScale', 'on', 'RelTol', 1e-3);
 
   % Frequency raster resolution to fit D scales
-  nsamples = 800;
-  omega = logspace(-1, 3, nsamples);
+  nsamples = 600;
+  omega_max = 3;
+  omega_min = -3;
+
+  omega = logspace(omega_min, omega_max, nsamples);
+  omega_range = {10^omega_min, 10^omega_max};
 
-  % Initial values for D-K iteration
+  % Initial values for D-K iteration are identity matrices
   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));
 
-  % degrees for approximations for D-scales, tuned by hand
-  fit_degrees = [
-    1, 1, 1, 1, 1, 1;  % alpha
-    1, 1, 1, 1, 1, 1;  % omega
-    1, 1, 1, 1, 1, 1;  % state
-    6, 1, 4, 2, 2, 1;  % perf
-  ];
+  % Maximum number of D-K iterations
+  niters = 5;
+  fprintf(' - Will do at most %d iterations.\n', niters);
 
-  % Number of D-K iterations
-  niters = size(fit_degrees, 2);
-
-  % Maximum degree of D-scales
-  d_scales_max_err = .01;% in percentage
-  d_scales_max_degree = 8;
+  % Maximum degree of D-scales and error
+  d_scales_max_degree = 3;
+  d_scales_max_err_p = .4; % in percentage
+  d_scales_improvement_p = .15; % in percentage
 
   % for plotting later
   mu_plot_legend = {};
@@ -157,21 +145,26 @@ if do_musyn
   % Start DK-iteration
   dkstart = tic;
   for it = 1:niters
-    fprintf(' - Running D-K iteration %d / %d...\n', it, niters);
+    fprintf(' * Running D-K iteration %d / %d...\n', it, niters);
     itstart = tic();
 
     % Find controller using H-infinity
-    [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl, hinfopt);
+    P_scaled = minreal(D_left * P * inv(D_right), [], false);
+    [P_scaled, ~] = prescale(P_scaled, omega_range);
+    [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt);
+
     fprintf('   H-infinity synthesis gamma: %g\n', gamma);
     if gamma == inf
-      fprintf('   Failed to synethesize H-infinity controller\n');
-      break;
+      error('Failed to synethesize H-infinity controller');
     end
 
     % Calculate frequency response of closed loop
-    N = minreal(lft(P, K), [], false); % slient
+    N = minreal(lft(P, K), [], false);
     M = minreal(N(idx.OutputUncertain, idx.InputUncertain), [], false);
 
+    [N, ~] = prescale(N, omega_range);
+    [M, ~] = prescale(M, omega_range);
+
     N_frd = frd(N, omega);
     M_frd = frd(M, omega);
 
@@ -187,13 +180,13 @@ if do_musyn
     fprintf('   SSV for Performance: %g, for Stability: %g\n', mu_rp, mu_rs);
 
     if do_plots
-      fprintf('   Plotting mu\n');
+      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), '-.');
+      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', ...
@@ -206,109 +199,123 @@ if do_musyn
     % Are we done yet?
     if mu_rp < 1
       fprintf(' - Found robust controller that meets performance.\n');
-      break
+      break;
+    end
+
+    if mu_rs < 1
+      fprintf('   Found robust controller that is stable.\n')
+      ctrl.musyn = struct('Name', '$\mu$-Synthesis', 'K', K, ...
+                          'mu_rp', mu_rp, 'mu_rs', mu_rs);
     end
 
     % Fit D-scales
-    % There are three complex, square, full block uncertainties and
-    % a non-square full complex block for performance
-    [D_left_samples, D_right_samples] = mussvunwrap(mu_info_rp);
+    [D_left_frd, D_right_frd] = mussvunwrap(mu_info_rp);
 
     fprintf('   Fitting D-scales\n');
 
-    i_alpha = 1;
-    i_omega = model.uncertain.BlockStructure(1, 1) + 1; % after first block
-    i_state = model.uncertain.BlockStructure(2, 1) + 1; % after second block
-    i_perf  = model.uncertain.BlockStructurePerf(3, 1) + 1; % after third block
-
-    D_samples = {
-      D_left_samples(i_alpha, i_alpha);
-      D_left_samples(i_omega, i_omega);
-      D_left_samples(i_state, i_state);
-      D_left_samples(i_perf, i_perf);
+    % There are three complex, square, full block uncertainties and
+    % a non-square full complex block for performance
+    i_alpha = [1, 1];
+    i_omega = model.uncertain.BlockStructure(1, :) + 1; % after first block
+    i_state = sum(model.uncertain.BlockStructure(1:2, :)) + 1; % after second block
+    i_perf  = sum(model.uncertain.BlockStructurePerf(1:3, :)) + 1; % after third block
+
+    D_frd = {
+      D_left_frd(i_alpha(1), i_alpha(1));
+      D_left_frd(i_omega(1), i_omega(1));
+      D_left_frd(i_state(1), i_state(1));
+      D_left_frd(i_perf(1), i_perf(1));
     };
 
     D_max_sv = {
-      max(max(sigma(D_samples{1})));
-      max(max(sigma(D_samples{2})));
-      max(max(sigma(D_samples{3})));
-      max(max(sigma(D_samples{4})));
+      max(max(sigma(D_frd{1, 1})));
+      max(max(sigma(D_frd{2, 1})));
+      max(max(sigma(D_frd{3, 1})));
+      max(max(sigma(D_frd{4, 1})));
     };
 
     D_names = {'alpha', 'omega', 'state', 'perf'};
     D_fitted = {};
 
+    % for each block
     for j = 1:4
+      % for each in left and right
       fprintf('      %s', D_names{j});
-
       best_fit_deg = inf;
       best_fit_err = inf;
-
-      for deg = fit_degrees(j, it):d_scales_max_degree
+      for deg = 0:d_scales_max_degree
         % Fit D-scale
-        % D_fit = fitmagfrd(D_samples{j}, deg);
-        D_fit = fitfrd(genphase(D_samples{j}), deg);
+        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
-          best_fit_deg = deg;
-          best_fit_err = fit_err;
-          D_fitted{j} = D_fit;
+          % Choose higher degree only if we improve by at least a specified
+          % percentage over the previous best fit (or we are at the first
+          % iteration). This is a heuristic to avoid adding too many states
+          % to the controller as it depends on the order of the D-scales.
+          if abs(best_fit_err - fit_err) / best_fit_err > d_scales_improvement_p || best_fit_err == inf
+              best_fit_deg = deg;
+              best_fit_err = fit_err;
+              D_fitted{j} = D_fit;
+          end
         end
 
-        if fit_err / D_max_sv{j} < d_scales_max_err
+        if (fit_err / D_max_sv{j} < d_scales_max_err_p)
           break;
         end
         fprintf('.');
       end
-      fprintf(' degree %d, error %g\n', best_fit_deg, best_fit_err);
+      fprintf(' degree %d, error %g (%g %%)\n', ...
+          best_fit_deg, best_fit_err, 100 * best_fit_err / D_max_sv{j});
     end
 
     % Construct full matrices
-    D_right = blkdiag(D_fitted{1} * eye(4), ...
-                      D_fitted{2} * eye(1), ...
-                      D_fitted{3} * eye(12), ...
-                      D_fitted{4} * eye(10), ...
-                      eye(5));
-
     D_left = blkdiag(D_fitted{1} * eye(4), ...
                      D_fitted{2} * eye(1), ...
                      D_fitted{3} * eye(12), ...
                      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_fitted{4} * eye(10), ...
+                      eye(5));
+
     % Compute peak of singular values for to estimate how good is the
     % approximation of the D-scales
-    sv_left_samples = sigma(D_left_samples);
-    max_sv_left_samples = max(max(sv_left_samples));
+    sv_left_frd = sigma(D_left_frd);
+    max_sv_left_frd = max(max(sv_left_frd));
 
     sv_left = sigma(D_left, omega);
     max_sv_left = max(max(sv_left));
 
-    fprintf('   Max SVD of D: %g, Dhat: %g\n', max_sv_left_samples, max_sv_left);
-    fprintf('   D scales fit abs. error: %g\n', abs(max_sv_left_samples - max_sv_left));
+    fprintf('   Max SVD of D: %g, Dhat: %g\n', max_sv_left_frd, max_sv_left);
+    fprintf('   D scales fit rel. error: %g %%\n', ...
+      100 * abs(max_sv_left_frd - max_sv_left) / max_sv_left_frd);
 
     % Plot fitted D-scales
     if do_plots
       fprintf('   Plotting D-scales');
       f = figure(101); clf(f); hold on;
 
-      bodemag(D_samples{1}, omega, 'r-');
+      bodemag(D_frd{1}, omega, 'r-');
       bodemag(D_fitted{1}, omega, 'b');
       fprintf('.');
 
-      bodemag(D_samples{2}, omega, 'r--');
+      bodemag(D_frd{2}, omega, 'r--');
       bodemag(D_fitted{2}, omega, 'b--');
       fprintf('.');
 
-      bodemag(D_samples{3}, omega, 'c-');
+      bodemag(D_frd{3}, omega, 'c-');
       bodemag(D_fitted{3}, omega, 'm-');
       fprintf('.');
 
-      bodemag(D_samples{4}, omega, 'c--');
+      bodemag(D_frd{4}, omega, 'c--');
       bodemag(D_fitted{4}, omega, 'm--');
       fprintf('.');
 
@@ -332,9 +339,39 @@ if do_musyn
   dkend = toc(dkstart);
   fprintf(' - D-K iteration took %.1f seconds\n', dkend);
 
-  if mu_rp > 1
-    fprintf(' - Failed to synthesize robust controller that meets the desired performance.\n');
-  else
-    ctrl.musyn = struct('K', K, 'mu', mu_rp);
+  if mu_rp > 1 && mu_rs > 1
+    error(' - Failed to synthesize robust controller that meets the desired performance.\n');
+  end
+
+  %% 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
+
+  % Save controller
+  ctrl.musyn = struct('Name', '$\mu$-Synthesis', ...
+                      'K', K, 'Delta', Delta, ...
+                      'mu_rp', mu_rp, 'mu_rs', mu_rs);
+
+  if mu_rp >= 1
+    fprintf(' - Synthetized robust stable controller does not meet the desired performance.\n');
   end
+
+%%  ------------------------------------------------------------------------
+% Measure Performance of mu synthesis design
+
+  fprintf('Simulating nominal closed loop...\n');
+
+  T = 60;
+  nsamples = 5000;
+  do_noise = true;
+
+  simout = uav_sim_step(params, model, ctrl.musyn, nsamples, T, do_plots, do_noise);
+
+  fprintf('Simulating worst-case closed loop...\n');
+
 end
diff --git a/uav_model.m b/uav_model.m
index 3fc1d0a..1f8f751 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -3,15 +3,18 @@
 % Copyright (C) 2024, Naoki Sean Pross, ETH Zürich.
 % This work is distributed under a permissive license, see LICENSE.txt
 %
-% This function generates three models: 
+% This function generates three plant models: 
 % 
-%  * A non-linear symbolic model (cannot be used directly).
-%  * A linear model obtained by linearizing the the non linear model at an
-%    operating point specified in the params struct argument. 
-%  * A uncertain linear model built atop of the linear model using SIMULINK.
-%    The uncertain model contains the performance and weighting transfer 
-%    function given in the arguments perf and params, and is stored in the
-%    SIMULINK file uav_model_uncertain.xls.
+%  * A non-linear symbolic model (cannot be used directly) derived from first
+%    principles (equations of motion).
+%
+%  * A linear model obtained by linearizing the the non linear plant model at
+%    an operating point specified in the params struct argument. 
+%
+%  * A uncertain linear model with reference trcking built atop of the linear
+%    model using SIMULINK. The uncertain model contains the performance and
+%    weighting transfer function given in the arguments perf and params, and is
+%    stored in the SIMULINK file uav_model_uncertain.xls.
 %
 % [MODEL] = UAV_MODEL(PARAMS, PERF, UNCERT)
 %
@@ -220,6 +223,27 @@ D = zeros(12, 5);
 [nx, nu] = size(B);
 [ny, ~] = size(C);
 
+% ------------------------------------------------------------------------
+% Scale inputs and outputs of linearized model
+
+S_actuators = blkdiag(...
+  eye(4) * 1 / params.actuators.ServoAbsMaxAngle, ...
+  eye(1) * 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);
+
+% Scale system matrices to have inputs and outputs between zero and one
+B = B * inv(S_actuators);
+C = inv(S_state) * C;
+D = D * inv(S_actuators);
+
 % Create state space object
 T = params.measurements.SensorFusionDelay;
 n = params.linearization.PadeApproxOrder;
@@ -230,7 +254,8 @@ model.linear = struct(...
   'Nx', nx, 'Nu', nu, 'Ny', ny, ... % number of states, inputs, and outputs
   'State', xi, 'Inputs', u, ... % state and input variables
   'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized
-  'StateSpace', sys ... % state space object
+  'StateSpace', sys, ... % state space object
+  'InputScalingMatrix', S_actuators, 'OutputScalingMatrix', S_state ...
 );
 
 % ------------------------------------------------------------------------
@@ -289,14 +314,20 @@ if rank(Wo) < nx
 end
 
 % ------------------------------------------------------------------------
+% Compute absolute value of error caused by linearization around set point
+
+
+% ------------------------------------------------------------------------
 % Model actuators
 
 % TODO: better model, this was tuned "by looking"
-w = 4 * params.actuators.ServoNominalAngularVelocity;
+w = 2 * params.actuators.ServoNominalAngularVelocity;
 zeta = .7;
 G_servo = tf(w^2, [1, 2 * zeta * w, w^2]);
-% figure; step(G_servo * pi / 3); grid on;
-G_prop = tf(1,1);
+
+w = 6e3;
+zeta = 1;
+G_prop = tf(w^2, [1, 2 * zeta * w, w^2]);
 
 model.actuators = struct('FlapServo', G_servo, 'ThrustPropeller', G_prop);
 
@@ -328,6 +359,21 @@ 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, ...
diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx
index b863139..d432718 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 7a17a21..7bcf2d9 100644
--- a/uav_model_uncertain_clp.slx
+++ b/uav_model_uncertain_clp.slx
diff --git a/uav_params.m b/uav_params.m
index a1c0e4a..e87e4fd 100644
--- a/uav_params.m
+++ b/uav_params.m
@@ -65,7 +65,7 @@ params.actuators = struct(...
 
 % IMU runs in NDOF mode
 params.measurements = struct(...
-  'SensorFusionDelay', 20e-3, ... % in s
+  'SensorFusionDelay', 50e-3, ... % in s
   'LIDARAccuracy', 10e-3, ... % in m
   'LIDARMaxDistance', 40, ... % inm
   'LIDARBandwidth', 100, ... % in Hz for z position
@@ -82,6 +82,11 @@ omega_max = params.actuators.PropellerMaxAngularVelocity;
 F_max = 38.637; % in N (measured)
 k_T = F_max / omega_max^2;
 
+% Lift coefficient from data
+% flap_stair = readtable('meas/Flap_Force_Stair.csv');
+% [~, istart] = min(abs(flap.Force));
+% [~, iend] = min(abs(flap.Force - 3));
+
 % FIXME: LiftCoefficient comes from
 % https://scienceworld.wolfram.com/physics/LiftCoefficient.html
 params.aerodynamics = struct(...
@@ -120,56 +125,15 @@ params.linearization = struct(...
 % ------------------------------------------------------------------------
 % Performance requirements
 
-params.performance = struct(...
-  'ReferencePosMaxDistance', 1, ... % m
-  'HorizontalPosMaxError', 2, ... % m
-  'HorizontalMaxSpeed', 2, ... % m / s
-  'HorizontalSettleTime', 6, ... % s
-  'VerticalPosMaxError', 2, ... % m
-  'VerticalMaxSpeed', 2, ... % m / s
-  'VerticalSettleTime', 4, ... % s
-  'AngleMaxPitchRoll', 10 * pi / 180, ... % in rad
-  'AngleMaxYaw', pi, ... % rad
-  'AngleMaxAngularRate', 20 * pi / 180 ... % rad / s
+params.normalization = struct(...
+  'HPosition', 1, ... % m
+  'VPosition', 1, ...% m
+  'HSpeed', 2, ... % m / s
+  'VSpeed', 2, ... % m / s
+  'PitchRollAngle', 10 * pi / 180, ... % rad
+  'YawAngle', 5 * pi / 180, ... % rad
+  'AngularRate', 2 * pi / 180, ... % rad / s
+  'WindSpeed', 2 ... % m / s
 );
-
-% Scaling matrices (because signals are normalized wrt parameter performance)
-p = params.performance;
-
-params.ErrorScalingMatrix = blkdiag(...
-  eye(4) * params.actuators.ServoAbsMaxAngle, ...
-  params.actuators.PropellerMaxAngularVelocity ...
-    - params.linearization.Inputs(5), ...
-  eye(2) * p.HorizontalPosMaxError, ...
-  p.VerticalPosMaxError, ...
-  eye(2) * p.HorizontalMaxSpeed, ...
-  p.VerticalMaxSpeed, ...
-  eye(2) * p.AngleMaxPitchRoll, ...
-  p.AngleMaxYaw);
-
-params.OutputScalingMatrix = blkdiag(...
-  eye(2) * p.HorizontalPosMaxError, ...
-  p.VerticalPosMaxError, ...
-  eye(2) * p.HorizontalMaxSpeed, ...
-  p.VerticalMaxSpeed, ...
-  eye(2) * p.AngleMaxPitchRoll, ...
-  p.AngleMaxYaw, ...
-  eye(3) * p.AngleMaxAngularRate);
-
-
-horiz_settle_speed = (1 - exp(-1)) / params.performance.HorizontalSettleTime;
-if horiz_settle_speed > params.performance.HorizontalMaxSpeed
-  fprintf(['Contradictory performance requirements: ' ...
-    'velocity needed to attain horizontal settle time is ' ...
-    'higher than HorizontalMaxSpeed']);
-end
-
-vert_settle_speed = (1 - exp(-1)) / params.performance.VerticalSettleTime;
-if vert_settle_speed > params.performance.VerticalMaxSpeed
-  fprintf(['Contradictory performance requirements: ' ...
-    'velocity needed to attain vertical settle time is ' ...
-    'higher than VerticalMaxSpeed']);
-end
-
 end
 % vim: ts=2 sw=2 et:
diff --git a/uav_performance_musyn.m b/uav_performance_musyn.m
index 180be8a..aba2160 100644
--- a/uav_performance_musyn.m
+++ b/uav_performance_musyn.m
@@ -18,22 +18,31 @@ function [perf] = uav_performance_musyn(params, do_plots)
 % Laplace variable
 s = tf('s');
 
-% TODO: set proper constraints
-G = 1e2 / (s + 1e2);
+% Bandwitdhs
+bw_alpha = params.actuators.ServoNominalAngularVelocity;
+bw_omega = 10;
 
-W_Palpha = G; 
-W_Pomega = G;
+bw_xy = .05;
+bw_z = .05;
 
-W_Pxy = G;
-W_Pz = G;
+bw_xydot = .5;
+bw_zdot = .1;
 
-W_Pxydot = tf(0);
-W_Pzdot = tf(0);
- 
-W_Pphitheta = tf(0);
-W_Ppsi = tf(0);
+bw_phitheta = .1;
+bw_psi = .08;
+
+% Inverse performance functions
+W_Palpha = .2 / (s / bw_alpha + 1); 
+W_Pomega = .2 / (s / bw_omega + 1);
 
-W_PTheta = tf(0);
+W_Pxy = 8 * bw_xy^2 / (s^2 + 2 * 1 * bw_xy * s + bw_xy^2);
+W_Pz = 1 * bw_z^2 / (s^2  + 2 * 1 * bw_z * s + bw_z^2);
+
+W_Pxydot = .2 / (s / bw_xydot + 1);
+W_Pzdot = .5 / (s / bw_zdot + 1);
+ 
+W_Pphitheta = 1 / (s / bw_phitheta + 1);
+W_Ppsi = .1 / (s / bw_psi + 1);
 
 % Construct performance vector by combining xy and z
 W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
@@ -70,37 +79,24 @@ if do_plots
 
   % Step response of position requirements
   figure; hold on;
-  step(W_Pxy); step(W_Pz);
-  step(W_Pxydot); step(W_Pzdot);
+
   step(W_Palpha);
   step(W_Pomega);
+  step(W_Pxy);
+  step(W_Pz);
+  step(W_Pxydot);
+  step(W_Pzdot);
+  step(W_Pphitheta);
+  step(W_Ppsi);
+
   grid on;
-  legend('$W_{P,xy}$', '$W_{P,z}$', ...
+  legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ...
+    '$W_{P,xy}$', '$W_{P,z}$', ...
     '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
-    '$W_{P,\alpha}$', '$W_{P,\omega}$', ...
+    '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ...
     'interpreter', 'latex', 'fontSize', 8);
   title('Step responses of performance requirements');
 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_musyn.m b/uav_sim_step.m
index 3af2a76..973a928 100644
--- a/uav_sim_step_musyn.m
+++ b/uav_sim_step.m
@@ -3,7 +3,7 @@
 % 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, do_plots, do_noise)
+function [simout] = uav_sim_step_musyn(params, model, ctrl, nsamp, T, do_plots, do_noise)
 
 % Load closed loop model and add controller
 % more or less equivalent to doing usys = lft(Pnom, K)
@@ -20,7 +20,7 @@ P_nom_clp = minreal(usys_clp(...
   [model.uncertain.index.OutputError; 
     model.uncertain.index.OutputNominal;
     model.uncertain.index.OutputPlots], ...
-  model.uncertain.index.InputExogenous));
+  model.uncertain.index.InputExogenous), [], false);
 
 % Indices for exogenous inputs
 Iwwind = (1:3)';
@@ -33,7 +33,6 @@ Ieomega = 5;
 IeP = (6:8)';
 IePdot = (9:11)';
 IeTheta = (12:14)';
-% size([Iealpha; Ieomega; IeP; IePdot; IeTheta])
 
 % Indices for y outputs (for plots)
 IP = (15:17)';
@@ -58,7 +57,7 @@ if do_noise
 end
 
 % Create step inputs (normalized)
-ref_step = ones(1, nsamp); % 1d step function
+ref_step = .5 * 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) ];
@@ -66,22 +65,39 @@ in_step_z = [ noise; zeros(2, nsamp); ref_step ];
 
 % Simulation time
 n_settle_times = 10;
-T_final_horiz = n_settle_times * params.performance.HorizontalSettleTime;
-T_final_vert = n_settle_times * params.performance.VerticalSettleTime;
-
-t_xy = linspace(0, T_final_horiz, nsamp);
-t_z = linspace(0, T_final_vert, nsamp);
+t = linspace(0, T, nsamp);
 
 % Simulate step responses
-out_step_x = lsim(P_nom_clp, in_step_x, t_xy);
-out_step_y = lsim(P_nom_clp, in_step_y, t_xy);
-out_step_z = lsim(P_nom_clp, in_step_z, t_z);
+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';
 
 % Return simulation
 simout = struct(...
-  'TimeXY', t_xy, ...
-  'StepX', out_step_x, ...
-  'StepY', out_step_y, ...
+  'Time', t, ...
+  'StepX', out_step_x_norm, ...
+  'StepY', out_step_y_norm, ...
+  'StepZ', out_step_z_norm, ...
   'Simulink', ulmod_clp, ...
   'StateSpace', P_nom_clp);
 
@@ -111,9 +127,9 @@ if do_plots
     ctrl.Name), 'Interpreter', 'latex');
 
   % Plot limits
-  ref_value     = params.performance.ReferencePosMaxDistance;
+  ref_value     = .5;
   alpha_max_deg = params.actuators.ServoAbsMaxAngle * to_deg;
-  euler_lim_deg = 1.5; % params.performance.AngleMaxPitchRoll * to_deg;
+  euler_lim_deg = 1.5;
   omega_max_rpm = (params.actuators.PropellerMaxAngularVelocity ...
     - params.linearization.Inputs(5)) * to_rpm;
   omega_min_rpm = -params.linearization.Inputs(5) * to_rpm;
@@ -121,14 +137,14 @@ if do_plots
   % Plot step response from x to alpha
   subplot(2, 3, 1);
   hold on;
-  plot(t_xy, out_step_x(:, Ialpha(1)) * to_deg);
-  plot(t_xy, out_step_x(:, Ialpha(2)) * to_deg);
-  plot(t_xy, out_step_x(:, Ialpha(3)) * to_deg);
-  plot(t_xy, out_step_x(:, Ialpha(4)) * to_deg);
-  plot([0, T_final_horiz], [1, 1] * alpha_max_deg, 'r--');
-  plot([0, T_final_horiz], [-1, -1] * alpha_max_deg, 'r--');
+  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([0, T], [1, 1] * alpha_max_deg, 'r--');
+  plot([0, T], [-1, -1] * alpha_max_deg, 'r--');
   grid on;
-  xlim([0, T_final_horiz]);
+  xlim([0, T]);
   ylim([-alpha_max_deg * 1.1, alpha_max_deg * 1.1]);
   title('Horizontal $x$ to Flaps', 'Interpreter', 'latex');
   ylabel('Flap Angle (degrees)', 'Interpreter', 'latex');
@@ -138,14 +154,14 @@ if do_plots
 
   % Plot step response from y to alpha
   subplot(2, 3, 2); hold on;
-  plot(t_xy, out_step_y(:, Ialpha(1)) * to_deg);
-  plot(t_xy, out_step_y(:, Ialpha(2)) * to_deg);
-  plot(t_xy, out_step_y(:, Ialpha(3)) * to_deg);
-  plot(t_xy, out_step_y(:, Ialpha(4)) * to_deg);
-  plot([0, T_final_horiz], [1, 1] * alpha_max_deg, 'r--');
-  plot([0, T_final_horiz], [-1, -1] * alpha_max_deg, 'r--');
+  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([0, T], [1, 1] * alpha_max_deg, 'r--');
+  plot([0, T], [-1, -1] * alpha_max_deg, 'r--');
   grid on;
-  xlim([0, T_final_horiz]);
+  xlim([0, T]);
   ylim([-alpha_max_deg * 1.1, alpha_max_deg * 1.1]);
   title('Horizontal $y$ to Flaps', 'Interpreter', 'latex');
   ylabel('Flap Angle (degrees)', 'Interpreter', 'latex');
@@ -155,14 +171,14 @@ if do_plots
 
   % Plot step response from z to alpha
   subplot(2, 3, 3); hold on;
-  plot(t_z, out_step_z(:, Ialpha(1)) * to_deg);
-  plot(t_z, out_step_z(:, Ialpha(2)) * to_deg);
-  plot(t_z, out_step_z(:, Ialpha(3)) * to_deg);
-  plot(t_z, out_step_z(:, Ialpha(4)) * to_deg);
-  plot([0, T_final_vert], [1, 1] * alpha_max_deg, 'r--');
-  plot([0, T_final_vert], [-1, -1] * alpha_max_deg, 'r--');
+  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([0, T], [1, 1] * alpha_max_deg, 'r--');
+  plot([0, T], [-1, -1] * alpha_max_deg, 'r--');
   grid on;
-  xlim([0, T_final_vert]);
+  xlim([0, T]);
   ylim([-alpha_max_deg * 1.1, alpha_max_deg * 1.1]);
   title('Vertical $z$ to Flaps', 'Interpreter', 'latex');
   ylabel('Flap Angle (degrees)', 'Interpreter', 'latex');
@@ -172,11 +188,11 @@ if do_plots
 
   % Plot step response from x to Theta
   subplot(2, 3, 4); hold on;
-  plot(t_xy, out_step_x(:, ITheta(1)) * to_deg);
-  plot(t_xy, out_step_x(:, ITheta(2)) * to_deg);
-  plot(t_xy, out_step_x(:, ITheta(3)) * to_deg);
+  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);
   grid on;
-  xlim([0, T_final_horiz]);
+  xlim([0, T]);
   ylim([-euler_lim_deg, euler_lim_deg]);
   title('Horizontal $x$ to Euler Angles', 'Interpreter', 'latex');
   ylabel('Euler Angle (degrees)', 'Interpreter', 'latex');
@@ -186,11 +202,11 @@ if do_plots
 
   % Plot step response from y to Theta
   subplot(2, 3, 5); hold on;
-  plot(t_xy, out_step_y(:, ITheta(1)) * to_deg);
-  plot(t_xy, out_step_y(:, ITheta(2)) * to_deg);
-  plot(t_xy, out_step_y(:, ITheta(3)) * to_deg);
+  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);
   grid on;
-  xlim([0, T_final_horiz]);
+  xlim([0, T]);
   ylim([-euler_lim_deg, euler_lim_deg]);
   title('Horizontal $y$ to Euler Angles', 'Interpreter', 'latex');
   ylabel('Euler Angle (degrees)', 'Interpreter', 'latex');
@@ -200,11 +216,11 @@ if do_plots
 
   % Plot step response from z to Theta
   subplot(2, 3, 6); hold on;
-  plot(t_z, out_step_z(:, ITheta(1)) * to_deg);
-  plot(t_z, out_step_z(:, ITheta(2)) * to_deg);
-  plot(t_z, out_step_z(:, ITheta(3)) * to_deg);
+  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);
   grid on;
-  xlim([0, T_final_vert]);
+  xlim([0, T]);
   ylim([-euler_lim_deg, euler_lim_deg]);
   title('Vertical $z$ to Euler Angles', 'Interpreter', 'latex');
   ylabel('Euler Angle (degrees)', 'Interpreter', 'latex');
@@ -219,9 +235,9 @@ if do_plots
     ctrl.Name), 'Interpreter', 'latex');
 
   hold on;
-  step(P_nom_clp(Iomega, Ir(3)) * to_rpm, T_final_vert);
-  plot([0, T_final_vert], [1, 1] * omega_min_rpm, 'r--');
-  plot([0, T_final_vert], [1, 1] * omega_max_rpm, 'r--');
+  step(P_nom_clp(Iomega, Ir(3)) * to_rpm, T);
+  plot([0, T], [1, 1] * omega_min_rpm, 'r--');
+  plot([0, T], [1, 1] * omega_max_rpm, 'r--');
   grid on;
   ylim([omega_min_rpm - 1, omega_max_rpm + 1]);
   title('Vertical $z$ to Thruster $\omega$', 'Interpreter', 'latex');
@@ -237,10 +253,10 @@ if do_plots
 
   % Plot step response from horizontal reference to horizontal position
   subplot(2, 2, 1); hold on;
-  plot(t_xy, out_step_x(:, IP(1)));
-  plot(t_xy, out_step_y(:, IP(2)));
-  % plot([0, T_final_horiz], [1, 1] * ref_value, 'r:');
-  % plot(t_xy, out_step_xydes, 'r--');
+  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--');
   grid on;
   title('Horizontal Position Error', 'Interpreter', 'latex');
   ylabel('Error (meters)', 'Interpreter', 'latex');
@@ -249,8 +265,8 @@ if do_plots
 
   % Plot step response horizontal reference to horizontal speed
   subplot(2, 2, 2); hold on;
-  plot(t_xy, out_step_x(:, IPdot(1)));
-  plot(t_xy, out_step_y(:, IPdot(2)));
+  plot(t, out_step_x(:, IPdot(1)));
+  plot(t, out_step_y(:, IPdot(2)));
   grid on;
   title('Horizontal Velocity', 'Interpreter', 'latex');
   ylabel('Velocity (m / s)', 'Interpreter', 'latex');
@@ -259,9 +275,9 @@ if do_plots
 
   % Plot step response from vertical reference to vertical position
   subplot(2, 2, 3); hold on;
-  plot(t_z, out_step_z(:, IP(3)));
-  % plot([0, T_final_vert], [1, 1] * ref_value, 'r:');
-  % plot(t_z, out_step_zdes, 'r--');
+  plot(t, out_step_z(:, IP(3)));
+  % plot([0, T], [1, 1] * ref_value, 'r:');
+  % plot(t, out_step_zdes, 'r--');
   grid on;
   title('Vertical Position Error', 'Interpreter', 'latex');
   ylabel('Error (meters)', 'Interpreter', 'latex');
@@ -270,7 +286,7 @@ if do_plots
 
   % Plot step response vertical reference to vertical speed
   subplot(2, 2, 4); hold on;
-  plot(t_z, out_step_z(:, IPdot(3)));
+  plot(t, out_step_z(:, IPdot(3)));
   grid on;
   title('Vertical Velocity', 'Interpreter', 'latex');
   ylabel('Velocity (m / s)', 'Interpreter', 'latex');
diff --git a/uav_sim_step_hinf.m b/uav_sim_step_hinf.m
deleted file mode 100644
index 5e448a1..0000000
--- a/uav_sim_step_hinf.m
+++ /dev/null
@@ -1,282 +0,0 @@
-% Simulate a step responses of ducted-fan VTOL micro-UAV.
-%
-% 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_hinf(params, model, ctrl, nsamp, 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);
-
-% 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);
-
-% 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)';
-% size([Iealpha; Ieomega; IeP; IePdot; IeTheta])
-
-% Indices for y outputs (for plots)
-IP = (15:17)';
-IPdot = (18:20)';
-ITheta = (21:23)';
-IOmega = (24:26)';
-
-% Indices for p outputs (for plots)
-Ialpha = (27:30)';
-Iomega = 31;
-
-Iualpha = (32:35)';
-Iuomega = 36;
-
-noise = zeros(7, nsamp); % no noise
-if do_noise
-  % Noise in percentage
-  noise_alpha_amp = (.5 * (pi / 180)) / params.actuators.ServoAbsMaxAngle;
-  noise_wind_amp = .1;
-  noise = [noise_wind_amp * randn(3, nsamp);
-    noise_alpha_amp * randn(4, nsamp)];
-end
-
-% Create step inputs (normalized)
-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 ];
-
-% Simulation time
-n_settle_times = 10;
-T_final_horiz = n_settle_times * params.performance.HorizontalSettleTime;
-T_final_vert = n_settle_times * params.performance.VerticalSettleTime;
-
-t_xy = linspace(0, T_final_horiz, nsamp);
-t_z = linspace(0, T_final_vert, nsamp);
-
-% Simulate step responses
-out_step_x = lsim(P_nom_clp, in_step_x, t_xy);
-out_step_y = lsim(P_nom_clp, in_step_y, t_xy);
-out_step_z = lsim(P_nom_clp, in_step_z, t_z);
-
-% Return simulation
-simout = struct(...
-  'TimeXY', t_xy, ...
-  'StepX', out_step_x, ...
-  'StepY', out_step_y, ...
-  '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);
-
-if do_plots
-  % Conversion factors
-  to_deg = 180 / pi; % radians to degrees
-  to_rpm = pi / 30; % rad / s to RPM
-
-  % Figure for flaps and Euler angles
-  figure;
-  sgtitle(sprintf(...
-    '\\bfseries Step Response of Flap and Euler Angles (%s)', ...
-    ctrl.Name), 'Interpreter', 'latex');
-
-  % Plot limits
-  ref_value     = params.performance.ReferencePosMaxDistance;
-  alpha_max_deg = params.actuators.ServoAbsMaxAngle * to_deg;
-  euler_lim_deg = 1.5; % params.performance.AngleMaxPitchRoll * to_deg;
-  omega_max_rpm = (params.actuators.PropellerMaxAngularVelocity ...
-    - params.linearization.Inputs(5)) * to_rpm;
-  omega_min_rpm = -params.linearization.Inputs(5) * to_rpm;
-
-  % Plot step response from x to alpha
-  subplot(2, 3, 1);
-  hold on;
-  plot(t_xy, out_step_x(:, Ialpha(1)) * to_deg);
-  plot(t_xy, out_step_x(:, Ialpha(2)) * to_deg);
-  plot(t_xy, out_step_x(:, Ialpha(3)) * to_deg);
-  plot(t_xy, out_step_x(:, Ialpha(4)) * to_deg);
-  plot([0, T_final_horiz], [1, 1] * alpha_max_deg, 'r--');
-  plot([0, T_final_horiz], [-1, -1] * alpha_max_deg, 'r--');
-  grid on;
-  xlim([0, T_final_horiz]);
-  ylim([-alpha_max_deg * 1.1, alpha_max_deg * 1.1]);
-  title('Horizontal $x$ to Flaps', 'Interpreter', 'latex');
-  ylabel('Flap Angle (degrees)', 'Interpreter', 'latex');
-  xlabel('Time (seconds)', 'Interpreter', 'latex');
-  legend('$\alpha_1(t)$', '$\alpha_2(t)$', '$\alpha_3(t)$', '$\alpha_4(t)$', ...
-    'Interpreter', 'latex');
-
-  % Plot step response from y to alpha
-  subplot(2, 3, 2); hold on;
-  plot(t_xy, out_step_y(:, Ialpha(1)) * to_deg);
-  plot(t_xy, out_step_y(:, Ialpha(2)) * to_deg);
-  plot(t_xy, out_step_y(:, Ialpha(3)) * to_deg);
-  plot(t_xy, out_step_y(:, Ialpha(4)) * to_deg);
-  plot([0, T_final_horiz], [1, 1] * alpha_max_deg, 'r--');
-  plot([0, T_final_horiz], [-1, -1] * alpha_max_deg, 'r--');
-  grid on;
-  xlim([0, T_final_horiz]);
-  ylim([-alpha_max_deg * 1.1, alpha_max_deg * 1.1]);
-  title('Horizontal $y$ to Flaps', 'Interpreter', 'latex');
-  ylabel('Flap Angle (degrees)', 'Interpreter', 'latex');
-  xlabel('Time (seconds)', 'Interpreter', 'latex');
-  legend('$\alpha_1(t)$', '$\alpha_2(t)$', '$\alpha_3(t)$', '$\alpha_4(t)$', ...
-    'Interpreter', 'latex');
-
-  % Plot step response from z to alpha
-  subplot(2, 3, 3); hold on;
-  plot(t_z, out_step_z(:, Ialpha(1)) * to_deg);
-  plot(t_z, out_step_z(:, Ialpha(2)) * to_deg);
-  plot(t_z, out_step_z(:, Ialpha(3)) * to_deg);
-  plot(t_z, out_step_z(:, Ialpha(4)) * to_deg);
-  plot([0, T_final_vert], [1, 1] * alpha_max_deg, 'r--');
-  plot([0, T_final_vert], [-1, -1] * alpha_max_deg, 'r--');
-  grid on;
-  xlim([0, T_final_vert]);
-  ylim([-alpha_max_deg * 1.1, alpha_max_deg * 1.1]);
-  title('Vertical $z$ to Flaps', 'Interpreter', 'latex');
-  ylabel('Flap Angle (degrees)', 'Interpreter', 'latex');
-  xlabel('Time (seconds)', 'Interpreter', 'latex');
-  legend('$\alpha_1(t)$', '$\alpha_2(t)$', '$\alpha_3(t)$', '$\alpha_4(t)$', ...
-    'Interpreter', 'latex');
-
-  % Plot step response from x to Theta
-  subplot(2, 3, 4); hold on;
-  plot(t_xy, out_step_x(:, ITheta(1)) * to_deg);
-  plot(t_xy, out_step_x(:, ITheta(2)) * to_deg);
-  plot(t_xy, out_step_x(:, ITheta(3)) * to_deg);
-  grid on;
-  xlim([0, T_final_horiz]);
-  ylim([-euler_lim_deg, euler_lim_deg]);
-  title('Horizontal $x$ to Euler Angles', 'Interpreter', 'latex');
-  ylabel('Euler Angle (degrees)', 'Interpreter', 'latex');
-  xlabel('Time (seconds)', 'Interpreter', 'latex');
-  legend('$\phi(t)$ Roll ', '$\theta(t)$ Pitch ', '$\psi(t)$ Yaw ', ...
-    'Interpreter', 'latex');
-
-  % Plot step response from y to Theta
-  subplot(2, 3, 5); hold on;
-  plot(t_xy, out_step_y(:, ITheta(1)) * to_deg);
-  plot(t_xy, out_step_y(:, ITheta(2)) * to_deg);
-  plot(t_xy, out_step_y(:, ITheta(3)) * to_deg);
-  grid on;
-  xlim([0, T_final_horiz]);
-  ylim([-euler_lim_deg, euler_lim_deg]);
-  title('Horizontal $y$ to Euler Angles', 'Interpreter', 'latex');
-  ylabel('Euler Angle (degrees)', 'Interpreter', 'latex');
-  xlabel('Time (seconds)', 'Interpreter', 'latex');
-  legend('$\phi(t)$ Roll ', '$\theta(t)$ Pitch ', '$\psi(t)$ Yaw ', ...
-    'Interpreter', 'latex');
-
-  % Plot step response from z to Theta
-  subplot(2, 3, 6); hold on;
-  plot(t_z, out_step_z(:, ITheta(1)) * to_deg);
-  plot(t_z, out_step_z(:, ITheta(2)) * to_deg);
-  plot(t_z, out_step_z(:, ITheta(3)) * to_deg);
-  grid on;
-  xlim([0, T_final_vert]);
-  ylim([-euler_lim_deg, euler_lim_deg]);
-  title('Vertical $z$ to Euler Angles', 'Interpreter', 'latex');
-  ylabel('Euler Angle (degrees)', 'Interpreter', 'latex');
-  xlabel('Time (seconds)', 'Interpreter', 'latex');
-  legend('$\phi(t)$ Roll ', '$\theta(t)$ Pitch ', '$\psi(t)$ Yaw ', ...
-    'Interpreter', 'latex');
-
-  % Plot step response from z to omega
-  figure;
-  sgtitle(sprintf(...
-    '\\bfseries Step Response to Propeller (%s)', ...
-    ctrl.Name), 'Interpreter', 'latex');
-
-  hold on;
-  step(P_nom_clp(Iomega, Ir(3)) * to_rpm, T_final_vert);
-  plot([0, T_final_vert], [1, 1] * omega_min_rpm, 'r--');
-  plot([0, T_final_vert], [1, 1] * omega_max_rpm, 'r--');
-  grid on;
-  ylim([omega_min_rpm - 1, omega_max_rpm + 1]);
-  title('Vertical $z$ to Thruster $\omega$', 'Interpreter', 'latex');
-  ylabel('Angular Velocity (RPM)', 'Interpreter', 'latex');
-  xlabel('Time (seconds)', 'Interpreter', 'latex');
-  legend('$\omega(t)$', 'Interpreter', 'latex');
-
-  % Figure for position and velocity
-  figure;
-  sgtitle(sprintf(...
-    '\\bfseries Step Response of Position and Speed (%s)', ...
-    ctrl.Name), 'Interpreter', 'latex');
-
-  % Plot step response from horizontal reference to horizontal position
-  subplot(2, 2, 1); hold on;
-  plot(t_xy, out_step_x(:, IP(1)));
-  plot(t_xy, out_step_y(:, IP(2)));
-  % plot([0, T_final_horiz], [1, 1] * ref_value, 'r:');
-  % plot(t_xy, out_step_xydes, 'r--');
-  grid on;
-  title('Horizontal Position Error', 'Interpreter', 'latex');
-  ylabel('Error (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_xy, out_step_x(:, IPdot(1)));
-  plot(t_xy, out_step_y(:, IPdot(2)));
-  grid on;
-  title('Horizontal Velocity', 'Interpreter', 'latex');
-  ylabel('Velocity (m / s)', 'Interpreter', 'latex');
-  xlabel('Time (seconds)', 'Interpreter', 'latex');
-  legend('$\dot{x}(t)$', '$\dot{y}(t)$', 'Interpreter', 'latex');
-
-  % Plot step response from vertical reference to vertical position
-  subplot(2, 2, 3); hold on;
-  plot(t_z, out_step_z(:, IP(3)));
-  % plot([0, T_final_vert], [1, 1] * ref_value, 'r:');
-  % plot(t_z, out_step_zdes, 'r--');
-  grid on;
-  title('Vertical Position Error', 'Interpreter', 'latex');
-  ylabel('Error (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_z, out_step_z(:, IPdot(3)));
-  grid on;
-  title('Vertical Velocity', 'Interpreter', 'latex');
-  ylabel('Velocity (m / s)', 'Interpreter', 'latex');
-  xlabel('Time (seconds)', 'Interpreter', 'latex');
-  legend('$\dot{z}(t)$', 'Interpreter', 'latex');
-end
-
-end
-% vim:ts=2 sw=2 et:
diff --git a/uav_uncertainty.m b/uav_uncertainty.m
index aff30ca..b664cee 100644
--- a/uav_uncertainty.m
+++ b/uav_uncertainty.m
@@ -37,9 +37,11 @@ eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega);
 % W_momega = eps_omega * 10 / (s + 10);
 % W_mState = .01 * 10 / (s + 10);
 
-W_malpha = eps_alpha * tf(1);
+b = 1e3;
+
+W_malpha = make_weight(b, 1 / eps_alpha, 1e-2);
 W_momega = tf(0);
-W_mState = .01 * tf(1);
+W_mState = 1 - 1 / (s / 50 + 1);
 
 uncert = struct(...
   'FlapAngle', W_malpha * eye(4), ...
@@ -61,4 +63,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: