diff options
Diffstat (limited to 'uav.m')
| -rw-r--r-- | uav.m | 91 |
1 files changed, 54 insertions, 37 deletions
@@ -9,8 +9,8 @@ clear; clc; close all; s = tf('s'); do_plots = true; % runs faster without -do_hinf = false; % midterm -do_musyn = true; % endterm +do_hinf = true; % midterm +do_musyn = false; % endterm if do_hinf & do_musyn error('Cannot do both H-infinity and mu synthesis.') @@ -31,14 +31,14 @@ end % Synthesized controllers will be stored here ctrl = struct(); -% ------------------------------------------------------------------------ -%% Define system parameters +%% ------------------------------------------------------------------------ +% Define system parameters fprintf('Generating system parameters...\n') params = uav_params(); -% ------------------------------------------------------------------------ -%% Define performance requirements +%% ------------------------------------------------------------------------ +% Define performance requirements if do_hinf fprintf('Generating performance requirements...\n') @@ -49,8 +49,8 @@ if do_musyn perf = uav_performance_musyn(params, do_plots); end -% ------------------------------------------------------------------------ -%% Define stability requirements +%% ------------------------------------------------------------------------ +% Define stability requirements % Note: for hinf it is needed to call uav_mode, but hinf will not actually % make use of this struct @@ -59,14 +59,14 @@ if do_hinf | do_musyn uncert = uav_uncertainty(params, do_plots); end -% ------------------------------------------------------------------------ -%% Create UAV model +%% ------------------------------------------------------------------------ +% Create UAV model fprintf('Generating system model...\n'); model = uav_model(params, perf, uncert); -% ------------------------------------------------------------------------ -%% Perform H-infinity design +%% ------------------------------------------------------------------------ +% Perform H-infinity design if do_hinf fprintf('Performing H-infinty controller design...\n') @@ -90,8 +90,8 @@ if do_hinf fprintf('Failed to syntesize controller (closed loop is unstable).\n') end -% ------------------------------------------------------------------------ -%% Measure Performance of H-infinity design +%% ------------------------------------------------------------------------ +% Measure Performance of H-infinity design fprintf('Simulating closed loop...\n'); @@ -109,12 +109,13 @@ if do_hinf writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') end -% ------------------------------------------------------------------------ -%% Perform mu-Analysis & DK iteration +%% ------------------------------------------------------------------------ +% Perform mu-Analysis & DK iteration if do_musyn 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], ... @@ -128,24 +129,23 @@ if do_musyn 'AutoScale', 'on', 'RelTol', 1e-2); % Frequency raster resolution to fit D scales - nsamples = 501; - omega = logspace(-3, 3, nsamples); + nsamples = 500; + omega = logspace(-5, 5, 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 + % degrees for approximations for 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 + 1, 1; % alpha + 6, 1; % omega + 1, 1; % state + 7, 1; % perf ]; % Number of D-K iterations niters = size(fit_degrees, 2); - % niters = 5; % for plotting later mu_plot_legend = {}; @@ -153,7 +153,7 @@ if do_musyn % Start DK-iteration dkstart = tic; for it = 1:niters - fprintf(' - Running D-K iteration %d...\n', it); + fprintf(' - Running D-K iteration %d / %d...\n', it, niters); itstart = tic(); % Find controller using H-infinity @@ -178,7 +178,8 @@ if do_musyn 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'); + title('\bfseries $\mu_\Delta(\omega)$ for both Stability and Performance', ... + 'interpreter', 'latex'); legend(mu_plot_legend, 'interpreter', 'latex'); grid on; drawnow; @@ -195,29 +196,37 @@ if do_musyn % a non-square full complex block for performance [D_left_samples, D_right_samples] = mussvunwrap(mu_info); + fprintf(' Fitting D-scales'); + % 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_alpha = fitmagfrd(D_left_samples_alpha, fit_degrees(1, it)); + % D_alpha = fitfrd(genphase(D_left_samples_alpha), fit_degrees(1, it)); + fprintf('.'); % 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_omega = fitmagfrd(D_left_samples_omega, fit_degrees(2, it)); + % D_omega = fitfrd(genphase(D_left_samples_omega), fit_degrees(2, it)); + fprintf('.'); % 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_state = fitmagfrd(D_left_samples_state, fit_degrees(3, it)); + % D_state = fitfrd(genphase(D_left_samples_state), fit_degrees(3, it)); + fprintf('.'); % 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)); + D_perf = fitmagfrd(D_left_samples_perf, fit_degrees(4, it)); + % D_perf = fitfrd(genphase(D_left_samples_perf), fit_degrees(4, it)); + fprintf('.'); + + fprintf('\n'); % Construct full matrices D_right = blkdiag(D_alpha * eye(4), ... @@ -232,6 +241,17 @@ if do_musyn D_perf * eye(14), ... eye(12)); + % 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 = 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)); + % Plot fitted D-scales if do_plots fprintf(' Plotting D-scales'); @@ -279,6 +299,3 @@ if do_musyn ctrl.musyn = struct('K', K, 'mu', mu_rp); end end - -% ------------------------------------------------------------------------ -%% Verify performance satisfaction via mu-analysis |