from control import lqe,ss,ctrb,obsv
from numpy import diag,array
from numpy.linalg import matrix_rank


A = array([ [0,1,0],
            [0,0,1],
            [0,0,0]])
B = array([[0],[0],[1]])
C = array([[1,0,0]])
D = array([[0]])

sys=ss(A,B,C,D)
print(matrix_rank(ctrb(A,B)))
print(matrix_rank(obsv(A,C)))

L,_,_=lqe(A,diag([1,1,1]),C,diag([1,1,1]),array([[1]]))
print(L)
L,_,_=lqe(sys,diag([1,1,1]),array([[1]]))

3
3
[[2.41421356]
 [2.41421356]
 [1.        ]]
Traceback (most recent call last):
  File "/home/paul/Documents/paul/personal/properly named/tippy control with better control/test_lqe.py", line 19, in <module>
    L,_,_=lqe(sys,diag([1,1,1]),array([[1]]))
          ~~~^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/home/paul/Documents/paul/personal/properly named/tippy control with better control/.venv/lib/python3.13/site-packages/control/stochsys.py", line 176, in lqe
    _check_shape("QN", QN, G.shape[1], G.shape[1])
    ~~~~~~~~~~~~^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/home/paul/Documents/paul/personal/properly named/tippy control with better control/.venv/lib/python3.13/site-packages/control/mateqn.py", line 643, in _check_shape
    raise ControlDimension("Incompatible dimensions of %s matrix" % name)
control.exception.ControlDimension: Incompatible dimensions of QN matrix