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

1 files changed, 95 insertions, 82 deletions

diff --git a/uav.m b/uav.m
index 453bd00..e5f49b2 100644
--- a/uav.m
+++ b/uav.m
@@ -1,75 +1,24 @@
-% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich
-%
 % Controller design for a ducted fan VTOL micro-UAV.
+%
+% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich
+% This work is distributed under a permissive license, see LICENSE.txt
 
 %  ------------------------------------------------------------------------
 % Clear environment and generate parameters
 
 clear; clc; close all; s = tf('s');
+
+fprintf('Generating system parameters...\n')
 params = uav_params();
+ctrl = struct();
 
-do_plots = false;
+do_plots = true;
 
 % ------------------------------------------------------------------------
 %% Define performance requirements
 
-% Mechanically, flaps are constrained to a max of 20~25 degrees,
-% and they have a maximal angular speed
-alpha_max = params.actuators.ServoAbsMaxAngle;
-alpha_dot_max = params.actuators.ServoNominalAngularVelocity;
-
-W_Palpha = (s + 100 * alpha_dot_max) / (s + alpha_dot_max);
-W_Palpha = alpha_max * W_Palpha / dcgain(W_Palpha); % adjust gain
-
-% Mechanically we have a maximal angular velocity for the propeller in the
-% thruster, also there are a lot of unmodelled dynamics in the thruster
-omega_max = params.actuators.TurbineMaxSpeed;
-W_Pomega = (s + 50 * omega_max) / (s + omega_max);
-
-% We want a nice and smooth movements
-v_xy_max = params.performance.MaxHorizontalSpeed;
-v_z_max = params.performance.MaxVerticalSpeed;
-
-W_Pxy = 1 / (s + 1 / v_xy_max);
-W_Pz = 1 / (s + 1 / v_z_max);
-
-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);
-
-  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');
-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, ...
-  'Position', W_PP, ...
-  'Velocity', W_PPdot, ...
-  'Angle', W_PTheta, ...
-  'AngularVelocity', W_POmega); 
+fprintf('Generating performance requirements...\n')
+perf = uav_requirements(params, do_plots);
 
 %  ------------------------------------------------------------------------
 %% Define stability requirements
@@ -78,48 +27,112 @@ W_malpha = tf(1,1);
 W_momega = tf(1,1);
 W_mState = tf(1,1);
 
-if do_plots
-  figure; hold on;
-
-  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')
-end
+% if do_plots
+%   figure; hold on;
+%
+%   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')
+% end
 
 uncert = struct(...
   'FlapAngle', W_malpha * eye(4), ...
   'Thrust', W_momega, ...
   'StateLinApprox', W_mState * eye(12));
-  
+
 % ------------------------------------------------------------------------
-% Create UAV model
+%% Create UAV model
 
+fprintf('Generating system model...\n');
 model = uav_model(params, perf, uncert);
 
 % ------------------------------------------------------------------------
+%% Perform LQR design
+
+% fprintf('Performing LQR controller design...\n')
+% ctrl.lqr = uav_ctrl_lqr(params, model);
+
+% ------------------------------------------------------------------------
 %% Perform H-infinity design
 
+fprintf('Performing H-infinty controller design...\n')
+
 idx = model.uncertain.index;
-idx_ey = [idx.OutputError; idx.OutputNominal];
-idx_wu = [idx.InputDisturbance; idx.InputReference; idx.InputNominal];
+P = model.uncertain.StateSpace;
+
+% Get nominal system without uncertainty (for lower LFT)
+P_nom = minreal(P([idx.OutputError; idx.OutputNominal], ...
+                  [idx.InputExogenous; 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));
+hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
+  'AutoScale', 'off', 'RelTol', 1e-3);
+[K_inf, ~, gamma, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt);
+ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_inf);
+
+if gamma >= 1
+  fprintf('Failed to syntesize controller (closed loop is unstable).\n')
+end
+
+%  ------------------------------------------------------------------------
+%% Measure Performance
+
+fprintf('Simulating closed loop...\n');
 
-hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', 'RelTol', 0.01);
-[K_inf, N_inf, gamma, info] = hinfsyn(G, nmeas, nctrl, hinfopt);
+nsamples = 500;
+do_noise = true;
+% uav_sim_step(params, model, ctrl.lqr, nsamples, do_plots);
+
+simout = uav_sim_step_hinf(params, model, ctrl.hinf, nsamples, do_plots, do_noise);
+
+fprintf('Writing simulation results...\n');
+cols = [
+    simout.StepX(:, simout.index.Position), ...
+    simout.StepX(:, simout.index.Velocity), ...
+    simout.StepX(:, simout.index.FlapAngles) * 180 / pi, ...
+    simout.StepX(:, simout.index.Angles) * 180 / pi];
+
+writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
 
 %  ------------------------------------------------------------------------
-% Verify performance satisfaction
+%% Verify performance satisfaction via mu-analysis
+
+% omega = logspace(-3, 3, 250);
+%
+% N_inf = lft(P, K_inf);
+% N_inf_frd = frd(N_inf, omega);
+%
+% % robust stability
+% [mu_inf_rs, ~] = mussv(N_inf_frd(idx.OutputUncertain, idx.InputUncertain), ...
+%             model.uncertain.BlockStructure);
+%
+% % robust performance
+% blk_perf = [model.uncertain.BlockStructure;
+%             model.uncertain.BlockStructurePerf];
+%
+% [mu_inf_rp, ~] = mussv(N_inf_frd, blk_perf);
+%
+% % nominal performance
+% mu_inf_np = svd(N_inf_frd(idx.OutputError, idx.InputExogenous));
+%
+% if do_plots
+%   figure; hold on;
+%   bodemag(mu_inf_rs(1));
+%   bodemag(mu_inf_np(1));
+%   bodemag(mu_inf_rs(2));
+%   bodemag(mu_inf_np(2));
+%
+%   grid on;
+%   title('$\mu_\Delta(N)$', 'Interpreter', 'latex');
+% end
 
 %  ------------------------------------------------------------------------
 % Perform mu-Analysis & DK iteration