Update DK iteration and model structure

1 files changed, 26 insertions, 19 deletions

diff --git a/uav_performance_hinf.m b/uav_performance_hinf.m
index ba2fd3b..4a6de8a 100644
--- a/uav_performance_hinf.m
+++ b/uav_performance_hinf.m
@@ -19,41 +19,43 @@ function [perf] = uav_performance_hinf(params, do_plots)
 s = tf('s');
 
 % Bandwitdhs
-bw_alpha = .7 * params.actuators.ServoNominalAngularVelocity;
-bw_omega = 8;
+T_alpha = params.actuators.ServoSecondsTo60Deg;
+T_omega = 0.1;
 
-bw_xy = .1;
-bw_z = .4;
+T_xy = 5;
+T_z = 10;
 
-bw_xydot = .5;
-bw_zdot = .1;
+% Inverse performance functions
+W_Palpha = make_weight(1/T_alpha, 10, 2);
+W_Pomega = make_weight(1/T_omega, 10, 2);
 
-bw_phitheta = bw_xy;
-bw_psi = .08;
+W_Rxy = 1 / ((s * T_xy)^2 + 2 * T_xy * .8 * s + 1);
+W_Rz = 1 / (s * T_z + 1);
 
-% Inverse performance functions
-W_Palpha = .25 / (s / bw_alpha + 1); 
-W_Pomega = .1 / (s / bw_omega + 1);
+W_Pxy = tf(5);
+W_Pz = tf(1);
 
-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_Pxydot = tf(.01);
+W_Pzdot = tf(.01);
 
-W_Pxydot = tf(.1); % .2 / (s / bw_xydot + 1);
-W_Pzdot = tf(.1); % .5 / (s / bw_zdot + 1);
- 
-W_Pphitheta = .01 / (s / bw_phitheta + 1);
-W_Ppsi = tf(.1); % .1 / (s / bw_psi + 1);
+W_Pphitheta = .001 / (s * T_xy + 1);
+W_Ppsi = tf(.1);
 
 % Construct performance vector by combining xy and z
+W_ref = 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);
 W_PTheta = blkdiag(W_Pphitheta * eye(2), W_Ppsi);
+
 perf = struct(...
   'FlapAngle', W_Palpha * eye(4), ...
   'Thrust', W_Pomega, ...
+  'ReferenceFilter', W_ref, ...
   'Position', W_PP, ...
   'Velocity', W_PPdot, ...
-  'Angles', W_PTheta);
+  'Angles', W_PTheta ...
+);
 
 if do_plots
   % Bode plots of performance requirements
@@ -61,6 +63,8 @@ if do_plots
 
   bodemag(W_Palpha);
   bodemag(W_Pomega);
+  bodemag(W_Rxy);
+  bodemag(W_Rz);
   bodemag(W_Pxy);
   bodemag(W_Pz);
   bodemag(W_Pxydot);
@@ -70,6 +74,7 @@ if do_plots
 
   grid on;
   legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ...
+    '$W_{R,xy}$', '$W_{R,z}$', ...
     '$W_{P,xy}$', '$W_{P,z}$', ...
     '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
     '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ...
@@ -80,11 +85,13 @@ if do_plots
   % Step response of position requirements
   figure; hold on;
   step(W_Pxy); step(W_Pz);
+  step(W_Rxy); step(W_Rz);
   step(W_Pxydot); step(W_Pzdot);
   step(W_Palpha);
   step(W_Pomega);
   grid on;
   legend('$W_{P,xy}$', '$W_{P,z}$', ...
+    '$W_{R,xy}$', '$W_{R,z}$', ...
     '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
     '$W_{P,\alpha}$', '$W_{P,\omega}$', ...
     'interpreter', 'latex', 'fontSize', 8);