% 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, 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); % 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); % Check that closed loop is actually stable eigvals = eig(P_nom_clp.A); nx = size(P_nom_clp.A, 1); for i = 1:nx if real(eigvals(i)) >= 0 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)'; % 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 = .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) ]; in_step_z = [ noise; zeros(2, nsamp); ref_step ]; % Simulation time n_settle_times = 10; t = linspace(0, T, nsamp); % 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'; % Return simulation simout = struct(... 'Time', t, ... 'StepX', out_step_x_norm, ... 'StepY', out_step_y_norm, ... 'StepZ', 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); 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 = .5; alpha_max_deg = params.actuators.ServoAbsMaxAngle * 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; % 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([0, T], [1, 1] * alpha_max_deg, 'r--'); plot([0, T], [-1, -1] * alpha_max_deg, 'r--'); grid on; 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'); 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, 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]); 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, 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]); 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, 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]); 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, 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]); 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, 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]); 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); % 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'); 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, 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'); 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))); 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, 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'); 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))); 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: