Fix assumption checks in uav_model

1 files changed, 58 insertions, 0 deletions

diff --git a/pbhtest.m b/pbhtest.m
new file mode 100644
index 0000000..f56aa21
--- /dev/null
+++ b/pbhtest.m
@@ -0,0 +1,58 @@
+% Do A PBH test on system. Given SYS returns:
+%
+%  - Nr. of states
+%  - Nr. of stable states
+%  - Nr. of controllable states
+%  - Nr. of unstabilizable states
+%  - Nr. of observable states
+%  - Nr. of undetectable states
+
+function [nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(sys)
+
+nx = length(sys.A);
+eigvals = eig(sys.A);
+
+% Count number of stable states
+nsta = 0;
+for i = 1:nx
+  if real(eigvals(i)) < 0
+    nsta = nsta + 1;
+  end
+end
+
+% Check system controllability / stabilizability
+Wc = ctrb(sys);
+nctrb = rank(Wc);
+if nctrb < nx
+  % Is the system at least stabilizable?
+  nustab = 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
+        nustab = nustab + 1;
+      end
+    end
+  end
+end
+
+% Check system observability / detectability
+Wo = obsv(sys);
+nobsv = rank(Wo);
+if nobsv < nx
+  % is the system at least detectable?
+  nudetb = 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
+        nudetb = nudetb + 1;
+      end
+    end
+  end
+end
+
+end
+