Update DK-iteration and clean up

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, ...