diff options
| -rw-r--r-- | uav.m | 229 | ||||
| -rw-r--r-- | uav_model.m | 70 | ||||
| -rw-r--r-- | uav_model_uncertain.slx | bin | 78345 -> 56206 bytes | |||
| -rw-r--r-- | uav_model_uncertain_clp.slx | bin | 85225 -> 163944 bytes | |||
| -rw-r--r-- | uav_params.m | 66 | ||||
| -rw-r--r-- | uav_performance_musyn.m | 68 | ||||
| -rw-r--r-- | uav_sim_step.m (renamed from uav_sim_step_musyn.m) | 142 | ||||
| -rw-r--r-- | uav_sim_step_hinf.m | 282 | ||||
| -rw-r--r-- | uav_uncertainty.m | 26 |
9 files changed, 341 insertions, 542 deletions
@@ -10,11 +10,7 @@ clear; clc; close all; s = tf('s'); do_plots = true; % runs faster without do_hinf = true; % midterm -do_musyn = false; % endterm - -if do_hinf & do_musyn - error('Cannot do both H-infinity and mu synthesis.') -end +do_musyn = true; % endterm fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n') fprintf('Will do:\n') @@ -40,24 +36,14 @@ params = uav_params(); %% ------------------------------------------------------------------------ % Define performance requirements -if do_hinf - fprintf('Generating performance requirements...\n') - perf = uav_performance_hinf(params, do_plots); -end -if do_musyn - fprintf('Generating performance requirements...\n') - perf = uav_performance_musyn(params, do_plots); -end +fprintf('Generating performance requirements...\n') +perf = uav_performance_musyn(params, do_plots); %% ------------------------------------------------------------------------ % Define stability requirements -% Note: for hinf it is needed to call uav_model, 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 +fprintf('Generating stability requirements...\n') +uncert = uav_uncertainty(params, do_plots); %% ------------------------------------------------------------------------ % Create UAV model @@ -81,38 +67,42 @@ if do_hinf nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; - hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ... + hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... '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); if gamma >= 1 - fprintf('Failed to syntesize controller (closed loop is unstable).\n') + error('Failed to syntesize controller (closed loop is unstable).') end %% ------------------------------------------------------------------------ % Measure Performance of H-infinity design - fprintf('Simulating closed loop...\n'); + fprintf(' - Simulating closed loop...\n'); + T = 60; nsamples = 500; do_noise = true; - simout = uav_sim_step_hinf(params, model, ctrl.hinf, nsamples, do_plots, do_noise); + simout = uav_sim_step(params, model, ctrl.hinf, nsamples, T, do_plots, do_noise); - fprintf('Writing simulation results...\n'); + 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]; - writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') + writematrix([simout.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') end %% ------------------------------------------------------------------------ % Perform mu-Analysis & DK iteration if do_musyn + drawnow; + fprintf('Performing mu-synthesis controller design...\n') % Get complete system (without debugging outputs for plots) @@ -125,31 +115,29 @@ if do_musyn % Options for H-infinity nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; - hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ... - 'AutoScale', 'on', 'RelTol', 1e-1); + hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... + 'AutoScale', 'on', 'RelTol', 1e-3); % Frequency raster resolution to fit D scales - nsamples = 800; - omega = logspace(-1, 3, nsamples); + nsamples = 600; + omega_max = 3; + omega_min = -3; + + omega = logspace(omega_min, omega_max, nsamples); + omega_range = {10^omega_min, 10^omega_max}; - % Initial values for D-K iteration + % Initial values for D-K iteration are identity matrices 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 for D-scales, tuned by hand - fit_degrees = [ - 1, 1, 1, 1, 1, 1; % alpha - 1, 1, 1, 1, 1, 1; % omega - 1, 1, 1, 1, 1, 1; % state - 6, 1, 4, 2, 2, 1; % perf - ]; + % Maximum number of D-K iterations + niters = 5; + fprintf(' - Will do at most %d iterations.\n', niters); - % Number of D-K iterations - niters = size(fit_degrees, 2); - - % Maximum degree of D-scales - d_scales_max_err = .01;% in percentage - d_scales_max_degree = 8; + % 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 % for plotting later mu_plot_legend = {}; @@ -157,21 +145,26 @@ if do_musyn % Start DK-iteration dkstart = tic; for it = 1:niters - fprintf(' - Running D-K iteration %d / %d...\n', it, niters); + fprintf(' * Running D-K iteration %d / %d...\n', it, niters); itstart = tic(); % Find controller using H-infinity - [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl, hinfopt); + P_scaled = minreal(D_left * P * inv(D_right), [], false); + [P_scaled, ~] = prescale(P_scaled, omega_range); + [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt); + fprintf(' H-infinity synthesis gamma: %g\n', gamma); if gamma == inf - fprintf(' Failed to synethesize H-infinity controller\n'); - break; + error('Failed to synethesize H-infinity controller'); end % Calculate frequency response of closed loop - N = minreal(lft(P, K), [], false); % slient + N = minreal(lft(P, K), [], false); M = minreal(N(idx.OutputUncertain, idx.InputUncertain), [], false); + [N, ~] = prescale(N, omega_range); + [M, ~] = prescale(M, omega_range); + N_frd = frd(N, omega); M_frd = frd(M, omega); @@ -187,13 +180,13 @@ if do_musyn fprintf(' SSV for Performance: %g, for Stability: %g\n', mu_rp, mu_rs); if do_plots - fprintf(' Plotting mu\n'); + 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), '-.'); + 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', ... @@ -206,109 +199,123 @@ if do_musyn % Are we done yet? if mu_rp < 1 fprintf(' - Found robust controller that meets performance.\n'); - break + break; + end + + if mu_rs < 1 + fprintf(' Found robust controller that is stable.\n') + ctrl.musyn = struct('Name', '$\mu$-Synthesis', 'K', K, ... + 'mu_rp', mu_rp, 'mu_rs', mu_rs); end % Fit D-scales - % 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_rp); + [D_left_frd, D_right_frd] = mussvunwrap(mu_info_rp); fprintf(' Fitting D-scales\n'); - i_alpha = 1; - i_omega = model.uncertain.BlockStructure(1, 1) + 1; % after first block - i_state = model.uncertain.BlockStructure(2, 1) + 1; % after second block - i_perf = model.uncertain.BlockStructurePerf(3, 1) + 1; % after third block - - D_samples = { - D_left_samples(i_alpha, i_alpha); - D_left_samples(i_omega, i_omega); - D_left_samples(i_state, i_state); - D_left_samples(i_perf, i_perf); + % There are three complex, square, full block uncertainties and + % a non-square full complex block for performance + i_alpha = [1, 1]; + i_omega = model.uncertain.BlockStructure(1, :) + 1; % after first block + i_state = sum(model.uncertain.BlockStructure(1:2, :)) + 1; % after second block + i_perf = sum(model.uncertain.BlockStructurePerf(1:3, :)) + 1; % after third block + + D_frd = { + D_left_frd(i_alpha(1), i_alpha(1)); + D_left_frd(i_omega(1), i_omega(1)); + D_left_frd(i_state(1), i_state(1)); + D_left_frd(i_perf(1), i_perf(1)); }; D_max_sv = { - max(max(sigma(D_samples{1}))); - max(max(sigma(D_samples{2}))); - max(max(sigma(D_samples{3}))); - max(max(sigma(D_samples{4}))); + max(max(sigma(D_frd{1, 1}))); + max(max(sigma(D_frd{2, 1}))); + max(max(sigma(D_frd{3, 1}))); + max(max(sigma(D_frd{4, 1}))); }; D_names = {'alpha', 'omega', 'state', 'perf'}; D_fitted = {}; + % for each block 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 = fit_degrees(j, it):d_scales_max_degree + for deg = 0:d_scales_max_degree % Fit D-scale - % D_fit = fitmagfrd(D_samples{j}, deg); - D_fit = fitfrd(genphase(D_samples{j}), deg); + 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 - best_fit_deg = deg; - best_fit_err = fit_err; - D_fitted{j} = D_fit; + % 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; + end end - if fit_err / D_max_sv{j} < d_scales_max_err + if (fit_err / D_max_sv{j} < d_scales_max_err_p) break; end fprintf('.'); end - fprintf(' degree %d, error %g\n', best_fit_deg, best_fit_err); + 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 - D_right = blkdiag(D_fitted{1} * eye(4), ... - D_fitted{2} * eye(1), ... - D_fitted{3} * eye(12), ... - D_fitted{4} * eye(10), ... - eye(5)); - D_left = blkdiag(D_fitted{1} * eye(4), ... D_fitted{2} * eye(1), ... D_fitted{3} * eye(12), ... 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_fitted{4} * eye(10), ... + eye(5)); + % 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_frd = sigma(D_left_frd); + max_sv_left_frd = max(max(sv_left_frd)); 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)); + fprintf(' Max SVD of D: %g, Dhat: %g\n', max_sv_left_frd, max_sv_left); + fprintf(' D scales fit rel. error: %g %%\n', ... + 100 * abs(max_sv_left_frd - max_sv_left) / max_sv_left_frd); % Plot fitted D-scales if do_plots fprintf(' Plotting D-scales'); f = figure(101); clf(f); hold on; - bodemag(D_samples{1}, omega, 'r-'); + bodemag(D_frd{1}, omega, 'r-'); bodemag(D_fitted{1}, omega, 'b'); fprintf('.'); - bodemag(D_samples{2}, omega, 'r--'); + bodemag(D_frd{2}, omega, 'r--'); bodemag(D_fitted{2}, omega, 'b--'); fprintf('.'); - bodemag(D_samples{3}, omega, 'c-'); + bodemag(D_frd{3}, omega, 'c-'); bodemag(D_fitted{3}, omega, 'm-'); fprintf('.'); - bodemag(D_samples{4}, omega, 'c--'); + bodemag(D_frd{4}, omega, 'c--'); bodemag(D_fitted{4}, omega, 'm--'); fprintf('.'); @@ -332,9 +339,39 @@ if do_musyn dkend = toc(dkstart); fprintf(' - D-K iteration took %.1f seconds\n', dkend); - 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); + if mu_rp > 1 && mu_rs > 1 + error(' - Failed to synthesize robust controller that meets the desired performance.\n'); + end + + %% 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 + + % Save controller + ctrl.musyn = struct('Name', '$\mu$-Synthesis', ... + 'K', K, 'Delta', Delta, ... + 'mu_rp', mu_rp, 'mu_rs', mu_rs); + + if mu_rp >= 1 + fprintf(' - Synthetized robust stable controller does not meet the desired performance.\n'); end + +%% ------------------------------------------------------------------------ +% Measure Performance of mu synthesis design + + fprintf('Simulating nominal closed loop...\n'); + + T = 60; + nsamples = 5000; + do_noise = true; + + simout = uav_sim_step(params, model, ctrl.musyn, nsamples, T, do_plots, do_noise); + + fprintf('Simulating worst-case closed loop...\n'); + end diff --git a/uav_model.m b/uav_model.m index 3fc1d0a..1f8f751 100644 --- a/uav_model.m +++ b/uav_model.m @@ -3,15 +3,18 @@ % Copyright (C) 2024, Naoki Sean Pross, ETH Zürich. % This work is distributed under a permissive license, see LICENSE.txt % -% This function generates three models: +% This function generates three plant models: % -% * A non-linear symbolic model (cannot be used directly). -% * A linear model obtained by linearizing the the non linear model at an -% operating point specified in the params struct argument. -% * A uncertain linear model built atop of the linear model using SIMULINK. -% The uncertain model contains the performance and weighting transfer -% function given in the arguments perf and params, and is stored in the -% SIMULINK file uav_model_uncertain.xls. +% * A non-linear symbolic model (cannot be used directly) derived from first +% principles (equations of motion). +% +% * A linear model obtained by linearizing the the non linear plant model at +% an operating point specified in the params struct argument. +% +% * A uncertain linear model with reference trcking built atop of the linear +% model using SIMULINK. The uncertain model contains the performance and +% weighting transfer function given in the arguments perf and params, and is +% stored in the SIMULINK file uav_model_uncertain.xls. % % [MODEL] = UAV_MODEL(PARAMS, PERF, UNCERT) % @@ -220,6 +223,27 @@ D = zeros(12, 5); [nx, nu] = size(B); [ny, ~] = size(C); +% ------------------------------------------------------------------------ +% Scale inputs and outputs of linearized model + +S_actuators = blkdiag(... + eye(4) * 1 / params.actuators.ServoAbsMaxAngle, ... + eye(1) * 1 / params.actuators.PropellerMaxAngularVelocity); + +S_state = blkdiag(... + eye(2) * params.normalization.HPosition, ... + eye(1) * params.normalization.VPosition, ... + eye(2) * params.normalization.HSpeed, ... + eye(1) * params.normalization.VSpeed, ... + eye(2) * params.normalization.PitchRollAngle, ... + eye(1) * params.normalization.YawAngle, ... + eye(3) * params.normalization.AngularRate); + +% Scale system matrices to have inputs and outputs between zero and one +B = B * inv(S_actuators); +C = inv(S_state) * C; +D = D * inv(S_actuators); + % Create state space object T = params.measurements.SensorFusionDelay; n = params.linearization.PadeApproxOrder; @@ -230,7 +254,8 @@ model.linear = struct(... 'Nx', nx, 'Nu', nu, 'Ny', ny, ... % number of states, inputs, and outputs 'State', xi, 'Inputs', u, ... % state and input variables 'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized - 'StateSpace', sys ... % state space object + 'StateSpace', sys, ... % state space object + 'InputScalingMatrix', S_actuators, 'OutputScalingMatrix', S_state ... ); % ------------------------------------------------------------------------ @@ -289,14 +314,20 @@ if rank(Wo) < nx end % ------------------------------------------------------------------------ +% Compute absolute value of error caused by linearization around set point + + +% ------------------------------------------------------------------------ % Model actuators % TODO: better model, this was tuned "by looking" -w = 4 * params.actuators.ServoNominalAngularVelocity; +w = 2 * params.actuators.ServoNominalAngularVelocity; zeta = .7; G_servo = tf(w^2, [1, 2 * zeta * w, w^2]); -% figure; step(G_servo * pi / 3); grid on; -G_prop = tf(1,1); + +w = 6e3; +zeta = 1; +G_prop = tf(w^2, [1, 2 * zeta * w, w^2]); model.actuators = struct('FlapServo', G_servo, 'ThrustPropeller', G_prop); @@ -328,6 +359,21 @@ blk_perf = [ 10, 14 % always full ]; +% ------------------------------------------------------------------------ +% Scale inputs and outputs of uncertain model + +% Scaling of reference is same as position +S_ref = blkdiag(... + eye(2) * 1 / params.normalization.HPosition, ... + eye(1) * 1 / params.normalization.VPosition); + +% TODO: finish +S_uncert_out = blkdiag(... + S_actuators, ... + S_state, ... + S_actuators, ... + S_state(1:9, 1:9)); + % Save uncertain model model.uncertain = struct(... 'Simulink', ulmod, ... diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx Binary files differindex b863139..d432718 100644 --- a/uav_model_uncertain.slx +++ b/uav_model_uncertain.slx diff --git a/uav_model_uncertain_clp.slx b/uav_model_uncertain_clp.slx Binary files differindex 7a17a21..7bcf2d9 100644 --- a/uav_model_uncertain_clp.slx +++ b/uav_model_uncertain_clp.slx diff --git a/uav_params.m b/uav_params.m index a1c0e4a..e87e4fd 100644 --- a/uav_params.m +++ b/uav_params.m @@ -65,7 +65,7 @@ params.actuators = struct(... % IMU runs in NDOF mode params.measurements = struct(... - 'SensorFusionDelay', 20e-3, ... % in s + 'SensorFusionDelay', 50e-3, ... % in s 'LIDARAccuracy', 10e-3, ... % in m 'LIDARMaxDistance', 40, ... % inm 'LIDARBandwidth', 100, ... % in Hz for z position @@ -82,6 +82,11 @@ omega_max = params.actuators.PropellerMaxAngularVelocity; F_max = 38.637; % in N (measured) k_T = F_max / omega_max^2; +% Lift coefficient from data +% flap_stair = readtable('meas/Flap_Force_Stair.csv'); +% [~, istart] = min(abs(flap.Force)); +% [~, iend] = min(abs(flap.Force - 3)); + % FIXME: LiftCoefficient comes from % https://scienceworld.wolfram.com/physics/LiftCoefficient.html params.aerodynamics = struct(... @@ -120,56 +125,15 @@ params.linearization = struct(... % ------------------------------------------------------------------------ % Performance requirements -params.performance = struct(... - 'ReferencePosMaxDistance', 1, ... % m - 'HorizontalPosMaxError', 2, ... % m - 'HorizontalMaxSpeed', 2, ... % m / s - 'HorizontalSettleTime', 6, ... % s - 'VerticalPosMaxError', 2, ... % m - 'VerticalMaxSpeed', 2, ... % m / s - 'VerticalSettleTime', 4, ... % s - 'AngleMaxPitchRoll', 10 * pi / 180, ... % in rad - 'AngleMaxYaw', pi, ... % rad - 'AngleMaxAngularRate', 20 * pi / 180 ... % rad / s +params.normalization = struct(... + 'HPosition', 1, ... % m + 'VPosition', 1, ...% m + 'HSpeed', 2, ... % m / s + 'VSpeed', 2, ... % m / s + 'PitchRollAngle', 10 * pi / 180, ... % rad + 'YawAngle', 5 * pi / 180, ... % rad + 'AngularRate', 2 * pi / 180, ... % rad / s + 'WindSpeed', 2 ... % m / s ); - -% Scaling matrices (because signals are normalized wrt parameter performance) -p = params.performance; - -params.ErrorScalingMatrix = blkdiag(... - eye(4) * params.actuators.ServoAbsMaxAngle, ... - params.actuators.PropellerMaxAngularVelocity ... - - params.linearization.Inputs(5), ... - eye(2) * p.HorizontalPosMaxError, ... - p.VerticalPosMaxError, ... - eye(2) * p.HorizontalMaxSpeed, ... - p.VerticalMaxSpeed, ... - eye(2) * p.AngleMaxPitchRoll, ... - p.AngleMaxYaw); - -params.OutputScalingMatrix = blkdiag(... - eye(2) * p.HorizontalPosMaxError, ... - p.VerticalPosMaxError, ... - eye(2) * p.HorizontalMaxSpeed, ... - p.VerticalMaxSpeed, ... - eye(2) * p.AngleMaxPitchRoll, ... - p.AngleMaxYaw, ... - eye(3) * p.AngleMaxAngularRate); - - -horiz_settle_speed = (1 - exp(-1)) / params.performance.HorizontalSettleTime; -if horiz_settle_speed > params.performance.HorizontalMaxSpeed - fprintf(['Contradictory performance requirements: ' ... - 'velocity needed to attain horizontal settle time is ' ... - 'higher than HorizontalMaxSpeed']); -end - -vert_settle_speed = (1 - exp(-1)) / params.performance.VerticalSettleTime; -if vert_settle_speed > params.performance.VerticalMaxSpeed - fprintf(['Contradictory performance requirements: ' ... - 'velocity needed to attain vertical settle time is ' ... - 'higher than VerticalMaxSpeed']); -end - end % vim: ts=2 sw=2 et: diff --git a/uav_performance_musyn.m b/uav_performance_musyn.m index 180be8a..aba2160 100644 --- a/uav_performance_musyn.m +++ b/uav_performance_musyn.m @@ -18,22 +18,31 @@ function [perf] = uav_performance_musyn(params, do_plots) % Laplace variable s = tf('s'); -% TODO: set proper constraints -G = 1e2 / (s + 1e2); +% Bandwitdhs +bw_alpha = params.actuators.ServoNominalAngularVelocity; +bw_omega = 10; -W_Palpha = G; -W_Pomega = G; +bw_xy = .05; +bw_z = .05; -W_Pxy = G; -W_Pz = G; +bw_xydot = .5; +bw_zdot = .1; -W_Pxydot = tf(0); -W_Pzdot = tf(0); - -W_Pphitheta = tf(0); -W_Ppsi = tf(0); +bw_phitheta = .1; +bw_psi = .08; + +% Inverse performance functions +W_Palpha = .2 / (s / bw_alpha + 1); +W_Pomega = .2 / (s / bw_omega + 1); -W_PTheta = tf(0); +W_Pxy = 8 * bw_xy^2 / (s^2 + 2 * 1 * bw_xy * s + bw_xy^2); +W_Pz = 1 * bw_z^2 / (s^2 + 2 * 1 * bw_z * s + bw_z^2); + +W_Pxydot = .2 / (s / bw_xydot + 1); +W_Pzdot = .5 / (s / bw_zdot + 1); + +W_Pphitheta = 1 / (s / bw_phitheta + 1); +W_Ppsi = .1 / (s / bw_psi + 1); % Construct performance vector by combining xy and z W_PP = blkdiag(W_Pxy * eye(2), W_Pz); @@ -70,37 +79,24 @@ if do_plots % 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); + step(W_Pxy); + step(W_Pz); + step(W_Pxydot); + step(W_Pzdot); + step(W_Pphitheta); + step(W_Ppsi); + grid on; - legend('$W_{P,xy}$', '$W_{P,z}$', ... + 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,\alpha}$', '$W_{P,\omega}$', ... + '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ... '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.m index 3af2a76..973a928 100644 --- a/uav_sim_step_musyn.m +++ b/uav_sim_step.m @@ -3,7 +3,7 @@ % 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) +function [simout] = uav_sim_step_musyn(params, model, ctrl, nsamp, T, do_plots, do_noise) % Load closed loop model and add controller % more or less equivalent to doing usys = lft(Pnom, K) @@ -20,7 +20,7 @@ P_nom_clp = minreal(usys_clp(... [model.uncertain.index.OutputError; model.uncertain.index.OutputNominal; model.uncertain.index.OutputPlots], ... - model.uncertain.index.InputExogenous)); + model.uncertain.index.InputExogenous), [], false); % Indices for exogenous inputs Iwwind = (1:3)'; @@ -33,7 +33,6 @@ 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)'; @@ -58,7 +57,7 @@ if do_noise end % Create step inputs (normalized) -ref_step = ones(1, nsamp); % 1d step function +ref_step = .5 * 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) ]; @@ -66,22 +65,39 @@ 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); +t = linspace(0, T, 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); +out_step_x_norm = lsim(P_nom_clp, in_step_x, t); +out_step_y_norm = lsim(P_nom_clp, in_step_y, t); +out_step_z_norm = lsim(P_nom_clp, in_step_z, t); + +% Scale outputs +S_actuators = blkdiag(... + eye(4) * params.actuators.ServoAbsMaxAngle, ... + eye(1) * params.actuators.PropellerMaxAngularVelocity); + +S_state = blkdiag(... + eye(2) * params.normalization.HPosition, ... + eye(1) * params.normalization.VPosition, ... + eye(2) * params.normalization.HSpeed, ... + eye(1) * params.normalization.VSpeed, ... + eye(2) * params.normalization.PitchRollAngle, ... + eye(1) * params.normalization.YawAngle, ... + eye(3) * params.normalization.AngularRate); + +S = blkdiag(S_actuators, S_state(1:9, 1:9), S_state, S_actuators, S_actuators); + +out_step_x = out_step_x_norm * S'; +out_step_y = out_step_y_norm * S'; +out_step_z = out_step_z_norm * S'; % Return simulation simout = struct(... - 'TimeXY', t_xy, ... - 'StepX', out_step_x, ... - 'StepY', out_step_y, ... + 'Time', t, ... + 'StepX', out_step_x_norm, ... + 'StepY', out_step_y_norm, ... + 'StepZ', out_step_z_norm, ... 'Simulink', ulmod_clp, ... 'StateSpace', P_nom_clp); @@ -111,9 +127,9 @@ if do_plots ctrl.Name), 'Interpreter', 'latex'); % Plot limits - ref_value = params.performance.ReferencePosMaxDistance; + ref_value = .5; alpha_max_deg = params.actuators.ServoAbsMaxAngle * to_deg; - euler_lim_deg = 1.5; % params.performance.AngleMaxPitchRoll * to_deg; + euler_lim_deg = 1.5; omega_max_rpm = (params.actuators.PropellerMaxAngularVelocity ... - params.linearization.Inputs(5)) * to_rpm; omega_min_rpm = -params.linearization.Inputs(5) * to_rpm; @@ -121,14 +137,14 @@ if do_plots % 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--'); + plot(t, out_step_x(:, Ialpha(1)) * to_deg); + plot(t, out_step_x(:, Ialpha(2)) * to_deg); + plot(t, out_step_x(:, Ialpha(3)) * to_deg); + plot(t, out_step_x(:, Ialpha(4)) * to_deg); + plot([0, T], [1, 1] * alpha_max_deg, 'r--'); + plot([0, T], [-1, -1] * alpha_max_deg, 'r--'); grid on; - xlim([0, T_final_horiz]); + xlim([0, T]); ylim([-alpha_max_deg * 1.1, alpha_max_deg * 1.1]); title('Horizontal $x$ to Flaps', 'Interpreter', 'latex'); ylabel('Flap Angle (degrees)', 'Interpreter', 'latex'); @@ -138,14 +154,14 @@ if do_plots % 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--'); + plot(t, out_step_y(:, Ialpha(1)) * to_deg); + plot(t, out_step_y(:, Ialpha(2)) * to_deg); + plot(t, out_step_y(:, Ialpha(3)) * to_deg); + plot(t, out_step_y(:, Ialpha(4)) * to_deg); + plot([0, T], [1, 1] * alpha_max_deg, 'r--'); + plot([0, T], [-1, -1] * alpha_max_deg, 'r--'); grid on; - xlim([0, T_final_horiz]); + xlim([0, T]); ylim([-alpha_max_deg * 1.1, alpha_max_deg * 1.1]); title('Horizontal $y$ to Flaps', 'Interpreter', 'latex'); ylabel('Flap Angle (degrees)', 'Interpreter', 'latex'); @@ -155,14 +171,14 @@ if do_plots % 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--'); + plot(t, out_step_z(:, Ialpha(1)) * to_deg); + plot(t, out_step_z(:, Ialpha(2)) * to_deg); + plot(t, out_step_z(:, Ialpha(3)) * to_deg); + plot(t, out_step_z(:, Ialpha(4)) * to_deg); + plot([0, T], [1, 1] * alpha_max_deg, 'r--'); + plot([0, T], [-1, -1] * alpha_max_deg, 'r--'); grid on; - xlim([0, T_final_vert]); + xlim([0, T]); ylim([-alpha_max_deg * 1.1, alpha_max_deg * 1.1]); title('Vertical $z$ to Flaps', 'Interpreter', 'latex'); ylabel('Flap Angle (degrees)', 'Interpreter', 'latex'); @@ -172,11 +188,11 @@ if do_plots % 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); + plot(t, out_step_x(:, ITheta(1)) * to_deg); + plot(t, out_step_x(:, ITheta(2)) * to_deg); + plot(t, out_step_x(:, ITheta(3)) * to_deg); grid on; - xlim([0, T_final_horiz]); + xlim([0, T]); ylim([-euler_lim_deg, euler_lim_deg]); title('Horizontal $x$ to Euler Angles', 'Interpreter', 'latex'); ylabel('Euler Angle (degrees)', 'Interpreter', 'latex'); @@ -186,11 +202,11 @@ if do_plots % 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); + plot(t, out_step_y(:, ITheta(1)) * to_deg); + plot(t, out_step_y(:, ITheta(2)) * to_deg); + plot(t, out_step_y(:, ITheta(3)) * to_deg); grid on; - xlim([0, T_final_horiz]); + xlim([0, T]); ylim([-euler_lim_deg, euler_lim_deg]); title('Horizontal $y$ to Euler Angles', 'Interpreter', 'latex'); ylabel('Euler Angle (degrees)', 'Interpreter', 'latex'); @@ -200,11 +216,11 @@ if do_plots % 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); + plot(t, out_step_z(:, ITheta(1)) * to_deg); + plot(t, out_step_z(:, ITheta(2)) * to_deg); + plot(t, out_step_z(:, ITheta(3)) * to_deg); grid on; - xlim([0, T_final_vert]); + xlim([0, T]); ylim([-euler_lim_deg, euler_lim_deg]); title('Vertical $z$ to Euler Angles', 'Interpreter', 'latex'); ylabel('Euler Angle (degrees)', 'Interpreter', 'latex'); @@ -219,9 +235,9 @@ if do_plots 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--'); + step(P_nom_clp(Iomega, Ir(3)) * to_rpm, T); + plot([0, T], [1, 1] * omega_min_rpm, 'r--'); + plot([0, T], [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'); @@ -237,10 +253,10 @@ if do_plots % 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--'); + plot(t, out_step_x(:, IP(1))); + plot(t, out_step_y(:, IP(2))); + % plot([0, T], [1, 1] * ref_value, 'r:'); + % plot(t, out_step_xydes, 'r--'); grid on; title('Horizontal Position Error', 'Interpreter', 'latex'); ylabel('Error (meters)', 'Interpreter', 'latex'); @@ -249,8 +265,8 @@ if do_plots % 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))); + plot(t, out_step_x(:, IPdot(1))); + plot(t, out_step_y(:, IPdot(2))); grid on; title('Horizontal Velocity', 'Interpreter', 'latex'); ylabel('Velocity (m / s)', 'Interpreter', 'latex'); @@ -259,9 +275,9 @@ if do_plots % 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--'); + plot(t, out_step_z(:, IP(3))); + % plot([0, T], [1, 1] * ref_value, 'r:'); + % plot(t, out_step_zdes, 'r--'); grid on; title('Vertical Position Error', 'Interpreter', 'latex'); ylabel('Error (meters)', 'Interpreter', 'latex'); @@ -270,7 +286,7 @@ if do_plots % Plot step response vertical reference to vertical speed subplot(2, 2, 4); hold on; - plot(t_z, out_step_z(:, IPdot(3))); + plot(t, out_step_z(:, IPdot(3))); grid on; title('Vertical Velocity', 'Interpreter', 'latex'); ylabel('Velocity (m / s)', 'Interpreter', 'latex'); diff --git a/uav_sim_step_hinf.m b/uav_sim_step_hinf.m deleted file mode 100644 index 5e448a1..0000000 --- a/uav_sim_step_hinf.m +++ /dev/null @@ -1,282 +0,0 @@ -% 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_hinf(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), [], false); - -% 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 aff30ca..b664cee 100644 --- a/uav_uncertainty.m +++ b/uav_uncertainty.m @@ -37,9 +37,11 @@ eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega); % W_momega = eps_omega * 10 / (s + 10); % W_mState = .01 * 10 / (s + 10); -W_malpha = eps_alpha * tf(1); +b = 1e3; + +W_malpha = make_weight(b, 1 / eps_alpha, 1e-2); W_momega = tf(0); -W_mState = .01 * tf(1); +W_mState = 1 - 1 / (s / 50 + 1); uncert = struct(... 'FlapAngle', W_malpha * eye(4), ... @@ -61,4 +63,24 @@ if do_plots 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: |