% 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'); do_plots = true; % runs faster without do_hinf = false; % midterm do_musyn = true; % endterm if do_hinf & do_musyn error('Cannot do both H-infinity and mu synthesis.') end fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n') fprintf('Will do:\n') if do_plots fprintf(' - Produce plots\n') end if do_hinf fprintf(' - H-infinity synthesis\n') end if do_musyn fprintf(' - Mu synthesis\n') end % Synthesized controllers will be stored here ctrl = struct(); % ------------------------------------------------------------------------ %% Define system parameters fprintf('Generating system parameters...\n') params = uav_params(); % ------------------------------------------------------------------------ %% Define performance requirements if do_hinf fprintf('Generating performance requirements...\n') perf = uav_performance_hinf(params, do_plots); end if do_musyn fprintf('Generating performance requirements...\n') perf = uav_performance_musyn(params, do_plots); end % ------------------------------------------------------------------------ %% Define stability requirements % Note: for hinf it is needed to call uav_mode, but hinf will not actually % make use of this struct if do_hinf | do_musyn fprintf('Generating stability requirements...\n') uncert = uav_uncertainty(params, do_plots); end % ------------------------------------------------------------------------ %% Create UAV model fprintf('Generating system model...\n'); model = uav_model(params, perf, uncert); % ------------------------------------------------------------------------ %% Perform H-infinity design if do_hinf fprintf('Performing H-infinty controller design...\n') idx = model.uncertain.index; P = model.uncertain.StateSpace; % Get nominal system without uncertainty (for lower LFT) P_nom = minreal(P([idx.OutputError; idx.OutputNominal], ... [idx.InputExogenous; idx.InputNominal]), [], false); nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; 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 of H-infinity design fprintf('Simulating closed loop...\n'); nsamples = 500; do_noise = true; 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') end % ------------------------------------------------------------------------ %% Perform mu-Analysis & DK iteration if do_musyn fprintf('Performing mu-synthesis controller design...\n') idx = model.uncertain.index; P = minreal(model.uncertain.StateSpace(... [idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ... [idx.InputUncertain; idx.InputExogenous; idx.InputNominal]), ... [], false); % Options for H-infinity nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... 'AutoScale', 'on', 'RelTol', 1e-2); % Frequency raster resolution to fit D scales nsamples = 501; omega = logspace(-3, 3, nsamples); % Initial values for D-K iteration D_left = tf(eye(model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny)); D_right = tf(eye(model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu)); % degrees for approximations of D-scales, tuned by hand fit_degrees = [ 2, 1, 1, 1; % 1, 1; % alpha 2, 4, 1, 1; % 1, 1; % omega 1, 2, 1, 1; % 1, 1; % state 3, 4, 1, 1; % 1, 1; % perf ]; % Number of D-K iterations niters = size(fit_degrees, 2); % niters = 5; % for plotting later mu_plot_legend = {}; % Start DK-iteration dkstart = tic; for it = 1:niters fprintf(' - Running D-K iteration %d...\n', it); itstart = tic(); % Find controller using H-infinity [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl, hinfopt); fprintf(' H-infinity synthesis gamma: %g\n', gamma); if gamma == inf fprintf(' Failed to synethesize H-infinity controller\n'); break; end % Calculate frequency response of closed loop N = minreal(lft(P, K), [], false); % slient N_frd = frd(N, omega); % Calculate upper bound D scaling [mu_bounds, mu_info] = mussv(N_frd, model.uncertain.BlockStructurePerf, 'sU'); mu_rp = norm(mu_bounds(1,1), inf, 1e-6); fprintf(' Mu value for RP: %g\n', mu_rp) if do_plots fprintf(' Plotting mu\n'); figure(100); hold on; bodemag(mu_bounds(1,1)); mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{%d}$', it)}; title('\bfseries $\mu_\Delta(\omega)$ for both Stability and Performance', 'interpreter', 'latex'); legend(mu_plot_legend, 'interpreter', 'latex'); grid on; drawnow; end % Are we done yet? if mu_rp < 1 fprintf(' - Found robust controller that meets performance.\n'); break end % Fit D-scales % There are three complex, square, full block uncertainties and % a non-square full complex block for performance [D_left_samples, D_right_samples] = mussvunwrap(mu_info); % D scale for alpha uncertainty (first block) i = 1; D_left_samples_alpha = D_left_samples(i, i); % D_alpha = fitmagfrd(D_left_samples_alpha, fit_degrees(1, it)); D_alpha = fitfrd(genphase(D_left_samples_alpha), fit_degrees(1, it)); % D scale for omega uncertainty (second block) i = model.uncertain.BlockStructure(1, 1) + 1; % after first block D_left_samples_omega = frd(D_left_samples(i, i)); % D_omega = fitmagfrd(D_left_samples_omega, fit_degrees(2, it)); D_omega = fitfrd(genphase(D_left_samples_omega), fit_degrees(2, it)); % D scale for state uncertainty (third block) i = model.uncertain.BlockStructure(2, 1) + 1; % after second block D_left_samples_state = D_left_samples(i, i); % D_state = fitmagfrd(D_left_samples_state, fit_degrees(3, it)); D_state = fitfrd(genphase(D_left_samples_state), fit_degrees(3, it)); % D scale for performance (non-square) i = model.uncertain.BlockStructurePerf(3, 1); % after third block D_left_samples_perf = D_left_samples(i, i); % D_perf = fitmagfrd(D_left_samples_perf, fit_degrees(4, it)); D_perf = fitfrd(genphase(D_left_samples_perf), fit_degrees(4, it)); % Construct full matrices D_right = blkdiag(D_alpha * eye(4), ... D_omega * eye(1), ... D_state * eye(12), ... D_perf * eye(10), ... eye(5)); D_left = blkdiag(D_alpha * eye(4), ... D_omega * eye(1), ... D_state * eye(12), ... D_perf * eye(14), ... eye(12)); % Plot fitted D-scales if do_plots fprintf(' Plotting D-scales'); f = figure(101); clf(f); hold on; bodemag(D_left_samples_alpha, omega, 'r-'); bodemag(D_alpha, omega, 'b'); fprintf('.'); bodemag(D_left_samples_omega, omega, 'r--'); bodemag(D_omega, omega, 'b--'); fprintf('.'); bodemag(D_left_samples_state, omega, 'c-'); bodemag(D_state, omega, 'm-'); fprintf('.'); bodemag(D_left_samples_perf, omega, 'c--'); bodemag(D_perf, omega, 'm--'); fprintf('.'); fprintf('\n'); title(sprintf('\\bfseries $D(\\omega)$ Scales Approximations at Iteration %d', it), ... 'interpreter', 'latex') legend(... '$D_{\alpha}$', '$\hat{D}_{\alpha}$', ... '$D_{\omega}$', '$\hat{D}_{\omega}$', ... '$D_{\mathbf{x}}$', '$\hat{D}_{\mathbf{x}}$', ... '$D_{\Delta}$', '$\hat{D}_{\Delta}$', ... 'interpreter', 'latex' ... ); grid on; drawnow; end itend = toc(itstart); fprintf(' Iteration took %.1f seconds\n', itend); end dkend = toc(dkstart); fprintf(' - D-K iteration took %.1f seconds\n', dkend); if mu_rp > 1 fprintf(' - Failed to synthesize robust controller that meets the desired performance.\n'); else ctrl.musyn = struct('K', K, 'mu', mu_rp); end end % ------------------------------------------------------------------------ %% Verify performance satisfaction via mu-analysis