diff options
Diffstat (limited to 'uav.m')
| -rw-r--r-- | uav.m | 187 |
1 files changed, 136 insertions, 51 deletions
@@ -9,11 +9,15 @@ clear; clc; close all; s = tf('s'); % Flags to speed up running for debugging -do_plots = true; +do_plots = true; % runs faster without do_lqr = false; % unused -do_hinf = true; % midterm +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 @@ -41,15 +45,21 @@ params = uav_params(); % ------------------------------------------------------------------------ %% Define performance requirements -if do_hinf | do_musyn +if do_hinf fprintf('Generating performance requirements...\n') - perf = uav_performance(params, do_plots); + 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 -if do_musyn +% 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 @@ -119,33 +129,48 @@ if do_musyn fprintf('Performing mu-synthesis controller design...\n') idx = model.uncertain.index; - P = model.uncertain.StateSpace([idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ... - [idx.InputUncertain; idx.InputExogenous; idx.InputNominal]); - - % Initial values for D-K iteration - D_left = eye(model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny); - D_right = eye(model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu); + 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', 'on', 'Method', 'RIC', ... - 'AutoScale', 'off', 'RelTol', 1e-3); + hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... + 'AutoScale', 'on', 'RelTol', 1e-2); % Number of D-K iterations - niters = 4; + niters = 8; % Frequency raster resolution to fit D scales - nsamples = 51; - omega = logspace(-2, 2, nsamples); + nsamples = 61; + omega = logspace(-2, 3, nsamples); + + % Initial values for D-K iteration + nleft = model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny; + nleft_clp = model.uncertain.Nz + model.uncertain.Ne; + + nright = model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu; + nright_clp = model.uncertain.Nv + model.uncertain.Nw; + + D_left = tf(eye(nleft)); + D_right = tf(eye(nright)); + + last_mu_rp = inf; + mu_plot_legend = {}; % Start DK-iteration for it = 1:niters fprintf(' - Running D-K iteration %d ...\n', it); % Find controller using H-infinity - [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl); + [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 @@ -156,43 +181,103 @@ if do_musyn 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 - [dsysl, dsysr] = mussvunwrap(mu_info); - D_left = fitfrd(genphase(dsysl(1,1)), 1); - D_right = fitfrd(genphase(dsysr(1,1)), 1); + % 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, 2); + + % 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, 3); + + % 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, 5); + + % 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, 2); + + % 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); + bodemag(D_alpha, omega); + fprintf('.'); + + bodemag(D_left_samples_omega, omega); + bodemag(D_omega, omega); + fprintf('.'); + + bodemag(D_left_samples_state, omega); + bodemag(D_state, omega); + fprintf('.'); + + bodemag(D_left_samples_perf, omega); + bodemag(D_perf, omega); + 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 + end + + 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 - -% 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 - -% vim: ts=2 sw=2 et: |