% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich
%
% Controller design for a ducted fan VTOL micro-UAV.

%  ------------------------------------------------------------------------
% Clear environment and generate parameters

clear; clc; close all; s = tf('s');
params = uav_params();

% ------------------------------------------------------------------------
% Define performance requirements

% Mechanically, flaps are constrained to a max of 20~25 degrees,
% and they have a maximal angular speed
alpha_max = params.actuators.ServoAbsMaxAngle;
alpha_dot_max = params.actuators.ServoNominalAngularVelocity;

W_Palpha = (s + 100 * alpha_dot_max) / (s + alpha_dot_max);
W_Palpha = alpha_max * W_Palpha / dcgain(W_Palpha); % adjust gain

% Mechanically we have a maximal angular velocity for the propeller in the
% thruster, also there are a lot of unmodelled dynamics in the thruster
omega_max = params.actuators.TurbineMaxSpeed;
W_Pomega = (s + 50 * omega_max) / (s + omega_max);

% We want a nice and smooth movements
v_xy_max = params.performance.MaxHorizontalSpeed;
v_z_max = params.performance.MaxVerticalSpeed;

W_Pxy = 1 / (s + 1 / v_xy_max);
W_Pz = 1 / (s + 1 / v_z_max);

% Bode plots of performance requirements
figure; hold on;

bodemag(1/W_Palpha);
bodemag(1/W_Pomega);
bodemag(1/W_Pxy);
bodemag(1/W_Pz);

grid on;
legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ...
  '$W_{P,xy}$', '$W_{P,z}$', ...
  'interpreter', 'latex', 'fontSize', 8);
title('Performance requirements');

% Step response of position requirements
figure; hold on;
step(W_Pxy); step(W_Pz);
grid on;
legend('$W_{P,xy}$', '$W_{P,z}$', 'interpreter', 'latex', 'fontSize', 8);
title('Step responses of position performance requirements');

% Construct performance for position vector by combining xy and z
W_PP = blkdiag(W_Pxy * eye(2), W_Pz);

perf = struct(...
  'FlapAngle', W_Palpha * eye(4), ...
  'Thrust', W_Pomega, ...
  'PositionAccuracy', W_PP);

%  ------------------------------------------------------------------------
% Define stability requirements

W_malpha = tf(1,1);
W_momega = tf(1,1);
W_mTheta = tf(1,1);
W_mOmega = tf(1,1);

figure; hold on;

bodemag(W_malpha);
bodemag(W_momega);
bodemag(W_mTheta);
bodemag(W_mOmega);

grid on;
legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ...
  '$W_{m,\Theta}$', '$W_{m,\Omega}$', ...
  'interpreter', 'latex', 'fontSize', 8);
title('Uncertainties')

uncert = struct(...
  'FlapAngle', W_malpha * eye(4), ...
  'Thrust', W_momega, ...
  'EulerAnglesApprox', W_mTheta * eye(3), ...
  'AngularRateApprox', W_mOmega * eye(3));
  
% ------------------------------------------------------------------------
% Create UAV model

model = uav_model(params, perf, uncert);

% ------------------------------------------------------------------------
% Perform H-infinity design

Gnom = model.linear.StateSpace;
G = model.uncertain.StateSpace;

%  ------------------------------------------------------------------------
% Verify performance satisfaction

%  ------------------------------------------------------------------------
% Perform mu-Analysis & DK iteration

% vim: ts=2 sw=2 et: