diff options
Diffstat (limited to 'uav_sim_step_musyn.m')
| -rw-r--r-- | uav_sim_step_musyn.m | 282 |
1 files changed, 0 insertions, 282 deletions
diff --git a/uav_sim_step_musyn.m b/uav_sim_step_musyn.m deleted file mode 100644 index 3af2a76..0000000 --- a/uav_sim_step_musyn.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_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: |