diff options
Diffstat (limited to 'uav.m')
| -rw-r--r-- | uav.m | 197 |
1 files changed, 110 insertions, 87 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; +do_musyn = true; fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n') fprintf('Will do:\n') @@ -47,15 +47,12 @@ fprintf('Generating stability requirements...\n') uncert = uav_uncertainty(params, do_plots); %% ------------------------------------------------------------------------ -% Create UAV model - -fprintf('Generating system model...\n'); -model = uav_model(params, perf_musyn, uncert); - -%% ------------------------------------------------------------------------ % Perform H-infinity design if do_hinf + fprintf('Generating system model for H-infinity design...\n'); + model = uav_model(params, perf_hinf, uncert); + fprintf('Performing H-infinty controller design...\n') idx = model.uncertain.index; @@ -125,17 +122,17 @@ if do_hinf fprintf('Simulating closed loop...\n'); - T = 40; - nsamples = 800; + T = 60; + nsamples = 5000; do_noise = false; - simout = uav_sim_step(params, model, ctrl.hinf, nsamples, T, do_plots, do_noise); + simout = uav_sim_step(params, model, ctrl.hinf, uncert, nsamples, T, do_plots, do_noise); fprintf(' - Writing simulation results...\n'); cols = [ simout.StepX(:, simout.index.Position), ... simout.StepX(:, simout.index.Velocity), ... - simout.StepX(:, simout.index.FlapAngles) * 180 / pi, ... - simout.StepX(:, simout.index.Angles) * 180 / pi]; + simout.StepX(:, simout.index.PlotAlpha) * 180 / pi, ... + simout.StepX(:, simout.index.EulerAngles) * 180 / pi]; writematrix([simout.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') end @@ -144,6 +141,8 @@ end drawnow; if do_musyn + fprintf('Generating system model for mu-synthesis design...\n'); + model = uav_model(params, perf_musyn, uncert); fprintf('Performing mu-synthesis controller design...\n') @@ -157,12 +156,12 @@ if do_musyn % Options for H-infinity nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; - hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ... + hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... 'AutoScale', 'on', 'RelTol', 1e-2); % Frequency raster resolution to fit D scales - omega_max = 3; - omega_min = -2; + omega_max = 2.5; + omega_min = -3.5; nsamples = (omega_max - omega_min) * 100; omega = logspace(omega_min, omega_max, nsamples); @@ -172,10 +171,6 @@ if do_musyn 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)); - % Maximum number of D-K iterations - niters = 5; - fprintf(' - Will do (max) %d iterations.\n', niters); - % Maximum degree of D-scales and error d_scales_max_degree = 4; d_scales_max_err = 15; @@ -203,21 +198,25 @@ if do_musyn gamma_max = 20; gamma_min = .8; + % Maximum number of D-K iterations + niters = 4; + fprintf(' - Will do (max) %d iterations.\n', niters); + %% Run D-K iteration % hand tuned degrees, inf means will pick automatically best fit % according to parameters given above d_scales_degrees = { - 0, 0, 0, inf, inf; % alpha - 0, inf, inf, inf, inf; % omega - 0, inf, inf, inf, inf; % state - 0, 0, 0, inf, inf; % perf + 0, 0, 0, 0, 0, inf, inf; % alpha + 1, 1, 3, 3, 0, inf, inf; % omega + 0, 1, 0, 0, 0, inf, inf; % state + 0, 0, 0, 0, 0, inf, inf; % perf }; if size(d_scales_degrees, 2) < niters 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"); @@ -225,7 +224,7 @@ if do_musyn 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}; @@ -239,6 +238,7 @@ if do_musyn dkstart = tic; for it = s:niters + fprintf('\n'); fprintf(' * Running D-K iteration %d / %d...\n', it, niters); itstart = tic(); @@ -250,8 +250,7 @@ if do_musyn P_scaled = minreal(D_left * P * inv(D_right), [], false); [P_scaled, ~] = prescale(P_scaled, omega_range); n = size(P_scaled.A, 1); - - % disabled + if false && n > scaled_plant_reduce_ord R = reducespec(P_scaled, 'balanced'); % or ncf R.Options.FreqIntervals = [omega_range{1}, omega_range{2}]; @@ -265,56 +264,66 @@ if do_musyn fprintf(' Scaled plant was reduced from order %d to %d\n', ... n, size(P_scaled.A, 1)) + + % update 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); + % For higher number of states we run into numerical problems + if n < 70 + % Scaled plant (whole) + [nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(P_scaled); - [~, ~, ~, nustab, ~, nudetb] = pbhtest(A); - if nustab > 0 || nudetb > 0 - fprintf(' Scaled plant has unstabilizable or undetectable A.\n'); - end + fprintf(' Scaled plant has %d modes:\n', nx); + fprintf(' %d stable, %d unstable\n', nsta, nx - nsta); + fprintf(' %d controllable, %d unstabilizable\n', nctrb, nustab); + fprintf(' %d observable, %d undetectable\n', nobsv, nudetb); - [~, ~, ~, nustab, ~, nudetb] = pbhtest(Bu); - if nustab > 0 || nudetb > 0 - fprintf(' Scaled plant has unstabilizable or undetectable Bu.\n'); - end + % Check scaled plant parts meet 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(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); + [~, ~, ~, A_nustab, ~, A_nudetb] = pbhtest(A); + [~, ~, ~, Bu_nustab, ~, Bu_nudetb] = pbhtest(Bu); + [~, ~, ~, Cy_nustab, ~, Cy_nudetb] = pbhtest(Cy); - 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 + Deu = P_scaled(idx.OutputError, idx.InputNominal); + Dyw = P_scaled(idx.OutputNominal, idx.InputExogenous); - 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 + Deu_fullrank = rank(ctrb(Deu)) < length(Deu.A) || rank(obsv(Deu)) < lenth(Deu.A); + Dyw_fullrank = rank(ctrb(Dyw)) < length(Dyw.A) || rank(obsv(Dyw)) < lenth(Dyw.A); - % Scaled pant - eigvals = eig(P_scaled.A); - stable_modes = 0; - for i = 1:n - if real(eigvals(i)) < 0 - stable_modes = stable_modes +1; + if A_nustab > 0 || A_nudetb > 0 + fprintf(' A has %d unstabilizable modes and %d undetectable modes!\n', ... + A_nustab, A_nudetb); + end + + if Bu_nustab > 0 || Bu_nudetb > 0 + fprintf(' Bu has %d unstabilizable modes and %d undetectable modes!\n', ... + Bu_nustab, Bu_nudetb); 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)); + if Cy_nustab > 0 || Cy_nudetb > 0 + fprintf(' Cy has %d unstabilizable modes and %d undetectable modes!\n', ... + Cy_nustab, Cy_nudetb); + end - 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); + if ~ Deu_fullrank + fprintf(' Deu is not full rank!\n'); + end + + if ~ Dyw_fullrank + fprintf(' Dyw is not full rank!\n'); + end else + eigvals = eig(P_scaled.A); + stable_modes = 0; + for i = 1:n + if real(eigvals(i)) < 0 + stable_modes = stable_modes +1; + end + end fprintf(' Scaled plant has %d modes: %d stable %d unstable.\n', ... n, stable_modes, n - stable_modes); end @@ -325,12 +334,12 @@ if do_musyn 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, hinfopt); + + % 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 = 1.2 * gamma; - [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'); @@ -364,13 +373,13 @@ if do_musyn 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'); @@ -396,6 +405,11 @@ if do_musyn 'mu_rp', mu_rp, 'mu_rs', mu_rs); end + % No need to find scales for last iteration + if it == niters + break; + end + % Fit D-scales [D_left_frd, D_right_frd] = mussvunwrap(mu_info_rp); @@ -428,7 +442,7 @@ if do_musyn % for each block for j = 1:4 fprintf(' %s', D_names{j}); - + % tuned by hand? if d_scales_degrees{j, it} < inf % D_fit = fitmagfrd(D_frd{j}, d_scales_degrees{j, it}); @@ -451,11 +465,11 @@ if do_musyn % 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 @@ -464,7 +478,7 @@ if do_musyn % the order of the D-scales. improvement = abs(best_fit_err - fit_err); improvement_p = improvement / best_fit_err; - + if improvement_p > d_scales_improvement_p ... || best_fit_err == inf @@ -473,7 +487,7 @@ if do_musyn D_fitted{j} = D_fit; end end - + if fit_err / D_max_sv{j} < d_scales_max_err_p ... || fit_err < d_scales_max_err break; @@ -489,15 +503,15 @@ if do_musyn 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), ... + D_left = blkdiag(D_fitted{1} * eye(4), ... % alpha uncert + D_fitted{2} * eye(1), ... % omega uncert + D_fitted{3} * eye(9), ... % state uncert D_fitted{4} * eye(14), ... eye(12)); - D_right = blkdiag(D_fitted{1} * eye(4), ... - D_fitted{2} * eye(1), ... - D_fitted{3} * eye(12), ... + D_right = blkdiag(D_fitted{1} * eye(4), ... % alpha uncert + D_fitted{2} * eye(1), ... % omega uncert + D_fitted{3} * eye(9), ... % state uncert D_fitted{4} * eye(10), ... eye(5)); @@ -581,6 +595,15 @@ if do_musyn %% ------------------------------------------------------------------------ % Measure Performance of mu synthesis design + 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 ... + ); + if do_plots fprintf(' - Plotting resulting controller...\n'); @@ -611,11 +634,11 @@ if do_musyn fprintf('Simulating nominal closed loop...\n'); - T = 40; + T = 60; nsamples = 5000; do_noise = false; - simout = uav_sim_step(params, model, ctrl.musyn, nsamples, T, do_plots, do_noise); + simout = uav_sim_step(params, model, ctrl.musyn, uncert, nsamples, T, do_plots, do_noise); fprintf('Simulating worst-case closed loop...\n'); |