Add more checks, update DK iteration

9 files changed, 361 insertions, 142 deletions

diff --git a/uav.m b/uav.m
index 059c944..3a0da71 100644
--- a/uav.m
+++ b/uav.m
@@ -9,7 +9,7 @@
 clear; clc; close all; s = tf('s');
 
 do_plots = true; % runs faster without
-do_hinf = true; % midterm
+do_hinf = false; % midterm
 do_musyn = true; % endterm
 
 fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n')
@@ -37,7 +37,8 @@ params = uav_params();
 % Define performance requirements
 
 fprintf('Generating performance requirements...\n')
-perf = uav_performance_musyn(params, do_plots);
+perf_hinf = uav_performance_hinf(params, do_plots);
+perf_musyn = uav_performance_musyn(params, do_plots);
 
 %%  ------------------------------------------------------------------------
 % Define stability requirements
@@ -49,7 +50,7 @@ uncert = uav_uncertainty(params, do_plots);
 % Create UAV model
 
 fprintf('Generating system model...\n');
-model = uav_model(params, perf, uncert);
+model = uav_model(params, perf_musyn, uncert);
 
 %% ------------------------------------------------------------------------
 % Perform H-infinity design
@@ -71,7 +72,21 @@ if do_hinf
     '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);
+
+  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 ...
+  );
+
+  ctrl.hinf = struct( ...
+    'Name', '$\mathcal{H}_{\infty}$', ...
+    'K', K_inf, ...
+    'index', index ...
+  );
 
   if gamma >= 1
     error('Failed to syntesize controller (closed loop is unstable).')
@@ -80,11 +95,39 @@ if do_hinf
 %%  ------------------------------------------------------------------------
 % Measure Performance of H-infinity design
 
-  fprintf(' - Simulating closed loop...\n');
+  if do_plots
+    fprintf(' - Plotting resulting controller...\n');
+
+    % Plot transfer functions
+    figure; hold on;
+    bode(ctrl.hinf.K(index.Ialpha(1), index.Ix));
+    bode(ctrl.hinf.K(index.Ialpha(2), index.Ix));
+
+    bode(ctrl.hinf.K(index.Ialpha(1), index.Iy));
+    bode(ctrl.hinf.K(index.Ialpha(2), index.Iy));
+
+    bode(ctrl.hinf.K(index.Iomega, index.Ix));
+    bode(ctrl.hinf.K(index.Iomega, index.Iy));
+    bode(ctrl.hinf.K(index.Iomega, index.Iz));
+
+    title(sprintf('\\bfseries %s Controller', ctrl.hinf.Name), ...
+      'interpreter', 'latex');
+    legend('$x \rightarrow \alpha_1$', ...
+      '$x \rightarrow \alpha_2$', ...
+      '$y \rightarrow \alpha_1$', ...
+      '$y \rightarrow \alpha_2$', ...
+      '$x \rightarrow \omega$', ...
+      '$y \rightarrow \omega$', ...
+      '$z \rightarrow \omega$', ...
+      'interpreter', 'latex');
+    grid on;
+  end
+
+  fprintf('Simulating closed loop...\n');
 
-  T = 60;
-  nsamples = 500;
-  do_noise = true;
+  T = 40;
+  nsamples = 800;
+  do_noise = false;
   simout = uav_sim_step(params, model, ctrl.hinf, nsamples, T, do_plots, do_noise);
 
   fprintf(' - Writing simulation results...\n');
@@ -116,10 +159,10 @@ if do_musyn
   nmeas = model.uncertain.Ny;
   nctrl = model.uncertain.Nu;
   hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
-    'AutoScale', 'on', 'RelTol', 1e-3);
+    'AutoScale', 'on', 'RelTol', 1e-2);
 
   % Frequency raster resolution to fit D scales
-  nsamples = 600;
+  nsamples = 300;
   omega_max = 3;
   omega_min = -3;
 
@@ -132,12 +175,28 @@ if do_musyn
 
   % Maximum number of D-K iterations
   niters = 5;
-  fprintf(' - Will do at most %d iterations.\n', niters);
+  fprintf(' - Will do (max) %d iterations.\n', niters);
 
   % 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
+  d_scales_max_degree = 10;
+  d_scales_max_err_p = .2; % in percentage
+  d_scales_improvement_p = .1; % in percentage, avoid diminishing returns
+
+  % hand tuned degrees, inf means will pick automatically best fit
+  % according to parameters given above
+  d_scales_degrees = {
+      0,   0, inf, inf, inf; % alpha
+      1, inf, inf, inf, inf; % omega
+      2, inf, inf, inf, inf; % state
+      0,   0, inf, inf, inf;
+  };
+
+  if size(d_scales_degrees, 2) < niters
+    error('');
+  end
+
+  scaled_plant_reduce_ord = 100;
+  scaled_plant_reduce_maxerr = 1e-3;
 
   % for plotting later
   mu_plot_legend = {};
@@ -148,9 +207,31 @@ if do_musyn
     fprintf(' * Running D-K iteration %d / %d...\n', it, niters);
     itstart = tic();
 
-    % Find controller using H-infinity
+    % Scale plant and reduce order, fitting the D-scales adds a lot of near
+    % zero modes that cause the plant to become very numerically ill
+    % conditioned. Since the D-scales are an approximation anyways (i.e.
+    % there is not mathematical proof for the fitting step), we limit the
+    % order of the scaled system to prevent poor scaling issues.
     P_scaled = minreal(D_left * P * inv(D_right), [], false);
     [P_scaled, ~] = prescale(P_scaled, omega_range);
+    n = size(P_scaled.A, 1);
+    
+    if n > scaled_plant_reduce_ord
+      R = reducespec(P_scaled, 'balanced'); % or ncf
+      R.Options.FreqIntervals = [omega_range{1}, omega_range{2}];
+      R.Options.Goal = 'absolute'; % better hinf norm
+      if do_plots
+        figure(102);
+        view(R);
+        drawnow;
+      end
+      P_scaled = getrom(R, MaxError=scaled_plant_reduce_maxerr);
+
+      fprintf('   Scaled plant was reduced from order %d to %d\n', ...
+        n, size(P_scaled.A, 1))
+    end
+
+    % Find controller using H-infinity
     [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt);
 
     fprintf('   H-infinity synthesis gamma: %g\n', gamma);
@@ -241,36 +322,55 @@ if do_musyn
     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 = 0:d_scales_max_degree
-        % 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
+      
+      % tuned by hand?
+      if d_scales_degrees{j, it} < inf
+        % D_fit = fitmagfrd(D_frd{j}, d_scales_degrees{j, it});
+        D_fit = fitfrd(genphase(D_frd{j}), d_scales_degrees{j, it});
+
         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 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;
+        D_fitted{j} = D_fit;
+
+        fprintf(' tuned degree %d, error %g (%g %%)\n', ...
+                d_scales_degrees{j, it}, fit_err, ...
+                100 * fit_err / D_max_sv{j});
+
+      else
+        % find best degree
+        best_fit_deg = inf;
+        best_fit_err = inf;
+
+        for deg = 0:d_scales_max_degree
+          % 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
+            % at the first iteration). This is a heuristic to prevent 
+            % 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_p)
+            break;
+          end
+          fprintf('.');
         end
-
-        if (fit_err / D_max_sv{j} < d_scales_max_err_p)
-          break;
-        end
-        fprintf('.');
+        fprintf(' degree %d, error %g (%g %%)\n', ...
+            best_fit_deg, best_fit_err, 100 * best_fit_err / D_max_sv{j});
       end
-      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
@@ -340,7 +440,7 @@ if do_musyn
   fprintf(' - D-K iteration took %.1f seconds\n', dkend);
 
   if mu_rp > 1 && mu_rs > 1
-    error(' - Failed to synthesize robust controller that meets the desired performance.\n');
+    error(' - Failed to synthesize robust controller that meets the desired performance.');
   end
 
   %% Fit worst-case perturbation
@@ -364,11 +464,39 @@ if do_musyn
 %%  ------------------------------------------------------------------------
 % Measure Performance of mu synthesis design
 
+  if do_plots
+    fprintf(' - Plotting resulting controller...\n');
+
+    % Plot transfer functions
+    figure; hold on;
+    bode(ctrl.musyn.K(index.Ialpha(1), index.Ix));
+    bode(ctrl.musyn.K(index.Ialpha(2), index.Ix));
+
+    bode(ctrl.musyn.K(index.Ialpha(1), index.Iy));
+    bode(ctrl.musyn.K(index.Ialpha(2), index.Iy));
+
+    bode(ctrl.musyn.K(index.Iomega, index.Ix));
+    bode(ctrl.musyn.K(index.Iomega, index.Iy));
+    bode(ctrl.musyn.K(index.Iomega, index.Iz));
+
+    title(sprintf('\\bfseries %s Controller', ctrl.musyn.Name), ...
+      'interpreter', 'latex');
+    legend('$x \rightarrow \alpha_1$', ...
+      '$x \rightarrow \alpha_2$', ...
+      '$y \rightarrow \alpha_1$', ...
+      '$y \rightarrow \alpha_2$', ...
+      '$x \rightarrow \omega$', ...
+      '$y \rightarrow \omega$', ...
+      '$z \rightarrow \omega$', ...
+      'interpreter', 'latex');
+    grid on;
+  end
+
   fprintf('Simulating nominal closed loop...\n');
 
-  T = 60;
+  T = 40;
   nsamples = 5000;
-  do_noise = true;
+  do_noise = false;
 
   simout = uav_sim_step(params, model, ctrl.musyn, nsamples, T, do_plots, do_noise);
 
diff --git a/uav_model.m b/uav_model.m
index 1f8f751..fda2745 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -9,7 +9,8 @@
 %    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. 
+%    an operating point specified in the params struct argument. And adding
+%    models for the actuators.
 %
 %  * A uncertain linear model with reference trcking built atop of the linear
 %    model using SIMULINK. The uncertain model contains the performance and
@@ -219,9 +220,22 @@ B = double(vpa(B));
 C = eye(size(A));
 D = zeros(12, 5);
 
-% Number of states, inputs and outputs
-[nx, nu] = size(B);
-[ny, ~] = size(C);
+% ------------------------------------------------------------------------
+% Model actuators
+
+% TODO: better model?
+w = params.actuators.ServoNominalAngularVelocity;
+zeta = 1;
+G_servo = tf(w^2, [1, 2 * zeta * w, w^2]);
+
+w = 60;
+zeta = 1;
+G_prop = tf(w^2, [1, 2 * zeta * w, w^2]);
+
+model.actuators = struct( ...
+  'FlapServo', G_servo, ...
+  'ThrustPropeller', G_prop, ...
+  'StateSpace', blkdiag(eye(4) * G_servo, G_prop));
 
 % ------------------------------------------------------------------------
 % Scale inputs and outputs of linearized model
@@ -247,21 +261,31 @@ D = D * inv(S_actuators);
 % Create state space object
 T = params.measurements.SensorFusionDelay;
 n = params.linearization.PadeApproxOrder;
-sys = minreal(pade(ss(A, B, C, D, 'OutputDelay', T), n), [], false); % slient
+sys = pade(ss(A, B, C, D, 'OutputDelay', T), n);
+
+% Add actuators
+sys = sys * model.actuators.StateSpace;
+
+% Remove unnecessary states
+sys = minreal(sys, [], false); % slient
+
+% Number of states, inputs and outputs
+[nx, nu] = size(sys.B);
+[ny, ~] = size(sys.C);
 
 % Save linearized dynamics (numerical)
 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 objec
   'InputScalingMatrix', S_actuators, 'OutputScalingMatrix', S_state ...
 );
 
 % ------------------------------------------------------------------------
 % Check properties of linearized model
 
-eigvals = eig(A);
+eigvals = eig(sys.A);
 
 % Check system controllability / stabilizability
 Wc = ctrb(sys);
@@ -274,7 +298,7 @@ if rank(Wc) < nx
   for i = 1:nx
     if real(eigvals(i)) >= 0
       % PBH test
-      W = [(A - eigvals(i) * eye(size(A))), B];
+      W = [(sys.A - eigvals(i) * eye(nx)), sys.B];
       if rank(W) < nx
         % fprintf('  State %d is not stabilizable\n', i);
         unstabilizable = unstabilizable + 1;
@@ -299,7 +323,7 @@ if rank(Wo) < nx
   for i = 1:nx
     if real(eigvals(i)) >= 0
       % PBH test
-      W = [C; (A - eigvals(i) * eye(size(A)))];
+      W = [sys.C; (sys.A - eigvals(i) * eye(nx))];
       if rank(W) < nx
         undetectable = undetectable + 1;
       end
@@ -313,31 +337,37 @@ if rank(Wo) < nx
   end
 end
 
-% ------------------------------------------------------------------------
-% Compute absolute value of error caused by linearization around set point
+stable = 0;
+unstable = 0;
+for i = 1:nx
+  if real(eigvals(i)) < 0
+    stable = stable + 1;
+  else
+    unstable = unstable + 1;
+  end
+end
 
+fprintf(' - Linearized system has %d states:\n', nx);
+fprintf('   %d controllable modes, %d observable modes.\n', rank(Wc), rank(Wo));
+fprintf('   %d stable modes, %d unstable modes.\n', stable, unstable);
 
-% ------------------------------------------------------------------------
-% Model actuators
-
-% TODO: better model, this was tuned "by looking"
-w = 2 * params.actuators.ServoNominalAngularVelocity;
-zeta = .7;
-G_servo = tf(w^2, [1, 2 * zeta * w, w^2]);
+if rank(Wc) < 12
+  error('Uncertain model has less than 12 controllable modes');
+end
 
-w = 6e3;
-zeta = 1;
-G_prop = tf(w^2, [1, 2 * zeta * w, w^2]);
+% ------------------------------------------------------------------------
+% Compute absolute value of error caused by linearization around set point
 
-model.actuators = struct('FlapServo', G_servo, 'ThrustPropeller', G_prop);
+% TODO
 
 % ------------------------------------------------------------------------
 % Add uncertainties using SIMULINK model
 
 % Load simulink model with uncertainties and pass in parameters
 h = load_system('uav_model_uncertain');
-hws = get_param('uav_model_uncertain', 'modelworkspace');
+set_param('uav_model_uncertain', SimulationMode='Normal');
 
+hws = get_param('uav_model_uncertain', 'modelworkspace');
 hws.assignin('params', params);
 hws.assignin('model', model);
 hws.assignin('perf', perf);
@@ -375,8 +405,8 @@ S_uncert_out = blkdiag(...
   S_state(1:9, 1:9));
 
 % Save uncertain model
-model.uncertain = struct(...
-  'Simulink', ulmod, ...
+model.uncertain = struct(... 
+  ... % 'Simulink', ulmod, ...
   'BlockStructure', blk_stab, ...
   'BlockStructurePerf', blk_perf, ...
   'StateSpace', usys ...
@@ -425,6 +455,73 @@ model.uncertain.Ny = max(size(idx.OutputNominal));
 % ------------------------------------------------------------------------
 % Check properties of uncertain model
 
+eigvals = eig(usys.A);
+nx = size(usys.A, 1);
+
+% Check system controllability / stabilizability
+Wc = ctrb(usys);
+if rank(Wc) < nx
+  fprintf(' - Uncertain system has %d uncontrollable states!\n', ...
+    (nx - rank(Wc)));
+
+  % Is the system at least stabilizable?
+  unstabilizable = 0;
+  for i = 1:nx
+    if real(eigvals(i)) >= 0
+      % PBH test
+      W = [(usys.A - eigvals(i) * eye(nx)), usys.B];
+      if rank(W) < nx
+        % fprintf('  State %d is not stabilizable\n', i);
+        unstabilizable = unstabilizable + 1;
+      end
+    end
+  end
+  if unstabilizable > 0
+    fprintf(' - Uncertain system has %d unstabilizable modes!\n', ...
+      unstabilizable);
+  else
+    fprintf('   However, it is stabilizable.\n');
+  end
+end
+
+% Check system observability / detectability
+Wo = obsv(usys);
+if rank(Wo) < nx
+  fprintf(' - Uncertain system has %d unobservable states!\n', ...
+    (nx - rank(Wo)));
+  % is the system at least detectable?
+  undetectable = 0;
+  for i = 1:nx
+    if real(eigvals(i)) >= 0
+      % PBH test
+      W = [usys.C; (usys.A - eigvals(i) * eye(nx))];
+      if rank(W) < nx
+        undetectable = undetectable + 1;
+      end
+    end
+  end
+  if undetectable > 0
+    fprintf(' - Uncertain system has %d undetectable modes!\n', ...
+      undetectable);
+  else
+    fprintf('   However, it is detectable.\n')
+  end
+end
+
+stable = 0;
+unstable = 0;
+for i = 1:nx
+  if real(eigvals(i)) < 0
+    stable = stable + 1;
+  else
+    unstable = unstable + 1;
+  end
+end
+
+fprintf(' - Uncertain system has %d states:\n', nx);
+fprintf('   %d controllable modes, %d observable modes.\n', rank(Wc), rank(Wo));
+fprintf('   %d stable modes, %d unstable modes.\n', stable, unstable);
+
 % % Check that (A, B_u, C_y) is stabilizable and detectable
 % A = model.uncertain.StateSpace(...
 %   model.uncertain.index.OutputUncertain, ...
diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx
index d432718..fd94980 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 7bcf2d9..36be668 100644
--- a/uav_model_uncertain_clp.slx
+++ b/uav_model_uncertain_clp.slx
diff --git a/uav_params.m b/uav_params.m
index e87e4fd..fdd519a 100644
--- a/uav_params.m
+++ b/uav_params.m
@@ -40,7 +40,7 @@ r_prop = 85e-3; % radius of propeller
 J_prop = .5 * m_prop * r_prop^2;
 
 params.mechanical = struct(...
-  'Mass', 3.0, ...
+  'Mass', 3.5, ...
   'DuctRadius', 46e-3, ...
   'DuctHeight', 171e-3, ...
   'FlapZDistance', 98e-3, ... % flap distance along z from center of mass
@@ -65,7 +65,7 @@ params.actuators = struct(...
 
 % IMU runs in NDOF mode
 params.measurements = struct(...
-  'SensorFusionDelay', 50e-3, ... % in s
+  'SensorFusionDelay', 30e-3, ... % in s
   'LIDARAccuracy', 10e-3, ... % in m
   'LIDARMaxDistance', 40, ... % inm
   'LIDARBandwidth', 100, ... % in Hz for z position
@@ -91,13 +91,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 %
+  'ThrustOmegaPropUncertainty', .01, ... % in %
   'FlapArea', 23e-3 * 10e-3, ... % in m^2
-  'FlapAreaUncertainty', .01, ... % in %
-  'DragCoefficients', [1, .1], ... % TODO
+  'FlapAreaUncertainty', .02, ... % in %
+  'DragCoefficients', [0, 0], ... % TODO
   'DragCoefficientsUncertainties', [.1, .1], ... % in %
   'LiftCoefficient', 2 * pi, ... % TODO
-  'LiftCoefficientUncertainty', .1 ...% in %
+  'LiftCoefficientUncertainty', .05 ...% in %
 );
 
 
@@ -114,7 +114,7 @@ k = params.aerodynamics.ThrustOmegaProp;
 omega_hover = sqrt(m * g / k);
 
 params.linearization = struct(...
-  'PadeApproxOrder', 2, ...
+  'PadeApproxOrder', 3, ...
   'Position', [0; 0; -2], ... % in inertial frame, z points down
   'Velocity', [0; 0; 0], ... % in inertial frame
   'Angles', [0; 0; pi / 4], ...   % in body frame
@@ -123,7 +123,7 @@ params.linearization = struct(...
 );
 
 % ------------------------------------------------------------------------
-% Performance requirements
+% Normalization (maximum values)
 
 params.normalization = struct(...
   'HPosition', 1, ... % m
diff --git a/uav_performance_hinf.m b/uav_performance_hinf.m
index 0a12b72..ba2fd3b 100644
--- a/uav_performance_hinf.m
+++ b/uav_performance_hinf.m
@@ -13,50 +13,41 @@
 %   PERF    Struct performance transfer functions
 
 
-function [perf] = uav_performance(params, do_plots)
+function [perf] = uav_performance_hinf(params, do_plots)
 
 % Laplace variable
 s = tf('s');
 
-alpha_max = params.actuators.ServoAbsMaxAngle;
-alpha_max_omega = params.actuators.ServoNominalAngularVelocity;
+% Bandwitdhs
+bw_alpha = .7 * params.actuators.ServoNominalAngularVelocity;
+bw_omega = 8;
 
-T_xy = params.performance.HorizontalSettleTime;
-T_z = params.performance.VerticalSettleTime;
+bw_xy = .1;
+bw_z = .4;
 
-omega_nxy = 5 / T_xy;
-omega_nz = 10 / T_z;
+bw_xydot = .5;
+bw_zdot = .1;
 
-% 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;
+bw_phitheta = bw_xy;
+bw_psi = .08;
 
-W_Palpha = make_weight(alpha_max_omega, 15, 1.1, 3);
-W_Pomega = make_weight(omega_nz, 50, 10);
+% Inverse performance functions
+W_Palpha = .25 / (s / bw_alpha + 1); 
+W_Pomega = .1 / (s / bw_omega + 1);
 
-% 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 = 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_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_Pxydot = tf(.1); % .2 / (s / bw_xydot + 1);
+W_Pzdot = tf(.1); % .5 / (s / bw_zdot + 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);
+W_Pphitheta = .01 / (s / bw_phitheta + 1);
+W_Ppsi = tf(.1); % .1 / (s / bw_psi + 1);
 
 % 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, ...
diff --git a/uav_performance_musyn.m b/uav_performance_musyn.m
index aba2160..57237fa 100644
--- a/uav_performance_musyn.m
+++ b/uav_performance_musyn.m
@@ -19,30 +19,30 @@ function [perf] = uav_performance_musyn(params, do_plots)
 s = tf('s');
 
 % Bandwitdhs
-bw_alpha = params.actuators.ServoNominalAngularVelocity;
-bw_omega = 10;
+bw_alpha = .7 * params.actuators.ServoNominalAngularVelocity;
+bw_omega = 8;
 
-bw_xy = .05;
-bw_z = .05;
+bw_xy = .1;
+bw_z = .4;
 
 bw_xydot = .5;
 bw_zdot = .1;
 
-bw_phitheta = .1;
+bw_phitheta = bw_xy;
 bw_psi = .08;
 
 % Inverse performance functions
-W_Palpha = .2 / (s / bw_alpha + 1); 
-W_Pomega = .2 / (s / bw_omega + 1);
+W_Palpha = .25 / (s / bw_alpha + 1); 
+W_Pomega = .1 / (s / bw_omega + 1);
 
-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_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 = .2 / (s / bw_xydot + 1);
-W_Pzdot = .5 / (s / bw_zdot + 1);
+W_Pxydot = tf(.1); % .2 / (s / bw_xydot + 1);
+W_Pzdot = tf(.1); % .5 / (s / bw_zdot + 1);
  
-W_Pphitheta = 1 / (s / bw_phitheta + 1);
-W_Ppsi = .1 / (s / bw_psi + 1);
+W_Pphitheta = .01 / (s / bw_phitheta + 1);
+W_Ppsi = tf(.1); % .1 / (s / bw_psi + 1);
 
 % Construct performance vector by combining xy and z
 W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
@@ -60,14 +60,14 @@ 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);
+  bodemag(1/W_Palpha);
+  bodemag(1/W_Pomega);
+  bodemag(1/W_Pxy);
+  bodemag(1/W_Pz);
+  bodemag(1/W_Pxydot);
+  bodemag(1/W_Pzdot);
+  bodemag(1/W_Pphitheta);
+  bodemag(1/W_Ppsi);
 
   grid on;
   legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ...
@@ -75,7 +75,8 @@ if do_plots
     '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
     '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ...
     'interpreter', 'latex', 'fontSize', 8);
-  title('Performance Requirements');
+  title('\bfseries Inverse Performance Requirements', ...
+    'interpreter', 'latex');
 
   % Step response of position requirements
   figure; hold on;
diff --git a/uav_sim_step.m b/uav_sim_step.m
index 973a928..82c05e9 100644
--- a/uav_sim_step.m
+++ b/uav_sim_step.m
@@ -22,6 +22,16 @@ P_nom_clp = minreal(usys_clp(...
     model.uncertain.index.OutputPlots], ...
   model.uncertain.index.InputExogenous), [], false);
 
+% Check that closed loop is actually stable
+eigvals = eig(P_nom_clp.A);
+nx = size(P_nom_clp.A, 1);
+
+for i = 1:nx
+  if real(eigvals(i)) >= 0
+    error('Closed loop is not stable!');
+  end
+end
+
 % Indices for exogenous inputs
 Iwwind = (1:3)';
 Iwalpha = (4:7)';
@@ -236,10 +246,10 @@ if do_plots
 
   hold on;
   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--');
+  % 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]);
+  % 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');
diff --git a/uav_uncertainty.m b/uav_uncertainty.m
index b664cee..53cd975 100644
--- a/uav_uncertainty.m
+++ b/uav_uncertainty.m
@@ -28,20 +28,12 @@ eps_d = params.aerodynamics.DragCoefficientsUncertainties(1);
 eps_omega = max(.5 * eps_T, eps_r);
 eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega);
 
-% TODO: set proper constraints
-% band pass parameters for W_malpha
-% wh = 20; % high freq
-% wl = .1; % low freq
+b = 10;
+G_highpass = make_weight(b, 1, .1);
 
-% 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);
-
-b = 1e3;
-
-W_malpha = make_weight(b, 1 / eps_alpha, 1e-2);
-W_momega = tf(0);
-W_mState = 1 - 1 / (s / 50 + 1);
+W_malpha = eps_alpha * G_highpass;
+W_momega = eps_omega * G_highpass;
+W_mState = .1 * G_highpass;
 
 uncert = struct(...
   'FlapAngle', W_malpha * eye(4), ...