Update weights

7 files changed, 52 insertions, 41 deletions

diff --git a/uav.m b/uav.m
index 2ee58da..7676148 100644
--- a/uav.m
+++ b/uav.m
@@ -157,11 +157,11 @@ if do_musyn
   nmeas = model.uncertain.Ny;
   nctrl = model.uncertain.Nu;
   hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
-    'AutoScale', 'on', 'RelTol', 1e-2);
+    'AutoScale', 'on', 'RelTol', 1e-3);
 
   % Frequency raster resolution to fit D scales
-  omega_max = 2.5;
-  omega_min = -3.5;
+  omega_max = 2;
+  omega_min = -3;
   nsamples = (omega_max - omega_min) * 100;
 
   omega = logspace(omega_min, omega_max, nsamples);
@@ -172,10 +172,10 @@ if do_musyn
   D_right = tf(eye(model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu));
 
   % Maximum degree of D-scales and error
-  d_scales_max_degree = 4;
+  d_scales_max_degree = 5;
   d_scales_max_err = 15;
-  d_scales_max_err_p = .1; % in percentage
-  d_scales_improvement_p = .5; % in percentage, avoid diminishing returns
+  d_scales_max_err_p = .05; % in percentage
+  d_scales_improvement_p = .25; % in percentage, avoid diminishing returns
 
   % Limit order of scaled plant
   scaled_plant_reduce_ord = 100;
@@ -199,7 +199,7 @@ if do_musyn
   gamma_min = .8;
 
   % Maximum number of D-K iterations
-  niters = 4;
+  niters = 6;
   fprintf(' - Will do (max) %d iterations.\n', niters);
 
   %% Run D-K iteration
@@ -208,8 +208,8 @@ if do_musyn
   % according to parameters given above
   d_scales_degrees = {
       0,   0,   0,   0,   0, inf, inf; % alpha
-      1,   1,   3,   3,   0, inf, inf; % omega
-      0,   1,   0,   0,   0, inf, inf; % state
+    inf, inf, inf, inf, inf, inf, inf; % omega
+      0,   0,   0,   0,   0, inf, inf; % state
       0,   0,   0,   0,   0, inf, inf; % perf
   };
 
@@ -251,6 +251,7 @@ if do_musyn
     [P_scaled, ~] = prescale(P_scaled, omega_range);
     n = size(P_scaled.A, 1);
 
+    % disabled, seems to work without
     if false && n > scaled_plant_reduce_ord
       R = reducespec(P_scaled, 'balanced'); % or ncf
       R.Options.FreqIntervals = [omega_range{1}, omega_range{2}];
diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx
index 6f1b050..437c82d 100644
--- a/uav_model_uncertain.slx
+++ b/uav_model_uncertain.slx
diff --git a/uav_params.m b/uav_params.m
index 2ac187e..8963b02 100644
--- a/uav_params.m
+++ b/uav_params.m
@@ -92,9 +92,9 @@ params.aerodynamics = struct(...
   'FlapArea', 23e-3 * 10e-3, ... % in m^2
   'FlapAreaUncertainty', .1, ... % in %
   'DragCoefficients', [0, 0], ... % TODO
-  'DragCoefficientsUncertainties', [.1, .1], ... % in %
+  'DragCoefficientsUncertainties', [.0, .0], ... % in %
   'LiftCoefficient', 2 * pi, ... % TODO
-  'LiftCoefficientUncertainty', .1 ...% in %
+  'LiftCoefficientUncertainty', .05 ...% in %
 );
 
 
diff --git a/uav_performance_hinf.m b/uav_performance_hinf.m
index 4a6de8a..c8df42a 100644
--- a/uav_performance_hinf.m
+++ b/uav_performance_hinf.m
@@ -23,26 +23,29 @@ T_alpha = params.actuators.ServoSecondsTo60Deg;
 T_omega = 0.1;
 
 T_xy = 5;
-T_z = 10;
+T_z = 4;
 
 % Inverse performance functions
-W_Palpha = make_weight(1/T_alpha, 10, 2);
-W_Pomega = make_weight(1/T_omega, 10, 2);
+W_Palpha = tf(.8);
+W_Pomega = tf(.5);
+
+W_Ralpha = 1 / ((s * T_alpha)^2 + 2 * T_alpha * s + 1);
+W_Romega = 1 / ((s * T_omega)^2 + 2 * T_omega * 1.2 * s + 1);
 
 W_Rxy = 1 / ((s * T_xy)^2 + 2 * T_xy * .8 * s + 1);
-W_Rz = 1 / (s * T_z + 1);
+W_Rz = 1 / ((s * T_z)^2 + 2 * T_z * s + 1);
 
-W_Pxy = tf(5);
+W_Pxy = tf(12);
 W_Pz = tf(1);
 
 W_Pxydot = tf(.01);
 W_Pzdot = tf(.01);
 
-W_Pphitheta = .001 / (s * T_xy + 1);
-W_Ppsi = tf(.1);
+W_Pphitheta = tf(.005);
+W_Ppsi = tf(.3);
 
 % Construct performance vector by combining xy and z
-W_ref = blkdiag(W_Rxy * eye(2), W_Rz);
+W_Rref = blkdiag(W_Rxy * eye(2), W_Rz);
 
 W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
 W_PPdot = blkdiag(W_Pxydot * eye(2), W_Pzdot);
@@ -51,7 +54,9 @@ W_PTheta = blkdiag(W_Pphitheta * eye(2), W_Ppsi);
 perf = struct(...
   'FlapAngle', W_Palpha * eye(4), ...
   'Thrust', W_Pomega, ...
-  'ReferenceFilter', W_ref, ...
+  'ReferenceFilter', W_Rref, ...
+  'FlapAngleFilter', W_Ralpha * eye(4), ...
+  'ThrusterFilter', W_Romega, ...
   'Position', W_PP, ...
   'Velocity', W_PPdot, ...
   'Angles', W_PTheta ...
diff --git a/uav_performance_musyn.m b/uav_performance_musyn.m
index 5ad5fa5..a521f81 100644
--- a/uav_performance_musyn.m
+++ b/uav_performance_musyn.m
@@ -22,28 +22,30 @@ s = tf('s');
 T_alpha = params.actuators.ServoSecondsTo60Deg;
 T_omega = 0.1;
 
-T_xy = 8;
-T_z = 10;
+T_xy = 6;
+T_z = 9;
 
 % Inverse performance functions
+W_Palpha = tf(.2);
+W_Pomega = tf(.2);
 
-W_Palpha = make_weight(1/T_alpha, 10, 4);
-W_Pomega = make_weight(1/T_omega, 10, 2);
+W_Ralpha = 1 / ((s * T_alpha)^2 + 2 * T_alpha * s + 1);
+W_Romega = 1 / ((s * T_omega)^2 + 2 * T_omega * s + 1);
 
 W_Rxy = 1 / ((s * T_xy)^2 + 2 * T_xy * .8 * s + 1);
-W_Rz = 1 / (s * T_z + 1);
+W_Rz = 1 / ((s * T_z)^2 + 2 * T_z * s + 1);
 
-W_Pxy = tf(5);
+W_Pxy = tf(1);
 W_Pz = tf(1);
 
 W_Pxydot = tf(.01);
 W_Pzdot = tf(.1);
 
-W_Pphitheta = .001 / (s * T_xy + 1);
-W_Ppsi = tf(.1);
+W_Pphitheta = tf(.005);
+W_Ppsi = tf(.2);
 
 % Construct performance vector by combining xy and z
-W_ref = blkdiag(W_Rxy * eye(2), W_Rz);
+W_Rref = blkdiag(W_Rxy * eye(2), W_Rz);
 
 W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
 W_PPdot = blkdiag(W_Pxydot * eye(2), W_Pzdot);
@@ -52,7 +54,9 @@ W_PTheta = blkdiag(W_Pphitheta * eye(2), W_Ppsi);
 perf = struct(...
   'FlapAngle', W_Palpha * eye(4), ...
   'Thrust', W_Pomega, ...
-  'ReferenceFilter', W_ref, ...
+  'ReferenceFilter', W_Rref, ...
+  'FlapAngleFilter', W_Ralpha * eye(4), ...
+  'ThrusterFilter', W_Romega, ...
   'Position', W_PP, ...
   'Velocity', W_PPdot, ...
   'Angles', W_PTheta);
@@ -61,14 +65,14 @@ if do_plots
   % Bode plots of performance requirements
   figure; hold on;
 
-  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);
+  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}$', ...
diff --git a/uav_sim_step.m b/uav_sim_step.m
index 65fb36f..2130a3d 100644
--- a/uav_sim_step.m
+++ b/uav_sim_step.m
@@ -104,7 +104,7 @@ ref_step = ones(1, nsamp); % 1d step function
 
 in_step_x = [ noise; step_size_h * ref_step; zeros(2, nsamp) ];
 in_step_y = [ noise; zeros(1, nsamp); step_size_h * ref_step; zeros(1, nsamp) ];
-in_step_z = [ noise; zeros(2, nsamp); step_size_v * ref_step ];
+in_step_z = [ noise; zeros(2, nsamp); -step_size_v * ref_step ]; % z points down
 
 % Simulation time
 t = linspace(0, T, nsamp);
@@ -152,7 +152,7 @@ if do_plots
 
   % Plot limits
   alpha_max_deg = params.actuators.ServoAbsMaxAngle * to_deg;
-  euler_lim_deg = 1.5;
+  euler_lim_deg = .5;
   omega_max_rpm = (params.actuators.PropellerMaxAngularVelocity ...
     - params.linearization.Inputs(5)) * to_rpm;
   omega_min_rpm = -params.linearization.Inputs(5) * to_rpm;
@@ -298,6 +298,7 @@ if do_plots
   % Plot step response from vertical reference to vertical position
   subplot(2, 2, 3); hold on;
   plot(t, out_step_z(:, idx.PlotPosition(3)));
+  plot(t, out_step_z(:, idx.PlotReference(3)), '--');
   grid on;
   title('Vertical Position', 'Interpreter', 'latex');
   ylabel('Distance (meters)', 'Interpreter', 'latex');
diff --git a/uav_uncertainty.m b/uav_uncertainty.m
index 03b7b3b..045321d 100644
--- a/uav_uncertainty.m
+++ b/uav_uncertainty.m
@@ -26,7 +26,7 @@ eps_d = params.aerodynamics.DragCoefficientsUncertainties(1);
 % eps_0 = params.aerodynamics.DragCoefficients(2);
 
 eps_omega = max(.5 * eps_T, eps_r);
-eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega)
+eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega);
 
 b = 12;
 T = 1;