% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich

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

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

%% ------------------------------------------------------------------------
% Create nominal plant

% Generate nominal model
model = uav_model(params);

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

alpha_max = params.actuators.ServoAbsMaxAngleDeg * pi / 180;
W_Palpha = tf(alpha_max,1);

W_Pomega = tf(1,1);
W_Pxy = tf(1,1);
W_Pz = tf(1,1);

figure(1); hold on;
bodemag(W_Palpha);
bodemag(W_Pomega);
bodemag(W_Pxy);
bodemag(W_Pz);

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


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

% Mechanically, flaps are constrained to a max of 20~25 degrees,
% we assume a precision of
% also, flaps tend to get less precise at higher frequencies
alpha_max = 2 * pi * 20;
W_malpha = alpha_max / (s + 2) * eye(4);

W_momega = tf(1,1);
W_mTheta = tf(1,1) * eye(3);
W_mOmega = tf(1,1) * eye(3);

uncert = struct(...
  'FlapAngle', W_malpha, ...
  'Thrust', W_momega, ...
  'EulerAnglesApprox', W_mTheta, ...
  'AngularRateApprox', W_mOmega);

%% ------------------------------------------------------------------------
% Create uncertain system

% Load simulink model with uncertainties
usys = linmod('uav_model_uncertain');
G = ss(usys.a, usys.b, usys.c, usys.d);

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

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

% vim: ts=2 sw=2 et: