diff options
Diffstat (limited to 'uav_model.m')
| -rw-r--r-- | uav_model.m | 70 |
1 files changed, 58 insertions, 12 deletions
diff --git a/uav_model.m b/uav_model.m index 3fc1d0a..1f8f751 100644 --- a/uav_model.m +++ b/uav_model.m @@ -3,15 +3,18 @@ % Copyright (C) 2024, Naoki Sean Pross, ETH Zürich. % This work is distributed under a permissive license, see LICENSE.txt % -% This function generates three models: +% This function generates three plant models: % -% * A non-linear symbolic model (cannot be used directly). -% * A linear model obtained by linearizing the the non linear model at an -% operating point specified in the params struct argument. -% * A uncertain linear model 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. +% * 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) % @@ -220,6 +223,27 @@ D = zeros(12, 5); [nx, nu] = size(B); [ny, ~] = size(C); +% ------------------------------------------------------------------------ +% Scale inputs and outputs of linearized model + +S_actuators = blkdiag(... + eye(4) * 1 / params.actuators.ServoAbsMaxAngle, ... + eye(1) * 1 / params.actuators.PropellerMaxAngularVelocity); + +S_state = blkdiag(... + eye(2) * params.normalization.HPosition, ... + eye(1) * params.normalization.VPosition, ... + eye(2) * params.normalization.HSpeed, ... + eye(1) * params.normalization.VSpeed, ... + eye(2) * params.normalization.PitchRollAngle, ... + eye(1) * params.normalization.YawAngle, ... + eye(3) * params.normalization.AngularRate); + +% Scale system matrices to have inputs and outputs between zero and one +B = B * inv(S_actuators); +C = inv(S_state) * C; +D = D * inv(S_actuators); + % Create state space object T = params.measurements.SensorFusionDelay; n = params.linearization.PadeApproxOrder; @@ -230,7 +254,8 @@ 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 + 'StateSpace', sys, ... % state space object + 'InputScalingMatrix', S_actuators, 'OutputScalingMatrix', S_state ... ); % ------------------------------------------------------------------------ @@ -289,14 +314,20 @@ if rank(Wo) < nx end % ------------------------------------------------------------------------ +% Compute absolute value of error caused by linearization around set point + + +% ------------------------------------------------------------------------ % Model actuators % TODO: better model, this was tuned "by looking" -w = 4 * params.actuators.ServoNominalAngularVelocity; +w = 2 * params.actuators.ServoNominalAngularVelocity; zeta = .7; G_servo = tf(w^2, [1, 2 * zeta * w, w^2]); -% figure; step(G_servo * pi / 3); grid on; -G_prop = tf(1,1); + +w = 6e3; +zeta = 1; +G_prop = tf(w^2, [1, 2 * zeta * w, w^2]); model.actuators = struct('FlapServo', G_servo, 'ThrustPropeller', G_prop); @@ -328,6 +359,21 @@ blk_perf = [ 10, 14 % always full ]; +% ------------------------------------------------------------------------ +% Scale inputs and outputs of uncertain model + +% Scaling of reference is same as position +S_ref = blkdiag(... + eye(2) * 1 / params.normalization.HPosition, ... + eye(1) * 1 / params.normalization.VPosition); + +% TODO: finish +S_uncert_out = blkdiag(... + S_actuators, ... + S_state, ... + S_actuators, ... + S_state(1:9, 1:9)); + % Save uncertain model model.uncertain = struct(... 'Simulink', ulmod, ... |