path: root/uav_model.m

% 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 plant models: 
% 
%  * A non-linear symbolic model (cannot be used directly) derived from first
%    principles (equations of motion).
%
%  * A linear model obtained by linearizing the the non linear plant model at
%    an operating point specified in the params struct argument. And adding
%    models for the actuators.
%
%  * A uncertain linear model with reference trcking built atop of the linear
%    model using SIMULINK. The uncertain model contains the performance and
%    weighting transfer function given in the arguments perf and params, and is
%    stored in the SIMULINK file uav_model_uncertain.xls.
%
% [MODEL] = UAV_MODEL(PARAMS, PERF, UNCERT)
%
% Arguments:
%   PARAMS  Struct of design parameters and constants generated from uav_params
%   PERF    Struct with transfer functions that describe performance
%           requirements (used for uncertain model).
%   UNCERT  Struct with weighting transfer functions for the uncertainty
%           blocks (used for uncertain model).
%
% Return value:
%   MODEL   Struct with models
%
% See also UAV_PARAMS


function [model] = uav_model(params, perf, uncert)

model = struct();

% ------------------------------------------------------------------------
% Symbolic variables

% Constant scalar physical quantities and dimensions
syms m g rho a b d S k_T c_d c_0 c_l J_r real;
syms J_1 J_2 J_3 real;
J = diag([J_1, J_2, J_3]);

% Scalar position, rotation and velocities
syms x y z xdot ydot zdot real;
syms phi theta p q r real;
psi = sym('psi', 'real'); % shadow MATLAB's psi() function

% Vector position, rotation and velocities
P = [x; y; z];             % position vector (inertial frame)
Pdot = [xdot; ydot; zdot]; % velocity vector (intertial frame)
Theta = [phi; theta; psi]; % attitude vector: [roll pitch yaw] (body frame)
Omega = [p; q; r];         % angular rates (body frame)

% Inputs: flap angles and ducted fan speed
syms alpha_1 alpha_2 alpha_3 alpha_4 omega real;
alpha = [alpha_1; alpha_2; alpha_3; alpha_4];

% Flap angles are measured relative to the body-frame z-axis and considered
% positive / negative with respect to the roll / pitch axis to which they
% are attached to. Reference table:
%
%  angle   attached axis   lift force direction
%                           when angle is positive
%  -------   -------------   ----------------------
%  alpha_1   pos. x axis    y direction
%  alpha_2   pos. y axis   -x direction
%  alpha_3   neg. x axis    y direction
%  alpha_4   neg. y axis   -x direction

% Rotation matrix to change between frames of reference:
% multiplying by R moves from the inertial frame to the body frame
% to go from body frame to inertial frame use R transpose (R is SO(3))
R = [
  cos(theta) * cos(psi), cos(theta) * sin(psi), -sin(theta);
  (sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi)), ...
    (sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi)), ...
    sin(phi) * cos(theta);
  (cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi)), ...
    (cos(phi) * sin(theta) * sin(psi) - sin(phi) * cos(psi)), ...
    cos(phi) * cos(theta);
];

% Matrix to relate Euler angles to angular velocity in the body frame
% i.e. dTheta/dt = U * Omega. To get the angular velocity in intertial
% coordinates use (R * U).
U = [
  1, sin(phi) * tan(theta), cos(phi) * tan(theta);
  0, cos(phi), -sin(phi);
  0, sin(phi) / cos(theta), cos(phi) / cos(theta);
];

% name of unit vectors in inertial frame
uvec_i = [1; 0; 0];
uvec_j = [0; 1; 0];
uvec_k = [0; 0; 1];

% name of unit vectors in body frame
uvec_x = [1; 0; 0];
uvec_y = [0; 1; 0];
uvec_z = [0; 0; 1];

% ------------------------------------------------------------------------
% Nonlinear system dynamics

% Approximate air velocity field magnitude collinear to uvec_z
nu = omega / pi * sqrt(k_T  / (2 * a * rho));

% Aerodynamic force caused by flaps in body frame
F_flap = @(alpha, uvec_n) rho * S * nu^2 / 2 * (...
  (c_d * alpha^2 + c_0) * uvec_z + c_l * alpha * uvec_n);

F_1 = F_flap(alpha_1, uvec_y);
F_2 = F_flap(alpha_2, uvec_x);
F_3 = F_flap(alpha_3, uvec_y);
F_4 = F_flap(alpha_4, uvec_x);

% Torque caused by aerodynamics forces in body frame
tau_1 = cross((d * uvec_z + a/3 * uvec_x), F_1);
tau_2 = cross((d * uvec_z + a/3 * uvec_y), F_2);
tau_3 = cross((d * uvec_z - a/3 * uvec_x), F_3);
tau_4 = cross((d * uvec_z - a/3 * uvec_y), F_4);

% Total force acting on the UAV in the body frame
F = R * (m * g * uvec_k) ... % gravity
  - k_T * omega^2 * uvec_z ... % thrust
  + F_1 + F_2 + F_3 + F_4; % flaps

% Total torque acting on the UAV in the body frame
tau = J_r * omega * R * cross(uvec_k, Omega) + ... % gyroscopic procession
  tau_1 + tau_2 + tau_3 + tau_4; % flaps

% State space form with state variable xi and input u
%
% The 12-dimensional state is given by
%
%  - absolute position (inertial frame) in R^3
%  - absolute velocity (intertial frame) in R^3
%  - Euler angles (body frame) in SO(3)
%  - Angular rates (body frame) in R^3
%
xi = [P; Pdot; Theta; Omega];
u = [alpha; omega];

% Right hand side of dynamics dxi = f(xi, u)
f = [
  Pdot;
  R' * F / m; % translational dynamics
  U * Omega;
  inv(J) * (tau - cross(Omega, J * Omega)); % rotational dynamics
];

% Save function to compute the rotation matrix
model.FrameRot = @(pitch, roll, yaw) ...
  subs(R, [phi, theta, psi], [pitch, roll, yaw]);

% Save equations of non-linear model (algebraic)
model.nonlinear = struct(...
  'State', xi, ...
  'Inputs', u, ...
  'Dynamics', f ...
);

% ------------------------------------------------------------------------
% Linearization at equilibrium

% Equilibrium point
xi_eq = [
  params.linearization.Position;
  params.linearization.Velocity;
  params.linearization.Angles;
  params.linearization.AngularVelocities;
];
u_eq = params.linearization.Inputs;

% Construct linearized state dynamics
A = subs(jacobian(f, xi), [xi; u], [xi_eq; u_eq]);
B = subs(jacobian(f, u), [xi; u], [xi_eq; u_eq]);

% Insert values of parameters
phy = struct(...
  'g', params.physics.Gravity, ...
  'rho', params.physics.AirDensity ...
);
A = subs(A, phy);
B = subs(B, phy);

mech = struct(...
  'm', params.mechanical.Mass, ...
  'a', params.mechanical.DuctRadius, ...
  'b', params.mechanical.DuctHeight, ...
  'd', params.mechanical.FlapZDistance, ...
  'J_1', params.mechanical.InertiaTensor(1, 1), ...
  'J_2', params.mechanical.InertiaTensor(2, 2), ...
  'J_3', params.mechanical.InertiaTensor(3, 3), ...
  'J_r', params.mechanical.GyroscopicInertiaZ ...
);
A = subs(A, mech);
B = subs(B, mech);

aero = struct(...
  'k_T', params.aerodynamics.ThrustOmegaProp, ...
  'S',   params.aerodynamics.FlapArea, ...
  'c_d', params.aerodynamics.DragCoefficients(1), ...
  'c_0', params.aerodynamics.DragCoefficients(2), ...
  'c_l', params.aerodynamics.LiftCoefficient ...
);
A = subs(A, aero);
B = subs(B, aero);

% Evaluate constants like pi, etc and convert to double
A = double(vpa(A));
B = double(vpa(B));

% The state is fully observed via hardware and refined with sensor fusion
% algorithms
C = eye(size(A));
D = zeros(12, 5);

% ------------------------------------------------------------------------
% Model actuators

% TODO: better model?
w = params.actuators.ServoNominalAngularVelocity;
zeta = 1;
G_servo = tf(w^2, [1, 2 * zeta * w, w^2]);

w = 60;
zeta = 1;
G_prop = tf(w^2, [1, 2 * zeta * w, w^2]);

model.actuators = struct( ...
  'FlapServo', G_servo, ...
  'ThrustPropeller', G_prop, ...
  'StateSpace', blkdiag(eye(4) * G_servo, G_prop));

% ------------------------------------------------------------------------
% Scale inputs and outputs of linearized model

S_actuators = blkdiag(...
  eye(4) * 1 / params.actuators.ServoAbsMaxAngle, ...
  eye(1) * 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);

% Scale system matrices to have inputs and outputs between zero and one
B = B * inv(S_actuators);
C = inv(S_state) * C;
D = D * inv(S_actuators);

% Create state space object
T = params.measurements.SensorFusionDelay;
n = params.linearization.PadeApproxOrder;
sys = pade(ss(A, B, C, D, 'OutputDelay', T), n);

% Add actuators
sys = sys * model.actuators.StateSpace;

% Remove unnecessary states
sys = minreal(sys, [], false); % slient

% Number of states, inputs and outputs
[nx, nu] = size(sys.B);
[ny, ~] = size(sys.C);

% Save linearized dynamics (numerical)
model.linear = struct(...
  'Nx', nx, 'Nu', nu, 'Ny', ny, ... % number of states, inputs, and outputs
  'State', xi, 'Inputs', u, ... % state and input variables
  'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized
  'StateSpace', sys, ... % state space objec
  'InputScalingMatrix', S_actuators, 'OutputScalingMatrix', S_state ...
);

% ------------------------------------------------------------------------
% Check properties of linearized model

eigvals = eig(sys.A);

% Check system controllability / stabilizability
Wc = ctrb(sys);
if rank(Wc) < nx
  fprintf(' - Linearized system has %d uncontrollable states!\n', ...
    (nx - rank(Wc)));

  % Is the system at least stabilizable?
  unstabilizable = 0;
  for i = 1:nx
    if real(eigvals(i)) >= 0
      % PBH test
      W = [(sys.A - eigvals(i) * eye(nx)), sys.B];
      if rank(W) < nx
        % fprintf('  State %d is not stabilizable\n', i);
        unstabilizable = unstabilizable + 1;
      end
    end
  end
  if unstabilizable > 0
    fprintf(' - Linearized system has %d unstabilizable modes!\n', ...
      unstabilizable);
  else
    fprintf('   However, it is stabilizable.\n');
  end
end

% Check system observability / detectability
Wo = obsv(sys);
if rank(Wo) < nx
  fprintf(' - Linearized system has %d unobservable states!\n', ...
    (nx - rank(Wo)));
  % is the system at least detectable?
  undetectable = 0;
  for i = 1:nx
    if real(eigvals(i)) >= 0
      % PBH test
      W = [sys.C; (sys.A - eigvals(i) * eye(nx))];
      if rank(W) < nx
        undetectable = undetectable + 1;
      end
    end
  end
  if undetectable > 0
    fprintf(' - Linearized system has %d undetectable modes!\n', ...
      undetectable);
  else
    fprintf('   However, it is detectable.\n')
  end
end

stable = 0;
unstable = 0;
for i = 1:nx
  if real(eigvals(i)) < 0
    stable = stable + 1;
  else
    unstable = unstable + 1;
  end
end

fprintf(' - Linearized system has %d states:\n', nx);
fprintf('   %d controllable modes, %d observable modes.\n', rank(Wc), rank(Wo));
fprintf('   %d stable modes, %d unstable modes.\n', stable, unstable);

if rank(Wc) < 12
  error('Uncertain model has less than 12 controllable modes');
end

% ------------------------------------------------------------------------
% Compute absolute value of error caused by linearization around set point

% TODO

% ------------------------------------------------------------------------
% Add uncertainties using SIMULINK model

% Load simulink model with uncertainties and pass in parameters
h = load_system('uav_model_uncertain');
set_param('uav_model_uncertain', SimulationMode='Normal');

hws = get_param('uav_model_uncertain', 'modelworkspace');
hws.assignin('params', params);
hws.assignin('model', model);
hws.assignin('perf', perf);
hws.assignin('uncert', uncert);

% Get uncertain model
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, 4; % alpha uncert, full
  1, 1; % omega uncert, full
  12, 12; % state uncert, full
];

blk_perf = [
  blk_stab;
  10, 14 % always full
];

% ------------------------------------------------------------------------
% Scale inputs and outputs of uncertain model

% Scaling of reference is same as position
S_ref = blkdiag(...
  eye(2) * 1 / params.normalization.HPosition, ...
  eye(1) * 1 / params.normalization.VPosition);

% TODO: finish
S_uncert_out = blkdiag(...
  S_actuators, ...
  S_state, ...
  S_actuators, ...
  S_state(1:9, 1:9));

% Save uncertain model
model.uncertain = struct(... 
  ... % 'Simulink', ulmod, ...
  'BlockStructure', blk_stab, ...
  'BlockStructurePerf', blk_perf, ...
  'StateSpace', usys ...
);

% The uncertain system is partitioned into the following matrix
%
% [ z ]   [ A    B_w   B_u  ] [ v ]
% [ e ] = [ C_e  D_ew  D_eu ] [ w ]
% [ y ]   [ C_y  D_yw  D_yu ] [ u ]
%
% Struct below provides indices for inputs and outputs of partitioning.
% Check for correctness of these values by inspecting:
%
%   - model.uncertain.Simulink.InputName(model.uncertain.index.InputX)
%   - model.uncertain.Simulink.OutputName(model.uncertain.index.OutputX)
%
% Function make_idx(start, size) is defined below.
model.uncertain.index = struct(...
  'InputUncertain',    make_idx( 1, 17), ... % 'v' 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, 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.Nw = max(size(idx.InputExogenous));
model.uncertain.Nu = max(size(idx.InputNominal));

model.uncertain.Nr = max(size(idx.InputReference));
% size of noise is (Nw - Nr)

% 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

eigvals = eig(usys.A);
nx = size(usys.A, 1);

% Check system controllability / stabilizability
Wc = ctrb(usys);
if rank(Wc) < nx
  fprintf(' - Uncertain system has %d uncontrollable states!\n', ...
    (nx - rank(Wc)));

  % Is the system at least stabilizable?
  unstabilizable = 0;
  for i = 1:nx
    if real(eigvals(i)) >= 0
      % PBH test
      W = [(usys.A - eigvals(i) * eye(nx)), usys.B];
      if rank(W) < nx
        % fprintf('  State %d is not stabilizable\n', i);
        unstabilizable = unstabilizable + 1;
      end
    end
  end
  if unstabilizable > 0
    fprintf(' - Uncertain system has %d unstabilizable modes!\n', ...
      unstabilizable);
  else
    fprintf('   However, it is stabilizable.\n');
  end
end

% Check system observability / detectability
Wo = obsv(usys);
if rank(Wo) < nx
  fprintf(' - Uncertain system has %d unobservable states!\n', ...
    (nx - rank(Wo)));
  % is the system at least detectable?
  undetectable = 0;
  for i = 1:nx
    if real(eigvals(i)) >= 0
      % PBH test
      W = [usys.C; (usys.A - eigvals(i) * eye(nx))];
      if rank(W) < nx
        undetectable = undetectable + 1;
      end
    end
  end
  if undetectable > 0
    fprintf(' - Uncertain system has %d undetectable modes!\n', ...
      undetectable);
  else
    fprintf('   However, it is detectable.\n')
  end
end

stable = 0;
unstable = 0;
for i = 1:nx
  if real(eigvals(i)) < 0
    stable = stable + 1;
  else
    unstable = unstable + 1;
  end
end

fprintf(' - Uncertain system has %d states:\n', nx);
fprintf('   %d controllable modes, %d observable modes.\n', rank(Wc), rank(Wo));
fprintf('   %d stable modes, %d unstable modes.\n', stable, unstable);

% % Check that (A, B_u, C_y) is stabilizable and detectable
% A = model.uncertain.StateSpace(...
%   model.uncertain.index.OutputUncertain, ...
%   model.uncertain.index.InputUncertain ...
% );
% 
% B_u = model.uncertain.StateSpace(...
%   model.uncertain.index.OutputUncertain, ...
%   model.uncertain.index.InputNominal ...
% );
%
% % TODO: do PBH test
% 
% % Check that D_eu and D_yw are full rank
% D_eu = model.uncertain.StateSpace(...
%   model.uncertain.index.OutputError, ...
%   model.uncertain.index.InputNominal ...
% );
% 
% D_yw = model.uncertain.StateSpace(...
%   model.uncertain.index.OutputNominal, ...
%   model.uncertain.index.InputDisturbance ...
% );

% if rank(D_eu) < min(size(D_eu))
%   fprintf('D_eu is not full rank!\n')
% end
% 
% if rank(D_yw) < min(size(D_yw))
%   fprintf('D_yw is not full rank!\n')
% end

end

function [indices] = make_idx(start, size)
  indices = [start:(start + size - 1)]';
end

% vim: ts=2 sw=2 et: