diff options
Diffstat (limited to 'uav_sim_step_lqr.m')
| -rw-r--r-- | uav_sim_step_lqr.m | 211 |
1 files changed, 211 insertions, 0 deletions
diff --git a/uav_sim_step_lqr.m b/uav_sim_step_lqr.m new file mode 100644 index 0000000..e672d35 --- /dev/null +++ b/uav_sim_step_lqr.m @@ -0,0 +1,211 @@ +% 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_lqr(params, model, ctrl, nsamp, do_plots) + +% TODO: Close loop + +% Create step inputs (normalized) +noise = zeros(7, nsamp); % no noise +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); + +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: |