% 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 = true; do_musyn = true; 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 fprintf('Generating performance requirements...\n') perf_hinf = uav_performance_hinf(params, do_plots); perf_musyn = uav_performance_musyn(params, do_plots); %% ------------------------------------------------------------------------ % Define stability requirements fprintf('Generating stability requirements...\n') uncert = uav_uncertainty(params, do_plots); %% ------------------------------------------------------------------------ % Perform H-infinity design if do_hinf fprintf('Generating system model for H-infinity design...\n'); model = uav_model(params, perf_hinf, uncert); 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', 'off', 'Method', 'RIC', ... 'AutoScale', 'off', 'RelTol', 1e-3); [K_inf, ~, gamma_hinf, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt); fprintf(' - H-infinity synthesis gamma: %g\n', gamma_hinf); index = struct( ... 'Ix', 1, 'Iy', 2, 'Iz', 3, ... 'IPdot', (4:6)', ... 'Iroll', 7, 'Ipitch', 8, 'Iyaw', 9, ... 'ITheta', (10:12)', ... 'Ialpha', (1:4)', ... 'Iomega', 5 ... ); ctrl.hinf = struct( ... 'Name', '$\mathcal{H}_{\infty}$', ... 'K', K_inf, ... 'index', index ... ); if gamma_hinf >= 1 error('Failed to syntesize controller (closed loop is unstable).') end %% ------------------------------------------------------------------------ % Measure Performance of H-infinity design if do_plots fprintf(' - Plotting resulting controller...\n'); % Plot transfer functions figure; hold on; bode(ctrl.hinf.K(index.Ialpha(1), index.Ix)); bode(ctrl.hinf.K(index.Ialpha(2), index.Ix)); bode(ctrl.hinf.K(index.Ialpha(1), index.Iy)); bode(ctrl.hinf.K(index.Ialpha(2), index.Iy)); bode(ctrl.hinf.K(index.Iomega, index.Ix)); bode(ctrl.hinf.K(index.Iomega, index.Iy)); bode(ctrl.hinf.K(index.Iomega, index.Iz)); title(sprintf('\\bfseries %s Controller', ctrl.hinf.Name), ... 'interpreter', 'latex'); legend('$x \rightarrow \alpha_1$', ... '$x \rightarrow \alpha_2$', ... '$y \rightarrow \alpha_1$', ... '$y \rightarrow \alpha_2$', ... '$x \rightarrow \omega$', ... '$y \rightarrow \omega$', ... '$z \rightarrow \omega$', ... 'interpreter', 'latex'); grid on; end fprintf('Simulating closed loop...\n'); T = 60; nsamples = 5000; do_noise = false; simout = uav_sim_step(params, model, ctrl.hinf, uncert, nsamples, T, do_plots, do_noise); fprintf(' - Writing simulation results...\n'); cols = [ simout.StepX(:, simout.index.Position), ... simout.StepX(:, simout.index.Velocity), ... simout.StepX(:, simout.index.PlotAlpha) * 180 / pi, ... simout.StepX(:, simout.index.EulerAngles) * 180 / pi]; writematrix([simout.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') end %% ------------------------------------------------------------------------ % Perform mu-Analysis & DK iteration drawnow; if do_musyn fprintf('Generating system model for mu-synthesis design...\n'); model = uav_model(params, perf_musyn, uncert); fprintf('Performing mu-synthesis controller design...\n') % Get complete system (without debugging outputs for plots) 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 omega_max = 2.5; omega_min = -3.5; nsamples = (omega_max - omega_min) * 100; omega = logspace(omega_min, omega_max, nsamples); omega_range = {10^omega_min, 10^omega_max}; % Initial values for D-K iteration are identity matrices 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)); % Maximum degree of D-scales and error d_scales_max_degree = 4; d_scales_max_err = 15; d_scales_max_err_p = .1; % in percentage d_scales_improvement_p = .5; % in percentage, avoid diminishing returns % Limit order of scaled plant scaled_plant_reduce_ord = 100; scaled_plant_reduce_maxerr = 1e-8; % for plotting later mu_plot_legend = {}; % For warm start K_prev = {}; gamma_prev = {}; K_prev{1} = 0; gamma_prev{1} = inf; mu_bounds_rp_prev = {}; mu_bounds_rs_prev = {}; D_left_prev = {}; D_right_prev = {}; % Start DK-iteration warmit = 0; % Do we have a lower bound for gamma? gamma_max = 20; gamma_min = .8; % Maximum number of D-K iterations niters = 4; fprintf(' - Will do (max) %d iterations.\n', niters); %% Run D-K iteration % hand tuned degrees, inf means will pick automatically best fit % according to parameters given above d_scales_degrees = { 0, 0, 0, 0, 0, inf, inf; % alpha 1, 1, 3, 3, 0, inf, inf; % omega 0, 1, 0, 0, 0, inf, inf; % state 0, 0, 0, 0, 0, inf, inf; % perf }; if size(d_scales_degrees, 2) < niters error('Number of columns of d_scales_degrees must match niter'); end % if iterations fails set warmit and re-run this code section if warmit > 0 fprintf("\n"); fprintf(" - Warm start, resuming from iteration %d.\n", warmit); K = K_prev{warmit}; gamma_max = gamma_prev{warmit}; mu_bounds_rp = mu_bounds_rp_prev{warmit}; mu_bounds_rs = mu_bounds_rs_prev{warmit}; D_left = D_left_prev{warmit}; D_right = D_right_prev{warmit}; s = warmit; else s = 1; end dkstart = tic; for it = s:niters fprintf('\n'); fprintf(' * Running D-K iteration %d / %d...\n', it, niters); itstart = tic(); % Scale plant and reduce order, fitting the D-scales adds a lot of near % zero modes that cause the plant to become very numerically ill % conditioned. Since the D-scales are an approximation anyways (i.e. % there is not mathematical proof for the fitting step), we limit the % order of the scaled system to prevent poor scaling issues. P_scaled = minreal(D_left * P * inv(D_right), [], false); [P_scaled, ~] = prescale(P_scaled, omega_range); n = size(P_scaled.A, 1); if false && n > scaled_plant_reduce_ord R = reducespec(P_scaled, 'balanced'); % or ncf R.Options.FreqIntervals = [omega_range{1}, omega_range{2}]; R.Options.Goal = 'absolute'; % better hinf norm if do_plots figure(102); view(R); drawnow; end P_scaled = getrom(R, MaxError=scaled_plant_reduce_maxerr, Method='truncate'); fprintf(' Scaled plant was reduced from order %d to %d\n', ... n, size(P_scaled.A, 1)) % update n n = size(P_scaled.A, 1); end % For higher number of states we run into numerical problems if n < 70 % Scaled plant (whole) [nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(P_scaled); fprintf(' Scaled plant has %d modes:\n', nx); fprintf(' %d stable, %d unstable\n', nsta, nx - nsta); fprintf(' %d controllable, %d unstabilizable\n', nctrb, nustab); fprintf(' %d observable, %d undetectable\n', nobsv, nudetb); % Check scaled plant parts meet H-infinity assumptions A = P_scaled(idx.OutputUncertain, idx.InputUncertain); Bu = P_scaled(idx.OutputUncertain, idx.InputNominal); Cy = P_scaled(idx.OutputNominal, idx.InputUncertain); [~, ~, ~, A_nustab, ~, A_nudetb] = pbhtest(A); [~, ~, ~, Bu_nustab, ~, Bu_nudetb] = pbhtest(Bu); [~, ~, ~, Cy_nustab, ~, Cy_nudetb] = pbhtest(Cy); Deu = P_scaled(idx.OutputError, idx.InputNominal); Dyw = P_scaled(idx.OutputNominal, idx.InputExogenous); Deu_fullrank = rank(ctrb(Deu)) < length(Deu.A) || rank(obsv(Deu)) < lenth(Deu.A); Dyw_fullrank = rank(ctrb(Dyw)) < length(Dyw.A) || rank(obsv(Dyw)) < lenth(Dyw.A); if A_nustab > 0 || A_nudetb > 0 fprintf(' A has %d unstabilizable modes and %d undetectable modes!\n', ... A_nustab, A_nudetb); end if Bu_nustab > 0 || Bu_nudetb > 0 fprintf(' Bu has %d unstabilizable modes and %d undetectable modes!\n', ... Bu_nustab, Bu_nudetb); end if Cy_nustab > 0 || Cy_nudetb > 0 fprintf(' Cy has %d unstabilizable modes and %d undetectable modes!\n', ... Cy_nustab, Cy_nudetb); end if ~ Deu_fullrank fprintf(' Deu is not full rank!\n'); end if ~ Dyw_fullrank fprintf(' Dyw is not full rank!\n'); end else eigvals = eig(P_scaled.A); stable_modes = 0; for i = 1:n if real(eigvals(i)) < 0 stable_modes = stable_modes +1; end end fprintf(' Scaled plant has %d modes: %d stable %d unstable.\n', ... n, stable_modes, n - stable_modes); end % Find controller using H-infinity if it > 1 && it ~= warmit K_prev{it} = K; gamma_prev{it} = gamma; end [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt); % fprintf(' Running H-infinity with gamma in [%g, %g]\n', gamma_min, gamma_max); % [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, [gamma_min, gamma_max], hinfopt); % gamma_max = 1.2 * gamma; fprintf(' H-infinity synthesis gamma: %g\n', gamma); if gamma == inf error('Failed to synethesize H-infinity controller'); end % Calculate frequency response of closed loop N = minreal(lft(P, K), [], false); M = minreal(N(idx.OutputUncertain, idx.InputUncertain), [], false); [N, ~] = prescale(N, omega_range); [M, ~] = prescale(M, omega_range); N_frd = frd(N, omega); M_frd = frd(M, omega); % if warm start we do not need to recompute this % as we are fiddling with D-scales if it == warmit fprintf(" Warm start, using SSVs from iteration %d\n", warmit); else % Calculate upper bound D scaling fprintf(' Computing Performance SSV... ') [mu_bounds_rp, mu_info_rp] = mussv(N_frd, model.uncertain.BlockStructurePerf, 'U'); fprintf(' Computing Stability SSV... ') [mu_bounds_rs, mu_info_rs] = mussv(M_frd, model.uncertain.BlockStructure, 'fU'); mu_bounds_rp_prev{it} = mu_bounds_rp; mu_bounds_rs_prev{it} = mu_bounds_rs; % Plot SSVs if do_plots fprintf(' Plotting SSV mu\n'); figure(100); hold on; bodemag(mu_bounds_rp(1,1)); mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{P,%d}$', it)}; bodemag(mu_bounds_rs(1,1), 'k:'); mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{S,%d}$', it)}; title('\bfseries $\mu_\Delta(\omega)$ for both Stability and Performance', ... 'interpreter', 'latex'); legend(mu_plot_legend, 'interpreter', 'latex'); grid on; drawnow; end end mu_rp = norm(mu_bounds_rp(1,1), inf, 1e-6); mu_rs = norm(mu_bounds_rs(1,1), inf, 1e-6); fprintf(' SSV for Performance: %g, for Stability: %g\n', mu_rp, mu_rs); % Are we done yet? if mu_rp < 1 fprintf(' - Found robust controller that meets performance.\n'); break; end if mu_rs < 1 fprintf(' Found robust controller that is stable.\n') ctrl.musyn = struct('Name', '$\mu$-Synthesis', 'K', K, ... 'mu_rp', mu_rp, 'mu_rs', mu_rs); end % No need to find scales for last iteration if it == niters break; end % Fit D-scales [D_left_frd, D_right_frd] = mussvunwrap(mu_info_rp); fprintf(' Fitting D-scales\n'); % There are three complex, square, full block uncertainties and % a non-square full complex block for performance i_alpha = [1, 1]; i_omega = model.uncertain.BlockStructure(1, :) + 1; % after first block i_state = sum(model.uncertain.BlockStructure(1:2, :)) + 1; % after second block i_perf = sum(model.uncertain.BlockStructurePerf(1:3, :)) + 1; % after third block D_frd = { D_left_frd(i_alpha(1), i_alpha(1)); D_left_frd(i_omega(1), i_omega(1)); D_left_frd(i_state(1), i_state(1)); D_left_frd(i_perf(1), i_perf(1)); }; D_max_sv = { max(max(sigma(D_frd{1, 1}))); max(max(sigma(D_frd{2, 1}))); max(max(sigma(D_frd{3, 1}))); max(max(sigma(D_frd{4, 1}))); }; D_names = {'alpha', 'omega', 'state', 'perf'}; D_fitted = {}; % for each block for j = 1:4 fprintf(' %s', D_names{j}); % tuned by hand? if d_scales_degrees{j, it} < inf % D_fit = fitmagfrd(D_frd{j}, d_scales_degrees{j, it}); D_fit = fitfrd(genphase(D_frd{j}), d_scales_degrees{j, it}); max_sv = max(max(sigma(D_fit, omega))); fit_err = abs(D_max_sv{j} - max_sv); D_fitted{j} = D_fit; fprintf(' tuned degree %d, error %g (%g %%)\n', ... d_scales_degrees{j, it}, fit_err, ... 100 * fit_err / D_max_sv{j}); else % find best degree best_fit_deg = inf; best_fit_err = inf; for deg = 0:d_scales_max_degree % Fit D-scale % D_fit = fitmagfrd(D_frd{j}, deg); D_fit = fitfrd(genphase(D_frd{j}), deg); % Check if it is a good fit max_sv = max(max(sigma(D_fit, omega))); fit_err = abs(D_max_sv{j} - max_sv); if fit_err < best_fit_err % Choose higher degree only if we improve by at least a % specified percentage over the previous best fit (or we are % at the first iteration). This is a heuristic to prevent % adding too many states to the controller as it depends on % the order of the D-scales. improvement = abs(best_fit_err - fit_err); improvement_p = improvement / best_fit_err; if improvement_p > d_scales_improvement_p ... || best_fit_err == inf best_fit_deg = deg; best_fit_err = fit_err; D_fitted{j} = D_fit; end end if fit_err / D_max_sv{j} < d_scales_max_err_p ... || fit_err < d_scales_max_err break; end fprintf('.'); end fprintf(' degree %d, error %g (%g %%)\n', ... best_fit_deg, best_fit_err, 100 * best_fit_err / D_max_sv{j}); end end % Construct full matrices D_left_prev{it} = D_left; D_right_prev{it} = D_right; D_left = blkdiag(D_fitted{1} * eye(4), ... % alpha uncert D_fitted{2} * eye(1), ... % omega uncert D_fitted{3} * eye(9), ... % state uncert D_fitted{4} * eye(14), ... eye(12)); D_right = blkdiag(D_fitted{1} * eye(4), ... % alpha uncert D_fitted{2} * eye(1), ... % omega uncert D_fitted{3} * eye(9), ... % state uncert D_fitted{4} * eye(10), ... eye(5)); % Compute peak of singular values for to estimate how good is the % approximation of the D-scales sv_left_frd = sigma(D_left_frd); max_sv_left_frd = max(max(sv_left_frd)); sv_left = sigma(D_left, omega); max_sv_left = max(max(sv_left)); d_fit_err = abs(max_sv_left_frd - max_sv_left); fprintf(' Max SVD of D: %g, Dhat: %g\n', max_sv_left_frd, max_sv_left); fprintf(' D scales fit abs. error: %g\n', d_fit_err) fprintf(' D scales fit rel. error: %g %%\n', ... 100 * d_fit_err / max_sv_left_frd); % Plot fitted D-scales if do_plots fprintf(' Plotting D-scales'); f = figure(101); clf(f); hold on; bodemag(D_frd{1}, omega, 'r-'); bodemag(D_fitted{1}, omega, 'b'); fprintf('.'); bodemag(D_frd{2}, omega, 'r--'); bodemag(D_fitted{2}, omega, 'b--'); fprintf('.'); bodemag(D_frd{3}, omega, 'c-'); bodemag(D_fitted{3}, omega, 'm-'); fprintf('.'); bodemag(D_frd{4}, omega, 'c--'); bodemag(D_fitted{4}, 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 && mu_rs > 1 error(' - Failed to synthesize robust controller that meets the desired performance.'); end %% Fit worst-case perturbation fprintf(' - Computing worst case perturbation.\n') % Find peak of mu [mu_upper_bound_rp, ~] = frdata(mu_bounds_rp(1,1)); max_mu_rp_idx = find(mu_rp == mu_upper_bound_rp, 1); Delta = mussvunwrap(mu_info_rp); % TODO: finish here % Save controller ctrl.musyn = struct('Name', '$\mu$-Synthesis', ... 'K', K, 'Delta', Delta, ... 'mu_rp', mu_rp, 'mu_rs', mu_rs); if mu_rp >= 1 fprintf(' - Synthetized robust stable controller does not meet the desired performance.\n'); end %% ------------------------------------------------------------------------ % Measure Performance of mu synthesis design index = struct( ... 'Ix', 1, 'Iy', 2, 'Iz', 3, ... 'IPdot', (4:6)', ... 'Iroll', 7, 'Ipitch', 8, 'Iyaw', 9, ... 'ITheta', (10:12)', ... 'Ialpha', (1:4)', ... 'Iomega', 5 ... ); if do_plots fprintf(' - Plotting resulting controller...\n'); % Plot transfer functions figure; hold on; bode(ctrl.musyn.K(index.Ialpha(1), index.Ix)); bode(ctrl.musyn.K(index.Ialpha(2), index.Ix)); bode(ctrl.musyn.K(index.Ialpha(1), index.Iy)); bode(ctrl.musyn.K(index.Ialpha(2), index.Iy)); bode(ctrl.musyn.K(index.Iomega, index.Ix)); bode(ctrl.musyn.K(index.Iomega, index.Iy)); bode(ctrl.musyn.K(index.Iomega, index.Iz)); title(sprintf('\\bfseries %s Controller', ctrl.musyn.Name), ... 'interpreter', 'latex'); legend('$x \rightarrow \alpha_1$', ... '$x \rightarrow \alpha_2$', ... '$y \rightarrow \alpha_1$', ... '$y \rightarrow \alpha_2$', ... '$x \rightarrow \omega$', ... '$y \rightarrow \omega$', ... '$z \rightarrow \omega$', ... 'interpreter', 'latex'); grid on; end fprintf('Simulating nominal closed loop...\n'); T = 60; nsamples = 5000; do_noise = false; simout = uav_sim_step(params, model, ctrl.musyn, uncert, nsamples, T, do_plots, do_noise); fprintf('Simulating worst-case closed loop...\n'); end