diff options
| -rw-r--r-- | uav.m | 187 | ||||
| -rw-r--r-- | uav_model.m | 6 | ||||
| -rw-r--r-- | uav_params.m | 10 | ||||
| -rw-r--r-- | uav_performance_hinf.m (renamed from uav_performance.m) | 4 | ||||
| -rw-r--r-- | uav_performance_musyn.m | 123 | ||||
| -rw-r--r-- | uav_sim_step_musyn.m | 282 | ||||
| -rw-r--r-- | uav_uncertainty.m | 38 |
7 files changed, 588 insertions, 62 deletions
@@ -9,11 +9,15 @@ clear; clc; close all; s = tf('s'); % Flags to speed up running for debugging -do_plots = true; +do_plots = true; % runs faster without do_lqr = false; % unused -do_hinf = true; % midterm +do_hinf = false; % midterm do_musyn = true; % endterm +if do_hinf & do_musyn + error('Cannot do both H-infinity and mu synthesis.') +end + fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n') fprintf('Will do:\n') if do_plots @@ -41,15 +45,21 @@ params = uav_params(); % ------------------------------------------------------------------------ %% Define performance requirements -if do_hinf | do_musyn +if do_hinf fprintf('Generating performance requirements...\n') - perf = uav_performance(params, do_plots); + perf = uav_performance_hinf(params, do_plots); +end +if do_musyn + fprintf('Generating performance requirements...\n') + perf = uav_performance_musyn(params, do_plots); end % ------------------------------------------------------------------------ %% Define stability requirements -if do_musyn +% Note: for hinf it is needed to call uav_mode, but hinf will not actually +% make use of this struct +if do_hinf | do_musyn fprintf('Generating stability requirements...\n') uncert = uav_uncertainty(params, do_plots); end @@ -119,33 +129,48 @@ if do_musyn fprintf('Performing mu-synthesis controller design...\n') idx = model.uncertain.index; - P = model.uncertain.StateSpace([idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ... - [idx.InputUncertain; idx.InputExogenous; idx.InputNominal]); - - % Initial values for D-K iteration - D_left = eye(model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny); - D_right = eye(model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu); + P = minreal(model.uncertain.StateSpace(... + [idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ... + [idx.InputUncertain; idx.InputExogenous; idx.InputNominal]), ... + [], false); % Options for H-infinity nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; - hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ... - 'AutoScale', 'off', 'RelTol', 1e-3); + hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... + 'AutoScale', 'on', 'RelTol', 1e-2); % Number of D-K iterations - niters = 4; + niters = 8; % Frequency raster resolution to fit D scales - nsamples = 51; - omega = logspace(-2, 2, nsamples); + nsamples = 61; + omega = logspace(-2, 3, nsamples); + + % Initial values for D-K iteration + nleft = model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny; + nleft_clp = model.uncertain.Nz + model.uncertain.Ne; + + nright = model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu; + nright_clp = model.uncertain.Nv + model.uncertain.Nw; + + D_left = tf(eye(nleft)); + D_right = tf(eye(nright)); + + last_mu_rp = inf; + mu_plot_legend = {}; % Start DK-iteration for it = 1:niters fprintf(' - Running D-K iteration %d ...\n', it); % Find controller using H-infinity - [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl); + [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl, hinfopt); fprintf(' H-infinity synthesis gamma: %g\n', gamma); + if gamma == inf + fprintf(' Failed to synethesize H-infinity controller\n'); + break; + end % Calculate frequency response of closed loop N = minreal(lft(P, K), [], false); % slient @@ -156,43 +181,103 @@ if do_musyn mu_rp = norm(mu_bounds(1,1), inf, 1e-6); fprintf(' Mu value for RP: %g\n', mu_rp) + if do_plots + fprintf(' Plotting mu\n'); + 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'); + 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'); + break + end + % Fit D-scales - [dsysl, dsysr] = mussvunwrap(mu_info); - D_left = fitfrd(genphase(dsysl(1,1)), 1); - D_right = fitfrd(genphase(dsysr(1,1)), 1); + % There are three complex, square, full block uncertainties and + % a non-square full complex block for performance + [D_left_samples, D_right_samples] = mussvunwrap(mu_info); + + % 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, 2); + + % 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, 3); + + % 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, 5); + + % 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, 2); + + % Construct full matrices + D_right = blkdiag(D_alpha * eye(4), ... + D_omega * eye(1), ... + D_state * eye(12), ... + D_perf * eye(10), ... + eye(5)); + + D_left = blkdiag(D_alpha * eye(4), ... + D_omega * eye(1), ... + D_state * eye(12), ... + D_perf * eye(14), ... + eye(12)); + + % Plot fitted D-scales + if do_plots + fprintf(' Plotting D-scales '); + f = figure(101); clf(f); hold on; + + bodemag(D_left_samples_alpha, omega); + bodemag(D_alpha, omega); + fprintf('.'); + + bodemag(D_left_samples_omega, omega); + bodemag(D_omega, omega); + fprintf('.'); + + bodemag(D_left_samples_state, omega); + bodemag(D_state, omega); + fprintf('.'); + + bodemag(D_left_samples_perf, omega); + bodemag(D_perf, omega); + fprintf('.'); + + fprintf('\n'); + title(sprintf('\bfseries $D(\\omega)$ Scales Approximations at Iteration %d', it), ... + 'interpreter', 'latex') + legend(... + '$D_{\alpha}$', '$\hat{D}_{\alpha}$', ... + '$D_{\omega}$', '$\hat{D}_{\omega}$', ... + '$D_{\mathbf{x}}$', '$\hat{D}_{\mathbf{x}}$', ... + '$D_{\Delta}$', '$\hat{D}_{\Delta}$', ... + 'interpreter', 'latex' ... + ); + grid on; + drawnow; + end + end + + if mu_rp > 1 + fprintf(' - Failed to synthesize robust controller that meets the desired performance.\n'); + else + ctrl.musyn = struct('K', K, 'mu', mu_rp); end end % ------------------------------------------------------------------------ %% Verify performance satisfaction via mu-analysis - -% omega = logspace(-3, 3, 250); -% -% N_inf = lft(P, K_inf); -% N_inf_frd = frd(N_inf, omega); -% -% % robust stability -% [mu_inf_rs, ~] = mussv(N_inf_frd(idx.OutputUncertain, idx.InputUncertain), ... -% model.uncertain.BlockStructure); -% -% % robust performance -% blk_perf = [model.uncertain.BlockStructure; -% model.uncertain.BlockStructurePerf]; -% -% [mu_inf_rp, ~] = mussv(N_inf_frd, blk_perf); -% -% % nominal performance -% mu_inf_np = svd(N_inf_frd(idx.OutputError, idx.InputExogenous)); -% -% if do_plots -% figure; hold on; -% bodemag(mu_inf_rs(1)); -% bodemag(mu_inf_np(1)); -% bodemag(mu_inf_rs(2)); -% bodemag(mu_inf_np(2)); -% -% grid on; -% title('$\mu_\Delta(N)$', 'Interpreter', 'latex'); -% end - -% vim: ts=2 sw=2 et: diff --git a/uav_model.m b/uav_model.m index 0ec9a67..3fc1d0a 100644 --- a/uav_model.m +++ b/uav_model.m @@ -318,14 +318,14 @@ usys = ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d); % Specify uncertainty block structure for mussv command blk_stab = [ - 4, 0; % alpha uncert, diagonal - 1, 0; % omega uncert, diagonal + 4, 4; % alpha uncert, full + 1, 1; % omega uncert, full 12, 12; % state uncert, full ]; blk_perf = [ blk_stab; - 10, 14; + 10, 14 % always full ]; % Save uncertain model diff --git a/uav_params.m b/uav_params.m index da8a19b..a1c0e4a 100644 --- a/uav_params.m +++ b/uav_params.m @@ -11,6 +11,7 @@ function [params] = uav_params() % Time in s % Frequencies in Hz % Angular velocities in rad / s +% Uncertainty / measurement errors in percentage (between 0 and 1) params = struct(); @@ -44,7 +45,8 @@ params.mechanical = struct(... 'DuctHeight', 171e-3, ... 'FlapZDistance', 98e-3, ... % flap distance along z from center of mass 'InertiaTensor', J, ... - 'GyroscopicInertiaZ', J_prop ... % assume small angle + 'GyroscopicInertiaZ', J_prop, ... % assume small angle + 'GyroscopicInertiaZUncertainty', .01 ... % in % ); % ------------------------------------------------------------------------ @@ -84,9 +86,13 @@ k_T = F_max / omega_max^2; % https://scienceworld.wolfram.com/physics/LiftCoefficient.html params.aerodynamics = struct(... 'ThrustOmegaProp', k_T, ... % in s^2 / (rad * N) + 'ThrustOmegaPropUncertainty', .05, ... % in % 'FlapArea', 23e-3 * 10e-3, ... % in m^2 + 'FlapAreaUncertainty', .01, ... % in % 'DragCoefficients', [1, .1], ... % TODO - 'LiftCoefficient', 2 * pi ... % TODO + 'DragCoefficientsUncertainties', [.1, .1], ... % in % + 'LiftCoefficient', 2 * pi, ... % TODO + 'LiftCoefficientUncertainty', .1 ...% in % ); diff --git a/uav_performance.m b/uav_performance_hinf.m index 4857a34..6ece761 100644 --- a/uav_performance.m +++ b/uav_performance_hinf.m @@ -13,7 +13,7 @@ % PERF Struct performance transfer functions -function [perf] = uav_performance(params, plot) +function [perf] = uav_performance(params, do_plots) % Laplace variable s = tf('s'); @@ -64,7 +64,7 @@ perf = struct(... 'Velocity', W_PPdot, ... 'Angles', W_PTheta); -if plot +if do_plots % Bode plots of performance requirements figure; hold on; diff --git a/uav_performance_musyn.m b/uav_performance_musyn.m new file mode 100644 index 0000000..6ece761 --- /dev/null +++ b/uav_performance_musyn.m @@ -0,0 +1,123 @@ +% Generate transfer functions for loop shaping performance requirements +% from parameters specified in uav_params.m +% +% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich +% This work is distributed under a permissive license, see LICENSE.txt +% +% Arguments: +% PARAMS Struct of design parameters and constants generated by uav_params +% PLOT When set to 'true' it plots the inverse magnitude of the +% performance transfer function +% +% Return value: +% PERF Struct performance transfer functions + + +function [perf] = uav_performance(params, do_plots) + +% Laplace variable +s = tf('s'); + +alpha_max = params.actuators.ServoAbsMaxAngle; +alpha_max_omega = params.actuators.ServoNominalAngularVelocity; + +T_xy = params.performance.HorizontalSettleTime; +T_z = params.performance.VerticalSettleTime; + +omega_nxy = 5 / T_xy; +omega_nz = 10 / T_z; + +% 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_Pphitheta = .01 * tf(1, [.1, 1]); +W_Ppsi = .01 * tf(1, 1); % don't care + +W_PTheta = tf(1, [.1, 1]) * eye(3); + +% Construct performance vector by combining xy and z +W_PP = blkdiag(W_Pxy * eye(2), W_Pz); +W_PPdot = blkdiag(W_Pxydot * eye(2), W_Pzdot); +W_PTheta = blkdiag(W_Pphitheta * eye(2), W_Ppsi); + +perf = struct(... + 'FlapAngle', W_Palpha * eye(4), ... + 'Thrust', W_Pomega, ... + 'Position', W_PP, ... + 'Velocity', W_PPdot, ... + 'Angles', W_PTheta); + +if do_plots + % Bode plots of performance requirements + figure; hold on; + + bodemag(W_Palpha); + bodemag(W_Pomega); + bodemag(W_Pxy); + bodemag(W_Pz); + bodemag(W_Pxydot); + bodemag(W_Pzdot); + bodemag(W_Pphitheta); + bodemag(W_Ppsi); + + grid on; + legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ... + '$W_{P,xy}$', '$W_{P,z}$', ... + '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ... + '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ... + 'interpreter', 'latex', 'fontSize', 8); + title('Performance Requirements'); + + % Step response of position requirements + figure; hold on; + step(W_Pxy); step(W_Pz); + step(W_Pxydot); step(W_Pzdot); + step(W_Palpha); + step(W_Pomega); + grid on; + legend('$W_{P,xy}$', '$W_{P,z}$', ... + '$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'); +end + +end + +% Make a n-order performance weight function +% +% Arguments: +% OMEGA Cutting frequency (-3dB) +% A Magnitude at DC, i.e. |Wp(0)| +% M Magnitude at infinity, i.e. |Wp(inf)| +% ORD Order +function [Wp] = make_weight(omega, A, M, ord) + +if nargin > 3 + n = ord; +else + n = 1; +end + +s = tf('s'); +Wp = (s / (M^(1/n)) + omega)^n / (s + omega * A^(1/n))^n; + +end +% vim: ts=2 sw=2 et: diff --git a/uav_sim_step_musyn.m b/uav_sim_step_musyn.m new file mode 100644 index 0000000..3af2a76 --- /dev/null +++ b/uav_sim_step_musyn.m @@ -0,0 +1,282 @@ +% Simulate a step responses of ducted-fan VTOL micro-UAV. +% +% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich +% This work is distributed under a permissive license, see LICENSE.txt + +function [simout] = uav_sim_step_musyn(params, model, ctrl, nsamp, do_plots, do_noise) + +% Load closed loop model and add controller +% more or less equivalent to doing usys = lft(Pnom, K) +h = load_system('uav_model_uncertain_clp'); +hws = get_param('uav_model_uncertain_clp', 'modelworkspace'); +hws.assignin('K', ctrl.K); + +ulmod_clp = linmod('uav_model_uncertain_clp'); +usys_clp = ss(ulmod_clp.a, ulmod_clp.b, ulmod_clp.c, ulmod_clp.d); + +% Take dynamics without uncertainty +% Also nominal ouput to make plots +P_nom_clp = minreal(usys_clp(... + [model.uncertain.index.OutputError; + model.uncertain.index.OutputNominal; + model.uncertain.index.OutputPlots], ... + model.uncertain.index.InputExogenous)); + +% Indices for exogenous inputs +Iwwind = (1:3)'; +Iwalpha = (4:7)'; +Ir = (8:10)'; + +% Indices for outputs +Iealpha = (1:4)'; +Ieomega = 5; +IeP = (6:8)'; +IePdot = (9:11)'; +IeTheta = (12:14)'; +% size([Iealpha; Ieomega; IeP; IePdot; IeTheta]) + +% Indices for y outputs (for plots) +IP = (15:17)'; +IPdot = (18:20)'; +ITheta = (21:23)'; +IOmega = (24:26)'; + +% Indices for p outputs (for plots) +Ialpha = (27:30)'; +Iomega = 31; + +Iualpha = (32:35)'; +Iuomega = 36; + +noise = zeros(7, nsamp); % no noise +if do_noise + % Noise in percentage + noise_alpha_amp = (.5 * (pi / 180)) / params.actuators.ServoAbsMaxAngle; + noise_wind_amp = .1; + noise = [noise_wind_amp * randn(3, nsamp); + noise_alpha_amp * randn(4, nsamp)]; +end + +% Create step inputs (normalized) +ref_step = ones(1, nsamp); % 1d step function + +in_step_x = [ noise; ref_step; zeros(2, nsamp) ]; +in_step_y = [ noise; zeros(1, nsamp); ref_step; zeros(1, nsamp) ]; +in_step_z = [ noise; zeros(2, nsamp); ref_step ]; + +% Simulation time +n_settle_times = 10; +T_final_horiz = n_settle_times * params.performance.HorizontalSettleTime; +T_final_vert = n_settle_times * params.performance.VerticalSettleTime; + +t_xy = linspace(0, T_final_horiz, nsamp); +t_z = linspace(0, T_final_vert, nsamp); + +% Simulate step responses +out_step_x = lsim(P_nom_clp, in_step_x, t_xy); +out_step_y = lsim(P_nom_clp, in_step_y, t_xy); +out_step_z = lsim(P_nom_clp, in_step_z, t_z); + +% Return simulation +simout = struct(... + 'TimeXY', t_xy, ... + 'StepX', out_step_x, ... + 'StepY', out_step_y, ... + 'Simulink', ulmod_clp, ... + 'StateSpace', P_nom_clp); + +simout.index = struct(... + 'ErrorFlapAngles', Iealpha, ... + 'ErrorThrust', Ieomega , ... + 'ErrorPos', IeP, ... + 'ErrorVel', IePdot, ... + 'ErrorAngles', IeTheta, ... + 'Position', IP, ... + 'Velocity', IPdot, ... + 'Angles', ITheta, ... + 'FlapAngles', Ialpha, ... + 'Thruster', Iomega, ... + 'InputFlapAngles', Iualpha, ... + 'InputThruster', Iuomega); + +if do_plots + % Conversion factors + to_deg = 180 / pi; % radians to degrees + to_rpm = pi / 30; % rad / s to RPM + + % Figure for flaps and Euler angles + figure; + sgtitle(sprintf(... + '\\bfseries Step Response of Flap and Euler Angles (%s)', ... + ctrl.Name), 'Interpreter', 'latex'); + + % Plot limits + ref_value = params.performance.ReferencePosMaxDistance; + alpha_max_deg = params.actuators.ServoAbsMaxAngle * to_deg; + euler_lim_deg = 1.5; % params.performance.AngleMaxPitchRoll * to_deg; + omega_max_rpm = (params.actuators.PropellerMaxAngularVelocity ... + - params.linearization.Inputs(5)) * to_rpm; + omega_min_rpm = -params.linearization.Inputs(5) * to_rpm; + + % Plot step response from x to alpha + subplot(2, 3, 1); + hold on; + plot(t_xy, out_step_x(:, Ialpha(1)) * to_deg); + plot(t_xy, out_step_x(:, Ialpha(2)) * to_deg); + plot(t_xy, out_step_x(:, Ialpha(3)) * to_deg); + plot(t_xy, out_step_x(:, Ialpha(4)) * to_deg); + plot([0, T_final_horiz], [1, 1] * alpha_max_deg, 'r--'); + plot([0, T_final_horiz], [-1, -1] * alpha_max_deg, 'r--'); + grid on; + xlim([0, T_final_horiz]); + ylim([-alpha_max_deg * 1.1, alpha_max_deg * 1.1]); + title('Horizontal $x$ to Flaps', 'Interpreter', 'latex'); + ylabel('Flap Angle (degrees)', 'Interpreter', 'latex'); + xlabel('Time (seconds)', 'Interpreter', 'latex'); + legend('$\alpha_1(t)$', '$\alpha_2(t)$', '$\alpha_3(t)$', '$\alpha_4(t)$', ... + 'Interpreter', 'latex'); + + % Plot step response from y to alpha + subplot(2, 3, 2); hold on; + plot(t_xy, out_step_y(:, Ialpha(1)) * to_deg); + plot(t_xy, out_step_y(:, Ialpha(2)) * to_deg); + plot(t_xy, out_step_y(:, Ialpha(3)) * to_deg); + plot(t_xy, out_step_y(:, Ialpha(4)) * to_deg); + plot([0, T_final_horiz], [1, 1] * alpha_max_deg, 'r--'); + plot([0, T_final_horiz], [-1, -1] * alpha_max_deg, 'r--'); + grid on; + xlim([0, T_final_horiz]); + ylim([-alpha_max_deg * 1.1, alpha_max_deg * 1.1]); + title('Horizontal $y$ to Flaps', 'Interpreter', 'latex'); + ylabel('Flap Angle (degrees)', 'Interpreter', 'latex'); + xlabel('Time (seconds)', 'Interpreter', 'latex'); + legend('$\alpha_1(t)$', '$\alpha_2(t)$', '$\alpha_3(t)$', '$\alpha_4(t)$', ... + 'Interpreter', 'latex'); + + % Plot step response from z to alpha + subplot(2, 3, 3); hold on; + plot(t_z, out_step_z(:, Ialpha(1)) * to_deg); + plot(t_z, out_step_z(:, Ialpha(2)) * to_deg); + plot(t_z, out_step_z(:, Ialpha(3)) * to_deg); + plot(t_z, out_step_z(:, Ialpha(4)) * to_deg); + plot([0, T_final_vert], [1, 1] * alpha_max_deg, 'r--'); + plot([0, T_final_vert], [-1, -1] * alpha_max_deg, 'r--'); + grid on; + xlim([0, T_final_vert]); + ylim([-alpha_max_deg * 1.1, alpha_max_deg * 1.1]); + title('Vertical $z$ to Flaps', 'Interpreter', 'latex'); + ylabel('Flap Angle (degrees)', 'Interpreter', 'latex'); + xlabel('Time (seconds)', 'Interpreter', 'latex'); + legend('$\alpha_1(t)$', '$\alpha_2(t)$', '$\alpha_3(t)$', '$\alpha_4(t)$', ... + 'Interpreter', 'latex'); + + % Plot step response from x to Theta + subplot(2, 3, 4); hold on; + plot(t_xy, out_step_x(:, ITheta(1)) * to_deg); + plot(t_xy, out_step_x(:, ITheta(2)) * to_deg); + plot(t_xy, out_step_x(:, ITheta(3)) * to_deg); + grid on; + xlim([0, T_final_horiz]); + ylim([-euler_lim_deg, euler_lim_deg]); + title('Horizontal $x$ to Euler Angles', 'Interpreter', 'latex'); + ylabel('Euler Angle (degrees)', 'Interpreter', 'latex'); + xlabel('Time (seconds)', 'Interpreter', 'latex'); + legend('$\phi(t)$ Roll ', '$\theta(t)$ Pitch ', '$\psi(t)$ Yaw ', ... + 'Interpreter', 'latex'); + + % Plot step response from y to Theta + subplot(2, 3, 5); hold on; + plot(t_xy, out_step_y(:, ITheta(1)) * to_deg); + plot(t_xy, out_step_y(:, ITheta(2)) * to_deg); + plot(t_xy, out_step_y(:, ITheta(3)) * to_deg); + grid on; + xlim([0, T_final_horiz]); + ylim([-euler_lim_deg, euler_lim_deg]); + title('Horizontal $y$ to Euler Angles', 'Interpreter', 'latex'); + ylabel('Euler Angle (degrees)', 'Interpreter', 'latex'); + xlabel('Time (seconds)', 'Interpreter', 'latex'); + legend('$\phi(t)$ Roll ', '$\theta(t)$ Pitch ', '$\psi(t)$ Yaw ', ... + 'Interpreter', 'latex'); + + % Plot step response from z to Theta + subplot(2, 3, 6); hold on; + plot(t_z, out_step_z(:, ITheta(1)) * to_deg); + plot(t_z, out_step_z(:, ITheta(2)) * to_deg); + plot(t_z, out_step_z(:, ITheta(3)) * to_deg); + grid on; + xlim([0, T_final_vert]); + ylim([-euler_lim_deg, euler_lim_deg]); + title('Vertical $z$ to Euler Angles', 'Interpreter', 'latex'); + ylabel('Euler Angle (degrees)', 'Interpreter', 'latex'); + xlabel('Time (seconds)', 'Interpreter', 'latex'); + legend('$\phi(t)$ Roll ', '$\theta(t)$ Pitch ', '$\psi(t)$ Yaw ', ... + 'Interpreter', 'latex'); + + % Plot step response from z to omega + figure; + sgtitle(sprintf(... + '\\bfseries Step Response to Propeller (%s)', ... + ctrl.Name), 'Interpreter', 'latex'); + + hold on; + step(P_nom_clp(Iomega, Ir(3)) * to_rpm, T_final_vert); + plot([0, T_final_vert], [1, 1] * omega_min_rpm, 'r--'); + plot([0, T_final_vert], [1, 1] * omega_max_rpm, 'r--'); + grid on; + ylim([omega_min_rpm - 1, omega_max_rpm + 1]); + title('Vertical $z$ to Thruster $\omega$', 'Interpreter', 'latex'); + ylabel('Angular Velocity (RPM)', 'Interpreter', 'latex'); + xlabel('Time (seconds)', 'Interpreter', 'latex'); + legend('$\omega(t)$', 'Interpreter', 'latex'); + + % Figure for position and velocity + figure; + sgtitle(sprintf(... + '\\bfseries Step Response of Position and Speed (%s)', ... + ctrl.Name), 'Interpreter', 'latex'); + + % Plot step response from horizontal reference to horizontal position + subplot(2, 2, 1); hold on; + plot(t_xy, out_step_x(:, IP(1))); + plot(t_xy, out_step_y(:, IP(2))); + % plot([0, T_final_horiz], [1, 1] * ref_value, 'r:'); + % plot(t_xy, out_step_xydes, 'r--'); + grid on; + title('Horizontal Position Error', 'Interpreter', 'latex'); + ylabel('Error (meters)', 'Interpreter', 'latex'); + xlabel('Time (seconds)', 'Interpreter', 'latex'); + legend('$x(t)$', '$y(t)$', 'Interpreter', 'latex'); + + % Plot step response horizontal reference to horizontal speed + subplot(2, 2, 2); hold on; + plot(t_xy, out_step_x(:, IPdot(1))); + plot(t_xy, out_step_y(:, IPdot(2))); + grid on; + title('Horizontal Velocity', 'Interpreter', 'latex'); + ylabel('Velocity (m / s)', 'Interpreter', 'latex'); + xlabel('Time (seconds)', 'Interpreter', 'latex'); + legend('$\dot{x}(t)$', '$\dot{y}(t)$', 'Interpreter', 'latex'); + + % Plot step response from vertical reference to vertical position + subplot(2, 2, 3); hold on; + plot(t_z, out_step_z(:, IP(3))); + % plot([0, T_final_vert], [1, 1] * ref_value, 'r:'); + % plot(t_z, out_step_zdes, 'r--'); + grid on; + title('Vertical Position Error', 'Interpreter', 'latex'); + ylabel('Error (meters)', 'Interpreter', 'latex'); + xlabel('Time (seconds)', 'Interpreter', 'latex'); + legend('$z(t)$', 'Interpreter', 'latex'); + + % Plot step response vertical reference to vertical speed + subplot(2, 2, 4); hold on; + plot(t_z, out_step_z(:, IPdot(3))); + grid on; + title('Vertical Velocity', 'Interpreter', 'latex'); + ylabel('Velocity (m / s)', 'Interpreter', 'latex'); + xlabel('Time (seconds)', 'Interpreter', 'latex'); + legend('$\dot{z}(t)$', 'Interpreter', 'latex'); +end + +end +% vim:ts=2 sw=2 et: diff --git a/uav_uncertainty.m b/uav_uncertainty.m index e1ddf76..6f28d8d 100644 --- a/uav_uncertainty.m +++ b/uav_uncertainty.m @@ -13,16 +13,46 @@ % UNCERT Struct of uncertainty transfer functions -function [uncert] = uav_performance(params, plot) +function [uncert] = uav_performance(params, do_plots) -W_malpha = tf(1,1); -W_momega = tf(1,1); -W_mState = tf(1,1); +s = tf('s'); + +% relative errors +eps_T = params.aerodynamics.ThrustOmegaPropUncertainty; +eps_r = params.mechanical.GyroscopicInertiaZUncertainty; +eps_S = params.aerodynamics.FlapAreaUncertainty; +eps_l = params.aerodynamics.LiftCoefficientUncertainty; +eps_d = params.aerodynamics.DragCoefficientsUncertainties(1); +% eps_0 = params.aerodynamics.DragCoefficients(2); + +eps_omega = max(.5 * eps_T, eps_r); +eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega); + +% band pass parameters for W_malpha +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); uncert = struct(... 'FlapAngle', W_malpha * eye(4), ... 'Thrust', W_momega, ... 'StateLinApprox', W_mState * eye(12)); +if do_plots + % Bode plots of performance requirements + figure; hold on; + bodemag(W_malpha); + bodemag(W_momega); + bodemag(W_mState); + + grid on; + legend('$W_{m,\alpha}$', '$W_{m,\omega}$', '$W_{m,\mathbf{P}}$', ... + 'interpreter', 'latex') + title('Stability (Uncertainty) Requirement') +end + end % vim: ts=2 sw=2 et: |