Move uncertain model design into uav_model and add more design parameter

4 files changed, 154 insertions, 101 deletions

diff --git a/midterm.m b/midterm.m
deleted file mode 100644
index eca35bc..0000000
--- a/midterm.m
+++ /dev/null
@@ -1,77 +0,0 @@
-% 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:
diff --git a/uav.m b/uav.m
new file mode 100644
index 0000000..ecb6971
--- /dev/null
+++ b/uav.m
@@ -0,0 +1,107 @@
+% 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:
diff --git a/uav_model.m b/uav_model.m
index 5650946..ce7cddb 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -2,7 +2,9 @@
 %
 % Compute model for UAV for given set of parameters.
 
-function [model] = uav_model(params)
+function [model] = uav_model(params, perf, uncert)
+
+model = struct();
 
 % ------------------------------------------------------------------------
 % Symbolic variables
@@ -121,6 +123,17 @@ f = [
   inv(J) * (tau - cross(Omega, J * Omega)); % rotational dynamics
 ];
 
+% Save function to compute the rotation matrix
+model.FrameRot = @(pitch, roll, yaw) ...
+  subs(R, [phi, theta, psi], [pitch, roll, yaw]);
+
+% Save equations of non-linear model (algebraic)
+model.nonlinear = struct(...
+  'State', xi, ...
+  'Inputs', u, ...
+  'Dynamics', f ...
+);
+
 % ------------------------------------------------------------------------
 % Linearization at equilibrium
 
@@ -187,6 +200,14 @@ sys = ss(A, B, C, D);
 % T = params.actuators.MeasurementDelay;
 % sys = ss(A, B, C, D, 'OutputDelay', T);
 
+% Save linearized dynamics (numerical)
+model.linear = struct(...
+  'Nx', nx, 'Nu', nu, 'Ny', ny, ... % number of states, inputs, and outputs
+  'State', xi, 'Inputs', u, ... % state and input variables
+  'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized
+  'StateSpace', sys ... % state space object
+);
+
 % ------------------------------------------------------------------------
 % Check properties of linearized model
 eigvals = eig(A);
@@ -242,27 +263,24 @@ if rank(Wo) < nx
 end
 
 % ------------------------------------------------------------------------
-% Save model
+% Add uncertainties using SIMULINK model
 
-model = struct();
+% Load simulink model with uncertainties and pass in parameters
+h = load_system('uav_model_uncertain');
+hws = get_param('uav_model_uncertain', 'modelworkspace');
 
-% Function to compute the rotation matrix
-model.FrameRot = @(pitch, roll, yaw) ...
-  subs(R, [phi, theta, psi], [pitch, roll, yaw]);
+hws.assignin('model', model);
+hws.assignin('perf', perf);
+hws.assignin('uncert', uncert);
 
-% Equations of non-linear model (algebraic)
-model.nonlinear = struct(...
-  'State', xi, ...
-  'Inputs', u, ...
-  'Dynamics', f ...
-);
+% Get uncertain model
+ulmod = linmod('uav_model_uncertain');
+usys = ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d);
 
-% Linearized dynamics (numerical)
-model.linear = struct(...
-  'Nx', nx, 'Nu', nu, 'Ny', ny, ... % number of states, inputs, and outputs
-  'State', xi, 'Inputs', u, ... % state and input variables
-  'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized
-  'StateSpace', sys ... % state space object
+% Save uncertain model
+model.uncertain = struct(...
+	'Simulink', ulmod, ...
+  'StateSpace', usys ...
 );
 
 end
diff --git a/uav_params.m b/uav_params.m
index 3ceda9e..0e82148 100644
--- a/uav_params.m
+++ b/uav_params.m
@@ -1,5 +1,5 @@
 % Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
-% 
+%
 % Retrieve / compute parameters for UAV.
 
 function [params] = uav_params()
@@ -34,16 +34,20 @@ params.mechanical = struct(...
 );
 
 % ------------------------------------------------------------------------
-% Actuator limits
+% Actuator limits and measurements
 
 % Cheap servos usually give a "speed" in seconds / 60 degrees without load
 servo_speed = 0.19; % seconds per 60 deg
 servo_angular_velocity = (pi / 3) / servo_speed;
 
 params.actuators = struct(...
-  'TurbineMaxSpeed', 620.7, ... % In Hz
-  'ServoAbsMaxAngleDeg', 25, ... % in Degrees
-  'ServoNominalAngularVelocity', servo_angular_velocity, ...
+  'TurbineMaxSpeed', 620.7, ... % in rad / s
+  'ServoAbsMaxAngle', 25 * pi / 180, ... % in radians
+  'ServoNominalAngularVelocity', servo_angular_velocity ...
+);
+
+params.measurements = struct(...
+  'SensorFusionBandwidth', 1e3, ...
   'MeasurementDelay', 8e-3 ... % delay caused by sensor fusion algorithm
 );
 
@@ -87,7 +91,8 @@ params.linearization = struct(...
 % Performance requirements
 
 params.performance = struct(...
-  'MaxHorizontalSpeed', 1, ...
+  'MaxHorizontalSpeed', 1e-2, ... % in m / s
+  'MaxVerticalSpeed', 5e-2 ... % in m / s
 );
 
 end