Replace LQR with H-infinity design

5 files changed, 153 insertions, 81 deletions

diff --git a/uav.m b/uav.m
index ecb6971..453bd00 100644
--- a/uav.m
+++ b/uav.m
@@ -8,8 +8,10 @@
 clear; clc; close all; s = tf('s');
 params = uav_params();
 
+do_plots = false;
+
 % ------------------------------------------------------------------------
-% Define performance requirements
+%% Define performance requirements
 
 % Mechanically, flaps are constrained to a max of 20~25 degrees,
 % and they have a maximal angular speed
@@ -31,61 +33,69 @@ 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;
+if do_plots
+  % Bode plots of performance requirements
+  figure; hold on;
+
+  bodemag(1/W_Palpha);
+  bodemag(1/W_Pomega);
+  bodemag(1/W_Pxy);
+  bodemag(1/W_Pz);
 
-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');
 
-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');
+  % 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');
+end
 
 % Construct performance for position vector by combining xy and z
 W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
+W_PPdot = tf(1,1) * eye(3);
+W_PTheta = tf(1,1) * eye(3);
+W_POmega = tf(1,1) * eye(3);
 
 perf = struct(...
   'FlapAngle', W_Palpha * eye(4), ...
   'Thrust', W_Pomega, ...
-  'PositionAccuracy', W_PP);
+  'Position', W_PP, ...
+  'Velocity', W_PPdot, ...
+  'Angle', W_PTheta, ...
+  'AngularVelocity', W_POmega); 
 
 %  ------------------------------------------------------------------------
-% Define stability requirements
+%% Define stability requirements
 
 W_malpha = tf(1,1);
 W_momega = tf(1,1);
-W_mTheta = tf(1,1);
-W_mOmega = tf(1,1);
+W_mState = tf(1,1);
 
-figure; hold on;
+if do_plots
+  figure; hold on;
 
-bodemag(W_malpha);
-bodemag(W_momega);
-bodemag(W_mTheta);
-bodemag(W_mOmega);
+  bodemag(W_malpha);
+  bodemag(W_momega);
+  bodemag(W_mState);
 
-grid on;
-legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ...
-  '$W_{m,\Theta}$', '$W_{m,\Omega}$', ...
-  'interpreter', 'latex', 'fontSize', 8);
-title('Uncertainties')
+  grid on;
+  legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ...
+    '$W_{m,\Theta}$', '$W_{m,\Omega}$', ...
+    'interpreter', 'latex', 'fontSize', 8);
+  title('Uncertainties')
+end
 
 uncert = struct(...
   'FlapAngle', W_malpha * eye(4), ...
   'Thrust', W_momega, ...
-  'EulerAnglesApprox', W_mTheta * eye(3), ...
-  'AngularRateApprox', W_mOmega * eye(3));
+  'StateLinApprox', W_mState * eye(12));
   
 % ------------------------------------------------------------------------
 % Create UAV model
@@ -93,10 +103,20 @@ uncert = struct(...
 model = uav_model(params, perf, uncert);
 
 % ------------------------------------------------------------------------
-% Perform H-infinity design
+%% Perform H-infinity design
+
+idx = model.uncertain.index;
+idx_ey = [idx.OutputError; idx.OutputNominal];
+idx_wu = [idx.InputDisturbance; idx.InputReference; idx.InputNominal];
+
+nmeas = max(size(idx.OutputNominal)); % size of y
+nctrl = max(size(idx.InputNominal)); % size of u
+
+% Get nominal system without uncertainty (lower LFT)
+G = minreal(model.uncertain.StateSpace(idx_ey, idx_wu));
 
-Gnom = model.linear.StateSpace;
-G = model.uncertain.StateSpace;
+hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', 'RelTol', 0.01);
+[K_inf, N_inf, gamma, info] = hinfsyn(G, nmeas, nctrl, hinfopt);
 
 %  ------------------------------------------------------------------------
 % Verify performance satisfaction
diff --git a/uav_ctrl_nominal.m b/uav_ctrl_nominal.m
deleted file mode 100644
index 1030233..0000000
--- a/uav_ctrl_nominal.m
+++ /dev/null
@@ -1,38 +0,0 @@
-% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
-% 
-% Design a nominal controller for UAV.
-
-function [ctrl] = uav_ctrl_nominal(params, model)
-
-% ------------------------------------------------------------------------
-% Design a nominal LQR controller
-
-% Define penalties according to following priorities
-
-q_pos = 1; % penalty on position
-q_vel = 10; % penalty on linear velocity
-q_ang = 100; % high penalty on angles
-q_rate = 100; % very high penalty on angular velocities
-
-r_ang = 1; % flap movement is cheap
-r_thrust = 100; % thrust is more expensive on the battery
-
-nx = model.linear.Nx;
-nu = model.linear.Nu;
-
-% LQR design matrices
-Q = kron(diag([q_pos, q_vel, q_ang, q_rate]), eye(3));
-R = diag([r_ang, r_ang, r_ang, r_ang, r_thrust]);
-N = zeros(nx, nu); % no cross terms
-
-% Compute controller
-[K, S, poles] = lqr(model.linear.StateSpace, Q, R, N);
-
-% ------------------------------------------------------------------------
-% Save controller
-
-ctrl = struct();
-ctrl.K = K;
-
-end
-% vim: ts=2 sw=2 et:
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:
diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx
index 1ba0805..b958b25 100644
--- a/uav_model_uncertain.slx
+++ b/uav_model_uncertain.slx
diff --git a/uav_params.m b/uav_params.m
index 0e82148..7bed89c 100644
--- a/uav_params.m
+++ b/uav_params.m
@@ -1,6 +1,6 @@
-% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
+% Retrieve or compute parameters for ducted-fan VTOL micro-UAV.
 %
-% Retrieve / compute parameters for UAV.
+% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
 
 function [params] = uav_params()
 
@@ -48,7 +48,7 @@ params.actuators = struct(...
 
 params.measurements = struct(...
   'SensorFusionBandwidth', 1e3, ...
-  'MeasurementDelay', 8e-3 ... % delay caused by sensor fusion algorithm
+  'SensorFusionDelay', 8e-3 ...
 );
 
 % ------------------------------------------------------------------------
@@ -73,6 +73,7 @@ params.aerodynamics = struct(...
 
 % Compute theoretical thrust required to make the UAV hover
 % from the relation mg = k * omega^2
+% FIXME: This value should probably be replaced with a measurement 
 g = params.physics.Gravity;
 m = params.mechanical.Mass;
 k = params.aerodynamics.ThrustOmegaProp;