diff options
Diffstat (limited to 'uav.m')
| -rw-r--r-- | uav.m | 165 |
1 files changed, 102 insertions, 63 deletions
@@ -52,7 +52,7 @@ end %% ------------------------------------------------------------------------ % Define stability requirements -% Note: for hinf it is needed to call uav_mode, but hinf will not actually +% 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') @@ -125,12 +125,12 @@ if do_musyn % Options for H-infinity nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; - hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... - 'AutoScale', 'on', 'RelTol', 1e-2); + hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ... + 'AutoScale', 'on', 'RelTol', 1e-1); % Frequency raster resolution to fit D scales - nsamples = 500; - omega = logspace(-5, 5, nsamples); + nsamples = 800; + omega = logspace(-1, 3, nsamples); % Initial values for D-K iteration D_left = tf(eye(model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny)); @@ -138,15 +138,19 @@ if do_musyn % degrees for approximations for D-scales, tuned by hand fit_degrees = [ - 1, 1; % alpha - 6, 1; % omega - 1, 1; % state - 7, 1; % perf + 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 ]; % 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; + % for plotting later mu_plot_legend = {}; @@ -166,18 +170,32 @@ if do_musyn % Calculate frequency response of closed loop N = minreal(lft(P, K), [], false); % slient + M = minreal(N(idx.OutputUncertain, idx.InputUncertain), [], false); + N_frd = frd(N, omega); + M_frd = frd(M, omega); % Calculate upper bound D scaling - [mu_bounds, mu_info] = mussv(N_frd, model.uncertain.BlockStructurePerf, 'sU'); - mu_rp = norm(mu_bounds(1,1), inf, 1e-6); - fprintf(' Mu value for RP: %g\n', mu_rp) + 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, 'U'); + + 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); 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)}; + + bodemag(mu_bounds_rp(1,1)); + mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{P,%d}$', it)}; + + bodemag(mu_bounds_rs(1,1), '-.'); + 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'); @@ -194,51 +212,72 @@ if do_musyn % 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); - - 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)); - 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)); - 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)); - 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)); - fprintf('.'); - - fprintf('\n'); + [D_left_samples, D_right_samples] = 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); + }; + + 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}))); + }; + + D_names = {'alpha', 'omega', 'state', 'perf'}; + D_fitted = {}; + + for j = 1:4 + fprintf(' %s', D_names{j}); + + best_fit_deg = inf; + best_fit_err = inf; + + for deg = fit_degrees(j, it):d_scales_max_degree + % Fit D-scale + % D_fit = fitmagfrd(D_samples{j}, deg); + D_fit = fitfrd(genphase(D_samples{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; + end + + if fit_err / D_max_sv{j} < d_scales_max_err + break; + end + fprintf('.'); + end + fprintf(' degree %d, error %g\n', best_fit_deg, best_fit_err); + end % Construct full matrices - D_right = blkdiag(D_alpha * eye(4), ... - D_omega * eye(1), ... - D_state * eye(12), ... - D_perf * eye(10), ... + 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_alpha * eye(4), ... - D_omega * eye(1), ... - D_state * eye(12), ... - D_perf * eye(14), ... + 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)); % Compute peak of singular values for to estimate how good is the @@ -257,20 +296,20 @@ if do_musyn fprintf(' Plotting D-scales'); f = figure(101); clf(f); hold on; - bodemag(D_left_samples_alpha, omega, 'r-'); - bodemag(D_alpha, omega, 'b'); + bodemag(D_samples{1}, omega, 'r-'); + bodemag(D_fitted{1}, omega, 'b'); fprintf('.'); - bodemag(D_left_samples_omega, omega, 'r--'); - bodemag(D_omega, omega, 'b--'); + bodemag(D_samples{2}, omega, 'r--'); + bodemag(D_fitted{2}, omega, 'b--'); fprintf('.'); - bodemag(D_left_samples_state, omega, 'c-'); - bodemag(D_state, omega, 'm-'); + bodemag(D_samples{3}, omega, 'c-'); + bodemag(D_fitted{3}, omega, 'm-'); fprintf('.'); - bodemag(D_left_samples_perf, omega, 'c--'); - bodemag(D_perf, omega, 'm--'); + bodemag(D_samples{4}, omega, 'c--'); + bodemag(D_fitted{4}, omega, 'm--'); fprintf('.'); fprintf('\n'); |