Add MATLAB code

4 files changed, 429 insertions, 0 deletions

diff --git a/midterm.m b/midterm.m
new file mode 100644
index 0000000..8f8e5e1
--- /dev/null
+++ b/midterm.m
@@ -0,0 +1,41 @@
+% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich
+
+%% ------------------------------------------------------------------------
+% Clear environment and generate parameters
+
+clear; clc; close all;
+params = uav_params();
+
+%% ------------------------------------------------------------------------
+% Create nominal plant and controller
+
+% Generate nominal model
+model = uav_model(params);
+ctrl = uav_ctrl_nominal(params, model);
+
+% Model dimensions
+nx = model.linear.Nx;
+
+% Open loop path
+L = model.linear.StateSpace * ctrl.K;
+
+% Closed loop path under unitary negative gain
+G = feedback(L, eye(nx, nx));
+
+%% ------------------------------------------------------------------------
+% Simulate nominal controller
+
+%% ------------------------------------------------------------------------
+% Define performance goals
+
+%% ------------------------------------------------------------------------
+% Create uncertain model
+
+%% ------------------------------------------------------------------------
+% Define stability goals
+
+%% ------------------------------------------------------------------------
+% Perform H-infinity design
+
+%% ------------------------------------------------------------------------
+% Perform mu-Analysis
\ No newline at end of file
diff --git a/uav_ctrl_nominal.m b/uav_ctrl_nominal.m
new file mode 100644
index 0000000..d46a1ef
--- /dev/null
+++ b/uav_ctrl_nominal.m
@@ -0,0 +1,37 @@
+% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
+% 
+% Design a nominal controller for UAV.
+
+function [ctrl] = uav_ctrl_nominal(params, model)
+
+% ------------------------------------------------------------------------
+% Design a nominal LQR controller
+
+% Define penalties according to following priorities
+
+q_pos = 10; % penalty on position
+q_vel = 1; % penalty on linear velocity
+q_ang = 100; % high penalty on angles
+q_rate = 1000; % very high penalty on angular velocities
+
+r_ang = 1; % flap movement is cheap
+r_thrust = 10; % thrust is more expensive on the battery
+
+nx = model.linear.Nx;
+nu = model.linear.Nu;
+
+% 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]);
+N = zeros(nx, nu); % no cross terms
+
+% Compute controller
+[K, S, poles] = lqr(model.linear.StateSpace, Q, R, N);
+
+% ------------------------------------------------------------------------
+% Save controller
+
+ctrl = struct();
+ctrl.K = K;
+
+end
\ No newline at end of file
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
diff --git a/uav_params.m b/uav_params.m
new file mode 100644
index 0000000..fb9121f
--- /dev/null
+++ b/uav_params.m
@@ -0,0 +1,81 @@
+% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
+% 
+% Retrieve / compute parameters for UAV.
+
+function [params] = uav_params()
+
+% Unit of measurements unless specified otherwise are
+%   Mass in kg
+%   Lenghts in m
+%   Time in s
+%   Frequencies in Hz
+%   Angular velocities in rad / s
+
+params = struct();
+
+% ------------------------------------------------------------------------
+% Physical constants
+
+params.physics = struct(...
+	'Gravity', 9.81, ...
+	'AirDensity', 1.204 ... % kg / m^3
+);
+
+% ------------------------------------------------------------------------
+% Mechanical measurements
+
+params.mechanical = struct(...
+    'Mass', 4.0, ...
+    'DuctRadius', 46e-3, ...
+    'DuctHeight', 171e-3, ...
+	'FlapZDistance', 98e-3, ... % flap distance along z from center of mass
+    'InertiaTensor', 5e-3 * eye(3), ... % TODO, assume diagonal
+    'GyroscopicInertiaZ', 0 ... % assume no gyroscopic effects for now
+);
+
+% ------------------------------------------------------------------------
+% Actuator limits
+
+params.actuators = struct(...
+	'TurbineMaxSpeed', 620.7, ... % In Hz
+    'ServoAbsMaxAngleDeg', 25, ... % in Degrees
+    'MeasurementDelay', 8e-3 ... % delay caused by sensor fusion algorithm
+);
+
+% ------------------------------------------------------------------------
+% Aerodynamics modelling
+
+% 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;
+F_max = 38.637; % in N (measured)
+k_T = sqrt(F_max / omega_max);
+
+params.aerodynamics = struct(...
+    'ThrustOmegaProp', k_T, ...
+    'FlapArea', 23e-3 * 10e-3 * 1.5, ... % FIXME factor 150% was chosen at random
+    'DragCoefficients', [1, 1] .* 1e-3, ... % TODO
+    'LiftCoefficient', 1e-3 ... % TODO
+);
+
+% ------------------------------------------------------------------------
+% Linearization point of non-linear dynamics.
+
+% Compute theoretical thrust required to make the UAV hover
+% from the relation mg = k * omega^2
+g = params.physics.Gravity;
+m = params.mechanical.Mass;
+k = params.aerodynamics.ThrustOmegaProp;
+
+omega_hover = sqrt(m * g / k);
+
+params.linearization = struct(...
+	'Position', [0; 0; -3], ... % in inertial frame, z points down
+	'Velocity', [0; 0; 0], ... % in inertial frame
+	'Angles', [0; 0; 0], ...   % in body frame
+	'AngularVelocities', [0; 0; 0], ... % in body frame
+	'Inputs', [0; 0; 0; 0; omega_hover] ... % Flaps at rest and turbine at X
+);
+
+end
\ No newline at end of file