diff options
Diffstat (limited to '')
| -rw-r--r-- | uav_sim_step.m (renamed from uav_sim_step_musyn.m) | 142 |
1 files changed, 79 insertions, 63 deletions
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'); |