Prepare structure for endterm

1 files changed, 123 insertions, 0 deletions

diff --git a/uav_performance.m b/uav_performance.m
new file mode 100644
index 0000000..4857a34
--- /dev/null
+++ b/uav_performance.m
@@ -0,0 +1,123 @@
+% Generate transfer functions for loop shaping performance requirements
+% from parameters specified in uav_params.m
+%
+% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
+% This work is distributed under a permissive license, see LICENSE.txt
+%
+% Arguments:
+%   PARAMS  Struct of design parameters and constants generated by uav_params
+%   PLOT    When set to 'true' it plots the inverse magnitude of the
+%           performance transfer function
+%
+% Return value:
+%   PERF    Struct performance transfer functions
+
+
+function [perf] = uav_performance(params, plot)
+
+% Laplace variable
+s = tf('s');
+
+alpha_max = params.actuators.ServoAbsMaxAngle;
+alpha_max_omega = params.actuators.ServoNominalAngularVelocity;
+
+T_xy = params.performance.HorizontalSettleTime;
+T_z = params.performance.VerticalSettleTime;
+
+omega_nxy = 5 / T_xy;
+omega_nz = 10 / T_z;
+
+% 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;
+
+W_Palpha = make_weight(alpha_max_omega, 15, 1.1, 3);
+W_Pomega = make_weight(omega_nz, 50, 10);
+
+% 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 = 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_Pphitheta = .01 * tf(1, [.1, 1]);
+W_Ppsi = .01 * tf(1, 1); % don't care
+
+W_PTheta = tf(1, [.1, 1]) * eye(3);
+
+% 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, ...
+  'Position', W_PP, ...
+  'Velocity', W_PPdot, ...
+  'Angles', W_PTheta);
+
+if plot
+  % 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);
+
+  grid on;
+  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,\phi\theta}$', '$W_{P,\psi}$', ...
+    'interpreter', 'latex', 'fontSize', 8);
+  title('Performance Requirements');
+
+  % 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);
+  grid on;
+  legend('$W_{P,xy}$', '$W_{P,z}$', ...
+    '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
+    '$W_{P,\alpha}$', '$W_{P,\omega}$', ...
+    '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: