LQE: added python 2 compatibility and unit test

sawyerbfuller · sawyerbfuller · commit e3125696a561 · 2019-12-12T11:05:01.000-08:00
diff --git a/control/statefbk.py b/control/statefbk.py
@@ -283,7 +283,10 @@ def lqe(A, G, C, QN, RN, NN=None):
     #NG = G @ NN
 
     #LT, P, E = lqr(A.T, C.T, G @ QN @ G.T, RN)
-    P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN)
+    #P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN)
+    A, G, C = np.array(A, ndmin=2), np.array(G, ndmin=2), np.array(C, ndmin=2)
+    QN, RN =  np.array(QN, ndmin=2), np.array(RN, ndmin=2)
+    P, E, LT = care(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN)
     return _ssmatrix(LT.T), _ssmatrix(P), _ssmatrix(E)
 
 
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
@@ -6,7 +6,7 @@
 from __future__ import print_function
 import unittest
 import numpy as np
-from control.statefbk import ctrb, obsv, place, place_varga, lqr, gram, acker
+from control.statefbk import ctrb, obsv, place, place_varga, lqr, lqe, gram, acker
 from control.matlab import *
 from control.exception import slycot_check, ControlDimension
 from control.mateqn import care, dare
@@ -299,6 +299,20 @@ def test_LQR_3args(self):
         K, S, poles = lqr(sys, Q, R)
         self.check_LQR(K, S, poles, Q, R)
 
+    def check_LQE(self, L, P, poles, G, QN, RN):
+        P_expected = np.array(np.sqrt(G*QN*G * RN))
+        L_expected = P_expected / RN
+        poles_expected = np.array([-L_expected], ndmin=2)
+        np.testing.assert_array_almost_equal(P, P_expected)
+        np.testing.assert_array_almost_equal(L, L_expected)
+        np.testing.assert_array_almost_equal(poles, poles_expected)
+
+    @unittest.skipIf(not slycot_check(), "slycot not installed")
+    def test_LQE(self):
+        A, G, C, QN, RN = 0., .1, 1., 10., 2.
+        L, P, poles = lqe(A, G, C, QN, RN)
+        self.check_LQE(L, P, poles, G, QN, RN)
+
     @unittest.skipIf(not slycot_check(), "slycot not installed")
     def test_care(self):
         #unit test for stabilizing and anti-stabilizing feedbacks
