Update DK-iteration and clean up

1 files changed, 32 insertions, 36 deletions

diff --git a/uav_performance_musyn.m b/uav_performance_musyn.m
index 180be8a..aba2160 100644
--- a/uav_performance_musyn.m
+++ b/uav_performance_musyn.m
@@ -18,22 +18,31 @@ function [perf] = uav_performance_musyn(params, do_plots)
 % Laplace variable
 s = tf('s');
 
-% TODO: set proper constraints
-G = 1e2 / (s + 1e2);
+% Bandwitdhs
+bw_alpha = params.actuators.ServoNominalAngularVelocity;
+bw_omega = 10;
 
-W_Palpha = G; 
-W_Pomega = G;
+bw_xy = .05;
+bw_z = .05;
 
-W_Pxy = G;
-W_Pz = G;
+bw_xydot = .5;
+bw_zdot = .1;
 
-W_Pxydot = tf(0);
-W_Pzdot = tf(0);
- 
-W_Pphitheta = tf(0);
-W_Ppsi = tf(0);
+bw_phitheta = .1;
+bw_psi = .08;
+
+% Inverse performance functions
+W_Palpha = .2 / (s / bw_alpha + 1); 
+W_Pomega = .2 / (s / bw_omega + 1);
 
-W_PTheta = tf(0);
+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_Pxydot = .2 / (s / bw_xydot + 1);
+W_Pzdot = .5 / (s / bw_zdot + 1);
+ 
+W_Pphitheta = 1 / (s / bw_phitheta + 1);
+W_Ppsi = .1 / (s / bw_psi + 1);
 
 % Construct performance vector by combining xy and z
 W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
@@ -70,37 +79,24 @@ if do_plots
 
   % 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);
+  step(W_Pxy);
+  step(W_Pz);
+  step(W_Pxydot);
+  step(W_Pzdot);
+  step(W_Pphitheta);
+  step(W_Ppsi);
+
   grid on;
-  legend('$W_{P,xy}$', '$W_{P,z}$', ...
+  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,\alpha}$', '$W_{P,\omega}$', ...
+    '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ...
     '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: