diff options
Diffstat (limited to 'uav.m')
| -rw-r--r-- | uav.m | 229 |
1 files changed, 133 insertions, 96 deletions
@@ -10,11 +10,7 @@ clear; clc; close all; s = tf('s'); do_plots = true; % runs faster without do_hinf = true; % midterm -do_musyn = false; % endterm - -if do_hinf & do_musyn - error('Cannot do both H-infinity and mu synthesis.') -end +do_musyn = true; % endterm fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n') fprintf('Will do:\n') @@ -40,24 +36,14 @@ 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 +fprintf('Generating performance requirements...\n') +perf = uav_performance_musyn(params, do_plots); %% ------------------------------------------------------------------------ % Define stability requirements -% Note: for hinf it is needed to call uav_model, 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 +fprintf('Generating stability requirements...\n') +uncert = uav_uncertainty(params, do_plots); %% ------------------------------------------------------------------------ % Create UAV model @@ -81,38 +67,42 @@ if do_hinf nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; - hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ... + hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... 'AutoScale', 'off', 'RelTol', 1e-3); [K_inf, ~, gamma, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt); + fprintf(' - H-infinity synthesis gamma: %g\n', gamma); ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_inf); if gamma >= 1 - fprintf('Failed to syntesize controller (closed loop is unstable).\n') + error('Failed to syntesize controller (closed loop is unstable).') end %% ------------------------------------------------------------------------ % Measure Performance of H-infinity design - fprintf('Simulating closed loop...\n'); + fprintf(' - Simulating closed loop...\n'); + T = 60; nsamples = 500; do_noise = true; - simout = uav_sim_step_hinf(params, model, ctrl.hinf, nsamples, do_plots, do_noise); + simout = uav_sim_step(params, model, ctrl.hinf, nsamples, T, do_plots, do_noise); - fprintf('Writing simulation results...\n'); + 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') + writematrix([simout.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') end %% ------------------------------------------------------------------------ % Perform mu-Analysis & DK iteration if do_musyn + drawnow; + fprintf('Performing mu-synthesis controller design...\n') % Get complete system (without debugging outputs for plots) @@ -125,31 +115,29 @@ if do_musyn % Options for H-infinity nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; - hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ... - 'AutoScale', 'on', 'RelTol', 1e-1); + hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... + 'AutoScale', 'on', 'RelTol', 1e-3); % Frequency raster resolution to fit D scales - nsamples = 800; - omega = logspace(-1, 3, nsamples); + nsamples = 600; + omega_max = 3; + omega_min = -3; + + omega = logspace(omega_min, omega_max, nsamples); + omega_range = {10^omega_min, 10^omega_max}; - % Initial values for D-K iteration + % 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)); - % degrees for approximations for D-scales, tuned by hand - fit_degrees = [ - 1, 1, 1, 1, 1, 1; % alpha - 1, 1, 1, 1, 1, 1; % omega - 1, 1, 1, 1, 1, 1; % state - 6, 1, 4, 2, 2, 1; % perf - ]; + % Maximum number of D-K iterations + niters = 5; + fprintf(' - Will do at most %d iterations.\n', niters); - % Number of D-K iterations - niters = size(fit_degrees, 2); - - % Maximum degree of D-scales - d_scales_max_err = .01;% in percentage - d_scales_max_degree = 8; + % Maximum degree of D-scales and error + d_scales_max_degree = 3; + d_scales_max_err_p = .4; % in percentage + d_scales_improvement_p = .15; % in percentage % for plotting later mu_plot_legend = {}; @@ -157,21 +145,26 @@ if do_musyn % Start DK-iteration dkstart = tic; for it = 1:niters - fprintf(' - Running D-K iteration %d / %d...\n', it, niters); + fprintf(' * Running D-K iteration %d / %d...\n', it, niters); itstart = tic(); % Find controller using H-infinity - [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl, hinfopt); + P_scaled = minreal(D_left * P * inv(D_right), [], false); + [P_scaled, ~] = prescale(P_scaled, omega_range); + [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt); + fprintf(' H-infinity synthesis gamma: %g\n', gamma); if gamma == inf - fprintf(' Failed to synethesize H-infinity controller\n'); - break; + error('Failed to synethesize H-infinity controller'); end % Calculate frequency response of closed loop - N = minreal(lft(P, K), [], false); % slient + 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); @@ -187,13 +180,13 @@ if do_musyn fprintf(' SSV for Performance: %g, for Stability: %g\n', mu_rp, mu_rs); if do_plots - fprintf(' Plotting mu\n'); + 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), '-.'); + 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', ... @@ -206,109 +199,123 @@ if do_musyn % Are we done yet? if mu_rp < 1 fprintf(' - Found robust controller that meets performance.\n'); - break + 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 % 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_rp); + [D_left_frd, D_right_frd] = mussvunwrap(mu_info_rp); fprintf(' Fitting D-scales\n'); - i_alpha = 1; - i_omega = model.uncertain.BlockStructure(1, 1) + 1; % after first block - i_state = model.uncertain.BlockStructure(2, 1) + 1; % after second block - i_perf = model.uncertain.BlockStructurePerf(3, 1) + 1; % after third block - - D_samples = { - D_left_samples(i_alpha, i_alpha); - D_left_samples(i_omega, i_omega); - D_left_samples(i_state, i_state); - D_left_samples(i_perf, i_perf); + % 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_samples{1}))); - max(max(sigma(D_samples{2}))); - max(max(sigma(D_samples{3}))); - max(max(sigma(D_samples{4}))); + 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 + % for each in left and right fprintf(' %s', D_names{j}); - best_fit_deg = inf; best_fit_err = inf; - - for deg = fit_degrees(j, it):d_scales_max_degree + for deg = 0:d_scales_max_degree % Fit D-scale - % D_fit = fitmagfrd(D_samples{j}, deg); - D_fit = fitfrd(genphase(D_samples{j}), deg); + 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 - best_fit_deg = deg; - best_fit_err = fit_err; - D_fitted{j} = D_fit; + % 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 avoid adding too many states + % to the controller as it depends on the order of the D-scales. + if abs(best_fit_err - fit_err) / best_fit_err > 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 + if (fit_err / D_max_sv{j} < d_scales_max_err_p) break; end fprintf('.'); end - fprintf(' degree %d, error %g\n', best_fit_deg, best_fit_err); + fprintf(' degree %d, error %g (%g %%)\n', ... + best_fit_deg, best_fit_err, 100 * best_fit_err / D_max_sv{j}); end % Construct full matrices - D_right = blkdiag(D_fitted{1} * eye(4), ... - D_fitted{2} * eye(1), ... - D_fitted{3} * eye(12), ... - D_fitted{4} * eye(10), ... - eye(5)); - D_left = blkdiag(D_fitted{1} * eye(4), ... D_fitted{2} * eye(1), ... D_fitted{3} * eye(12), ... D_fitted{4} * eye(14), ... eye(12)); + D_right = blkdiag(D_fitted{1} * eye(4), ... + D_fitted{2} * eye(1), ... + D_fitted{3} * eye(12), ... + 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_samples = sigma(D_left_samples); - max_sv_left_samples = max(max(sv_left_samples)); + 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)); - fprintf(' Max SVD of D: %g, Dhat: %g\n', max_sv_left_samples, max_sv_left); - fprintf(' D scales fit abs. error: %g\n', abs(max_sv_left_samples - max_sv_left)); + fprintf(' Max SVD of D: %g, Dhat: %g\n', max_sv_left_frd, max_sv_left); + fprintf(' D scales fit rel. error: %g %%\n', ... + 100 * abs(max_sv_left_frd - max_sv_left) / max_sv_left_frd); % Plot fitted D-scales if do_plots fprintf(' Plotting D-scales'); f = figure(101); clf(f); hold on; - bodemag(D_samples{1}, omega, 'r-'); + bodemag(D_frd{1}, omega, 'r-'); bodemag(D_fitted{1}, omega, 'b'); fprintf('.'); - bodemag(D_samples{2}, omega, 'r--'); + bodemag(D_frd{2}, omega, 'r--'); bodemag(D_fitted{2}, omega, 'b--'); fprintf('.'); - bodemag(D_samples{3}, omega, 'c-'); + bodemag(D_frd{3}, omega, 'c-'); bodemag(D_fitted{3}, omega, 'm-'); fprintf('.'); - bodemag(D_samples{4}, omega, 'c--'); + bodemag(D_frd{4}, omega, 'c--'); bodemag(D_fitted{4}, omega, 'm--'); fprintf('.'); @@ -332,9 +339,39 @@ if do_musyn 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); + if mu_rp > 1 && mu_rs > 1 + error(' - Failed to synthesize robust controller that meets the desired performance.\n'); + 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 + + fprintf('Simulating nominal closed loop...\n'); + + T = 60; + nsamples = 5000; + do_noise = true; + + simout = uav_sim_step(params, model, ctrl.musyn, nsamples, T, do_plots, do_noise); + + fprintf('Simulating worst-case closed loop...\n'); + end |