diff options
Diffstat (limited to 'uav.m')
| -rw-r--r-- | uav.m | 73 |
1 files changed, 64 insertions, 9 deletions
@@ -9,7 +9,7 @@ clear; clc; close all; s = tf('s'); do_plots = true; % runs faster without -do_hinf = true; +do_hinf = false; do_musyn = true; fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n') @@ -134,7 +134,7 @@ if do_hinf simout.StepX(:, simout.index.PlotAlpha) * 180 / pi, ... simout.StepX(:, simout.index.EulerAngles) * 180 / pi]; - writematrix([simout.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') + writematrix([simout.Time', cols], 'fig/stepsim_hinf.dat', 'Delimiter', 'tab') end %% ------------------------------------------------------------------------ % Perform mu-Analysis & DK iteration @@ -208,7 +208,7 @@ if do_musyn % according to parameters given above d_scales_degrees = { 0, 0, 0, 0, 0, inf, inf; % alpha - inf, inf, inf, inf, inf, inf, inf; % omega + 5, 1, inf, inf, inf, inf, inf; % omega 0, 0, 0, 0, 0, inf, inf; % state 0, 0, 0, 0, 0, inf, inf; % perf }; @@ -346,6 +346,10 @@ if do_musyn error('Failed to synethesize H-infinity controller'); end + if it == 1 + K_hinf = K; + end + % Calculate frequency response of closed loop N = minreal(lft(P, K), [], false); M = minreal(N(idx.OutputUncertain, idx.InputUncertain), [], false); @@ -370,6 +374,12 @@ if do_musyn mu_bounds_rp_prev{it} = mu_bounds_rp; mu_bounds_rs_prev{it} = mu_bounds_rs; + % Save for worst case perturbation analysis + if it == 1 + mu_bounds_rp_first = mu_bounds_rp; + mu_info_rp_first = mu_info_rp; + end + % Plot SSVs if do_plots fprintf(' Plotting SSV mu\n'); @@ -578,13 +588,43 @@ if do_musyn %% Fit worst-case perturbation fprintf(' - Computing worst case perturbation.\n') - % Find peak of mu - [mu_upper_bound_rp, ~] = frdata(mu_bounds_rp(1,1)); - max_mu_rp_idx = find(mu_rp == mu_upper_bound_rp, 1); - Delta = mussvunwrap(mu_info_rp); - % TODO: finish here + % Find worst case perturbation on first controller + max_mus = max(frdata(mu_bounds_rp_first)); + max_idx = find(max_mus == max(max_mus)); + max_idx = max_idx(1); + + Delta_frd = mussvunwrap(mu_info_rp_first); + Delta_frd = frdata(Delta_frd); + Delta_frd = Delta_frd(:, :, max_idx); + + % Fit a plant + Delta = ss(zeros(model.uncertain.Nv, model.uncertain.Nz)); + s = tf('s'); + + for r = 1:model.uncertain.Nv + for c = 1:model.uncertain.Nz + d = Delta_frd(r, c); + g = abs(d); + + if g < 1e-6 + continue + end + + if imag(d) > 0 + d = -1 * d; + g = -1 * g; + end + x = real(d) / abs(g); + tau = 2 * omega(max_idx) * (sqrt((1 + x) / (1 - x))); + Delta(r,c) = g * (-s + tau / 2) / (s + tau / 2); + end + end + + Delta = Delta / norm(Delta, inf); + + % Save controllers + ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_hinf); - % Save controller ctrl.musyn = struct('Name', '$\mu$-Synthesis', ... 'K', K, 'Delta', Delta, ... 'mu_rp', mu_rp, 'mu_rs', mu_rs); @@ -640,7 +680,22 @@ if do_musyn do_noise = false; simout = uav_sim_step(params, model, ctrl.musyn, uncert, 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.PlotAlpha) * 180 / pi, ... + % simout.StepX(:, simout.index.EulerAngles) * 180 / pi]; + % + % writematrix([simout.Time', cols], 'fig/stepsim_musyn.dat', 'Delimiter', 'tab') + + %% Worst case analysis fprintf('Simulating worst-case closed loop...\n'); + uncert.Delta = ctrl.musyn.Delta; + simout = uav_sim_step(params, model, ctrl.musyn, uncert, nsamples, T, do_plots, do_noise); + simout = uav_sim_step(params, model, ctrl.hinf, uncert, nsamples, 5, do_plots, do_noise); + uncert = rmfield(uncert, 'Delta'); end |