Merge "Add a helper class for implementing intrusive linked lists"
diff --git a/doc/TODO.bazel b/doc/TODO.bazel
deleted file mode 100644
index 9ffd12b..0000000
--- a/doc/TODO.bazel
+++ /dev/null
@@ -1,4 +0,0 @@
-//y2014/prime:All needs to conditionally have //y2014/wpilib:wpilib_interface.
- y2015 and bot3 too
-
-Need to get tcmalloc hooked in.
diff --git a/doc/clang-3.5.ctl b/doc/clang-3.5.ctl
deleted file mode 100644
index b20c52a..0000000
--- a/doc/clang-3.5.ctl
+++ /dev/null
@@ -1,13 +0,0 @@
-Source: llvm-toolchain-snapshot
-Section: misc
-Priority: optional
-Homepage: http://www.llvm.org/
-Standards-Version: 3.9.2
-Package: clang-3.5
-Version: 1:3.5~svn201561
-Maintainer: FRC Team 971 <spartanrobotics.org>
-Depends: libbsd0, libc6, libedit2, libncurses5, libpython2.7, libtinfo5, zlib1g
-Architecture: amd64
-Description: Clang 3.5 with included GCC 4.8 so it actually works.
- This is just 1 massive package with a newer clang that doesn't conflict with
- other stuff like the official clang-3.5 package does.
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index bb2a754..fa94d01 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -9,6 +9,7 @@
'libcdd.py',
],
deps = [
+ '//external:python-glog',
'//external:slycot',
],
data = [
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 6494ce1..8a63620 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -12,6 +12,7 @@
import numpy
import slycot
import scipy.signal.cont2discrete
+import glog
class Error (Exception):
"""Base class for all control loop exceptions."""
@@ -94,7 +95,7 @@
def ctrb(A, B):
"""Returns the controlability matrix.
- This matrix must have full rank for all the states to be controlable.
+ This matrix must have full rank for all the states to be controllable.
"""
n = A.shape[0]
output = B
@@ -133,9 +134,16 @@
"""
I = numpy.matrix(numpy.eye(Q.shape[0]))
Z = numpy.matrix(numpy.zeros(Q.shape[0]))
+ n = A.shape[0]
+ m = C.shape[0]
+
+ controllability_rank = numpy.linalg.matrix_rank(ctrb(A.T, C.T))
+ if controlability_rank != n:
+ glog.warning('Observability of %d != %d, unobservable state',
+ controlability_rank, n)
# Compute the steady state covariance matrix.
- P_prior = scipy.linalg.solve_discrete_are(A.T, C.T, Q, R)
+ P_prior, rcond, w, S, T = slycot.sb02od(n=n, m=m, A=A.T, B=C.T, Q=Q, R=R, dico='D')
S = C * P_prior * C.T + R
K = numpy.linalg.lstsq(S.T, (P_prior * C.T).T)[0].T
P = (I - K * C) * P_prior