Fix assumption checks in uav_model

1 files changed, 74 insertions, 156 deletions

diff --git a/uav_model.m b/uav_model.m
index fda2745..6a7f9da 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -285,74 +285,19 @@ model.linear = struct(...
 % ------------------------------------------------------------------------
 % Check properties of linearized model
 
-eigvals = eig(sys.A);
-
-% Check system controllability / stabilizability
-Wc = ctrb(sys);
-if rank(Wc) < nx
-  fprintf(' - Linearized 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 = [(sys.A - eigvals(i) * eye(nx)), sys.B];
-      if rank(W) < nx
-        % fprintf('  State %d is not stabilizable\n', i);
-        unstabilizable = unstabilizable + 1;
-      end
-    end
-  end
-  if unstabilizable > 0
-    fprintf(' - Linearized system has %d unstabilizable modes!\n', ...
-      unstabilizable);
-  else
-    fprintf('   However, it is stabilizable.\n');
-  end
-end
+[nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(sys);
 
-% Check system observability / detectability
-Wo = obsv(sys);
-if rank(Wo) < nx
-  fprintf(' - Linearized 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 = [sys.C; (sys.A - eigvals(i) * eye(nx))];
-      if rank(W) < nx
-        undetectable = undetectable + 1;
-      end
-    end
-  end
-  if undetectable > 0
-    fprintf(' - Linearized system has %d undetectable modes!\n', ...
-      undetectable);
-  else
-    fprintf('   However, it is detectable.\n')
-  end
-end
+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);
 
-stable = 0;
-unstable = 0;
-for i = 1:nx
-  if real(eigvals(i)) < 0
-    stable = stable + 1;
-  else
-    unstable = unstable + 1;
-  end
+if nctrb < 12
+  error('Linearized model has less than 12 controllable modes!');
 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);
-
-if rank(Wc) < 12
-  error('Uncertain model has less than 12 controllable modes');
+if nustab > 0 || nudetb > 0
+  error('Linearized model has unstabilizable or undetectable modes!');
 end
 
 % ------------------------------------------------------------------------
@@ -455,109 +400,82 @@ 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
+[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);
 
-% 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
+if nctrb < 12
+  error('Uncertain model has less than 12 controllable modes!');
 end
 
-stable = 0;
-unstable = 0;
-for i = 1:nx
-  if real(eigvals(i)) < 0
-    stable = stable + 1;
-  else
-    unstable = unstable + 1;
-  end
+if nustab > 0 || nudetb > 0
+  error('Uncertain model has unstabilizable or undetectable modes!');
 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, ...
-%   model.uncertain.index.InputUncertain ...
-% );
-% 
-% B_u = model.uncertain.StateSpace(...
-%   model.uncertain.index.OutputUncertain, ...
-%   model.uncertain.index.InputNominal ...
-% );
-%
-% % TODO: do PBH test
-% 
-% % 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(D_eu) < min(size(D_eu))
-%   fprintf('D_eu is not full rank!\n')
-% end
-% 
-% if rank(D_yw) < min(size(D_yw))
-%   fprintf('D_yw is not full rank!\n')
-% end
+A = model.uncertain.StateSpace(...
+  model.uncertain.index.OutputUncertain, ...
+  model.uncertain.index.InputUncertain ...
+);
+
+[~, ~, ~, nustab, ~, nudetb] = pbhtest(A);
+
+if nustab > 0 || nudetb > 0
+  fprintf('   Uncertain system has undetectable or uncontrollable A.\n');
+end
+
+B_u = model.uncertain.StateSpace(...
+  model.uncertain.index.OutputUncertain, ...
+  model.uncertain.index.InputNominal ...
+);
+
+[~, ~, ~, nustab, ~, nudetb] = pbhtest(B_u);
+
+if nustab > 0 || nudetb > 0
+  fprintf('   Uncertain system has undetectable or uncontrollable Bu.\n');
+end
+
+C_y = model.uncertain.StateSpace(...
+  model.uncertain.index.OutputNominal, ...
+  model.uncertain.index.InputUncertain ...
+);
+
+[~, ~, ~, nustab, ~, nudetb] = pbhtest(C_y);
+
+if nustab > 0 || nudetb > 0
+  fprintf('   Uncertain system has undetectable or uncontrollable 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
 
 function [indices] = make_idx(start, size)
-  indices = [start:(start + size - 1)]';
+  indices = (start:(start + size - 1))';
 end
 
 % vim: ts=2 sw=2 et: