From 86f87bd7997105572c44f74714f9b03c2f7ea4fe Mon Sep 17 00:00:00 2001 From: Nao Pross Date: Mon, 13 May 2024 16:42:23 +0200 Subject: Prepare structure for endterm --- uav.m | 108 ++++++++++++++++++++++------------------------ uav_model.m | 2 +- uav_performance.m | 123 +++++++++++++++++++++++++++++++++++++++++++++++++++++ uav_requirements.m | 123 ----------------------------------------------------- uav_uncertainty.m | 28 ++++++++++++ 5 files changed, 203 insertions(+), 181 deletions(-) create mode 100644 uav_performance.m delete mode 100644 uav_requirements.m create mode 100644 uav_uncertainty.m diff --git a/uav.m b/uav.m index e5f49b2..c646de2 100644 --- a/uav.m +++ b/uav.m @@ -12,39 +12,27 @@ fprintf('Generating system parameters...\n') params = uav_params(); ctrl = struct(); +% Flags to speed up running for debugging do_plots = true; +do_lqr = false; % unused +do_hinf = false; % midterm +do_musyn = true; % endterm % ------------------------------------------------------------------------ %% Define performance requirements -fprintf('Generating performance requirements...\n') -perf = uav_requirements(params, do_plots); +if do_hinf | do_musyn + fprintf('Generating performance requirements...\n') + perf = uav_performance(params, do_plots); +end % ------------------------------------------------------------------------ %% Define stability requirements -W_malpha = tf(1,1); -W_momega = tf(1,1); -W_mState = tf(1,1); - -% if do_plots -% figure; hold on; -% -% bodemag(W_malpha); -% bodemag(W_momega); -% bodemag(W_mState); -% -% grid on; -% legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ... -% '$W_{m,\Theta}$', '$W_{m,\Omega}$', ... -% 'Interpreter', 'latex', 'fontSize', 8); -% title('Uncertainties') -% end - -uncert = struct(... - 'FlapAngle', W_malpha * eye(4), ... - 'Thrust', W_momega, ... - 'StateLinApprox', W_mState * eye(12)); +if do_musyn + fprintf('Generating stability requirements...\n') + uncert = uav_uncertainty(params, do_plots); +end % ------------------------------------------------------------------------ %% Create UAV model @@ -55,52 +43,61 @@ model = uav_model(params, perf, uncert); % ------------------------------------------------------------------------ %% Perform LQR design -% fprintf('Performing LQR controller design...\n') -% ctrl.lqr = uav_ctrl_lqr(params, model); +if do_lqr + fprintf('Performing LQR controller design...\n') + ctrl.lqr = uav_ctrl_lqr(params, model); +end % ------------------------------------------------------------------------ %% Perform H-infinity design -fprintf('Performing H-infinty controller design...\n') +if do_hinf + fprintf('Performing H-infinty controller design...\n') -idx = model.uncertain.index; -P = model.uncertain.StateSpace; + idx = model.uncertain.index; + P = model.uncertain.StateSpace; -% Get nominal system without uncertainty (for lower LFT) -P_nom = minreal(P([idx.OutputError; idx.OutputNominal], ... - [idx.InputExogenous; idx.InputNominal])); + % Get nominal system without uncertainty (for lower LFT) + P_nom = minreal(P([idx.OutputError; idx.OutputNominal], ... + [idx.InputExogenous; idx.InputNominal])); -nmeas = max(size(idx.OutputNominal)); % size of y -nctrl = max(size(idx.InputNominal)); % size of u + nmeas = max(size(idx.OutputNominal)); % size of y + nctrl = max(size(idx.InputNominal)); % size of u -hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ... - 'AutoScale', 'off', 'RelTol', 1e-3); -[K_inf, ~, gamma, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt); -ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_inf); + hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ... + 'AutoScale', 'off', 'RelTol', 1e-3); + [K_inf, ~, gamma, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt); + ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_inf); -if gamma >= 1 - fprintf('Failed to syntesize controller (closed loop is unstable).\n') -end + if gamma >= 1 + fprintf('Failed to syntesize controller (closed loop is unstable).\n') + end % ------------------------------------------------------------------------ -%% Measure Performance +%% Measure Performance of H-infinity design -fprintf('Simulating closed loop...\n'); + fprintf('Simulating closed loop...\n'); -nsamples = 500; -do_noise = true; -% uav_sim_step(params, model, ctrl.lqr, nsamples, do_plots); + nsamples = 500; + do_noise = true; + simout = uav_sim_step_hinf(params, model, ctrl.hinf, nsamples, do_plots, do_noise); -simout = uav_sim_step_hinf(params, model, ctrl.hinf, nsamples, 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]; -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') +end + +% ------------------------------------------------------------------------ +%% Perform mu-Analysis & DK iteration -writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') +if do_musyn + +end % ------------------------------------------------------------------------ %% Verify performance satisfaction via mu-analysis @@ -134,7 +131,4 @@ writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') % title('$\mu_\Delta(N)$', 'Interpreter', 'latex'); % end -% ------------------------------------------------------------------------ -% Perform mu-Analysis & DK iteration - % vim: ts=2 sw=2 et: diff --git a/uav_model.m b/uav_model.m index 4fddae9..3b691f4 100644 --- a/uav_model.m +++ b/uav_model.m @@ -11,7 +11,7 @@ % * 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 fil euav_model_uncertain.xls. +% SIMULINK file uav_model_uncertain.xls. % % [MODEL] = UAV_MODEL(PARAMS, PERF, UNCERT) % diff --git a/uav_performance.m b/uav_performance.m new file mode 100644 index 0000000..4857a34 --- /dev/null +++ b/uav_performance.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, plot) + +% 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 plot + % 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_requirements.m b/uav_requirements.m deleted file mode 100644 index 9ccaeea..0000000 --- a/uav_requirements.m +++ /dev/null @@ -1,123 +0,0 @@ -% 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: -% MODEL Struct performance transfer functions - - -function [perf] = uav_requirements(params, plot) - -% 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 plot - % 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_uncertainty.m b/uav_uncertainty.m new file mode 100644 index 0000000..e1ddf76 --- /dev/null +++ b/uav_uncertainty.m @@ -0,0 +1,28 @@ +% Generate transfer functions for loop shaping stability (uncertainty) +% 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: +% UNCERT Struct of uncertainty transfer functions + + +function [uncert] = uav_performance(params, plot) + +W_malpha = tf(1,1); +W_momega = tf(1,1); +W_mState = tf(1,1); + +uncert = struct(... + 'FlapAngle', W_malpha * eye(4), ... + 'Thrust', W_momega, ... + 'StateLinApprox', W_mState * eye(12)); + +end +% vim: ts=2 sw=2 et: -- cgit v1.2.1