Improve H-infinity, system parameters, add simulations and plots

1 files changed, 55 insertions, 11 deletions

diff --git a/uav_model.m b/uav_model.m
index 747f1b7..4fddae9 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -1,6 +1,8 @@
 % Compute models for Ducted Fan VTOL micro-UAV for given set of parameters. 
 %
 % Copyright (C) 2024, Naoki Sean Pross, ETH Zürich.
+% This work is distributed under a permissive license, see LICENSE.txt
+%
 % This function generates three models: 
 % 
 %  * A non-linear symbolic model (cannot be used directly).
@@ -219,10 +221,9 @@ D = zeros(12, 5);
 [ny, ~] = size(C);
 
 % Create state space object
-sys = ss(A, B, C, D);
-
-% T = params.actuators.MeasurementDelay;
-% sys = ss(A, B, C, D, 'OutputDelay', T);
+T = params.measurements.SensorFusionDelay;
+n = params.linearization.PadeApproxOrder;
+sys = minreal(pade(ss(A, B, C, D, 'OutputDelay', T), n));
 
 % Save linearized dynamics (numerical)
 model.linear = struct(...
@@ -263,7 +264,7 @@ if rank(Wc) < nx
   end
 end
 
-% Check  system observability / detectability
+% Check system observability / detectability
 Wo = obsv(sys);
 if rank(Wo) < nx
   fprintf('Linearized system has %d unobservable states!\n', ...
@@ -283,11 +284,23 @@ if rank(Wo) < nx
     fprintf('Linearized system has %d undetectable modes!\n', ...
       undetectable);
   else
-    fprintf('However, it is detectable.')
+    fprintf('However, it is detectable.\n')
   end
 end
 
 % ------------------------------------------------------------------------
+% Model actuators
+
+% TODO: better model, this was tuned "by looking"
+w = 4 * params.actuators.ServoNominalAngularVelocity;
+zeta = .7;
+G_servo = tf(w^2, [1, 2 * zeta * w, w^2]);
+% figure; step(G_servo * pi / 3); grid on;
+G_prop = tf(1,1);
+
+model.actuators = struct('FlapServo', G_servo, 'ThrustPropeller', G_prop);
+
+% ------------------------------------------------------------------------
 % Add uncertainties using SIMULINK model
 
 % Load simulink model with uncertainties and pass in parameters
@@ -303,9 +316,25 @@ hws.assignin('uncert', uncert);
 ulmod = linmod('uav_model_uncertain');
 usys = ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d);
 
+% Specify uncertainty block structure for mussv command
+blk_stab = [
+  4, 0; % alpha
+  1, 0; % omega
+  12, 12; % state
+];
+
+blk_perf = [
+  4, 0; % alpha
+  1, 0; % omega
+  3, 3; % position
+  3, 3; % velocity
+];
+
 % Save uncertain model
 model.uncertain = struct(...
   'Simulink', ulmod, ...
+  'BlockStructure', blk_stab, ...
+  'BlockStructurePerf', blk_perf, ...
   'StateSpace', usys ...
 );
 
@@ -324,14 +353,29 @@ model.uncertain = struct(...
 % Function make_idx(start, size) is defined below.
 model.uncertain.index = struct(...
   'InputUncertain',    make_idx( 1, 17), ... % 'v' inputs
-  'InputDisturbance',  make_idx(18, 12), ... % 'w' inputs for noise
-  'InputReference',    make_idx(30, 12), ... % 'w' inputs for reference
-  'InputNominal',      make_idx(42,  5), ... % 'u' inputs
+  'InputDisturbance',  make_idx(18,  7), ... % 'w' inputs for noise
+  'InputReference',    make_idx(25,  3), ... % 'w' inputs for reference
+  'InputExogenous',    make_idx(18, 10), ... % 'w' inputs (all of them)
+  'InputNominal',      make_idx(28,  5), ... % 'u' inputs
   'OutputUncertain',   make_idx( 1, 17), ... % 'z' outputs
-  'OutputError',       make_idx(18, 17), ... % 'e' outputs
-  'OutputNominal',     make_idx(35, 12)  ... % 'y' outputs
+  'OutputError',       make_idx(18, 14), ... % 'e' outputs
+  'OutputNominal',     make_idx(32, 12), ... % 'y' outputs
+  'OutputPlots',       make_idx(44, 10)  ... % 'p' outputs for plots in closed loop
 );
 
+idx = model.uncertain.index;
+
+% Number of inputs
+model.uncertain.Nv = max(size(idx.InputUncertain));
+model.uncertain.Nr = max(size(idx.InputReference));
+model.uncertain.Nw = max(size(idx.InputExogenous));
+model.uncertain.Nu = max(size(idx.InputNominal));
+
+% Number of outputs
+model.uncertain.Nz = max(size(idx.OutputUncertain));
+model.uncertain.Ne = max(size(idx.OutputError));
+model.uncertain.Ny = max(size(idx.OutputNominal));
+
 % ------------------------------------------------------------------------
 % Check properties of uncertain model