diff options
Diffstat (limited to 'uav.m')
| -rw-r--r-- | uav.m | 214 |
1 files changed, 171 insertions, 43 deletions
@@ -9,7 +9,7 @@ clear; clc; close all; s = tf('s'); do_plots = true; % runs faster without -do_hinf = true; % midterm +do_hinf = false; % midterm do_musyn = true; % endterm fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n') @@ -37,7 +37,8 @@ params = uav_params(); % Define performance requirements fprintf('Generating performance requirements...\n') -perf = uav_performance_musyn(params, do_plots); +perf_hinf = uav_performance_hinf(params, do_plots); +perf_musyn = uav_performance_musyn(params, do_plots); %% ------------------------------------------------------------------------ % Define stability requirements @@ -49,7 +50,7 @@ uncert = uav_uncertainty(params, do_plots); % Create UAV model fprintf('Generating system model...\n'); -model = uav_model(params, perf, uncert); +model = uav_model(params, perf_musyn, uncert); %% ------------------------------------------------------------------------ % Perform H-infinity design @@ -71,7 +72,21 @@ if do_hinf '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); + + index = struct( ... + 'Ix', 1, 'Iy', 2, 'Iz', 3, ... + 'IPdot', (4:6)', ... + 'Iroll', 7, 'Ipitch', 8, 'Iyaw', 9, ... + 'ITheta', (10:12)', ... + 'Ialpha', (1:4)', ... + 'Iomega', 5 ... + ); + + ctrl.hinf = struct( ... + 'Name', '$\mathcal{H}_{\infty}$', ... + 'K', K_inf, ... + 'index', index ... + ); if gamma >= 1 error('Failed to syntesize controller (closed loop is unstable).') @@ -80,11 +95,39 @@ if do_hinf %% ------------------------------------------------------------------------ % Measure Performance of H-infinity design - fprintf(' - Simulating closed loop...\n'); + if do_plots + fprintf(' - Plotting resulting controller...\n'); + + % Plot transfer functions + figure; hold on; + bode(ctrl.hinf.K(index.Ialpha(1), index.Ix)); + bode(ctrl.hinf.K(index.Ialpha(2), index.Ix)); + + bode(ctrl.hinf.K(index.Ialpha(1), index.Iy)); + bode(ctrl.hinf.K(index.Ialpha(2), index.Iy)); + + bode(ctrl.hinf.K(index.Iomega, index.Ix)); + bode(ctrl.hinf.K(index.Iomega, index.Iy)); + bode(ctrl.hinf.K(index.Iomega, index.Iz)); + + title(sprintf('\\bfseries %s Controller', ctrl.hinf.Name), ... + 'interpreter', 'latex'); + legend('$x \rightarrow \alpha_1$', ... + '$x \rightarrow \alpha_2$', ... + '$y \rightarrow \alpha_1$', ... + '$y \rightarrow \alpha_2$', ... + '$x \rightarrow \omega$', ... + '$y \rightarrow \omega$', ... + '$z \rightarrow \omega$', ... + 'interpreter', 'latex'); + grid on; + end + + fprintf('Simulating closed loop...\n'); - T = 60; - nsamples = 500; - do_noise = true; + T = 40; + nsamples = 800; + do_noise = false; simout = uav_sim_step(params, model, ctrl.hinf, nsamples, T, do_plots, do_noise); fprintf(' - Writing simulation results...\n'); @@ -116,10 +159,10 @@ if do_musyn nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... - 'AutoScale', 'on', 'RelTol', 1e-3); + 'AutoScale', 'on', 'RelTol', 1e-2); % Frequency raster resolution to fit D scales - nsamples = 600; + nsamples = 300; omega_max = 3; omega_min = -3; @@ -132,12 +175,28 @@ if do_musyn % Maximum number of D-K iterations niters = 5; - fprintf(' - Will do at most %d iterations.\n', niters); + fprintf(' - Will do (max) %d iterations.\n', niters); % 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 + d_scales_max_degree = 10; + d_scales_max_err_p = .2; % in percentage + d_scales_improvement_p = .1; % in percentage, avoid diminishing returns + + % 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; + }; + + if size(d_scales_degrees, 2) < niters + error(''); + end + + scaled_plant_reduce_ord = 100; + scaled_plant_reduce_maxerr = 1e-3; % for plotting later mu_plot_legend = {}; @@ -148,9 +207,31 @@ if do_musyn fprintf(' * Running D-K iteration %d / %d...\n', it, niters); itstart = tic(); - % Find controller using H-infinity + % Scale plant and reduce order, fitting the D-scales adds a lot of near + % zero modes that cause the plant to become very numerically ill + % conditioned. Since the D-scales are an approximation anyways (i.e. + % there is not mathematical proof for the fitting step), we limit the + % order of the scaled system to prevent poor scaling issues. P_scaled = minreal(D_left * P * inv(D_right), [], false); [P_scaled, ~] = prescale(P_scaled, omega_range); + n = size(P_scaled.A, 1); + + if 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 + if do_plots + figure(102); + view(R); + drawnow; + end + P_scaled = getrom(R, MaxError=scaled_plant_reduce_maxerr); + + fprintf(' Scaled plant was reduced from order %d to %d\n', ... + n, size(P_scaled.A, 1)) + end + + % Find controller using H-infinity [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt); fprintf(' H-infinity synthesis gamma: %g\n', gamma); @@ -241,36 +322,55 @@ if do_musyn 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 = 0:d_scales_max_degree - % Fit D-scale - D_fit = fitmagfrd(D_frd{j}, deg); - % D_fit = fitfrd(genphase(D_frd{j}), deg); - - % Check if it is a good fit + + % tuned by hand? + if d_scales_degrees{j, it} < inf + % D_fit = fitmagfrd(D_frd{j}, d_scales_degrees{j, it}); + D_fit = fitfrd(genphase(D_frd{j}), d_scales_degrees{j, it}); + max_sv = max(max(sigma(D_fit, omega))); fit_err = abs(D_max_sv{j} - max_sv); - - if fit_err < best_fit_err - % 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; + D_fitted{j} = D_fit; + + fprintf(' tuned degree %d, error %g (%g %%)\n', ... + d_scales_degrees{j, it}, fit_err, ... + 100 * fit_err / D_max_sv{j}); + + else + % find best degree + best_fit_deg = inf; + best_fit_err = inf; + + for deg = 0:d_scales_max_degree + % Fit D-scale + % 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 + % 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 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 + 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) + break; + end + fprintf('.'); end - - if (fit_err / D_max_sv{j} < d_scales_max_err_p) - break; - end - fprintf('.'); + fprintf(' degree %d, error %g (%g %%)\n', ... + best_fit_deg, best_fit_err, 100 * best_fit_err / D_max_sv{j}); end - 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 @@ -340,7 +440,7 @@ if do_musyn fprintf(' - D-K iteration took %.1f seconds\n', dkend); if mu_rp > 1 && mu_rs > 1 - error(' - Failed to synthesize robust controller that meets the desired performance.\n'); + error(' - Failed to synthesize robust controller that meets the desired performance.'); end %% Fit worst-case perturbation @@ -364,11 +464,39 @@ if do_musyn %% ------------------------------------------------------------------------ % Measure Performance of mu synthesis design + if do_plots + fprintf(' - Plotting resulting controller...\n'); + + % Plot transfer functions + figure; hold on; + bode(ctrl.musyn.K(index.Ialpha(1), index.Ix)); + bode(ctrl.musyn.K(index.Ialpha(2), index.Ix)); + + bode(ctrl.musyn.K(index.Ialpha(1), index.Iy)); + bode(ctrl.musyn.K(index.Ialpha(2), index.Iy)); + + bode(ctrl.musyn.K(index.Iomega, index.Ix)); + bode(ctrl.musyn.K(index.Iomega, index.Iy)); + bode(ctrl.musyn.K(index.Iomega, index.Iz)); + + title(sprintf('\\bfseries %s Controller', ctrl.musyn.Name), ... + 'interpreter', 'latex'); + legend('$x \rightarrow \alpha_1$', ... + '$x \rightarrow \alpha_2$', ... + '$y \rightarrow \alpha_1$', ... + '$y \rightarrow \alpha_2$', ... + '$x \rightarrow \omega$', ... + '$y \rightarrow \omega$', ... + '$z \rightarrow \omega$', ... + 'interpreter', 'latex'); + grid on; + end + fprintf('Simulating nominal closed loop...\n'); - T = 60; + T = 40; nsamples = 5000; - do_noise = true; + do_noise = false; simout = uav_sim_step(params, model, ctrl.musyn, nsamples, T, do_plots, do_noise); |