diff options
| -rw-r--r-- | uav.m | 219 |
1 files changed, 168 insertions, 51 deletions
@@ -70,8 +70,8 @@ if do_hinf 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); + [K_inf, ~, gamma_hinf, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt); + fprintf(' - H-infinity synthesis gamma: %g\n', gamma_hinf); index = struct( ... 'Ix', 1, 'Iy', 2, 'Iz', 3, ... @@ -88,7 +88,7 @@ if do_hinf 'index', index ... ); - if gamma >= 1 + if gamma_hinf >= 1 error('Failed to syntesize controller (closed loop is unstable).') end @@ -139,12 +139,11 @@ if do_hinf writematrix([simout.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') end - %% ------------------------------------------------------------------------ % Perform mu-Analysis & DK iteration +drawnow; if do_musyn - drawnow; fprintf('Performing mu-synthesis controller design...\n') @@ -158,13 +157,13 @@ if do_musyn % Options for H-infinity nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; - hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... + hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ... 'AutoScale', 'on', 'RelTol', 1e-2); % Frequency raster resolution to fit D scales - nsamples = 300; omega_max = 3; - omega_min = -3; + omega_min = -2; + nsamples = (omega_max - omega_min) * 100; omega = logspace(omega_min, omega_max, nsamples); omega_range = {10^omega_min, 10^omega_max}; @@ -178,32 +177,68 @@ if do_musyn fprintf(' - Will do (max) %d iterations.\n', niters); % Maximum degree of D-scales and error - d_scales_max_degree = 10; - d_scales_max_err_p = .2; % in percentage - d_scales_improvement_p = .1; % in percentage, avoid diminishing returns + d_scales_max_degree = 4; + d_scales_max_err = 15; + d_scales_max_err_p = .1; % in percentage + d_scales_improvement_p = .5; % in percentage, avoid diminishing returns + + % Limit order of scaled plant + scaled_plant_reduce_ord = 100; + scaled_plant_reduce_maxerr = 1e-8; + + % for plotting later + mu_plot_legend = {}; + + % For warm start + K_prev = {}; gamma_prev = {}; + K_prev{1} = 0; gamma_prev{1} = inf; + + mu_bounds_rp_prev = {}; mu_bounds_rs_prev = {}; + D_left_prev = {}; D_right_prev = {}; + + % Start DK-iteration + warmit = 0; + + % Do we have a lower bound for gamma? + gamma_max = 20; + gamma_min = .8; + + %% Run D-K iteration % hand tuned degrees, inf means will pick automatically best fit % according to parameters given above d_scales_degrees = { - 0, 0, inf, inf, inf; % alpha - 1, inf, inf, inf, inf; % omega - 2, inf, inf, inf, inf; % state - 0, 0, inf, inf, inf; + 0, 0, 0, inf, inf; % alpha + 0, inf, inf, inf, inf; % omega + 0, inf, inf, inf, inf; % state + 0, 0, 0, inf, inf; % perf }; if size(d_scales_degrees, 2) < niters - error(''); + error('Number of columns of d_scales_degrees must match niter'); end + + % if iterations fails set warmit and re-run this code section + if warmit > 0 + fprintf("\n"); + fprintf(" - Warm start, resuming from iteration %d.\n", warmit); - scaled_plant_reduce_ord = 100; - scaled_plant_reduce_maxerr = 1e-3; + K = K_prev{warmit}; + gamma_max = gamma_prev{warmit}; + + mu_bounds_rp = mu_bounds_rp_prev{warmit}; + mu_bounds_rs = mu_bounds_rs_prev{warmit}; - % for plotting later - mu_plot_legend = {}; + D_left = D_left_prev{warmit}; + D_right = D_right_prev{warmit}; + + s = warmit; + else + s = 1; + end - % Start DK-iteration dkstart = tic; - for it = 1:niters + for it = s:niters fprintf(' * Running D-K iteration %d / %d...\n', it, niters); itstart = tic(); @@ -216,7 +251,8 @@ if do_musyn [P_scaled, ~] = prescale(P_scaled, omega_range); n = size(P_scaled.A, 1); - if n > scaled_plant_reduce_ord + % disabled + if false && n > scaled_plant_reduce_ord R = reducespec(P_scaled, 'balanced'); % or ncf R.Options.FreqIntervals = [omega_range{1}, omega_range{2}]; R.Options.Goal = 'absolute'; % better hinf norm @@ -225,15 +261,76 @@ if do_musyn view(R); drawnow; end - P_scaled = getrom(R, MaxError=scaled_plant_reduce_maxerr); + P_scaled = getrom(R, MaxError=scaled_plant_reduce_maxerr, Method='truncate'); fprintf(' Scaled plant was reduced from order %d to %d\n', ... n, size(P_scaled.A, 1)) end + % Check scaled plant meets H-infinity assumptions + A = P_scaled(idx.OutputUncertain, idx.InputUncertain); + Bu = P_scaled(idx.OutputUncertain, idx.InputNominal); + Cy = P_scaled(idx.OutputNominal, idx.InputUncertain); + + [~, ~, ~, nustab, ~, nudetb] = pbhtest(A); + if nustab > 0 || nudetb > 0 + fprintf(' Scaled plant has unstabilizable or undetectable A.\n'); + end + + [~, ~, ~, nustab, ~, nudetb] = pbhtest(Bu); + if nustab > 0 || nudetb > 0 + fprintf(' Scaled plant has unstabilizable or undetectable Bu.\n'); + end + + [~, ~, ~, nustab, ~, nudetb] = pbhtest(Cy); + if nustab > 0 || nudetb > 0 + fprintf(' Scaled plant has unstabilizable or undetectable Cy.\n'); + end + + Deu = P_scaled(idx.OutputError, idx.InputNominal); + Dyw = P_scaled(idx.OutputNominal, idx.InputExogenous); + + if rank(ctrb(Deu)) < length(Deu.A) || rank(obsv(Deu)) < lenth(Deu.A) + fprintf(' Scaled plant has Deu that is not full rank.\n'); + end + + if rank(ctrb(Dyw)) < length(Dyw.A) || rank(obsv(Dyw)) < lenth(Dyw.A) + fprintf(' Scaled plant has Dyw that is not full rank.\n'); + end + + % Scaled pant + eigvals = eig(P_scaled.A); + stable_modes = 0; + for i = 1:n + if real(eigvals(i)) < 0 + stable_modes = stable_modes +1; + end + end + + % For higher number of states we get numerical problems + if n < 100 + ctrb_modes = rank(ctrb(P_scaled)); + obsv_modes = rank(obsv(P_scaled)); + + fprintf(' Scaled plant has %d modes: %d ctrb %d obsv %d stable %d unstable.\n', ... + n, ctrb_modes, obsv_modes, stable_modes, n - stable_modes); + else + fprintf(' Scaled plant has %d modes: %d stable %d unstable.\n', ... + n, stable_modes, n - stable_modes); + end + % Find controller using H-infinity - [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt); + if it > 1 && it ~= warmit + K_prev{it} = K; + gamma_prev{it} = gamma; + end + + gamma_max=inf; + fprintf(' Running H-infinity with gamma in [%g, %g]\n', ... + gamma_min, gamma_max); + [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, [gamma_min, gamma_max], hinfopt); + gamma_max = 2 * gamma; fprintf(' H-infinity synthesis gamma: %g\n', gamma); if gamma == inf error('Failed to synethesize H-infinity controller'); @@ -249,34 +346,44 @@ if do_musyn N_frd = frd(N, omega); M_frd = frd(M, omega); - % Calculate upper bound D scaling - 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'); + % if warm start we do not need to recompute this + % as we are fiddling with D-scales + if it == warmit + fprintf(" Warm start, using SSVs from iteration %d\n", warmit); + else + % Calculate upper bound D scaling + 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, 'fU'); + + mu_bounds_rp_prev{it} = mu_bounds_rp; + mu_bounds_rs_prev{it} = mu_bounds_rs; + + % Plot SSVs + if do_plots + 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), 'k:'); + 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'); + grid on; + drawnow; + end + end 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 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), 'k:'); - 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'); - grid on; - drawnow; - end - % Are we done yet? if mu_rp < 1 fprintf(' - Found robust controller that meets performance.\n'); @@ -320,7 +427,6 @@ if do_musyn % for each block for j = 1:4 - % for each in left and right fprintf(' %s', D_names{j}); % tuned by hand? @@ -356,14 +462,20 @@ if do_musyn % at the first iteration). This is a heuristic to prevent % 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 + improvement = abs(best_fit_err - fit_err); + improvement_p = improvement / best_fit_err; + + if improvement_p > 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_p) + if fit_err / D_max_sv{j} < d_scales_max_err_p ... + || fit_err < d_scales_max_err break; end fprintf('.'); @@ -374,6 +486,9 @@ if do_musyn end % Construct full matrices + D_left_prev{it} = D_left; + D_right_prev{it} = D_right; + D_left = blkdiag(D_fitted{1} * eye(4), ... D_fitted{2} * eye(1), ... D_fitted{3} * eye(12), ... @@ -393,10 +508,12 @@ if do_musyn sv_left = sigma(D_left, omega); max_sv_left = max(max(sv_left)); + d_fit_err = abs(max_sv_left_frd - max_sv_left); fprintf(' Max SVD of D: %g, Dhat: %g\n', max_sv_left_frd, max_sv_left); + fprintf(' D scales fit abs. error: %g\n', d_fit_err) fprintf(' D scales fit rel. error: %g %%\n', ... - 100 * abs(max_sv_left_frd - max_sv_left) / max_sv_left_frd); + 100 * d_fit_err / max_sv_left_frd); % Plot fitted D-scales if do_plots |