Add MATLAB code

1 files changed, 270 insertions, 0 deletions

diff --git a/uav_model.m b/uav_model.m
new file mode 100644
index 0000000..7b2bf9d
--- /dev/null
+++ b/uav_model.m
@@ -0,0 +1,270 @@
+% 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