Add more checks, update DK iteration

1 files changed, 122 insertions, 25 deletions

diff --git a/uav_model.m b/uav_model.m
index 1f8f751..fda2745 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -9,7 +9,8 @@
 %    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. 
+%    an operating point specified in the params struct argument. And adding
+%    models for the actuators.
 %
 %  * A uncertain linear model with reference trcking built atop of the linear
 %    model using SIMULINK. The uncertain model contains the performance and
@@ -219,9 +220,22 @@ B = double(vpa(B));
 C = eye(size(A));
 D = zeros(12, 5);
 
-% Number of states, inputs and outputs
-[nx, nu] = size(B);
-[ny, ~] = size(C);
+% ------------------------------------------------------------------------
+% Model actuators
+
+% TODO: better model?
+w = params.actuators.ServoNominalAngularVelocity;
+zeta = 1;
+G_servo = tf(w^2, [1, 2 * zeta * w, w^2]);
+
+w = 60;
+zeta = 1;
+G_prop = tf(w^2, [1, 2 * zeta * w, w^2]);
+
+model.actuators = struct( ...
+  'FlapServo', G_servo, ...
+  'ThrustPropeller', G_prop, ...
+  'StateSpace', blkdiag(eye(4) * G_servo, G_prop));
 
 % ------------------------------------------------------------------------
 % Scale inputs and outputs of linearized model
@@ -247,21 +261,31 @@ D = D * inv(S_actuators);
 % Create state space object
 T = params.measurements.SensorFusionDelay;
 n = params.linearization.PadeApproxOrder;
-sys = minreal(pade(ss(A, B, C, D, 'OutputDelay', T), n), [], false); % slient
+sys = pade(ss(A, B, C, D, 'OutputDelay', T), n);
+
+% 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 object
+  'StateSpace', sys, ... % state space objec
   'InputScalingMatrix', S_actuators, 'OutputScalingMatrix', S_state ...
 );
 
 % ------------------------------------------------------------------------
 % Check properties of linearized model
 
-eigvals = eig(A);
+eigvals = eig(sys.A);
 
 % Check system controllability / stabilizability
 Wc = ctrb(sys);
@@ -274,7 +298,7 @@ if rank(Wc) < nx
   for i = 1:nx
     if real(eigvals(i)) >= 0
       % PBH test
-      W = [(A - eigvals(i) * eye(size(A))), B];
+      W = [(sys.A - eigvals(i) * eye(nx)), sys.B];
       if rank(W) < nx
         % fprintf('  State %d is not stabilizable\n', i);
         unstabilizable = unstabilizable + 1;
@@ -299,7 +323,7 @@ if rank(Wo) < nx
   for i = 1:nx
     if real(eigvals(i)) >= 0
       % PBH test
-      W = [C; (A - eigvals(i) * eye(size(A)))];
+      W = [sys.C; (sys.A - eigvals(i) * eye(nx))];
       if rank(W) < nx
         undetectable = undetectable + 1;
       end
@@ -313,31 +337,37 @@ if rank(Wo) < nx
   end
 end
 
-% ------------------------------------------------------------------------
-% Compute absolute value of error caused by linearization around set point
+stable = 0;
+unstable = 0;
+for i = 1:nx
+  if real(eigvals(i)) < 0
+    stable = stable + 1;
+  else
+    unstable = unstable + 1;
+  end
+end
 
+fprintf(' - Linearized system has %d states:\n', nx);
+fprintf('   %d controllable modes, %d observable modes.\n', rank(Wc), rank(Wo));
+fprintf('   %d stable modes, %d unstable modes.\n', stable, unstable);
 
-% ------------------------------------------------------------------------
-% Model actuators
-
-% TODO: better model, this was tuned "by looking"
-w = 2 * params.actuators.ServoNominalAngularVelocity;
-zeta = .7;
-G_servo = tf(w^2, [1, 2 * zeta * w, w^2]);
+if rank(Wc) < 12
+  error('Uncertain model has less than 12 controllable modes');
+end
 
-w = 6e3;
-zeta = 1;
-G_prop = tf(w^2, [1, 2 * zeta * w, w^2]);
+% ------------------------------------------------------------------------
+% Compute absolute value of error caused by linearization around set point
 
-model.actuators = struct('FlapServo', G_servo, 'ThrustPropeller', G_prop);
+% TODO
 
 % ------------------------------------------------------------------------
 % Add uncertainties using SIMULINK model
 
 % Load simulink model with uncertainties and pass in parameters
 h = load_system('uav_model_uncertain');
-hws = get_param('uav_model_uncertain', 'modelworkspace');
+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);
@@ -375,8 +405,8 @@ S_uncert_out = blkdiag(...
   S_state(1:9, 1:9));
 
 % Save uncertain model
-model.uncertain = struct(...
-  'Simulink', ulmod, ...
+model.uncertain = struct(... 
+  ... % 'Simulink', ulmod, ...
   'BlockStructure', blk_stab, ...
   'BlockStructurePerf', blk_perf, ...
   'StateSpace', usys ...
@@ -425,6 +455,73 @@ model.uncertain.Ny = max(size(idx.OutputNominal));
 % ------------------------------------------------------------------------
 % Check properties of uncertain model
 
+eigvals = eig(usys.A);
+nx = size(usys.A, 1);
+
+% Check system controllability / stabilizability
+Wc = ctrb(usys);
+if rank(Wc) < nx
+  fprintf(' - Uncertain system has %d uncontrollable states!\n', ...
+    (nx - rank(Wc)));
+
+  % Is the system at least stabilizable?
+  unstabilizable = 0;
+  for i = 1:nx
+    if real(eigvals(i)) >= 0
+      % PBH test
+      W = [(usys.A - eigvals(i) * eye(nx)), usys.B];
+      if rank(W) < nx
+        % fprintf('  State %d is not stabilizable\n', i);
+        unstabilizable = unstabilizable + 1;
+      end
+    end
+  end
+  if unstabilizable > 0
+    fprintf(' - Uncertain system has %d unstabilizable modes!\n', ...
+      unstabilizable);
+  else
+    fprintf('   However, it is stabilizable.\n');
+  end
+end
+
+% Check system observability / detectability
+Wo = obsv(usys);
+if rank(Wo) < nx
+  fprintf(' - Uncertain system has %d unobservable states!\n', ...
+    (nx - rank(Wo)));
+  % is the system at least detectable?
+  undetectable = 0;
+  for i = 1:nx
+    if real(eigvals(i)) >= 0
+      % PBH test
+      W = [usys.C; (usys.A - eigvals(i) * eye(nx))];
+      if rank(W) < nx
+        undetectable = undetectable + 1;
+      end
+    end
+  end
+  if undetectable > 0
+    fprintf(' - Uncertain system has %d undetectable modes!\n', ...
+      undetectable);
+  else
+    fprintf('   However, it is detectable.\n')
+  end
+end
+
+stable = 0;
+unstable = 0;
+for i = 1:nx
+  if real(eigvals(i)) < 0
+    stable = stable + 1;
+  else
+    unstable = unstable + 1;
+  end
+end
+
+fprintf(' - Uncertain system has %d states:\n', nx);
+fprintf('   %d controllable modes, %d observable modes.\n', rank(Wc), rank(Wo));
+fprintf('   %d stable modes, %d unstable modes.\n', stable, unstable);
+
 % % Check that (A, B_u, C_y) is stabilizable and detectable
 % A = model.uncertain.StateSpace(...
 %   model.uncertain.index.OutputUncertain, ...