diff options
Diffstat (limited to 'uav_sim_step.m')
| -rw-r--r-- | uav_sim_step.m | 260 |
1 files changed, 135 insertions, 125 deletions
diff --git a/uav_sim_step.m b/uav_sim_step.m index 82c05e9..65fb36f 100644 --- a/uav_sim_step.m +++ b/uav_sim_step.m @@ -3,127 +3,141 @@ % 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, T, do_plots, do_noise) +function [simout] = uav_sim_step(params, model, ctrl, uncert, nsamp, T, 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); +if isfield(ctrl, 'K') + hws.assignin('K', ctrl.K); +else + error('You need to provide a controller ctrl.K'); +end -% 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); +% There is an uncertainty block +if isfield(uncert, 'Delta') + hws.assignin('Delta', uncert.Delta); +else + fprintf(' - No uncertainty error Delta was provided, setting to zero.\n'); + Delta = tf(0) * zeros(sum(model.uncertain.BlockStructure)); + hws.assignin('Delta', Delta); +end -% Check that closed loop is actually stable -eigvals = eig(P_nom_clp.A); -nx = size(P_nom_clp.A, 1); +ulmod_clp = linmod('uav_model_uncertain_clp'); +P_clp = minreal(ss(ulmod_clp.a, ulmod_clp.b, ulmod_clp.c, ulmod_clp.d), [], false); -for i = 1:nx - if real(eigvals(i)) >= 0 +% Check that closed loop is actually stable +nx = size(P_clp.A, 1); +fprintf(' - Closed loop dynamics have %d states\n', nx); +if nx < 60 + [~, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(P_clp); + if nx ~= nsta error('Closed loop is not stable!'); end -end - -% 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)'; - -% Indices for y outputs (for plots) -IP = (15:17)'; -IPdot = (18:20)'; -ITheta = (21:23)'; -IOmega = (24:26)'; + fprintf(' %d stable modes, %d unstable modes\n', nsta, nx - nsta); + fprintf(' %d controllable modes, %d unstabilizable\n', nctrb, nustab); + fprintf(' %d observable modes, %d undetectable\n', nobsv, nudetb); +else + eigvals = eig(P_clp.A); + for i = 1:nx + if real(eigvals(i)) >= 0 + error('Closed loop is not stable!'); + end + end +end -% Indices for p outputs (for plots) -Ialpha = (27:30)'; -Iomega = 31; +% Generate indices for closed loop plant +o = model.uncertain.dims.output; +i = model.uncertain.dims.input; + +dims = struct(... + 'ErrorAlpha', o.OutputErrorAlpha, ... + 'ErrorOmega', o.OutputErrorOmega, ... + 'ErrorPosition', o.OutputErrorPosition, ... + 'ErrorVelocity', o.OutputErrorVelocity, ... + 'ErrorEulerAngles', o.OutputErrorEulerAngles, ... + 'Position', o.OutputNominalPosition, ... + 'Velocity', o.OutputNominalVelocity, ... + 'EulerAngles', o.OutputNominalEulerAngles, ... + 'AngularRates', o.OutputNominalAngularRates, ... + 'PlotAlpha', o.OutputPlotAlpha, ... + 'PlotOmega', o.OutputPlotOmega, ... + 'PlotReference', o.OutputPlotReference, ... + 'PlotPosition', o.OutputPlotPosition, ... + 'InputAlpha', i.InputNominalAlpha, ... + 'InputOmega', i.InputNominalOmega ... +); + +idx = struct(); +start = 1; fields = fieldnames(dims); +for f = fields' + dim = getfield(dims, f{1}); + i = (start:(start + dim - 1))'; + idx = setfield(idx, f{1}, i); + start = start + dim; +end -Iualpha = (32:35)'; -Iuomega = 36; +% Input indices +idx.InputDisturbanceWind = (1:3)'; +idx.InputDisturbanceFlaps = (4:7)'; +idx.InputReference = (8:10)'; -noise = zeros(7, nsamp); % no noise +% Create noise +noise = zeros(7, nsamp); if do_noise - % Noise in percentage + % Noise (normalized) noise_alpha_amp = (.5 * (pi / 180)) / params.actuators.ServoAbsMaxAngle; - noise_wind_amp = .1; + noise_wind_amp = .01; noise = [noise_wind_amp * randn(3, nsamp); noise_alpha_amp * randn(4, nsamp)]; end +% Step size +step_size_h = .5 / params.normalization.HPosition; +step_size_v = .5 / params.normalization.VPosition; + % Create step inputs (normalized) -ref_step = .5 * ones(1, nsamp); % 1d step function +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 ]; +in_step_x = [ noise; step_size_h * ref_step; zeros(2, nsamp) ]; +in_step_y = [ noise; zeros(1, nsamp); step_size_h * ref_step; zeros(1, nsamp) ]; +in_step_z = [ noise; zeros(2, nsamp); step_size_v * ref_step ]; % Simulation time -n_settle_times = 10; t = linspace(0, T, nsamp); +% Scale simulation outputs +S = blkdiag(... + model.uncertain.scaling.OutputErrorScaling, ... + model.uncertain.scaling.OutputNominalScaling, ... + model.uncertain.scaling.OutputPlotScaling, ... + model.uncertain.scaling.InputNominalScaling ... +); + % Simulate step responses -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'; +out_step_x_norm = lsim(P_clp, in_step_x, t, 'foh'); +out_step_y_norm = lsim(P_clp, in_step_y, t, 'foh'); +out_step_z_norm = lsim(P_clp, in_step_z, t, 'foh'); + +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(... 'Time', t, ... - 'StepX', out_step_x_norm, ... - 'StepY', out_step_y_norm, ... - 'StepZ', out_step_z_norm, ... + 'StepX', out_step_x, ... + 'StepY', out_step_y, ... + 'StepZ', out_step_z, ... + 'StepXNorm', out_step_x_norm, ... + 'StepYNorm', out_step_y_norm, ... + 'StepZNorm', out_step_z_norm, ... '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); + 'StateSpace', P_clp, ... + 'index', idx); if do_plots % Conversion factors @@ -137,7 +151,6 @@ if do_plots ctrl.Name), 'Interpreter', 'latex'); % Plot limits - ref_value = .5; alpha_max_deg = params.actuators.ServoAbsMaxAngle * to_deg; euler_lim_deg = 1.5; omega_max_rpm = (params.actuators.PropellerMaxAngularVelocity ... @@ -147,10 +160,10 @@ if do_plots % Plot step response from x to alpha subplot(2, 3, 1); hold on; - 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(t, out_step_x(:, idx.PlotAlpha(1)) * to_deg); + plot(t, out_step_x(:, idx.PlotAlpha(2)) * to_deg); + plot(t, out_step_x(:, idx.PlotAlpha(3)) * to_deg); + plot(t, out_step_x(:, idx.PlotAlpha(4)) * to_deg); plot([0, T], [1, 1] * alpha_max_deg, 'r--'); plot([0, T], [-1, -1] * alpha_max_deg, 'r--'); grid on; @@ -164,10 +177,10 @@ if do_plots % Plot step response from y to alpha subplot(2, 3, 2); hold on; - 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(t, out_step_y(:, idx.PlotAlpha(1)) * to_deg); + plot(t, out_step_y(:, idx.PlotAlpha(2)) * to_deg); + plot(t, out_step_y(:, idx.PlotAlpha(3)) * to_deg); + plot(t, out_step_y(:, idx.PlotAlpha(4)) * to_deg); plot([0, T], [1, 1] * alpha_max_deg, 'r--'); plot([0, T], [-1, -1] * alpha_max_deg, 'r--'); grid on; @@ -181,10 +194,10 @@ if do_plots % Plot step response from z to alpha subplot(2, 3, 3); hold on; - 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(t, out_step_z(:, idx.PlotAlpha(1)) * to_deg); + plot(t, out_step_z(:, idx.PlotAlpha(2)) * to_deg); + plot(t, out_step_z(:, idx.PlotAlpha(3)) * to_deg); + plot(t, out_step_z(:, idx.PlotAlpha(4)) * to_deg); plot([0, T], [1, 1] * alpha_max_deg, 'r--'); plot([0, T], [-1, -1] * alpha_max_deg, 'r--'); grid on; @@ -198,9 +211,9 @@ if do_plots % Plot step response from x to Theta subplot(2, 3, 4); hold on; - 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); + plot(t, out_step_x(:, idx.EulerAngles(1)) * to_deg); + plot(t, out_step_x(:, idx.EulerAngles(2)) * to_deg); + plot(t, out_step_x(:, idx.EulerAngles(3)) * to_deg); grid on; xlim([0, T]); ylim([-euler_lim_deg, euler_lim_deg]); @@ -212,9 +225,9 @@ if do_plots % Plot step response from y to Theta subplot(2, 3, 5); hold on; - 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); + plot(t, out_step_y(:, idx.EulerAngles(1)) * to_deg); + plot(t, out_step_y(:, idx.EulerAngles(2)) * to_deg); + plot(t, out_step_y(:, idx.EulerAngles(3)) * to_deg); grid on; xlim([0, T]); ylim([-euler_lim_deg, euler_lim_deg]); @@ -226,9 +239,9 @@ if do_plots % Plot step response from z to Theta subplot(2, 3, 6); hold on; - 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); + plot(t, out_step_z(:, idx.EulerAngles(1)) * to_deg); + plot(t, out_step_z(:, idx.EulerAngles(2)) * to_deg); + plot(t, out_step_z(:, idx.EulerAngles(3)) * to_deg); grid on; xlim([0, T]); ylim([-euler_lim_deg, euler_lim_deg]); @@ -245,7 +258,7 @@ if do_plots ctrl.Name), 'Interpreter', 'latex'); hold on; - step(P_nom_clp(Iomega, Ir(3)) * to_rpm, T); + step(P_clp(idx.PlotOmega, idx.InputReference(3)) * to_rpm, T); % plot([0, T], [1, 1] * omega_min_rpm, 'r--'); % plot([0, T], [1, 1] * omega_max_rpm, 'r--'); grid on; @@ -263,20 +276,19 @@ if do_plots % Plot step response from horizontal reference to horizontal position subplot(2, 2, 1); hold on; - 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--'); + plot(t, out_step_x(:, idx.PlotPosition(1))); + plot(t, out_step_y(:, idx.PlotPosition(2))); + plot(t, out_step_x(:, idx.PlotReference(1)), '--'); grid on; - title('Horizontal Position Error', 'Interpreter', 'latex'); - ylabel('Error (meters)', 'Interpreter', 'latex'); + title('Horizontal Position', 'Interpreter', 'latex'); + ylabel('Distance (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, out_step_x(:, IPdot(1))); - plot(t, out_step_y(:, IPdot(2))); + plot(t, out_step_x(:, idx.Velocity(1))); + plot(t, out_step_y(:, idx.Velocity(2))); grid on; title('Horizontal Velocity', 'Interpreter', 'latex'); ylabel('Velocity (m / s)', 'Interpreter', 'latex'); @@ -285,18 +297,16 @@ if do_plots % Plot step response from vertical reference to vertical position subplot(2, 2, 3); hold on; - plot(t, out_step_z(:, IP(3))); - % plot([0, T], [1, 1] * ref_value, 'r:'); - % plot(t, out_step_zdes, 'r--'); + plot(t, out_step_z(:, idx.PlotPosition(3))); grid on; - title('Vertical Position Error', 'Interpreter', 'latex'); - ylabel('Error (meters)', 'Interpreter', 'latex'); + title('Vertical Position', 'Interpreter', 'latex'); + ylabel('Distance (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, out_step_z(:, IPdot(3))); + plot(t, out_step_z(:, idx.ErrorVelocity(3))); grid on; title('Vertical Velocity', 'Interpreter', 'latex'); ylabel('Velocity (m / s)', 'Interpreter', 'latex'); |