Improve H-infinity, system parameters, add simulations and plots

9 files changed, 894 insertions, 113 deletions

diff --git a/uav.m b/uav.m
index 453bd00..e5f49b2 100644
--- a/uav.m
+++ b/uav.m
@@ -1,75 +1,24 @@
-% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich
-%
 % Controller design for a 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
 
 %  ------------------------------------------------------------------------
 % Clear environment and generate parameters
 
 clear; clc; close all; s = tf('s');
+
+fprintf('Generating system parameters...\n')
 params = uav_params();
+ctrl = struct();
 
-do_plots = false;
+do_plots = true;
 
 % ------------------------------------------------------------------------
 %% Define performance requirements
 
-% Mechanically, flaps are constrained to a max of 20~25 degrees,
-% and they have a maximal angular speed
-alpha_max = params.actuators.ServoAbsMaxAngle;
-alpha_dot_max = params.actuators.ServoNominalAngularVelocity;
-
-W_Palpha = (s + 100 * alpha_dot_max) / (s + alpha_dot_max);
-W_Palpha = alpha_max * W_Palpha / dcgain(W_Palpha); % adjust gain
-
-% Mechanically we have a maximal angular velocity for the propeller in the
-% thruster, also there are a lot of unmodelled dynamics in the thruster
-omega_max = params.actuators.TurbineMaxSpeed;
-W_Pomega = (s + 50 * omega_max) / (s + omega_max);
-
-% We want a nice and smooth movements
-v_xy_max = params.performance.MaxHorizontalSpeed;
-v_z_max = params.performance.MaxVerticalSpeed;
-
-W_Pxy = 1 / (s + 1 / v_xy_max);
-W_Pz = 1 / (s + 1 / v_z_max);
-
-if do_plots
-  % Bode plots of performance requirements
-  figure; hold on;
-
-  bodemag(1/W_Palpha);
-  bodemag(1/W_Pomega);
-  bodemag(1/W_Pxy);
-  bodemag(1/W_Pz);
-
-  grid on;
-  legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ...
-    '$W_{P,xy}$', '$W_{P,z}$', ...
-    'interpreter', 'latex', 'fontSize', 8);
-  title('Performance requirements');
-
-
-  % Step response of position requirements
-  figure; hold on;
-  step(W_Pxy); step(W_Pz);
-  grid on;
-  legend('$W_{P,xy}$', '$W_{P,z}$', 'interpreter', 'latex', 'fontSize', 8);
-  title('Step responses of position performance requirements');
-end
-
-% Construct performance for position vector by combining xy and z
-W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
-W_PPdot = tf(1,1) * eye(3);
-W_PTheta = tf(1,1) * eye(3);
-W_POmega = tf(1,1) * eye(3);
-
-perf = struct(...
-  'FlapAngle', W_Palpha * eye(4), ...
-  'Thrust', W_Pomega, ...
-  'Position', W_PP, ...
-  'Velocity', W_PPdot, ...
-  'Angle', W_PTheta, ...
-  'AngularVelocity', W_POmega); 
+fprintf('Generating performance requirements...\n')
+perf = uav_requirements(params, do_plots);
 
 %  ------------------------------------------------------------------------
 %% Define stability requirements
@@ -78,48 +27,112 @@ W_malpha = tf(1,1);
 W_momega = tf(1,1);
 W_mState = tf(1,1);
 
-if do_plots
-  figure; hold on;
-
-  bodemag(W_malpha);
-  bodemag(W_momega);
-  bodemag(W_mState);
-
-  grid on;
-  legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ...
-    '$W_{m,\Theta}$', '$W_{m,\Omega}$', ...
-    'interpreter', 'latex', 'fontSize', 8);
-  title('Uncertainties')
-end
+% if do_plots
+%   figure; hold on;
+%
+%   bodemag(W_malpha);
+%   bodemag(W_momega);
+%   bodemag(W_mState);
+%
+%   grid on;
+%   legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ...
+%     '$W_{m,\Theta}$', '$W_{m,\Omega}$', ...
+%     'Interpreter', 'latex', 'fontSize', 8);
+%   title('Uncertainties')
+% end
 
 uncert = struct(...
   'FlapAngle', W_malpha * eye(4), ...
   'Thrust', W_momega, ...
   'StateLinApprox', W_mState * eye(12));
-  
+
 % ------------------------------------------------------------------------
-% Create UAV model
+%% Create UAV model
 
+fprintf('Generating system model...\n');
 model = uav_model(params, perf, uncert);
 
 % ------------------------------------------------------------------------
+%% Perform LQR design
+
+% fprintf('Performing LQR controller design...\n')
+% ctrl.lqr = uav_ctrl_lqr(params, model);
+
+% ------------------------------------------------------------------------
 %% Perform H-infinity design
 
+fprintf('Performing H-infinty controller design...\n')
+
 idx = model.uncertain.index;
-idx_ey = [idx.OutputError; idx.OutputNominal];
-idx_wu = [idx.InputDisturbance; idx.InputReference; idx.InputNominal];
+P = model.uncertain.StateSpace;
+
+% Get nominal system without uncertainty (for lower LFT)
+P_nom = minreal(P([idx.OutputError; idx.OutputNominal], ...
+                  [idx.InputExogenous; idx.InputNominal]));
 
 nmeas = max(size(idx.OutputNominal)); % size of y
 nctrl = max(size(idx.InputNominal)); % size of u
 
-% Get nominal system without uncertainty (lower LFT)
-G = minreal(model.uncertain.StateSpace(idx_ey, idx_wu));
+hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
+  'AutoScale', 'off', 'RelTol', 1e-3);
+[K_inf, ~, gamma, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt);
+ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_inf);
+
+if gamma >= 1
+  fprintf('Failed to syntesize controller (closed loop is unstable).\n')
+end
+
+%  ------------------------------------------------------------------------
+%% Measure Performance
+
+fprintf('Simulating closed loop...\n');
 
-hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', 'RelTol', 0.01);
-[K_inf, N_inf, gamma, info] = hinfsyn(G, nmeas, nctrl, hinfopt);
+nsamples = 500;
+do_noise = true;
+% uav_sim_step(params, model, ctrl.lqr, nsamples, do_plots);
+
+simout = uav_sim_step_hinf(params, model, ctrl.hinf, nsamples, do_plots, do_noise);
+
+fprintf('Writing simulation results...\n');
+cols = [
+    simout.StepX(:, simout.index.Position), ...
+    simout.StepX(:, simout.index.Velocity), ...
+    simout.StepX(:, simout.index.FlapAngles) * 180 / pi, ...
+    simout.StepX(:, simout.index.Angles) * 180 / pi];
+
+writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
 
 %  ------------------------------------------------------------------------
-% Verify performance satisfaction
+%% Verify performance satisfaction via mu-analysis
+
+% omega = logspace(-3, 3, 250);
+%
+% N_inf = lft(P, K_inf);
+% N_inf_frd = frd(N_inf, omega);
+%
+% % robust stability
+% [mu_inf_rs, ~] = mussv(N_inf_frd(idx.OutputUncertain, idx.InputUncertain), ...
+%             model.uncertain.BlockStructure);
+%
+% % robust performance
+% blk_perf = [model.uncertain.BlockStructure;
+%             model.uncertain.BlockStructurePerf];
+%
+% [mu_inf_rp, ~] = mussv(N_inf_frd, blk_perf);
+%
+% % nominal performance
+% mu_inf_np = svd(N_inf_frd(idx.OutputError, idx.InputExogenous));
+%
+% if do_plots
+%   figure; hold on;
+%   bodemag(mu_inf_rs(1));
+%   bodemag(mu_inf_np(1));
+%   bodemag(mu_inf_rs(2));
+%   bodemag(mu_inf_np(2));
+%
+%   grid on;
+%   title('$\mu_\Delta(N)$', 'Interpreter', 'latex');
+% end
 
 %  ------------------------------------------------------------------------
 % Perform mu-Analysis & DK iteration
diff --git a/uav_ctrl_lqr.m b/uav_ctrl_lqr.m
new file mode 100644
index 0000000..8063c64
--- /dev/null
+++ b/uav_ctrl_lqr.m
@@ -0,0 +1,39 @@
+% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
+% 
+% Design a nominal controller for UAV.
+
+function [ctrl_lqr] = uav_ctrl_lqr(params, model)
+
+% ------------------------------------------------------------------------
+% Design a Kalman filter for state estimation
+
+G = model.linear.StateSpace
+[L, P, E] = lqe(G.A, G.
+
+% ------------------------------------------------------------------------
+% Design a nominal LQR controller
+
+% Define penalties according to following priorities
+
+q_pos = 1; % penalty on position
+q_vel = 10; % penalty on linear velocity
+q_ang = 10; % high penalty on angles
+q_rate = 10; % very high penalty on angular velocities
+
+r_ang = 1; % flap movement is cheap
+r_thrust = 10; % thrust is more expensive on the battery
+
+% LQR design matrices
+Q = kron(diag([q_pos, q_vel, q_ang, q_rate]), eye(3));
+R = diag([r_ang, r_ang, r_ang, r_ang, r_thrust]);
+
+% Compute controller using output lqr
+[K, S, poles] = lqry(model.linear.StateSpace, Q, R);
+
+% ------------------------------------------------------------------------
+% Save controller
+
+ctrl_lqr = struct('Name', 'LQR', 'K', K);
+
+end
+% vim: ts=2 sw=2 et:
diff --git a/uav_model.m b/uav_model.m
index 747f1b7..4fddae9 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -1,6 +1,8 @@
 % Compute models for Ducted Fan VTOL micro-UAV for given set of parameters. 
 %
 % Copyright (C) 2024, Naoki Sean Pross, ETH Zürich.
+% This work is distributed under a permissive license, see LICENSE.txt
+%
 % This function generates three models: 
 % 
 %  * A non-linear symbolic model (cannot be used directly).
@@ -219,10 +221,9 @@ D = zeros(12, 5);
 [ny, ~] = size(C);
 
 % Create state space object
-sys = ss(A, B, C, D);
-
-% T = params.actuators.MeasurementDelay;
-% sys = ss(A, B, C, D, 'OutputDelay', T);
+T = params.measurements.SensorFusionDelay;
+n = params.linearization.PadeApproxOrder;
+sys = minreal(pade(ss(A, B, C, D, 'OutputDelay', T), n));
 
 % Save linearized dynamics (numerical)
 model.linear = struct(...
@@ -263,7 +264,7 @@ if rank(Wc) < nx
   end
 end
 
-% Check  system observability / detectability
+% Check system observability / detectability
 Wo = obsv(sys);
 if rank(Wo) < nx
   fprintf('Linearized system has %d unobservable states!\n', ...
@@ -283,11 +284,23 @@ if rank(Wo) < nx
     fprintf('Linearized system has %d undetectable modes!\n', ...
       undetectable);
   else
-    fprintf('However, it is detectable.')
+    fprintf('However, it is detectable.\n')
   end
 end
 
 % ------------------------------------------------------------------------
+% Model actuators
+
+% TODO: better model, this was tuned "by looking"
+w = 4 * params.actuators.ServoNominalAngularVelocity;
+zeta = .7;
+G_servo = tf(w^2, [1, 2 * zeta * w, w^2]);
+% figure; step(G_servo * pi / 3); grid on;
+G_prop = tf(1,1);
+
+model.actuators = struct('FlapServo', G_servo, 'ThrustPropeller', G_prop);
+
+% ------------------------------------------------------------------------
 % Add uncertainties using SIMULINK model
 
 % Load simulink model with uncertainties and pass in parameters
@@ -303,9 +316,25 @@ hws.assignin('uncert', uncert);
 ulmod = linmod('uav_model_uncertain');
 usys = ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d);
 
+% Specify uncertainty block structure for mussv command
+blk_stab = [
+  4, 0; % alpha
+  1, 0; % omega
+  12, 12; % state
+];
+
+blk_perf = [
+  4, 0; % alpha
+  1, 0; % omega
+  3, 3; % position
+  3, 3; % velocity
+];
+
 % Save uncertain model
 model.uncertain = struct(...
   'Simulink', ulmod, ...
+  'BlockStructure', blk_stab, ...
+  'BlockStructurePerf', blk_perf, ...
   'StateSpace', usys ...
 );
 
@@ -324,14 +353,29 @@ model.uncertain = struct(...
 % Function make_idx(start, size) is defined below.
 model.uncertain.index = struct(...
   'InputUncertain',    make_idx( 1, 17), ... % 'v' inputs
-  'InputDisturbance',  make_idx(18, 12), ... % 'w' inputs for noise
-  'InputReference',    make_idx(30, 12), ... % 'w' inputs for reference
-  'InputNominal',      make_idx(42,  5), ... % 'u' inputs
+  'InputDisturbance',  make_idx(18,  7), ... % 'w' inputs for noise
+  'InputReference',    make_idx(25,  3), ... % 'w' inputs for reference
+  'InputExogenous',    make_idx(18, 10), ... % 'w' inputs (all of them)
+  'InputNominal',      make_idx(28,  5), ... % 'u' inputs
   'OutputUncertain',   make_idx( 1, 17), ... % 'z' outputs
-  'OutputError',       make_idx(18, 17), ... % 'e' outputs
-  'OutputNominal',     make_idx(35, 12)  ... % 'y' outputs
+  'OutputError',       make_idx(18, 14), ... % 'e' outputs
+  'OutputNominal',     make_idx(32, 12), ... % 'y' outputs
+  'OutputPlots',       make_idx(44, 10)  ... % 'p' outputs for plots in closed loop
 );
 
+idx = model.uncertain.index;
+
+% Number of inputs
+model.uncertain.Nv = max(size(idx.InputUncertain));
+model.uncertain.Nr = max(size(idx.InputReference));
+model.uncertain.Nw = max(size(idx.InputExogenous));
+model.uncertain.Nu = max(size(idx.InputNominal));
+
+% Number of outputs
+model.uncertain.Nz = max(size(idx.OutputUncertain));
+model.uncertain.Ne = max(size(idx.OutputError));
+model.uncertain.Ny = max(size(idx.OutputNominal));
+
 % ------------------------------------------------------------------------
 % Check properties of uncertain model
 
diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx
index b958b25..b863139 100644
--- a/uav_model_uncertain.slx
+++ b/uav_model_uncertain.slx
diff --git a/uav_model_uncertain_clp.slx b/uav_model_uncertain_clp.slx
new file mode 100644
index 0000000..7a17a21
--- /dev/null
+++ b/uav_model_uncertain_clp.slx
diff --git a/uav_params.m b/uav_params.m
index 7bed89c..da8a19b 100644
--- a/uav_params.m
+++ b/uav_params.m
@@ -1,6 +1,7 @@
 % Retrieve or compute parameters for 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 [params] = uav_params()
 
@@ -24,31 +25,49 @@ params.physics = struct(...
 % ------------------------------------------------------------------------
 % Mechanical measurements
 
+% Inertia marix at center of mass,
+% numbers from CAD are in g * mm^2, convert to kg * m^2
+J = [
+  5.856E+08, 1.121E+06, -3.506E+05;
+  1.121E+06, 4.222E+08, 6.643E+06;
+  -3.506E+05, 6.643E+06, 4.678E+08
+] * 1e-9;
+
+% Approximate propeller with a disk
+m_prop = 200e-3; % mass of propeller
+r_prop = 85e-3; % radius of propeller
+J_prop = .5 * m_prop * r_prop^2;
+
 params.mechanical = struct(...
-  'Mass', 4.0, ...
+  'Mass', 3.0, ...
   'DuctRadius', 46e-3, ...
   'DuctHeight', 171e-3, ...
   'FlapZDistance', 98e-3, ... % flap distance along z from center of mass
-  'InertiaTensor', 5e-3 * eye(3), ... % TODO true values, assume diagonal
-  'GyroscopicInertiaZ', 0 ... % assume no gyroscopic effects for now
+  'InertiaTensor', J, ...
+  'GyroscopicInertiaZ', J_prop ... % assume small angle
 );
 
 % ------------------------------------------------------------------------
 % Actuator limits and measurements
 
-% Cheap servos usually give a "speed" in seconds / 60 degrees without load
-servo_speed = 0.19; % seconds per 60 deg
-servo_angular_velocity = (pi / 3) / servo_speed;
+% Servos usually give a "speed" in seconds / 60 degrees without load
+servo_speed = 0.13; % seconds / 60 deg
+servo_angular_velocity = (60 / servo_speed) * (pi / 180); % rad / s
 
 params.actuators = struct(...
-  'TurbineMaxSpeed', 620.7, ... % in rad / s
-  'ServoAbsMaxAngle', 25 * pi / 180, ... % in radians
+  'PropellerMaxAngularVelocity', 620.7, ... % in rad / s
+  'ServoAbsMaxAngle', 20 * pi / 180, ... % in radians
+  'ServoMaxTorque', 4.3 * 1e-2, ... % in kg / m
   'ServoNominalAngularVelocity', servo_angular_velocity ...
 );
 
+% IMU runs in NDOF mode
 params.measurements = struct(...
-  'SensorFusionBandwidth', 1e3, ...
-  'SensorFusionDelay', 8e-3 ...
+  'SensorFusionDelay', 20e-3, ... % in s
+  'LIDARAccuracy', 10e-3, ... % in m
+  'LIDARMaxDistance', 40, ... % inm
+  'LIDARBandwidth', 100, ... % in Hz for z position
+  'IMUBandwidth', 100 ... % in Hz
 );
 
 % ------------------------------------------------------------------------
@@ -57,23 +76,26 @@ params.measurements = struct(...
 % Compute thrust proportionality factor from actuator limits
 % from thrust relation F = k * omega^2.
 % FIXME: this is not ideal, need better measurements
-omega_max = params.actuators.TurbineMaxSpeed;
+omega_max = params.actuators.PropellerMaxAngularVelocity;
 F_max = 38.637; % in N (measured)
-k_T = sqrt(F_max / omega_max);
+k_T = F_max / omega_max^2;
 
+% FIXME: LiftCoefficient comes from
+% https://scienceworld.wolfram.com/physics/LiftCoefficient.html
 params.aerodynamics = struct(...
   'ThrustOmegaProp', k_T, ... % in s^2 / (rad * N)
-  'FlapArea', 23e-3 * 10e-3 * 1.5, ... % FIXME factor 150% was chosen at random
-  'DragCoefficients', [1, 4] .* 1e-3, ... % TODO
-  'LiftCoefficient', 1e-3 ... % TODO
+  'FlapArea', 23e-3 * 10e-3, ... % in m^2
+  'DragCoefficients', [1, .1], ... % TODO
+  'LiftCoefficient', 2 * pi ... % TODO
 );
 
+
 % ------------------------------------------------------------------------
 % Linearization point of non-linear dynamics.
 
 % Compute theoretical thrust required to make the UAV hover
 % from the relation mg = k * omega^2
-% FIXME: This value should probably be replaced with a measurement 
+% FIXME: This value should probably be replaced with a measurement
 g = params.physics.Gravity;
 m = params.mechanical.Mass;
 k = params.aerodynamics.ThrustOmegaProp;
@@ -81,9 +103,10 @@ k = params.aerodynamics.ThrustOmegaProp;
 omega_hover = sqrt(m * g / k);
 
 params.linearization = struct(...
-  'Position', [0; 0; -3], ... % in inertial frame, z points down
+  'PadeApproxOrder', 2, ...
+  'Position', [0; 0; -2], ... % in inertial frame, z points down
   'Velocity', [0; 0; 0], ... % in inertial frame
-  'Angles', [0; 0; 0], ...   % in body frame
+  'Angles', [0; 0; pi / 4], ...   % in body frame
   'AngularVelocities', [0; 0; 0], ... % in body frame
   'Inputs', [0; 0; 0; 0; omega_hover] ... % Flaps at rest and turbine at X
 );
@@ -92,9 +115,55 @@ params.linearization = struct(...
 % Performance requirements
 
 params.performance = struct(...
-  'MaxHorizontalSpeed', 1e-2, ... % in m / s
-  'MaxVerticalSpeed', 5e-2 ... % in m / s
+  'ReferencePosMaxDistance', 1, ... % m
+  'HorizontalPosMaxError', 2, ... % m
+  'HorizontalMaxSpeed', 2, ... % m / s
+  'HorizontalSettleTime', 6, ... % s
+  'VerticalPosMaxError', 2, ... % m
+  'VerticalMaxSpeed', 2, ... % m / s
+  'VerticalSettleTime', 4, ... % s
+  'AngleMaxPitchRoll', 10 * pi / 180, ... % in rad
+  'AngleMaxYaw', pi, ... % rad
+  'AngleMaxAngularRate', 20 * pi / 180 ... % rad / s
 );
 
+% Scaling matrices (because signals are normalized wrt parameter performance)
+p = params.performance;
+
+params.ErrorScalingMatrix = blkdiag(...
+  eye(4) * params.actuators.ServoAbsMaxAngle, ...
+  params.actuators.PropellerMaxAngularVelocity ...
+    - params.linearization.Inputs(5), ...
+  eye(2) * p.HorizontalPosMaxError, ...
+  p.VerticalPosMaxError, ...
+  eye(2) * p.HorizontalMaxSpeed, ...
+  p.VerticalMaxSpeed, ...
+  eye(2) * p.AngleMaxPitchRoll, ...
+  p.AngleMaxYaw);
+
+params.OutputScalingMatrix = blkdiag(...
+  eye(2) * p.HorizontalPosMaxError, ...
+  p.VerticalPosMaxError, ...
+  eye(2) * p.HorizontalMaxSpeed, ...
+  p.VerticalMaxSpeed, ...
+  eye(2) * p.AngleMaxPitchRoll, ...
+  p.AngleMaxYaw, ...
+  eye(3) * p.AngleMaxAngularRate);
+
+
+horiz_settle_speed = (1 - exp(-1)) / params.performance.HorizontalSettleTime;
+if horiz_settle_speed > params.performance.HorizontalMaxSpeed
+  fprintf(['Contradictory performance requirements: ' ...
+    'velocity needed to attain horizontal settle time is ' ...
+    'higher than HorizontalMaxSpeed']);
+end
+
+vert_settle_speed = (1 - exp(-1)) / params.performance.VerticalSettleTime;
+if vert_settle_speed > params.performance.VerticalMaxSpeed
+  fprintf(['Contradictory performance requirements: ' ...
+    'velocity needed to attain vertical settle time is ' ...
+    'higher than VerticalMaxSpeed']);
+end
+
 end
 % vim: ts=2 sw=2 et:
diff --git a/uav_requirements.m b/uav_requirements.m
new file mode 100644
index 0000000..9ccaeea
--- /dev/null
+++ b/uav_requirements.m
@@ -0,0 +1,123 @@
+% Generate transfer functions for loop shaping performance requirements
+% from parameters specified in uav_params.m
+%
+% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
+% This work is distributed under a permissive license, see LICENSE.txt
+%
+% Arguments:
+%   PARAMS  Struct of design parameters and constants generated by uav_params
+%   PLOT    When set to 'true' it plots the inverse magnitude of the
+%           performance transfer function
+%
+% Return value:
+%   MODEL   Struct performance transfer functions
+
+
+function [perf] = uav_requirements(params, plot)
+
+% Laplace variable
+s = tf('s');
+
+alpha_max = params.actuators.ServoAbsMaxAngle;
+alpha_max_omega = params.actuators.ServoNominalAngularVelocity;
+
+T_xy = params.performance.HorizontalSettleTime;
+T_z = params.performance.VerticalSettleTime;
+
+omega_nxy = 5 / T_xy;
+omega_nz = 10 / T_z;
+
+% W_Palpha = 1 / (s^2 + 2 * alpha_max_omega * s + alpha_max_omega^2);
+% W_Palpha = (1 - W_Palpha / dcgain(W_Palpha)) * .8;
+% W_Pomega = (1 - 1 / (T_z / 5 * s + 1)) * .1;
+
+W_Palpha = make_weight(alpha_max_omega, 15, 1.1, 3);
+W_Pomega = make_weight(omega_nz, 50, 10);
+
+% zeta = 1; % Almost critically damped
+% W_Pxy = 1 / (s^2  + 2 * zeta * omega_nxy * s + omega_nxy^2);
+% W_Pxy = 1 * W_Pxy / dcgain(W_Pxy);
+% W_Pz  = 1 / (s^2 + 2 * zeta * omega_nz * s + omega_nz^2);
+% W_Pz  = 1 * W_Pz / dcgain(W_Pz);
+
+W_Pxy = make_weight(omega_nxy, 2, 5);
+W_Pz = make_weight(omega_nz, 1, 10);
+
+% Set a speed limit
+W_Pxydot = .2 * tf(1, 1);
+W_Pzdot = .2 * tf(1, 1);
+ 
+W_Pphitheta = .01 * tf(1, [.1, 1]);
+W_Ppsi = .01 * tf(1, 1); % don't care
+
+W_PTheta = tf(1, [.1, 1]) * eye(3);
+
+% Construct performance vector by combining xy and z
+W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
+W_PPdot = blkdiag(W_Pxydot * eye(2), W_Pzdot);
+W_PTheta = blkdiag(W_Pphitheta * eye(2), W_Ppsi);
+
+perf = struct(...
+  'FlapAngle', W_Palpha * eye(4), ...
+  'Thrust', W_Pomega, ...
+  'Position', W_PP, ...
+  'Velocity', W_PPdot, ...
+  'Angles', W_PTheta);
+
+if plot
+  % Bode plots of performance requirements
+  figure; hold on;
+
+  bodemag(W_Palpha);
+  bodemag(W_Pomega);
+  bodemag(W_Pxy);
+  bodemag(W_Pz);
+  bodemag(W_Pxydot);
+  bodemag(W_Pzdot);
+  bodemag(W_Pphitheta);
+  bodemag(W_Ppsi);
+
+  grid on;
+  legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ...
+    '$W_{P,xy}$', '$W_{P,z}$', ...
+    '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
+    '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ...
+    'interpreter', 'latex', 'fontSize', 8);
+  title('Performance Requirements');
+
+  % Step response of position requirements
+  figure; hold on;
+  step(W_Pxy); step(W_Pz);
+  step(W_Pxydot); step(W_Pzdot);
+  step(W_Palpha);
+  step(W_Pomega);
+  grid on;
+  legend('$W_{P,xy}$', '$W_{P,z}$', ...
+    '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
+    '$W_{P,\alpha}$', '$W_{P,\omega}$', ...
+    'interpreter', 'latex', 'fontSize', 8);
+  title('Step responses of performance requirements');
+end
+
+end
+
+% Make a n-order performance weight function
+%
+% Arguments:
+%   OMEGA  Cutting frequency (-3dB)
+%   A      Magnitude at DC, i.e. |Wp(0)|
+%   M      Magnitude at infinity, i.e. |Wp(inf)|
+%   ORD    Order
+function [Wp] = make_weight(omega, A, M, ord)
+
+if nargin > 3
+  n = ord;
+else
+  n = 1;
+end
+
+s = tf('s');
+Wp = (s / (M^(1/n)) + omega)^n / (s + omega * A^(1/n))^n;
+
+end
+% vim: ts=2 sw=2 et:
diff --git a/uav_sim_step_hinf.m b/uav_sim_step_hinf.m
new file mode 100644
index 0000000..71510b8
--- /dev/null
+++ b/uav_sim_step_hinf.m
@@ -0,0 +1,282 @@
+% 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_hinf(params, model, ctrl, nsamp, 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));
+
+% 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)';
+% size([Iealpha; Ieomega; IeP; IePdot; IeTheta])
+
+% 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 = 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);
+
+% Return simulation
+simout = struct(...
+  'TimeXY', t_xy, ...
+  'StepX', out_step_x, ...
+  'StepY', out_step_y, ...
+  '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     = 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:
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: