280 lines
12 KiB
Python
280 lines
12 KiB
Python
|
#!/usr/bin/env python
|
||
|
|
#
|
||
|
|
# bdalg_test.py - test suit for block diagram algebra
|
||
|
|
# RMM, 30 Mar 2011 (based on TestBDAlg from v0.4a)
|
||
|
|
|
||
|
|
import unittest
|
||
|
|
import numpy as np
|
||
|
|
from numpy import sort
|
||
|
|
import control as ctrl
|
||
|
|
from control.xferfcn import TransferFunction
|
||
|
|
from control.statesp import StateSpace
|
||
|
|
from control.bdalg import feedback
|
||
|
|
from control.lti import zero, pole
|
||
|
|
|
||
|
|
class TestFeedback(unittest.TestCase):
|
||
|
|
"""These are tests for the feedback function in bdalg.py. Currently, some
|
||
|
|
of the tests are not implemented, or are not working properly. TODO: these
|
||
|
|
need to be fixed."""
|
||
|
|
|
||
|
|
def setUp(self):
|
||
|
|
"""This contains some random LTI systems and scalars for testing."""
|
||
|
|
|
||
|
|
# Two random SISO systems.
|
||
|
|
self.sys1 = TransferFunction([1, 2], [1, 2, 3])
|
||
|
|
self.sys2 = StateSpace([[1., 4.], [3., 2.]], [[1.], [-4.]],
|
||
|
|
[[1., 0.]], [[0.]])
|
||
|
|
# Two random scalars.
|
||
|
|
self.x1 = 2.5
|
||
|
|
self.x2 = -3.
|
||
|
|
|
||
|
|
def testScalarScalar(self):
|
||
|
|
"""Scalar system with scalar feedback block."""
|
||
|
|
|
||
|
|
ans1 = feedback(self.x1, self.x2)
|
||
|
|
ans2 = feedback(self.x1, self.x2, 1.)
|
||
|
|
|
||
|
|
self.assertAlmostEqual(ans1.num[0][0][0] / ans1.den[0][0][0],
|
||
|
|
-2.5 / 6.5)
|
||
|
|
self.assertAlmostEqual(ans2.num[0][0][0] / ans2.den[0][0][0], 2.5 / 8.5)
|
||
|
|
|
||
|
|
def testScalarSS(self):
|
||
|
|
"""Scalar system with state space feedback block."""
|
||
|
|
|
||
|
|
ans1 = feedback(self.x1, self.sys2)
|
||
|
|
ans2 = feedback(self.x1, self.sys2, 1.)
|
||
|
|
|
||
|
|
np.testing.assert_array_almost_equal(ans1.A, [[-1.5, 4.], [13., 2.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.B, [[2.5], [-10.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.C, [[-2.5, 0.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.D, [[2.5]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.A, [[3.5, 4.], [-7., 2.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.B, [[2.5], [-10.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.C, [[2.5, 0.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.D, [[2.5]])
|
||
|
|
|
||
|
|
# Make sure default arugments work as well
|
||
|
|
ans3 = feedback(self.sys2, 1)
|
||
|
|
ans4 = feedback(self.sys2)
|
||
|
|
np.testing.assert_array_almost_equal(ans3.A, ans4.A)
|
||
|
|
np.testing.assert_array_almost_equal(ans3.B, ans4.B)
|
||
|
|
np.testing.assert_array_almost_equal(ans3.C, ans4.C)
|
||
|
|
np.testing.assert_array_almost_equal(ans3.D, ans4.D)
|
||
|
|
|
||
|
|
def testScalarTF(self):
|
||
|
|
"""Scalar system with transfer function feedback block."""
|
||
|
|
|
||
|
|
ans1 = feedback(self.x1, self.sys1)
|
||
|
|
ans2 = feedback(self.x1, self.sys1, 1.)
|
||
|
|
|
||
|
|
np.testing.assert_array_almost_equal(ans1.num, [[[2.5, 5., 7.5]]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.den, [[[1., 4.5, 8.]]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.num, [[[2.5, 5., 7.5]]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.den, [[[1., -0.5, -2.]]])
|
||
|
|
|
||
|
|
# Make sure default arugments work as well
|
||
|
|
ans3 = feedback(self.sys1, 1)
|
||
|
|
ans4 = feedback(self.sys1)
|
||
|
|
np.testing.assert_array_almost_equal(ans3.num, ans4.num)
|
||
|
|
np.testing.assert_array_almost_equal(ans3.den, ans4.den)
|
||
|
|
|
||
|
|
def testSSScalar(self):
|
||
|
|
"""State space system with scalar feedback block."""
|
||
|
|
|
||
|
|
ans1 = feedback(self.sys2, self.x1)
|
||
|
|
ans2 = feedback(self.sys2, self.x1, 1.)
|
||
|
|
|
||
|
|
np.testing.assert_array_almost_equal(ans1.A, [[-1.5, 4.], [13., 2.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.B, [[1.], [-4.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.C, [[1., 0.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.D, [[0.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.A, [[3.5, 4.], [-7., 2.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.B, [[1.], [-4.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.C, [[1., 0.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.D, [[0.]])
|
||
|
|
|
||
|
|
def testSSSS1(self):
|
||
|
|
"""State space system with state space feedback block."""
|
||
|
|
|
||
|
|
ans1 = feedback(self.sys2, self.sys2)
|
||
|
|
ans2 = feedback(self.sys2, self.sys2, 1.)
|
||
|
|
|
||
|
|
np.testing.assert_array_almost_equal(ans1.A, [[1., 4., -1., 0.],
|
||
|
|
[3., 2., 4., 0.], [1., 0., 1., 4.], [-4., 0., 3., 2]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.B, [[1.], [-4.], [0.], [0.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.C, [[1., 0., 0., 0.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.D, [[0.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.A, [[1., 4., 1., 0.],
|
||
|
|
[3., 2., -4., 0.], [1., 0., 1., 4.], [-4., 0., 3., 2.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.B, [[1.], [-4.], [0.], [0.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.C, [[1., 0., 0., 0.]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.D, [[0.]])
|
||
|
|
|
||
|
|
def testSSSS2(self):
|
||
|
|
"""State space system with state space feedback block, including a
|
||
|
|
direct feedthrough term."""
|
||
|
|
|
||
|
|
sys3 = StateSpace([[-1., 4.], [2., -3]], [[2.], [3.]], [[-3., 1.]],
|
||
|
|
[[-2.]])
|
||
|
|
sys4 = StateSpace([[-3., -2.], [1., 4.]], [[-2.], [-6.]], [[2., -3.]],
|
||
|
|
[[3.]])
|
||
|
|
|
||
|
|
ans1 = feedback(sys3, sys4)
|
||
|
|
ans2 = feedback(sys3, sys4, 1.)
|
||
|
|
|
||
|
|
np.testing.assert_array_almost_equal(ans1.A,
|
||
|
|
[[-4.6, 5.2, 0.8, -1.2], [-3.4, -1.2, 1.2, -1.8],
|
||
|
|
[-1.2, 0.4, -1.4, -4.4], [-3.6, 1.2, 5.8, -3.2]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.B,
|
||
|
|
[[-0.4], [-0.6], [-0.8], [-2.4]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.C, [[0.6, -0.2, -0.8, 1.2]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.D, [[0.4]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.A,
|
||
|
|
[[-3.57142857142857, 4.85714285714286, 0.571428571428571,
|
||
|
|
-0.857142857142857],
|
||
|
|
[-1.85714285714286, -1.71428571428571, 0.857142857142857,
|
||
|
|
-1.28571428571429],
|
||
|
|
[0.857142857142857, -0.285714285714286, -1.85714285714286,
|
||
|
|
-3.71428571428571],
|
||
|
|
[2.57142857142857, -0.857142857142857, 4.42857142857143,
|
||
|
|
-1.14285714285714]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.B, [[0.285714285714286],
|
||
|
|
[0.428571428571429], [0.571428571428571], [1.71428571428571]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.C, [[-0.428571428571429,
|
||
|
|
0.142857142857143, -0.571428571428571, 0.857142857142857]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.D, [[-0.285714285714286]])
|
||
|
|
|
||
|
|
|
||
|
|
def testSSTF(self):
|
||
|
|
"""State space system with transfer function feedback block."""
|
||
|
|
|
||
|
|
# This functionality is not implemented yet.
|
||
|
|
pass
|
||
|
|
|
||
|
|
def testTFScalar(self):
|
||
|
|
"""Transfer function system with scalar feedback block."""
|
||
|
|
|
||
|
|
ans1 = feedback(self.sys1, self.x1)
|
||
|
|
ans2 = feedback(self.sys1, self.x1, 1.)
|
||
|
|
|
||
|
|
np.testing.assert_array_almost_equal(ans1.num, [[[1., 2.]]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.den, [[[1., 4.5, 8.]]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.num, [[[1., 2.]]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.den, [[[1., -0.5, -2.]]])
|
||
|
|
|
||
|
|
def testTFSS(self):
|
||
|
|
"""Transfer function system with state space feedback block."""
|
||
|
|
|
||
|
|
# This functionality is not implemented yet.
|
||
|
|
pass
|
||
|
|
|
||
|
|
def testTFTF(self):
|
||
|
|
"""Transfer function system with transfer function feedback block."""
|
||
|
|
|
||
|
|
ans1 = feedback(self.sys1, self.sys1)
|
||
|
|
ans2 = feedback(self.sys1, self.sys1, 1.)
|
||
|
|
|
||
|
|
np.testing.assert_array_almost_equal(ans1.num, [[[1., 4., 7., 6.]]])
|
||
|
|
np.testing.assert_array_almost_equal(ans1.den,
|
||
|
|
[[[1., 4., 11., 16., 13.]]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.num, [[[1., 4., 7., 6.]]])
|
||
|
|
np.testing.assert_array_almost_equal(ans2.den, [[[1., 4., 9., 8., 5.]]])
|
||
|
|
|
||
|
|
def testLists(self):
|
||
|
|
"""Make sure that lists of various lengths work for operations"""
|
||
|
|
sys1 = ctrl.tf([1, 1], [1, 2])
|
||
|
|
sys2 = ctrl.tf([1, 3], [1, 4])
|
||
|
|
sys3 = ctrl.tf([1, 5], [1, 6])
|
||
|
|
sys4 = ctrl.tf([1, 7], [1, 8])
|
||
|
|
sys5 = ctrl.tf([1, 9], [1, 0])
|
||
|
|
|
||
|
|
# Series
|
||
|
|
sys1_2 = ctrl.series(sys1, sys2)
|
||
|
|
np.testing.assert_array_almost_equal(sort(pole(sys1_2)), [-4., -2.])
|
||
|
|
np.testing.assert_array_almost_equal(sort(zero(sys1_2)), [-3., -1.])
|
||
|
|
|
||
|
|
sys1_3 = ctrl.series(sys1, sys2, sys3);
|
||
|
|
np.testing.assert_array_almost_equal(sort(pole(sys1_3)),
|
||
|
|
[-6., -4., -2.])
|
||
|
|
np.testing.assert_array_almost_equal(sort(zero(sys1_3)),
|
||
|
|
[-5., -3., -1.])
|
||
|
|
|
||
|
|
sys1_4 = ctrl.series(sys1, sys2, sys3, sys4);
|
||
|
|
np.testing.assert_array_almost_equal(sort(pole(sys1_4)),
|
||
|
|
[-8., -6., -4., -2.])
|
||
|
|
np.testing.assert_array_almost_equal(sort(zero(sys1_4)),
|
||
|
|
[-7., -5., -3., -1.])
|
||
|
|
|
||
|
|
sys1_5 = ctrl.series(sys1, sys2, sys3, sys4, sys5);
|
||
|
|
np.testing.assert_array_almost_equal(sort(pole(sys1_5)),
|
||
|
|
[-8., -6., -4., -2., -0.])
|
||
|
|
np.testing.assert_array_almost_equal(sort(zero(sys1_5)),
|
||
|
|
[-9., -7., -5., -3., -1.])
|
||
|
|
|
||
|
|
# Parallel
|
||
|
|
sys1_2 = ctrl.parallel(sys1, sys2)
|
||
|
|
np.testing.assert_array_almost_equal(sort(pole(sys1_2)), [-4., -2.])
|
||
|
|
np.testing.assert_array_almost_equal(sort(zero(sys1_2)),
|
||
|
|
sort(zero(sys1 + sys2)))
|
||
|
|
|
||
|
|
sys1_3 = ctrl.parallel(sys1, sys2, sys3);
|
||
|
|
np.testing.assert_array_almost_equal(sort(pole(sys1_3)),
|
||
|
|
[-6., -4., -2.])
|
||
|
|
np.testing.assert_array_almost_equal(sort(zero(sys1_3)),
|
||
|
|
sort(zero(sys1 + sys2 + sys3)))
|
||
|
|
|
||
|
|
sys1_4 = ctrl.parallel(sys1, sys2, sys3, sys4);
|
||
|
|
np.testing.assert_array_almost_equal(sort(pole(sys1_4)),
|
||
|
|
[-8., -6., -4., -2.])
|
||
|
|
np.testing.assert_array_almost_equal(sort(zero(sys1_4)),
|
||
|
|
sort(zero(sys1 + sys2 +
|
||
|
|
sys3 + sys4)))
|
||
|
|
|
||
|
|
|
||
|
|
sys1_5 = ctrl.parallel(sys1, sys2, sys3, sys4, sys5);
|
||
|
|
np.testing.assert_array_almost_equal(sort(pole(sys1_5)),
|
||
|
|
[-8., -6., -4., -2., -0.])
|
||
|
|
np.testing.assert_array_almost_equal(sort(zero(sys1_5)),
|
||
|
|
sort(zero(sys1 + sys2 +
|
||
|
|
sys3 + sys4 + sys5)))
|
||
|
|
def testMimoSeries(self):
|
||
|
|
"""regression: bdalg.series reverses order of arguments"""
|
||
|
|
g1 = ctrl.ss([],[],[],[[1,2],[0,3]])
|
||
|
|
g2 = ctrl.ss([],[],[],[[1,0],[2,3]])
|
||
|
|
ref = g2*g1
|
||
|
|
tst = ctrl.series(g1,g2)
|
||
|
|
# assert_array_equal on mismatched matrices gives
|
||
|
|
# "repr failed for <matrix>: ..."
|
||
|
|
def assert_equal(x,y):
|
||
|
|
np.testing.assert_array_equal(np.asarray(x),
|
||
|
|
np.asarray(y))
|
||
|
|
assert_equal(ref.A, tst.A)
|
||
|
|
assert_equal(ref.B, tst.B)
|
||
|
|
assert_equal(ref.C, tst.C)
|
||
|
|
assert_equal(ref.D, tst.D)
|
||
|
|
|
||
|
|
def test_feedback_args(self):
|
||
|
|
# Added 25 May 2019 to cover missing exception handling in feedback()
|
||
|
|
# If first argument is not LTI or convertable, generate an exception
|
||
|
|
args = ([1], self.sys2)
|
||
|
|
self.assertRaises(TypeError, ctrl.feedback, *args)
|
||
|
|
|
||
|
|
# If second argument is not LTI or convertable, generate an exception
|
||
|
|
args = (self.sys1, np.array([1]))
|
||
|
|
self.assertRaises(TypeError, ctrl.feedback, *args)
|
||
|
|
|
||
|
|
# Convert first argument to FRD, if needed
|
||
|
|
h = TransferFunction([1], [1, 2, 2])
|
||
|
|
omega = np.logspace(-1, 2, 10)
|
||
|
|
frd = ctrl.FRD(h, omega)
|
||
|
|
sys = ctrl.feedback(1, frd)
|
||
|
|
self.assertTrue(isinstance(sys, ctrl.FRD))
|
||
|
|
|
||
|
|
|
||
|
|
def suite():
|
||
|
|
return unittest.TestLoader().loadTestsFromTestCase(TestFeedback)
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == "__main__":
|
||
|
|
unittest.main()
|