Fix DK iteration scales, add realistic values

also separate performance requirements from hinf and musyn

7 files changed, 588 insertions, 62 deletions

diff --git a/uav.m b/uav.m
index 62f6543..ca33346 100644
--- a/uav.m
+++ b/uav.m
@@ -9,11 +9,15 @@
 clear; clc; close all; s = tf('s');
 
 % Flags to speed up running for debugging
-do_plots = true;
+do_plots = true; % runs faster without
 do_lqr = false; % unused
-do_hinf = true; % midterm
+do_hinf = false; % midterm
 do_musyn = true; % endterm
 
+if do_hinf & do_musyn
+  error('Cannot do both H-infinity and mu synthesis.')
+end
+
 fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n')
 fprintf('Will do:\n')
 if do_plots
@@ -41,15 +45,21 @@ params = uav_params();
 % ------------------------------------------------------------------------
 %% Define performance requirements
 
-if do_hinf | do_musyn
+if do_hinf
   fprintf('Generating performance requirements...\n')
-  perf = uav_performance(params, do_plots);
+  perf = uav_performance_hinf(params, do_plots);
+end
+if do_musyn
+  fprintf('Generating performance requirements...\n')
+  perf = uav_performance_musyn(params, do_plots);
 end
 
 %  ------------------------------------------------------------------------
 %% Define stability requirements
 
-if do_musyn
+% Note: for hinf it is needed to call uav_mode, 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
@@ -119,33 +129,48 @@ if do_musyn
   fprintf('Performing mu-synthesis controller design...\n')
 
   idx = model.uncertain.index;
-  P = model.uncertain.StateSpace([idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ...
-                                 [idx.InputUncertain; idx.InputExogenous; idx.InputNominal]);
-
-  % Initial values for D-K iteration
-  D_left = eye(model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny);
-  D_right = eye(model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu);
+  P = minreal(model.uncertain.StateSpace(...
+        [idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ...
+        [idx.InputUncertain; idx.InputExogenous; idx.InputNominal]), ...
+        [], false);
 
   % Options for H-infinity
   nmeas = model.uncertain.Ny;
   nctrl = model.uncertain.Nu;
-  hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
-    'AutoScale', 'off', 'RelTol', 1e-3);
+  hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
+    'AutoScale', 'on', 'RelTol', 1e-2);
 
   % Number of D-K iterations
-  niters = 4;
+  niters = 8;
 
   % Frequency raster resolution to fit D scales
-  nsamples = 51;
-  omega = logspace(-2, 2, nsamples);
+  nsamples = 61;
+  omega = logspace(-2, 3, nsamples);
+
+  % Initial values for D-K iteration
+  nleft = model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny;
+  nleft_clp = model.uncertain.Nz + model.uncertain.Ne;
+
+  nright = model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu;
+  nright_clp = model.uncertain.Nv + model.uncertain.Nw;
+
+  D_left = tf(eye(nleft));
+  D_right = tf(eye(nright));
+
+  last_mu_rp = inf;
+  mu_plot_legend = {};
 
   % Start DK-iteration
   for it = 1:niters
     fprintf(' - Running D-K iteration %d ...\n', it);
 
     % Find controller using H-infinity
-    [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl);
+    [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl, hinfopt);
     fprintf('   H-infinity synthesis gamma: %g\n', gamma);
+    if gamma == inf
+      fprintf('   Failed to synethesize H-infinity controller\n');
+      break;
+    end
 
     % Calculate frequency response of closed loop
     N = minreal(lft(P, K), [], false); % slient
@@ -156,43 +181,103 @@ if do_musyn
     mu_rp = norm(mu_bounds(1,1), inf, 1e-6);
     fprintf('   Mu value for RP: %g\n', mu_rp)
 
+    if do_plots
+      fprintf('   Plotting mu\n');
+      figure(100); hold on;
+      bodemag(mu_bounds(1,1));
+      mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{%d}$', it)};
+      title('\bfseries $\mu_\Delta(\omega)$ for both Stability and Performance', 'interpreter', 'latex');
+      legend(mu_plot_legend, 'interpreter', 'latex');
+      grid on;
+      drawnow;
+    end
+
+    % Are we done yet?
+    if mu_rp < 1
+      fprintf(' - Found robust controller that meets performance.\n');
+      break
+    end
+
     % Fit D-scales
-    [dsysl, dsysr] = mussvunwrap(mu_info);
-    D_left = fitfrd(genphase(dsysl(1,1)), 1);
-    D_right = fitfrd(genphase(dsysr(1,1)), 1);
+    % 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);
+
+    % D scale for alpha uncertainty (first block)
+    i = 1;
+    D_left_samples_alpha = D_left_samples(i, i);
+    D_alpha = fitmagfrd(D_left_samples_alpha, 2);
+
+    % D scale for omega uncertainty (second block)
+    i = model.uncertain.BlockStructure(1, 1) + 1; % after first block
+    D_left_samples_omega = frd(D_left_samples(i, i));
+    D_omega = fitmagfrd(D_left_samples_omega, 3);
+
+    % D scale for state uncertainty (third block)
+    i = model.uncertain.BlockStructure(2, 1) + 1; % after second block
+    D_left_samples_state = D_left_samples(i, i);
+    D_state = fitmagfrd(D_left_samples_state, 5);
+
+    % D scale for performance (non-square)
+    i = model.uncertain.BlockStructurePerf(3, 1); % after third block
+    D_left_samples_perf = D_left_samples(i, i);
+    D_perf = fitmagfrd(D_left_samples_perf, 2);
+
+    % Construct full matrices
+    D_right = blkdiag(D_alpha * eye(4), ...
+                      D_omega * eye(1), ...
+                      D_state * eye(12), ...
+                      D_perf * eye(10), ...
+                      eye(5));
+
+    D_left = blkdiag(D_alpha * eye(4), ...
+                     D_omega * eye(1), ...
+                     D_state * eye(12), ...
+                     D_perf * eye(14), ...
+                     eye(12));
+
+    % Plot fitted D-scales
+    if do_plots
+      fprintf('   Plotting D-scales ');
+      f = figure(101); clf(f); hold on;
+
+      bodemag(D_left_samples_alpha, omega);
+      bodemag(D_alpha, omega);
+      fprintf('.');
+
+      bodemag(D_left_samples_omega, omega);
+      bodemag(D_omega, omega);
+      fprintf('.');
+
+      bodemag(D_left_samples_state, omega);
+      bodemag(D_state, omega);
+      fprintf('.');
+
+      bodemag(D_left_samples_perf, omega);
+      bodemag(D_perf, omega);
+      fprintf('.');
+
+      fprintf('\n');
+      title(sprintf('\bfseries $D(\\omega)$ Scales Approximations at Iteration %d', it), ...
+            'interpreter', 'latex')
+      legend(...
+        '$D_{\alpha}$', '$\hat{D}_{\alpha}$', ...
+        '$D_{\omega}$', '$\hat{D}_{\omega}$', ...
+        '$D_{\mathbf{x}}$', '$\hat{D}_{\mathbf{x}}$', ...
+        '$D_{\Delta}$', '$\hat{D}_{\Delta}$', ...
+        'interpreter', 'latex' ...
+      );
+      grid on;
+      drawnow;
+    end
+  end
+
+  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);
   end
 end
 
 %  ------------------------------------------------------------------------
 %% Verify performance satisfaction via mu-analysis
-
-% omega = logspace(-3, 3, 250);
-%
-% N_inf = lft(P, K_inf);
-% N_inf_frd = frd(N_inf, omega);
-%
-% % robust stability
-% [mu_inf_rs, ~] = mussv(N_inf_frd(idx.OutputUncertain, idx.InputUncertain), ...
-%             model.uncertain.BlockStructure);
-%
-% % robust performance
-% blk_perf = [model.uncertain.BlockStructure;
-%             model.uncertain.BlockStructurePerf];
-%
-% [mu_inf_rp, ~] = mussv(N_inf_frd, blk_perf);
-%
-% % nominal performance
-% mu_inf_np = svd(N_inf_frd(idx.OutputError, idx.InputExogenous));
-%
-% if do_plots
-%   figure; hold on;
-%   bodemag(mu_inf_rs(1));
-%   bodemag(mu_inf_np(1));
-%   bodemag(mu_inf_rs(2));
-%   bodemag(mu_inf_np(2));
-%
-%   grid on;
-%   title('$\mu_\Delta(N)$', 'Interpreter', 'latex');
-% end
-
-% vim: ts=2 sw=2 et:
diff --git a/uav_model.m b/uav_model.m
index 0ec9a67..3fc1d0a 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -318,14 +318,14 @@ usys = ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d);
 
 % Specify uncertainty block structure for mussv command
 blk_stab = [
-  4, 0; % alpha uncert, diagonal
-  1, 0; % omega uncert, diagonal
+  4, 4; % alpha uncert, full
+  1, 1; % omega uncert, full
   12, 12; % state uncert, full
 ];
 
 blk_perf = [
   blk_stab;
-  10, 14;
+  10, 14 % always full
 ];
 
 % Save uncertain model
diff --git a/uav_params.m b/uav_params.m
index da8a19b..a1c0e4a 100644
--- a/uav_params.m
+++ b/uav_params.m
@@ -11,6 +11,7 @@ function [params] = uav_params()
 % Time in s
 % Frequencies in Hz
 % Angular velocities in rad / s
+% Uncertainty / measurement errors in percentage (between 0 and 1)
 
 params = struct();
 
@@ -44,7 +45,8 @@ params.mechanical = struct(...
   'DuctHeight', 171e-3, ...
   'FlapZDistance', 98e-3, ... % flap distance along z from center of mass
   'InertiaTensor', J, ...
-  'GyroscopicInertiaZ', J_prop ... % assume small angle
+  'GyroscopicInertiaZ', J_prop, ... % assume small angle
+  'GyroscopicInertiaZUncertainty', .01 ... % in %
 );
 
 % ------------------------------------------------------------------------
@@ -84,9 +86,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', .05, ... % in %
   'FlapArea', 23e-3 * 10e-3, ... % in m^2
+  'FlapAreaUncertainty', .01, ... % in %
   'DragCoefficients', [1, .1], ... % TODO
-  'LiftCoefficient', 2 * pi ... % TODO
+  'DragCoefficientsUncertainties', [.1, .1], ... % in %
+  'LiftCoefficient', 2 * pi, ... % TODO
+  'LiftCoefficientUncertainty', .1 ...% in %
 );
 
 
diff --git a/uav_performance.m b/uav_performance_hinf.m
index 4857a34..6ece761 100644
--- a/uav_performance.m
+++ b/uav_performance_hinf.m
@@ -13,7 +13,7 @@
 %   PERF    Struct performance transfer functions
 
 
-function [perf] = uav_performance(params, plot)
+function [perf] = uav_performance(params, do_plots)
 
 % Laplace variable
 s = tf('s');
@@ -64,7 +64,7 @@ perf = struct(...
   'Velocity', W_PPdot, ...
   'Angles', W_PTheta);
 
-if plot
+if do_plots
   % Bode plots of performance requirements
   figure; hold on;
 
diff --git a/uav_performance_musyn.m b/uav_performance_musyn.m
new file mode 100644
index 0000000..6ece761
--- /dev/null
+++ b/uav_performance_musyn.m
@@ -0,0 +1,123 @@
+% Generate transfer functions for loop shaping performance requirements
+% from parameters specified in uav_params.m
+%
+% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
+% This work is distributed under a permissive license, see LICENSE.txt
+%
+% Arguments:
+%   PARAMS  Struct of design parameters and constants generated by uav_params
+%   PLOT    When set to 'true' it plots the inverse magnitude of the
+%           performance transfer function
+%
+% Return value:
+%   PERF    Struct performance transfer functions
+
+
+function [perf] = uav_performance(params, do_plots)
+
+% Laplace variable
+s = tf('s');
+
+alpha_max = params.actuators.ServoAbsMaxAngle;
+alpha_max_omega = params.actuators.ServoNominalAngularVelocity;
+
+T_xy = params.performance.HorizontalSettleTime;
+T_z = params.performance.VerticalSettleTime;
+
+omega_nxy = 5 / T_xy;
+omega_nz = 10 / T_z;
+
+% W_Palpha = 1 / (s^2 + 2 * alpha_max_omega * s + alpha_max_omega^2);
+% W_Palpha = (1 - W_Palpha / dcgain(W_Palpha)) * .8;
+% W_Pomega = (1 - 1 / (T_z / 5 * s + 1)) * .1;
+
+W_Palpha = make_weight(alpha_max_omega, 15, 1.1, 3);
+W_Pomega = make_weight(omega_nz, 50, 10);
+
+% zeta = 1; % Almost critically damped
+% W_Pxy = 1 / (s^2  + 2 * zeta * omega_nxy * s + omega_nxy^2);
+% W_Pxy = 1 * W_Pxy / dcgain(W_Pxy);
+% W_Pz  = 1 / (s^2 + 2 * zeta * omega_nz * s + omega_nz^2);
+% W_Pz  = 1 * W_Pz / dcgain(W_Pz);
+
+W_Pxy = make_weight(omega_nxy, 2, 5);
+W_Pz = make_weight(omega_nz, 1, 10);
+
+% Set a speed limit
+W_Pxydot = .2 * tf(1, 1);
+W_Pzdot = .2 * tf(1, 1);
+ 
+W_Pphitheta = .01 * tf(1, [.1, 1]);
+W_Ppsi = .01 * tf(1, 1); % don't care
+
+W_PTheta = tf(1, [.1, 1]) * eye(3);
+
+% Construct performance vector by combining xy and z
+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, ...
+  'Position', W_PP, ...
+  'Velocity', W_PPdot, ...
+  'Angles', W_PTheta);
+
+if do_plots
+  % Bode plots of performance requirements
+  figure; hold on;
+
+  bodemag(W_Palpha);
+  bodemag(W_Pomega);
+  bodemag(W_Pxy);
+  bodemag(W_Pz);
+  bodemag(W_Pxydot);
+  bodemag(W_Pzdot);
+  bodemag(W_Pphitheta);
+  bodemag(W_Ppsi);
+
+  grid on;
+  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,\phi\theta}$', '$W_{P,\psi}$', ...
+    'interpreter', 'latex', 'fontSize', 8);
+  title('Performance Requirements');
+
+  % 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);
+  grid on;
+  legend('$W_{P,xy}$', '$W_{P,z}$', ...
+    '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
+    '$W_{P,\alpha}$', '$W_{P,\omega}$', ...
+    '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_musyn.m
new file mode 100644
index 0000000..3af2a76
--- /dev/null
+++ b/uav_sim_step_musyn.m
@@ -0,0 +1,282 @@
+% 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_musyn(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));
+
+% 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 e1ddf76..6f28d8d 100644
--- a/uav_uncertainty.m
+++ b/uav_uncertainty.m
@@ -13,16 +13,46 @@
 %   UNCERT  Struct of uncertainty transfer functions
 
 
-function [uncert] = uav_performance(params, plot)
+function [uncert] = uav_performance(params, do_plots)
 
-W_malpha = tf(1,1);
-W_momega = tf(1,1);
-W_mState = tf(1,1);
+s = tf('s');
+
+% relative errors
+eps_T = params.aerodynamics.ThrustOmegaPropUncertainty;
+eps_r = params.mechanical.GyroscopicInertiaZUncertainty;
+eps_S = params.aerodynamics.FlapAreaUncertainty;
+eps_l = params.aerodynamics.LiftCoefficientUncertainty;
+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);
+
+% band pass parameters for W_malpha
+wh = 20; % high freq
+wl = .1; % low freq
+
+W_malpha = eps_alpha * (s / wl) / ((s / wh + 1) * (s / wl + 1));
+W_momega = eps_omega * 10 / (s + 10);
+W_mState = .01 * 10 / (s + 10);
 
 uncert = struct(...
   'FlapAngle', W_malpha * eye(4), ...
   'Thrust', W_momega, ...
   'StateLinApprox', W_mState * eye(12));
 
+if do_plots
+  % Bode plots of performance requirements
+  figure; hold on;
+  bodemag(W_malpha);
+  bodemag(W_momega);
+  bodemag(W_mState);
+
+  grid on;
+  legend('$W_{m,\alpha}$', '$W_{m,\omega}$', '$W_{m,\mathbf{P}}$', ...
+    'interpreter', 'latex')
+  title('Stability (Uncertainty) Requirement')
+end
+
 end
 % vim: ts=2 sw=2 et: