Update a lot of small things and add SIMULINK model

5 files changed, 84 insertions, 35 deletions

diff --git a/midterm.m b/midterm.m
index 8f8e5e1..eca35bc 100644
--- a/midterm.m
+++ b/midterm.m
@@ -3,39 +3,75 @@
 %% ------------------------------------------------------------------------
 % Clear environment and generate parameters
 
-clear; clc; close all;
+clear; clc; close all; s = tf('s');
 params = uav_params();
 
 %% ------------------------------------------------------------------------
-% Create nominal plant and controller
+% Create nominal plant
 
 % Generate nominal model
 model = uav_model(params);
-ctrl = uav_ctrl_nominal(params, model);
 
-% Model dimensions
-nx = model.linear.Nx;
+%% ------------------------------------------------------------------------
+% Define performance requirements
 
-% Open loop path
-L = model.linear.StateSpace * ctrl.K;
+alpha_max = params.actuators.ServoAbsMaxAngleDeg * pi / 180;
+W_Palpha = tf(alpha_max,1);
 
-% Closed loop path under unitary negative gain
-G = feedback(L, eye(nx, nx));
+W_Pomega = tf(1,1);
+W_Pxy = tf(1,1);
+W_Pz = tf(1,1);
 
-%% ------------------------------------------------------------------------
-% Simulate nominal controller
+figure(1); hold on;
+bodemag(W_Palpha);
+bodemag(W_Pomega);
+bodemag(W_Pxy);
+bodemag(W_Pz);
 
-%% ------------------------------------------------------------------------
-% Define performance goals
+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);
 
 %% ------------------------------------------------------------------------
-% Create uncertain model
+% 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);
 
 %% ------------------------------------------------------------------------
-% Define stability goals
+% 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
\ No newline at end of file
+% Perform mu-Analysis & DK iteration
+
+% vim: ts=2 sw=2 et:
diff --git a/uav_ctrl_nominal.m b/uav_ctrl_nominal.m
index d46a1ef..1030233 100644
--- a/uav_ctrl_nominal.m
+++ b/uav_ctrl_nominal.m
@@ -9,13 +9,13 @@ function [ctrl] = uav_ctrl_nominal(params, model)
 
 % Define penalties according to following priorities
 
-q_pos = 10; % penalty on position
-q_vel = 1; % penalty on linear velocity
+q_pos = 1; % penalty on position
+q_vel = 10; % penalty on linear velocity
 q_ang = 100; % high penalty on angles
-q_rate = 1000; % very high penalty on angular velocities
+q_rate = 100; % very high penalty on angular velocities
 
 r_ang = 1; % flap movement is cheap
-r_thrust = 10; % thrust is more expensive on the battery
+r_thrust = 100; % thrust is more expensive on the battery
 
 nx = model.linear.Nx;
 nu = model.linear.Nu;
@@ -34,4 +34,5 @@ N = zeros(nx, nu); % no cross terms
 ctrl = struct();
 ctrl.K = K;
 
-end
\ No newline at end of file
+end
+% vim: ts=2 sw=2 et:
diff --git a/uav_model.m b/uav_model.m
index 2ae4d96..5650946 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -1,5 +1,5 @@
 % Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
-% 
+%
 % Compute model for UAV for given set of parameters.
 
 function [model] = uav_model(params)
@@ -95,7 +95,7 @@ tau_4 = cross((d * uvec_z - a/3 * uvec_y), F_4);
 % Total force acting on the UAV in the body frame
 F = R * (m * g * uvec_k) ... % gravity
   - k_T * omega^2 * uvec_z ... % thrust
-  + F_1 + F_2 + F_3 + F_4; % flaps  
+  + F_1 + F_2 + F_3 + F_4; % flaps
 
 % Total torque acting on the UAV in the body frame
 tau = J_r * omega * R * cross(uvec_k, Omega) + ... % gyroscopic procession
@@ -117,7 +117,7 @@ u = [alpha; omega];
 f = [
   Pdot;
   R' * F / m; % translational dynamics
-  U * Omega; 
+  U * Omega;
   inv(J) * (tau - cross(Omega, J * Omega)); % rotational dynamics
 ];
 
@@ -184,10 +184,8 @@ D = zeros(12, 5);
 % Create state space object
 sys = ss(A, B, C, D);
 
-% ------------------------------------------------------------------------
-% Measurement model
-
-% TODO: add sensor fusion delay
+% T = params.actuators.MeasurementDelay;
+% sys = ss(A, B, C, D, 'OutputDelay', T);
 
 % ------------------------------------------------------------------------
 % Check properties of linearized model
@@ -198,7 +196,7 @@ Wc = ctrb(sys);
 if rank(Wc) < nx
   fprintf('Linearized system has %d uncontrollable states!\n', ...
     (nx - rank(Wc)));
-    
+
   % Is the system at least stabilizable?
   unstabilizable = 0;
   for i = 1:nx
@@ -222,10 +220,10 @@ end
 % Check  system observability / detectability
 Wo = obsv(sys);
 if rank(Wo) < nx
-  fprintf('Linearized system has %d unobservable state!\n', ...
+  fprintf('Linearized system has %d unobservable states!\n', ...
     (nx - rank(Wo)));
   % is the system at least detectable?
-  undetectable = 0
+  undetectable = 0;
   for i = 1:nx
     if real(eigvals(i)) >= 0
       % PBH test
@@ -268,3 +266,4 @@ model.linear = struct(...
 );
 
 end
+% vim: ts=2 sw=2 et:
diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx
new file mode 100644
index 0000000..1ba0805
--- /dev/null
+++ b/uav_model_uncertain.slx
diff --git a/uav_params.m b/uav_params.m
index dd0c101..3ceda9e 100644
--- a/uav_params.m
+++ b/uav_params.m
@@ -29,16 +29,21 @@ params.mechanical = struct(...
   'DuctRadius', 46e-3, ...
   'DuctHeight', 171e-3, ...
   'FlapZDistance', 98e-3, ... % flap distance along z from center of mass
-  'InertiaTensor', 5e-3 * eye(3), ... % TODO, assume diagonal
+  'InertiaTensor', 5e-3 * eye(3), ... % TODO true values, assume diagonal
   'GyroscopicInertiaZ', 0 ... % assume no gyroscopic effects for now
 );
 
 % ------------------------------------------------------------------------
 % Actuator limits
 
+% 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, ...
   'MeasurementDelay', 8e-3 ... % delay caused by sensor fusion algorithm
 );
 
@@ -53,9 +58,9 @@ F_max = 38.637; % in N (measured)
 k_T = sqrt(F_max / omega_max);
 
 params.aerodynamics = struct(...
-  'ThrustOmegaProp', k_T, ...
+  'ThrustOmegaProp', k_T, ... % in s^2 / (rad * N)
   'FlapArea', 23e-3 * 10e-3 * 1.5, ... % FIXME factor 150% was chosen at random
-  'DragCoefficients', [1, 1] .* 1e-3, ... % TODO
+  'DragCoefficients', [1, 4] .* 1e-3, ... % TODO
   'LiftCoefficient', 1e-3 ... % TODO
 );
 
@@ -78,4 +83,12 @@ params.linearization = struct(...
   'Inputs', [0; 0; 0; 0; omega_hover] ... % Flaps at rest and turbine at X
 );
 
-end
\ No newline at end of file
+% ------------------------------------------------------------------------
+% Performance requirements
+
+params.performance = struct(...
+  'MaxHorizontalSpeed', 1, ...
+);
+
+end
+% vim: ts=2 sw=2 et: