Update DK iteration and model structure

1 files changed, 135 insertions, 125 deletions

diff --git a/uav_sim_step.m b/uav_sim_step.m
index 82c05e9..65fb36f 100644
--- a/uav_sim_step.m
+++ b/uav_sim_step.m
@@ -3,127 +3,141 @@
 % 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)
+function [simout] = uav_sim_step(params, model, ctrl, uncert, 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);
+if isfield(ctrl, 'K')
+  hws.assignin('K', ctrl.K);
+else
+  error('You need to provide a controller ctrl.K');
+end
 
-% 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);
+% There is an uncertainty block
+if isfield(uncert, 'Delta')
+  hws.assignin('Delta', uncert.Delta);
+else
+  fprintf(' - No uncertainty error Delta was provided, setting to zero.\n');
+  Delta = tf(0) * zeros(sum(model.uncertain.BlockStructure));
+  hws.assignin('Delta', Delta);
+end
 
-% Check that closed loop is actually stable
-eigvals = eig(P_nom_clp.A);
-nx = size(P_nom_clp.A, 1);
+ulmod_clp = linmod('uav_model_uncertain_clp');
+P_clp = minreal(ss(ulmod_clp.a, ulmod_clp.b, ulmod_clp.c, ulmod_clp.d), [], false);
 
-for i = 1:nx
-  if real(eigvals(i)) >= 0
+% Check that closed loop is actually stable
+nx = size(P_clp.A, 1);
+fprintf(' - Closed loop dynamics have %d states\n', nx);
+if nx < 60
+  [~, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(P_clp);
+  if nx ~= nsta
     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)';
+  fprintf('      %d stable modes, %d unstable modes\n', nsta, nx - nsta);
+  fprintf('      %d controllable modes, %d unstabilizable\n', nctrb, nustab);
+  fprintf('      %d observable modes, %d undetectable\n', nobsv, nudetb);
+else
+  eigvals = eig(P_clp.A);
+  for i = 1:nx
+    if real(eigvals(i)) >= 0
+      error('Closed loop is not stable!');
+    end
+  end
+end
 
-% Indices for p outputs (for plots)
-Ialpha = (27:30)';
-Iomega = 31;
+% Generate indices for closed loop plant
+o = model.uncertain.dims.output;
+i = model.uncertain.dims.input;
+
+dims = struct(...
+  'ErrorAlpha', o.OutputErrorAlpha, ...
+  'ErrorOmega', o.OutputErrorOmega, ...
+  'ErrorPosition', o.OutputErrorPosition, ...
+  'ErrorVelocity', o.OutputErrorVelocity, ...
+  'ErrorEulerAngles', o.OutputErrorEulerAngles, ...
+  'Position', o.OutputNominalPosition, ...
+  'Velocity', o.OutputNominalVelocity, ...
+  'EulerAngles', o.OutputNominalEulerAngles, ...
+  'AngularRates', o.OutputNominalAngularRates, ...
+  'PlotAlpha', o.OutputPlotAlpha, ...
+  'PlotOmega', o.OutputPlotOmega, ...
+  'PlotReference', o.OutputPlotReference, ...
+  'PlotPosition', o.OutputPlotPosition, ...
+  'InputAlpha', i.InputNominalAlpha, ...
+  'InputOmega', i.InputNominalOmega ...
+);
+
+idx = struct();
+start = 1; fields = fieldnames(dims);
+for f = fields'
+  dim = getfield(dims, f{1});
+  i = (start:(start + dim - 1))';
+  idx = setfield(idx, f{1}, i);
+  start = start + dim;
+end
 
-Iualpha = (32:35)';
-Iuomega = 36;
+% Input indices
+idx.InputDisturbanceWind = (1:3)';
+idx.InputDisturbanceFlaps = (4:7)';
+idx.InputReference = (8:10)';
 
-noise = zeros(7, nsamp); % no noise
+% Create noise
+noise = zeros(7, nsamp);
 if do_noise
-  % Noise in percentage
+  % Noise (normalized)
   noise_alpha_amp = (.5 * (pi / 180)) / params.actuators.ServoAbsMaxAngle;
-  noise_wind_amp = .1;
+  noise_wind_amp = .01;
   noise = [noise_wind_amp * randn(3, nsamp);
     noise_alpha_amp * randn(4, nsamp)];
 end
 
+% Step size
+step_size_h = .5  / params.normalization.HPosition;
+step_size_v = .5  / params.normalization.VPosition;
+
 % Create step inputs (normalized)
-ref_step = .5 * ones(1, nsamp); % 1d step function
+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 ];
+in_step_x = [ noise; step_size_h * ref_step; zeros(2, nsamp) ];
+in_step_y = [ noise; zeros(1, nsamp); step_size_h * ref_step; zeros(1, nsamp) ];
+in_step_z = [ noise; zeros(2, nsamp); step_size_v * ref_step ];
 
 % Simulation time
-n_settle_times = 10;
 t = linspace(0, T, nsamp);
 
+% Scale simulation outputs
+S = blkdiag(...
+  model.uncertain.scaling.OutputErrorScaling, ...
+  model.uncertain.scaling.OutputNominalScaling, ...
+  model.uncertain.scaling.OutputPlotScaling, ...
+  model.uncertain.scaling.InputNominalScaling ...
+);
+
 % 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';
+out_step_x_norm = lsim(P_clp, in_step_x, t, 'foh');
+out_step_y_norm = lsim(P_clp, in_step_y, t, 'foh');
+out_step_z_norm = lsim(P_clp, in_step_z, t, 'foh');
+
+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, ...
+  'StepX', out_step_x, ...
+  'StepY', out_step_y, ...
+  'StepZ', out_step_z, ...
+  'StepXNorm', out_step_x_norm, ...
+  'StepYNorm', out_step_y_norm, ...
+  'StepZNorm', 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);
+  'StateSpace', P_clp, ...
+  'index', idx);
 
 if do_plots
   % Conversion factors
@@ -137,7 +151,6 @@ if do_plots
     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 ...
@@ -147,10 +160,10 @@ if do_plots
   % 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(t, out_step_x(:, idx.PlotAlpha(1)) * to_deg);
+  plot(t, out_step_x(:, idx.PlotAlpha(2)) * to_deg);
+  plot(t, out_step_x(:, idx.PlotAlpha(3)) * to_deg);
+  plot(t, out_step_x(:, idx.PlotAlpha(4)) * to_deg);
   plot([0, T], [1, 1] * alpha_max_deg, 'r--');
   plot([0, T], [-1, -1] * alpha_max_deg, 'r--');
   grid on;
@@ -164,10 +177,10 @@ if do_plots
 
   % 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(t, out_step_y(:, idx.PlotAlpha(1)) * to_deg);
+  plot(t, out_step_y(:, idx.PlotAlpha(2)) * to_deg);
+  plot(t, out_step_y(:, idx.PlotAlpha(3)) * to_deg);
+  plot(t, out_step_y(:, idx.PlotAlpha(4)) * to_deg);
   plot([0, T], [1, 1] * alpha_max_deg, 'r--');
   plot([0, T], [-1, -1] * alpha_max_deg, 'r--');
   grid on;
@@ -181,10 +194,10 @@ if do_plots
 
   % 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(t, out_step_z(:, idx.PlotAlpha(1)) * to_deg);
+  plot(t, out_step_z(:, idx.PlotAlpha(2)) * to_deg);
+  plot(t, out_step_z(:, idx.PlotAlpha(3)) * to_deg);
+  plot(t, out_step_z(:, idx.PlotAlpha(4)) * to_deg);
   plot([0, T], [1, 1] * alpha_max_deg, 'r--');
   plot([0, T], [-1, -1] * alpha_max_deg, 'r--');
   grid on;
@@ -198,9 +211,9 @@ if do_plots
 
   % 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);
+  plot(t, out_step_x(:, idx.EulerAngles(1)) * to_deg);
+  plot(t, out_step_x(:, idx.EulerAngles(2)) * to_deg);
+  plot(t, out_step_x(:, idx.EulerAngles(3)) * to_deg);
   grid on;
   xlim([0, T]);
   ylim([-euler_lim_deg, euler_lim_deg]);
@@ -212,9 +225,9 @@ if do_plots
 
   % 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);
+  plot(t, out_step_y(:, idx.EulerAngles(1)) * to_deg);
+  plot(t, out_step_y(:, idx.EulerAngles(2)) * to_deg);
+  plot(t, out_step_y(:, idx.EulerAngles(3)) * to_deg);
   grid on;
   xlim([0, T]);
   ylim([-euler_lim_deg, euler_lim_deg]);
@@ -226,9 +239,9 @@ if do_plots
 
   % 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);
+  plot(t, out_step_z(:, idx.EulerAngles(1)) * to_deg);
+  plot(t, out_step_z(:, idx.EulerAngles(2)) * to_deg);
+  plot(t, out_step_z(:, idx.EulerAngles(3)) * to_deg);
   grid on;
   xlim([0, T]);
   ylim([-euler_lim_deg, euler_lim_deg]);
@@ -245,7 +258,7 @@ if do_plots
     ctrl.Name), 'Interpreter', 'latex');
 
   hold on;
-  step(P_nom_clp(Iomega, Ir(3)) * to_rpm, T);
+  step(P_clp(idx.PlotOmega, idx.InputReference(3)) * to_rpm, T);
   % plot([0, T], [1, 1] * omega_min_rpm, 'r--');
   % plot([0, T], [1, 1] * omega_max_rpm, 'r--');
   grid on;
@@ -263,20 +276,19 @@ if do_plots
 
   % 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--');
+  plot(t, out_step_x(:, idx.PlotPosition(1)));
+  plot(t, out_step_y(:, idx.PlotPosition(2)));
+  plot(t, out_step_x(:, idx.PlotReference(1)), '--');
   grid on;
-  title('Horizontal Position Error', 'Interpreter', 'latex');
-  ylabel('Error (meters)', 'Interpreter', 'latex');
+  title('Horizontal Position', 'Interpreter', 'latex');
+  ylabel('Distance (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)));
+  plot(t, out_step_x(:, idx.Velocity(1)));
+  plot(t, out_step_y(:, idx.Velocity(2)));
   grid on;
   title('Horizontal Velocity', 'Interpreter', 'latex');
   ylabel('Velocity (m / s)', 'Interpreter', 'latex');
@@ -285,18 +297,16 @@ if do_plots
 
   % 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--');
+  plot(t, out_step_z(:, idx.PlotPosition(3)));
   grid on;
-  title('Vertical Position Error', 'Interpreter', 'latex');
-  ylabel('Error (meters)', 'Interpreter', 'latex');
+  title('Vertical Position', 'Interpreter', 'latex');
+  ylabel('Distance (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)));
+  plot(t, out_step_z(:, idx.ErrorVelocity(3)));
   grid on;
   title('Vertical Velocity', 'Interpreter', 'latex');
   ylabel('Velocity (m / s)', 'Interpreter', 'latex');