% 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. % % * 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 (normalized to [-1, 1]!) T = params.actuators.ServoSecondsTo60Deg; G_servo = tf(1, [T, 1]); % TODO: measurements for propeller G_prop = tf(1, [.1, 1]); model.actuators = struct( ... 'FlapServo', G_servo, ... 'ThrustPropeller', G_prop, ... 'StateSpace', blkdiag(eye(4) * G_servo, G_prop)); % ------------------------------------------------------------------------ % Scale inputs and outputs of linearized model % All scaling matrices scale UP FROM RANGE (-1, 1), i.e. for outputs they can % be used as-is, while for inputs they need to be inverted S_actuators = blkdiag(... eye(4) * params.normalization.FlapAngle, ... eye(1) * params.normalization.ThrustAngularVelocity ... ); S_positions = blkdiag(... eye(2) * params.normalization.HPosition, ... eye(1) * params.normalization.VPosition ... ); S_velocities = blkdiag(... eye(2) * params.normalization.HSpeed, ... eye(1) * params.normalization.VSpeed ... ); S_angles = blkdiag(... eye(2) * params.normalization.PitchRollAngle, ... eye(1) * params.normalization.YawAngle ... ); S_angular_velocities = eye(3) * params.normalization.AngularRate; S_state = blkdiag(S_positions, S_velocities, S_angles, S_angular_velocities); % Scale system matrices to have inputs and outputs between zero and one B = B * S_actuators; C = inv(S_state) * C; D = D * S_actuators; % Create state space object T = params.measurements.SensorFusionDelay; n = params.linearization.PadeApproxOrder; sys = pade(ss(A, B, C, D, 'OutputDelay', T), n); % sys = ss(A, B, C, D); % 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 'InputScaling', S_actuators, 'OutputScaling', S_state ... ); % ------------------------------------------------------------------------ % Check properties of linearized model [nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(sys); fprintf(' - Linearized system has %d states:\n', nx); fprintf(' %d stable modes, %d unstable modes.\n', nsta, nx - nsta); fprintf(' %d controllable modes, %d unstabilizable modes.\n', nctrb, nustab); fprintf(' %d observable modes, %d undetectable modes.\n', nobsv, nustab); if nctrb < 12 fprintf(' Linearized model has less than 12 controllable modes!\n'); end if nustab > 0 || nudetb > 0 error('Linearized model has unstabilizable or undetectable modes!'); end % ------------------------------------------------------------------------ % 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 = minreal(ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d), [], false); % Specify uncertainty block structure for mussv command blk_stab = [ 4, 4; % alpha uncert, full 1, 1; % omega uncert, full 9, 9; % state uncert, full ]; blk_perf = [ blk_stab; 10, 14 % always full ]; % Save uncertain model model.uncertain = struct(... 'Simulink', ulmod, ... 'BlockStructure', blk_stab, ... 'BlockStructurePerf', blk_perf, ... 'StateSpace', usys ... ); % ------------------------------------------------------------------------ % Create indices to take subsystems % 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) % % Size of signals to generate indices input_dims = struct(... 'InputUncertainAlpha', 4, ... % 'v' inputs 'InputUncertainOmega', 1, ... % . 'InputUncertainState', 9, ... % . 'InputDisturbanceWind', 3, ... % 'w' inputs 'InputDisturbanceAlpha', 4, ... % . 'InputReference', 3, ... % . 'InputNominalAlpha', 4, ... % 'u' inputs 'InputNominalOmega', 1 ... % . ); output_dims = struct(... 'OutputUncertainAlpha', 4, ... % 'z' outputs 'OutputUncertainOmega', 1, ... % . 'OutputUncertainState', 9, ... % . 'OutputErrorAlpha', 4, ... % 'e' outputs 'OutputErrorOmega', 1, ... % . 'OutputErrorPosition', 3, ... % . 'OutputErrorVelocity', 3, ... % . 'OutputErrorEulerAngles', 3, ... % . 'OutputNominalPosition', 3, ... % 'y' outputs 'OutputNominalVelocity', 3, ... % . 'OutputNominalEulerAngles', 3, ... % . 'OutputNominalAngularRates', 3, ... % . 'OutputPlotAlpha', 4, ... % plot / "debugging" outputs 'OutputPlotOmega', 1, ... % . 'OutputPlotReference', 3, ... % . 'OutputPlotPosition', 3 ... % . ); model.uncertain.dims = struct(... 'input', input_dims, ... 'output', output_dims ... ); % model.uncertain.dims.OutputError = sum([ % model.uncertain.dims.OutputErrorAlpha; % model.uncertain.dims.OutputErrorOmega; % model.uncertain.dims.OutputErrorPosition; % model.uncertain.dims.OutputErrorVelocity; % model.uncertain.dims.OutputErrorEulerAngles; % ]); % % model.uncertain.dims.OutputNominal; = sum([ % model.uncertain.dims.OutputNominalPosition; % model.uncertain.dims.OutputNominalVelocity; % model.uncertain.dims.OutputNominalEulerAngles; % model.uncertain.dims.OutputNominalAngularRates; % ]); % % model.uncertain.dims.OutputPlot = sum([ % model.uncertain.dims.OutputPlotAlpha; % model.uncertain.dims.OutputPlotOmega; % model.uncertain.dims.OutputPlotReference; % ]); % Create indices model.uncertain.index = struct(); % Create indices for inputs start = 1; fields = fieldnames(input_dims); for f = fields' dim = getfield(input_dims, f{1}); i = (start:(start + dim - 1))'; model.uncertain.index = setfield(model.uncertain.index, f{1}, i); start = start + dim; end % Create indices for output start = 1; fields = fieldnames(output_dims); for f = fields' dim = getfield(output_dims, f{1}); i = (start:(start + dim - 1))'; model.uncertain.index = setfield(model.uncertain.index, f{1}, i); start = start + dim; end % Create groups of inputs model.uncertain.index.InputUncertain = [ model.uncertain.index.InputUncertainAlpha; model.uncertain.index.InputUncertainOmega; model.uncertain.index.InputUncertainState; ]; model.uncertain.index.InputDisturbance = [ model.uncertain.index.InputDisturbanceWind; model.uncertain.index.InputDisturbanceAlpha; ]; model.uncertain.index.InputExogenous = [ model.uncertain.index.InputDisturbance; model.uncertain.index.InputReference; ]; model.uncertain.index.InputNominal = [ model.uncertain.index.InputNominalAlpha; model.uncertain.index.InputNominalOmega; ]; % Create groups of outputs model.uncertain.index.OutputUncertain = [ model.uncertain.index.OutputUncertainAlpha; model.uncertain.index.OutputUncertainOmega; model.uncertain.index.OutputUncertainState; ]; model.uncertain.index.OutputError = [ model.uncertain.index.OutputErrorAlpha; model.uncertain.index.OutputErrorOmega; model.uncertain.index.OutputErrorPosition; model.uncertain.index.OutputErrorVelocity; model.uncertain.index.OutputErrorEulerAngles; ]; model.uncertain.index.OutputNominal = [ model.uncertain.index.OutputNominalPosition; model.uncertain.index.OutputNominalVelocity; model.uncertain.index.OutputNominalEulerAngles; model.uncertain.index.OutputNominalAngularRates; ]; model.uncertain.index.OutputPlot = [ model.uncertain.index.OutputPlotAlpha; model.uncertain.index.OutputPlotOmega; model.uncertain.index.OutputPlotReference; ]; % Number of inputs model.uncertain.Nv = max(size(model.uncertain.index.InputUncertain)); model.uncertain.Nw = max(size(model.uncertain.index.InputExogenous)); model.uncertain.Nu = max(size(model.uncertain.index.InputNominal)); model.uncertain.Nr = max(size(model.uncertain.index.InputReference)); % size of noise is (Nw - Nr) % Number of outputs model.uncertain.Nz = max(size(model.uncertain.index.OutputUncertain)); model.uncertain.Ne = max(size(model.uncertain.index.OutputError)); model.uncertain.Ny = max(size(model.uncertain.index.OutputNominal)); % ------------------------------------------------------------------------ % Scaling matrices for uncertain model % Note howerver that the uncertain (SIMULINK) model is entirely described in % normalized coordinates, so these matrices are to scale the result e.g. for % plotting. % Input matrices scale DOWN TO range (-1, 1) % Output matrices scale UP FROM range (-1, 1) model.uncertain.scaling = struct(); % TODO Scale for 'v' inputs if necessary % Scale for 'w' inputs model.uncertain.scaling.InputUncertainScaling = blkdiag(... S_actuators, ... S_positions ... % reference use same as position ); % Scale for 'u' inputs model.uncertain.scaling.InputNominalScaling = S_actuators; % TODO Scale for 'z' outputs if necessary % Scale for 'e' outputs model.uncertain.scaling.OutputErrorScaling = blkdiag(... S_actuators, ... S_positions, ... S_velocities, ... S_angles ... ); % Scale for 'y' outputs model.uncertain.scaling.OutputNominalScaling = S_state; % Scale for 'p' outputs model.uncertain.scaling.OutputPlotScaling = blkdiag(... S_actuators, ... S_positions, ... % reference same as position S_positions ... ); % ------------------------------------------------------------------------ % Check properties of uncertain model [nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(usys); fprintf(' - Uncertain system has %d states:\n', nx); fprintf(' %d stable modes, %d unstable modes.\n', nsta, nx - nsta); fprintf(' %d controllable modes, %d unstabilizable modes.\n', nctrb, nustab); fprintf(' %d observable modes, %d undetectable modes.\n', nobsv, nustab); if nctrb < 12 fprintf(' Uncertain model has less than 12 controllable modes!'); end if nustab > 0 || nudetb > 0 error('Uncertain model has unstabilizable or undetectable modes!'); end % % Check that (A, B_u, C_y) is stabilizable and detectable A = model.uncertain.StateSpace(... model.uncertain.index.OutputUncertain, ... model.uncertain.index.InputUncertain ... ); [~, ~, ~, nustab, ~, nudetb] = pbhtest(A); if nustab > 0 fprintf(' Uncertain system has unstabilizable A! (%d modes)\n', ... nustab); end if nudetb > 0 fprintf(' Uncertain system has undetectable A! (%d modes)\n', ... nudetb); end B_u = model.uncertain.StateSpace(... model.uncertain.index.OutputUncertain, ... model.uncertain.index.InputNominal ... ); [~, ~, ~, nustab, ~, nudetb] = pbhtest(B_u); if nustab > 0 fprintf(' Uncertain system has unstabilizable Bu! (%d modes)\n', ... nustab); end if nudetb > 0 fprintf(' Uncertain system has undetectable Bu! (%d modes)\n', ... nudetb); end C_y = model.uncertain.StateSpace(... model.uncertain.index.OutputNominal, ... model.uncertain.index.InputUncertain ... ); [~, ~, ~, nustab, ~, nudetb] = pbhtest(C_y); if nustab > 0 fprintf(' Uncertain system has unstabilizable Cy!\n'); end if nudetb > 0 fprintf(' Uncertain system has undetectable Cy!\n'); end % 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(ctrb(D_eu)) < length(D_eu.A) || rank(obsv(D_eu)) < length(D_eu.A) fprintf(' D_eu is not full rank!\n') end if rank(ctrb(D_yw)) < length(D_yw.A) || rank(obsv(D_yw)) < length(D_yw.A) fprintf(' D_yw is not full rank!\n') end % TODO: column rank checks end % vim: ts=2 sw=2 et: