diff options
Diffstat (limited to '')
| -rw-r--r-- | uav.m | 177 | ||||
| -rw-r--r-- | uav_ctrl_lqr.m | 39 | ||||
| -rw-r--r-- | uav_model.m | 66 | ||||
| -rw-r--r-- | uav_model_uncertain.slx | bin | 54074 -> 78345 bytes | |||
| -rw-r--r-- | uav_model_uncertain_clp.slx | bin | 0 -> 85225 bytes | |||
| -rw-r--r-- | uav_params.m | 109 | ||||
| -rw-r--r-- | uav_requirements.m | 123 | ||||
| -rw-r--r-- | uav_sim_step_hinf.m | 282 | ||||
| -rw-r--r-- | uav_sim_step_lqr.m | 211 |
9 files changed, 894 insertions, 113 deletions
@@ -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 Binary files differindex 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 Binary files differnew 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: |