Update DK iteration and model structure

1 files changed, 236 insertions, 73 deletions

diff --git a/uav_model.m b/uav_model.m
index 6a7f9da..af0be20 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -221,16 +221,13 @@ C = eye(size(A));
 D = zeros(12, 5);
 
 % ------------------------------------------------------------------------
-% Model actuators
+% Model actuators (normalized to [-1, 1]!)
 
-% TODO: better model?
-w = params.actuators.ServoNominalAngularVelocity;
-zeta = 1;
-G_servo = tf(w^2, [1, 2 * zeta * w, w^2]);
+T = params.actuators.ServoSecondsTo60Deg;
+G_servo = tf(1, [T, 1]);
 
-w = 60;
-zeta = 1;
-G_prop = tf(w^2, [1, 2 * zeta * w, w^2]);
+% TODO: measurements for propeller
+G_prop = tf(1, [.1, 1]);
 
 model.actuators = struct( ...
   'FlapServo', G_servo, ...
@@ -240,28 +237,43 @@ model.actuators = struct( ...
 % ------------------------------------------------------------------------
 % 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) * 1 / params.actuators.ServoAbsMaxAngle, ...
-  eye(1) * 1 / params.actuators.PropellerMaxAngularVelocity);
+  eye(4) * params.normalization.FlapAngle, ...
+  eye(1) * params.normalization.ThrustAngularVelocity ...
+);
 
-S_state = blkdiag(...
+S_positions = blkdiag(...
   eye(2) * params.normalization.HPosition, ...
-  eye(1) * params.normalization.VPosition, ...
+  eye(1) * params.normalization.VPosition ...
+);
+
+S_velocities = blkdiag(...
   eye(2) * params.normalization.HSpeed, ...
-  eye(1) * params.normalization.VSpeed, ...
+  eye(1) * params.normalization.VSpeed ...
+);
+
+S_angles = blkdiag(...
   eye(2) * params.normalization.PitchRollAngle, ...
-  eye(1) * params.normalization.YawAngle, ...
-  eye(3) * params.normalization.AngularRate);
+  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 * inv(S_actuators);
+B = B * S_actuators;
 C = inv(S_state) * C;
-D = D * inv(S_actuators);
+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;
@@ -279,7 +291,7 @@ model.linear = struct(...
   'State', xi, 'Inputs', u, ... % state and input variables
   'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized
   'StateSpace', sys, ... % state space objec
-  'InputScalingMatrix', S_actuators, 'OutputScalingMatrix', S_state ...
+  'InputScaling', S_actuators, 'OutputScaling', S_state ...
 );
 
 % ------------------------------------------------------------------------
@@ -293,7 +305,7 @@ fprintf('   %d controllable modes, %d unstabilizable modes.\n', nctrb, nustab);
 fprintf('   %d observable modes, %d undetectable modes.\n', nobsv, nustab);
 
 if nctrb < 12
-  error('Linearized model has less than 12 controllable modes!');
+  fprintf('   Linearized model has less than 12 controllable modes!\n');
 end
 
 if nustab > 0 || nudetb > 0
@@ -301,11 +313,6 @@ if nustab > 0 || nudetb > 0
 end
 
 % ------------------------------------------------------------------------
-% Compute absolute value of error caused by linearization around set point
-
-% TODO
-
-% ------------------------------------------------------------------------
 % Add uncertainties using SIMULINK model
 
 % Load simulink model with uncertainties and pass in parameters
@@ -320,13 +327,13 @@ hws.assignin('uncert', uncert);
 
 % Get uncertain model
 ulmod = linmod('uav_model_uncertain');
-usys = ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d);
+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
-  12, 12; % state uncert, full
+  9, 9; % state uncert, full
 ];
 
 blk_perf = [
@@ -334,29 +341,17 @@ 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, ...
+  '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 ]
@@ -369,33 +364,189 @@ model.uncertain = struct(...
 %   - model.uncertain.Simulink.InputName(model.uncertain.index.InputX)
 %   - model.uncertain.Simulink.OutputName(model.uncertain.index.OutputX)
 %
-% Function make_idx(start, size) is defined below.
-model.uncertain.index = struct(...
-  'InputUncertain',    make_idx( 1, 17), ... % 'v' inputs
-  'InputDisturbance',  make_idx(18,  7), ... % 'w' inputs for noise
-  'InputReference',    make_idx(25,  3), ... % 'w' inputs for reference
-  'InputExogenous',    make_idx(18, 10), ... % 'w' inputs (all of them)
-  'InputNominal',      make_idx(28,  5), ... % 'u' inputs
-  'OutputUncertain',   make_idx( 1, 17), ... % 'z' outputs
-  'OutputError',       make_idx(18, 14), ... % 'e' outputs
-  'OutputNominal',     make_idx(32, 12), ... % 'y' outputs
-  'OutputPlots',       make_idx(44, 10)  ... % 'p' outputs for plots in closed loop
+
+% 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 ...  %  .
 );
 
-idx = model.uncertain.index;
+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 ...  %  .
+);
 
-% Number of inputs
-model.uncertain.Nv = max(size(idx.InputUncertain));
-model.uncertain.Nw = max(size(idx.InputExogenous));
-model.uncertain.Nu = max(size(idx.InputNominal));
+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;
+];
 
-model.uncertain.Nr = max(size(idx.InputReference));
+% 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(idx.OutputUncertain));
-model.uncertain.Ne = max(size(idx.OutputError));
-model.uncertain.Ny = max(size(idx.OutputNominal));
+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
@@ -408,7 +559,7 @@ fprintf('   %d controllable modes, %d unstabilizable modes.\n', nctrb, nustab);
 fprintf('   %d observable modes, %d undetectable modes.\n', nobsv, nustab);
 
 if nctrb < 12
-  error('Uncertain model has less than 12 controllable modes!');
+  fprintf('   Uncertain model has less than 12 controllable modes!');
 end
 
 if nustab > 0 || nudetb > 0
@@ -424,8 +575,14 @@ A = model.uncertain.StateSpace(...
 
 [~, ~, ~, nustab, ~, nudetb] = pbhtest(A);
 
-if nustab > 0 || nudetb > 0
-  fprintf('   Uncertain system has undetectable or uncontrollable A.\n');
+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(...
@@ -435,8 +592,14 @@ B_u = model.uncertain.StateSpace(...
 
 [~, ~, ~, nustab, ~, nudetb] = pbhtest(B_u);
 
-if nustab > 0 || nudetb > 0
-  fprintf('   Uncertain system has undetectable or uncontrollable Bu.\n');
+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(...
@@ -446,8 +609,12 @@ C_y = model.uncertain.StateSpace(...
 
 [~, ~, ~, nustab, ~, nudetb] = pbhtest(C_y);
 
-if nustab > 0 || nudetb > 0
-  fprintf('   Uncertain system has undetectable or uncontrollable Cy.\n');
+if nustab > 0
+    fprintf('   Uncertain system has unstabilizable Cy!\n');
+end
+
+if nudetb > 0
+  fprintf('   Uncertain system has undetectable Cy!\n');
 end
 
 
@@ -474,8 +641,4 @@ end
 
 end
 
-function [indices] = make_idx(start, size)
-  indices = (start:(start + size - 1))';
-end
-
 % vim: ts=2 sw=2 et: