Update DK-iteration and clean up

1 files changed, 298 insertions, 0 deletions

diff --git a/uav_sim_step.m b/uav_sim_step.m
new file mode 100644
index 0000000..973a928
--- /dev/null
+++ b/uav_sim_step.m
@@ -0,0 +1,298 @@
+% 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);
+
+% 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: