Update DK-iteration and clean up

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');