Add more checks, update DK iteration

1 files changed, 19 insertions, 28 deletions

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, ...