% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
% 
% Compute model for UAV for given set of parameters.

function [model] = uav_model(params)

% ------------------------------------------------------------------------
% 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
];

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

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

% Create state space object
sys = ss(A, B, C, D);

% ------------------------------------------------------------------------
% Measurement model

% TODO: add sensor fusion delay

% ------------------------------------------------------------------------
% Check properties of linearized model
eigvals = eig(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 = [(A - eigvals(i) * eye(size(A))), 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 state!\n', ...
    (nx - rank(Wo)));
  % is the system at least detectable?
  undetectable = 0
  for i = 1:nx
    if real(eigvals(i)) >= 0
      % PBH test
      W = [C; (A - eigvals(i) * eye(size(A)))];
      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.')
  end
end

% ------------------------------------------------------------------------
% Save model

model = struct();

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

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

% 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 object
);

end