diff options
| -rw-r--r-- | uav.m | 81 |
1 files changed, 41 insertions, 40 deletions
@@ -8,9 +8,7 @@ clear; clc; close all; s = tf('s'); -% Flags to speed up running for debugging do_plots = true; % runs faster without -do_lqr = false; % unused do_hinf = false; % midterm do_musyn = true; % endterm @@ -23,9 +21,6 @@ fprintf('Will do:\n') if do_plots fprintf(' - Produce plots\n') end -if do_lqr - fprintf(' - LQR synthesis\n') -end if do_hinf fprintf(' - H-infinity synthesis\n') end @@ -71,14 +66,6 @@ fprintf('Generating system model...\n'); model = uav_model(params, perf, uncert); % ------------------------------------------------------------------------ -%% Perform LQR design - -if do_lqr - fprintf('Performing LQR controller design...\n') - ctrl.lqr = uav_ctrl_lqr(params, model); -end - -% ------------------------------------------------------------------------ %% Perform H-infinity design if do_hinf @@ -140,29 +127,34 @@ if do_musyn hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... 'AutoScale', 'on', 'RelTol', 1e-2); - % Number of D-K iterations - niters = 8; - % Frequency raster resolution to fit D scales - nsamples = 61; - omega = logspace(-2, 3, nsamples); + nsamples = 501; + omega = logspace(-3, 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; + 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)); - nright = model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu; - nright_clp = model.uncertain.Nv + model.uncertain.Nw; + % 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 + ]; - D_left = tf(eye(nleft)); - D_right = tf(eye(nright)); + % Number of D-K iterations + niters = size(fit_degrees, 2); + % niters = 5; - last_mu_rp = inf; + % for plotting later mu_plot_legend = {}; % Start DK-iteration + dkstart = tic; for it = 1:niters - fprintf(' - Running D-K iteration %d ...\n', it); + 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); @@ -206,22 +198,26 @@ if do_musyn % 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_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, 3); + % 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, 5); + % 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, 2); + % 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), ... @@ -238,27 +234,27 @@ if do_musyn % Plot fitted D-scales if do_plots - fprintf(' Plotting D-scales '); + fprintf(' Plotting D-scales'); f = figure(101); clf(f); hold on; - bodemag(D_left_samples_alpha, omega); - bodemag(D_alpha, omega); + bodemag(D_left_samples_alpha, omega, 'r-'); + bodemag(D_alpha, omega, 'b'); fprintf('.'); - bodemag(D_left_samples_omega, omega); - bodemag(D_omega, omega); + bodemag(D_left_samples_omega, omega, 'r--'); + bodemag(D_omega, omega, 'b--'); fprintf('.'); - bodemag(D_left_samples_state, omega); - bodemag(D_state, omega); + bodemag(D_left_samples_state, omega, 'c-'); + bodemag(D_state, omega, 'm-'); fprintf('.'); - bodemag(D_left_samples_perf, omega); - bodemag(D_perf, omega); + 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), ... + title(sprintf('\\bfseries $D(\\omega)$ Scales Approximations at Iteration %d', it), ... 'interpreter', 'latex') legend(... '$D_{\alpha}$', '$\hat{D}_{\alpha}$', ... @@ -270,7 +266,12 @@ if do_musyn 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'); |