Replace LQR with H-infinity design

1 files changed, 57 insertions, 37 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