Replace LQR with H-infinity design

1 files changed, 92 insertions, 3 deletions

diff --git a/uav_model.m b/uav_model.m
index ce7cddb..747f1b7 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -1,6 +1,30 @@
-% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
+% Compute models for Ducted Fan VTOL micro-UAV for given set of parameters. 
 %
-% Compute model for UAV for given set of parameters.
+% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich.
+% This function generates three models: 
+% 
+%  * A non-linear symbolic model (cannot be used directly).
+%  * A linear model obtained by linearizing the the non linear model at an
+%    operating point specified in the params struct argument. 
+%  * A uncertain linear model built atop of the linear model using SIMULINK.
+%    The uncertain model contains the performance and weighting transfer 
+%    function given in the arguments perf and params, and is stored in the
+%    SIMULINK fil euav_model_uncertain.xls.
+%
+% [MODEL] = UAV_MODEL(PARAMS, PERF, UNCERT)
+%
+% Arguments:
+%   PARAMS  Struct of design parameters and constants generated from uav_params
+%   PERF    Struct with transfer functions that describe performance
+%           requirements (used for uncertain model).
+%   UNCERT  Struct with weighting transfer functions for the uncertainty
+%           blocks (used for uncertain model).
+%
+% Return value:
+%   MODEL   Struct with models
+%
+% See also UAV_PARAMS
+
 
 function [model] = uav_model(params, perf, uncert)
 
@@ -210,6 +234,7 @@ model.linear = struct(...
 
 % ------------------------------------------------------------------------
 % Check properties of linearized model
+
 eigvals = eig(A);
 
 % Check system controllability / stabilizability
@@ -269,6 +294,7 @@ end
 h = load_system('uav_model_uncertain');
 hws = get_param('uav_model_uncertain', 'modelworkspace');
 
+hws.assignin('params', params);
 hws.assignin('model', model);
 hws.assignin('perf', perf);
 hws.assignin('uncert', uncert);
@@ -279,9 +305,72 @@ usys = ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d);
 
 % Save uncertain model
 model.uncertain = struct(...
-	'Simulink', ulmod, ...
+  'Simulink', ulmod, ...
   'StateSpace', usys ...
 );
 
+% The uncertain system is partitioned into the following matrix
+%
+% [ z ]   [ A    B_w   B_u  ] [ v ]
+% [ e ] = [ C_e  D_ew  D_eu ] [ w ]
+% [ y ]   [ C_y  D_yw  D_yu ] [ u ]
+%
+% Struct below provides indices for inputs and outputs of partitioning.
+% Check for correctness of these values by inspecting:
+%
+%   - model.uncertain.Simulink.InputName(model.uncertain.index.InputX)
+%   - model.uncertain.Simulink.OutputName(model.uncertain.index.OutputX)
+%
+% 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
+  'OutputUncertain',   make_idx( 1, 17), ... % 'z' outputs
+  'OutputError',       make_idx(18, 17), ... % 'e' outputs
+  'OutputNominal',     make_idx(35, 12)  ... % 'y' outputs
+);
+
+% ------------------------------------------------------------------------
+% Check properties of uncertain model
+
+% % Check that (A, B_u, C_y) is stabilizable and detectable
+% A = model.uncertain.StateSpace(...
+%   model.uncertain.index.OutputUncertain, ...
+%   model.uncertain.index.InputUncertain ...
+% );
+% 
+% B_u = model.uncertain.StateSpace(...
+%   model.uncertain.index.OutputUncertain, ...
+%   model.uncertain.index.InputNominal ...
+% );
+%
+% % TODO: do PBH test
+% 
+% % Check that D_eu and D_yw are full rank
+% D_eu = model.uncertain.StateSpace(...
+%   model.uncertain.index.OutputError, ...
+%   model.uncertain.index.InputNominal ...
+% );
+% 
+% D_yw = model.uncertain.StateSpace(...
+%   model.uncertain.index.OutputNominal, ...
+%   model.uncertain.index.InputDisturbance ...
+% );
+
+% if rank(D_eu) < min(size(D_eu))
+%   fprintf('D_eu is not full rank!\n')
+% end
+% 
+% if rank(D_yw) < min(size(D_yw))
+%   fprintf('D_yw is not full rank!\n')
+% end
+
+end
+
+function [indices] = make_idx(start, size)
+  indices = [start:(start + size - 1)]';
 end
+
 % vim: ts=2 sw=2 et: