Improve H-infinity, system parameters, add simulations and plots

1 files changed, 89 insertions, 20 deletions

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: