diff options
| -rw-r--r-- | uav.m | 91 | ||||
| -rw-r--r-- | uav_performance_hinf.m | 6 | ||||
| -rw-r--r-- | uav_performance_musyn.m | 41 | ||||
| -rw-r--r-- | uav_uncertainty.m | 20 |
4 files changed, 83 insertions, 75 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; % midterm +do_musyn = false; % endterm if do_hinf & do_musyn error('Cannot do both H-infinity and mu synthesis.') @@ -31,14 +31,14 @@ end % Synthesized controllers will be stored here ctrl = struct(); -% ------------------------------------------------------------------------ -%% Define system parameters +%% ------------------------------------------------------------------------ +% Define system parameters fprintf('Generating system parameters...\n') params = uav_params(); -% ------------------------------------------------------------------------ -%% Define performance requirements +%% ------------------------------------------------------------------------ +% Define performance requirements if do_hinf fprintf('Generating performance requirements...\n') @@ -49,8 +49,8 @@ if do_musyn perf = uav_performance_musyn(params, do_plots); end -% ------------------------------------------------------------------------ -%% Define stability requirements +%% ------------------------------------------------------------------------ +% Define stability requirements % Note: for hinf it is needed to call uav_mode, but hinf will not actually % make use of this struct @@ -59,14 +59,14 @@ if do_hinf | do_musyn uncert = uav_uncertainty(params, do_plots); end -% ------------------------------------------------------------------------ -%% Create UAV model +%% ------------------------------------------------------------------------ +% Create UAV model fprintf('Generating system model...\n'); model = uav_model(params, perf, uncert); -% ------------------------------------------------------------------------ -%% Perform H-infinity design +%% ------------------------------------------------------------------------ +% Perform H-infinity design if do_hinf fprintf('Performing H-infinty controller design...\n') @@ -90,8 +90,8 @@ if do_hinf fprintf('Failed to syntesize controller (closed loop is unstable).\n') end -% ------------------------------------------------------------------------ -%% Measure Performance of H-infinity design +%% ------------------------------------------------------------------------ +% Measure Performance of H-infinity design fprintf('Simulating closed loop...\n'); @@ -109,12 +109,13 @@ if do_hinf writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') end -% ------------------------------------------------------------------------ -%% Perform mu-Analysis & DK iteration +%% ------------------------------------------------------------------------ +% Perform mu-Analysis & DK iteration if do_musyn fprintf('Performing mu-synthesis controller design...\n') + % Get complete system (without debugging outputs for plots) idx = model.uncertain.index; P = minreal(model.uncertain.StateSpace(... [idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ... @@ -128,24 +129,23 @@ if do_musyn 'AutoScale', 'on', 'RelTol', 1e-2); % Frequency raster resolution to fit D scales - nsamples = 501; - omega = logspace(-3, 3, nsamples); + nsamples = 500; + omega = logspace(-5, 5, nsamples); % Initial values for D-K iteration 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)); - % degrees for approximations of D-scales, tuned by hand + % degrees for approximations for D-scales, tuned by hand fit_degrees = [ - 2, 1, 1, 1; % 1, 1; % alpha - 2, 4, 1, 1; % 1, 1; % omega - 1, 2, 1, 1; % 1, 1; % state - 3, 4, 1, 1; % 1, 1; % perf + 1, 1; % alpha + 6, 1; % omega + 1, 1; % state + 7, 1; % perf ]; % Number of D-K iterations niters = size(fit_degrees, 2); - % niters = 5; % for plotting later mu_plot_legend = {}; @@ -153,7 +153,7 @@ if do_musyn % Start DK-iteration dkstart = tic; for it = 1:niters - fprintf(' - Running D-K iteration %d...\n', it); + fprintf(' - Running D-K iteration %d / %d...\n', it, niters); itstart = tic(); % Find controller using H-infinity @@ -178,7 +178,8 @@ if do_musyn figure(100); hold on; bodemag(mu_bounds(1,1)); mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{%d}$', it)}; - title('\bfseries $\mu_\Delta(\omega)$ for both Stability and Performance', 'interpreter', 'latex'); + title('\bfseries $\mu_\Delta(\omega)$ for both Stability and Performance', ... + 'interpreter', 'latex'); legend(mu_plot_legend, 'interpreter', 'latex'); grid on; drawnow; @@ -195,29 +196,37 @@ if do_musyn % 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)); + 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)); + 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)); + 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)); + 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'); % Construct full matrices D_right = blkdiag(D_alpha * eye(4), ... @@ -232,6 +241,17 @@ if do_musyn D_perf * eye(14), ... eye(12)); + % Compute peak of singular values for to estimate how good is the + % approximation of the D-scales + sv_left_samples = sigma(D_left_samples); + max_sv_left_samples = max(max(sv_left_samples)); + + sv_left = sigma(D_left, omega); + max_sv_left = max(max(sv_left)); + + fprintf(' Max SVD of D: %g, Dhat: %g\n', max_sv_left_samples, max_sv_left); + fprintf(' D scales fit abs. error: %g\n', abs(max_sv_left_samples - max_sv_left)); + % Plot fitted D-scales if do_plots fprintf(' Plotting D-scales'); @@ -279,6 +299,3 @@ if do_musyn ctrl.musyn = struct('K', K, 'mu', mu_rp); end end - -% ------------------------------------------------------------------------ -%% Verify performance satisfaction via mu-analysis diff --git a/uav_performance_hinf.m b/uav_performance_hinf.m index 6ece761..0a12b72 100644 --- a/uav_performance_hinf.m +++ b/uav_performance_hinf.m @@ -83,7 +83,8 @@ if do_plots '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ... '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ... 'interpreter', 'latex', 'fontSize', 8); - title('Performance Requirements'); + title('\bfseries Performance Requirements ($\mathcal{H}_\infty$ Weights)', ... + 'interpreter', 'latex'); % Step response of position requirements figure; hold on; @@ -96,7 +97,8 @@ if do_plots '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ... '$W_{P,\alpha}$', '$W_{P,\omega}$', ... 'interpreter', 'latex', 'fontSize', 8); - title('Step responses of performance requirements'); + title('\bfseries Step responses of $\mathcal{H}_\infty$ Weights', ... + 'interpreter', 'latex'); end end diff --git a/uav_performance_musyn.m b/uav_performance_musyn.m index 6ece761..180be8a 100644 --- a/uav_performance_musyn.m +++ b/uav_performance_musyn.m @@ -13,44 +13,27 @@ % PERF Struct performance transfer functions -function [perf] = uav_performance(params, do_plots) +function [perf] = uav_performance_musyn(params, do_plots) % Laplace variable s = tf('s'); -alpha_max = params.actuators.ServoAbsMaxAngle; -alpha_max_omega = params.actuators.ServoNominalAngularVelocity; +% TODO: set proper constraints +G = 1e2 / (s + 1e2); -T_xy = params.performance.HorizontalSettleTime; -T_z = params.performance.VerticalSettleTime; +W_Palpha = G; +W_Pomega = G; -omega_nxy = 5 / T_xy; -omega_nz = 10 / T_z; +W_Pxy = G; +W_Pz = G; -% W_Palpha = 1 / (s^2 + 2 * alpha_max_omega * s + alpha_max_omega^2); -% W_Palpha = (1 - W_Palpha / dcgain(W_Palpha)) * .8; -% W_Pomega = (1 - 1 / (T_z / 5 * s + 1)) * .1; - -W_Palpha = make_weight(alpha_max_omega, 15, 1.1, 3); -W_Pomega = make_weight(omega_nz, 50, 10); - -% zeta = 1; % Almost critically damped -% W_Pxy = 1 / (s^2 + 2 * zeta * omega_nxy * s + omega_nxy^2); -% W_Pxy = 1 * W_Pxy / dcgain(W_Pxy); -% W_Pz = 1 / (s^2 + 2 * zeta * omega_nz * s + omega_nz^2); -% W_Pz = 1 * W_Pz / dcgain(W_Pz); - -W_Pxy = make_weight(omega_nxy, 2, 5); -W_Pz = make_weight(omega_nz, 1, 10); - -% Set a speed limit -W_Pxydot = .2 * tf(1, 1); -W_Pzdot = .2 * tf(1, 1); +W_Pxydot = tf(0); +W_Pzdot = tf(0); -W_Pphitheta = .01 * tf(1, [.1, 1]); -W_Ppsi = .01 * tf(1, 1); % don't care +W_Pphitheta = tf(0); +W_Ppsi = tf(0); -W_PTheta = tf(1, [.1, 1]) * eye(3); +W_PTheta = tf(0); % Construct performance vector by combining xy and z W_PP = blkdiag(W_Pxy * eye(2), W_Pz); diff --git a/uav_uncertainty.m b/uav_uncertainty.m index 6f28d8d..aff30ca 100644 --- a/uav_uncertainty.m +++ b/uav_uncertainty.m @@ -28,13 +28,18 @@ eps_d = params.aerodynamics.DragCoefficientsUncertainties(1); eps_omega = max(.5 * eps_T, eps_r); eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega); +% TODO: set proper constraints % band pass parameters for W_malpha -wh = 20; % high freq -wl = .1; % low freq +% wh = 20; % high freq +% wl = .1; % low freq -W_malpha = eps_alpha * (s / wl) / ((s / wh + 1) * (s / wl + 1)); -W_momega = eps_omega * 10 / (s + 10); -W_mState = .01 * 10 / (s + 10); +% W_malpha = eps_alpha * (s / wl) / ((s / wh + 1) * (s / wl + 1)); +% W_momega = eps_omega * 10 / (s + 10); +% W_mState = .01 * 10 / (s + 10); + +W_malpha = eps_alpha * tf(1); +W_momega = tf(0); +W_mState = .01 * tf(1); uncert = struct(... 'FlapAngle', W_malpha * eye(4), ... @@ -50,8 +55,9 @@ if do_plots grid on; legend('$W_{m,\alpha}$', '$W_{m,\omega}$', '$W_{m,\mathbf{P}}$', ... - 'interpreter', 'latex') - title('Stability (Uncertainty) Requirement') + 'interpreter', 'latex') + title('\bfseries Stability Requirement (only for $\mu$-Synthesis)', ... + 'interpreter', 'latex') end end |