696 lines
25 KiB
Python
696 lines
25 KiB
Python
#!/usr/bin/env python
|
|
#
|
|
# matlab_test.py - test MATLAB compatibility
|
|
# RMM, 30 Mar 2011 (based on TestMatlab from v0.4a)
|
|
#
|
|
# This test suite just goes through and calls all of the MATLAB
|
|
# functions using different systems and arguments to make sure that
|
|
# nothing crashes. It doesn't test actual functionality; the module
|
|
# specific unit tests will do that.
|
|
|
|
from __future__ import print_function
|
|
import unittest
|
|
import numpy as np
|
|
from scipy.linalg import eigvals
|
|
import scipy as sp
|
|
from control.matlab import *
|
|
from control.frdata import FRD
|
|
from control.exception import slycot_check
|
|
import warnings
|
|
|
|
# for running these through Matlab or Octave
|
|
'''
|
|
siso_ss1 = ss([1. -2.; 3. -4.], [5.; 7.], [6. 8.], [0])
|
|
|
|
siso_tf1 = tf([1], [1, 2, 1])
|
|
siso_tf2 = tf([1, 1], [1, 2, 3, 1])
|
|
|
|
siso_tf3 = tf(siso_ss1)
|
|
siso_ss2 = ss(siso_tf2)
|
|
siso_ss3 = ss(siso_tf3)
|
|
siso_tf4 = tf(siso_ss2)
|
|
|
|
A =[ 1. -2. 0. 0.;
|
|
3. -4. 0. 0.;
|
|
0. 0. 1. -2.;
|
|
0. 0. 3. -4. ]
|
|
B = [ 5. 0.;
|
|
7. 0.;
|
|
0. 5.;
|
|
0. 7. ]
|
|
C = [ 6. 8. 0. 0.;
|
|
0. 0. 6. 8. ]
|
|
D = [ 9. 0.;
|
|
0. 9. ]
|
|
mimo_ss1 = ss(A, B, C, D)
|
|
|
|
% all boring, since no cross-over
|
|
margin(siso_tf1)
|
|
margin(siso_tf2)
|
|
margin(siso_ss1)
|
|
margin(siso_ss2)
|
|
|
|
% make a bit better
|
|
[gm, pm, gmc, pmc] = margin(siso_ss2*siso_ss2*2)
|
|
|
|
'''
|
|
|
|
class TestMatlab(unittest.TestCase):
|
|
def setUp(self):
|
|
"""Set up some systems for testing out MATLAB functions"""
|
|
A = np.matrix("1. -2.; 3. -4.")
|
|
B = np.matrix("5.; 7.")
|
|
C = np.matrix("6. 8.")
|
|
D = np.matrix("9.")
|
|
self.siso_ss1 = ss(A,B,C,D)
|
|
|
|
# Create some transfer functions
|
|
self.siso_tf1 = tf([1], [1, 2, 1]);
|
|
self.siso_tf2 = tf([1, 1], [1, 2, 3, 1]);
|
|
|
|
# Conversions
|
|
self.siso_tf3 = tf(self.siso_ss1);
|
|
self.siso_ss2 = ss(self.siso_tf2);
|
|
self.siso_ss3 = tf2ss(self.siso_tf3);
|
|
self.siso_tf4 = ss2tf(self.siso_ss2);
|
|
|
|
#Create MIMO system, contains ``siso_ss1`` twice
|
|
A = np.matrix("1. -2. 0. 0.;"
|
|
"3. -4. 0. 0.;"
|
|
"0. 0. 1. -2.;"
|
|
"0. 0. 3. -4. ")
|
|
B = np.matrix("5. 0.;"
|
|
"7. 0.;"
|
|
"0. 5.;"
|
|
"0. 7. ")
|
|
C = np.matrix("6. 8. 0. 0.;"
|
|
"0. 0. 6. 8. ")
|
|
D = np.matrix("9. 0.;"
|
|
"0. 9. ")
|
|
self.mimo_ss1 = ss(A, B, C, D)
|
|
|
|
# get consistent test results
|
|
np.random.seed(0)
|
|
|
|
def testParallel(self):
|
|
sys1 = parallel(self.siso_ss1, self.siso_ss2)
|
|
sys1 = parallel(self.siso_ss1, self.siso_tf2)
|
|
sys1 = parallel(self.siso_tf1, self.siso_ss2)
|
|
sys1 = parallel(1, self.siso_ss2)
|
|
sys1 = parallel(1, self.siso_tf2)
|
|
sys1 = parallel(self.siso_ss1, 1)
|
|
sys1 = parallel(self.siso_tf1, 1)
|
|
|
|
def testSeries(self):
|
|
sys1 = series(self.siso_ss1, self.siso_ss2)
|
|
sys1 = series(self.siso_ss1, self.siso_tf2)
|
|
sys1 = series(self.siso_tf1, self.siso_ss2)
|
|
sys1 = series(1, self.siso_ss2)
|
|
sys1 = series(1, self.siso_tf2)
|
|
sys1 = series(self.siso_ss1, 1)
|
|
sys1 = series(self.siso_tf1, 1)
|
|
|
|
def testFeedback(self):
|
|
sys1 = feedback(self.siso_ss1, self.siso_ss2)
|
|
sys1 = feedback(self.siso_ss1, self.siso_tf2)
|
|
sys1 = feedback(self.siso_tf1, self.siso_ss2)
|
|
sys1 = feedback(1, self.siso_ss2)
|
|
sys1 = feedback(1, self.siso_tf2)
|
|
sys1 = feedback(self.siso_ss1, 1)
|
|
sys1 = feedback(self.siso_tf1, 1)
|
|
|
|
def testPoleZero(self):
|
|
pole(self.siso_ss1);
|
|
pole(self.siso_tf1);
|
|
pole(self.siso_tf2);
|
|
zero(self.siso_ss1);
|
|
zero(self.siso_tf1);
|
|
zero(self.siso_tf2);
|
|
|
|
def testPZmap(self):
|
|
# pzmap(self.siso_ss1); not implemented
|
|
# pzmap(self.siso_ss2); not implemented
|
|
pzmap(self.siso_tf1);
|
|
pzmap(self.siso_tf2);
|
|
pzmap(self.siso_tf2, Plot=False);
|
|
|
|
def testStep(self):
|
|
t = np.linspace(0, 1, 10)
|
|
# Test transfer function
|
|
yout, tout = step(self.siso_tf1, T=t)
|
|
youttrue = np.array([0, 0.0057, 0.0213, 0.0446, 0.0739,
|
|
0.1075, 0.1443, 0.1832, 0.2235, 0.2642])
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
|
|
# Test SISO system with direct feedthrough
|
|
sys = self.siso_ss1
|
|
youttrue = np.array([9., 17.6457, 24.7072, 30.4855, 35.2234, 39.1165,
|
|
42.3227, 44.9694, 47.1599, 48.9776])
|
|
|
|
yout, tout = step(sys, T=t)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
|
|
# Play with arguments
|
|
yout, tout = step(sys, T=t, X0=0)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
|
|
X0 = np.array([0, 0]);
|
|
yout, tout = step(sys, T=t, X0=X0)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
|
|
yout, tout, xout = step(sys, T=t, X0=0, return_x=True)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
|
|
if slycot_check():
|
|
# Test MIMO system, which contains ``siso_ss1`` twice
|
|
sys = self.mimo_ss1
|
|
y_00, _t = step(sys, T=t, input=0, output=0)
|
|
y_11, _t = step(sys, T=t, input=1, output=1)
|
|
np.testing.assert_array_almost_equal(y_00, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(y_11, youttrue, decimal=4)
|
|
|
|
def testImpulse(self):
|
|
t = np.linspace(0, 1, 10)
|
|
# test transfer function
|
|
yout, tout = impulse(self.siso_tf1, T=t)
|
|
youttrue = np.array([0., 0.0994, 0.1779, 0.2388, 0.2850, 0.3188,
|
|
0.3423, 0.3573, 0.3654, 0.3679])
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
|
|
# produce a warning for a system with direct feedthrough
|
|
with warnings.catch_warnings():
|
|
warnings.simplefilter("ignore")
|
|
#Test SISO system
|
|
sys = self.siso_ss1
|
|
youttrue = np.array([86., 70.1808, 57.3753, 46.9975, 38.5766, 31.7344,
|
|
26.1668, 21.6292, 17.9245, 14.8945])
|
|
yout, tout = impulse(sys, T=t)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
|
|
# Play with arguments
|
|
yout, tout = impulse(sys, T=t, X0=0)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
|
|
X0 = np.array([0, 0]);
|
|
yout, tout = impulse(sys, T=t, X0=X0)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
|
|
yout, tout, xout = impulse(sys, T=t, X0=0, return_x=True)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
|
|
if slycot_check():
|
|
#Test MIMO system, which contains ``siso_ss1`` twice
|
|
sys = self.mimo_ss1
|
|
y_00, _t = impulse(sys, T=t, input=0, output=0)
|
|
y_11, _t = impulse(sys, T=t, input=1, output=1)
|
|
np.testing.assert_array_almost_equal(y_00, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(y_11, youttrue, decimal=4)
|
|
|
|
def testInitial(self):
|
|
#Test SISO system
|
|
sys = self.siso_ss1
|
|
t = np.linspace(0, 1, 10)
|
|
x0 = np.matrix(".5; 1.")
|
|
youttrue = np.array([11., 8.1494, 5.9361, 4.2258, 2.9118, 1.9092,
|
|
1.1508, 0.5833, 0.1645, -0.1391])
|
|
yout, tout = initial(sys, T=t, X0=x0)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
|
|
# Play with arguments
|
|
yout, tout, xout = initial(sys, T=t, X0=x0, return_x=True)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
|
|
if slycot_check():
|
|
#Test MIMO system, which contains ``siso_ss1`` twice
|
|
sys = self.mimo_ss1
|
|
x0 = np.matrix(".5; 1.; .5; 1.")
|
|
y_00, _t = initial(sys, T=t, X0=x0, input=0, output=0)
|
|
y_11, _t = initial(sys, T=t, X0=x0, input=1, output=1)
|
|
np.testing.assert_array_almost_equal(y_00, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(y_11, youttrue, decimal=4)
|
|
|
|
def testLsim(self):
|
|
t = np.linspace(0, 1, 10)
|
|
|
|
#compute step response - test with state space, and transfer function
|
|
#objects
|
|
u = np.array([1., 1, 1, 1, 1, 1, 1, 1, 1, 1])
|
|
youttrue = np.array([9., 17.6457, 24.7072, 30.4855, 35.2234, 39.1165,
|
|
42.3227, 44.9694, 47.1599, 48.9776])
|
|
yout, tout, _xout = lsim(self.siso_ss1, u, t)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
np.testing.assert_array_almost_equal(tout, t)
|
|
yout, _t, _xout = lsim(self.siso_tf3, u, t)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
|
|
#test with initial value and special algorithm for ``U=0``
|
|
u=0
|
|
x0 = np.matrix(".5; 1.")
|
|
youttrue = np.array([11., 8.1494, 5.9361, 4.2258, 2.9118, 1.9092,
|
|
1.1508, 0.5833, 0.1645, -0.1391])
|
|
yout, _t, _xout = lsim(self.siso_ss1, u, t, x0)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
|
|
if slycot_check():
|
|
#Test MIMO system, which contains ``siso_ss1`` twice
|
|
#first system: initial value, second system: step response
|
|
u = np.array([[0., 1.], [0, 1], [0, 1], [0, 1], [0, 1],
|
|
[0, 1], [0, 1], [0, 1], [0, 1], [0, 1]])
|
|
x0 = np.matrix(".5; 1; 0; 0")
|
|
youttrue = np.array([[11., 9.], [8.1494, 17.6457],
|
|
[5.9361, 24.7072], [4.2258, 30.4855],
|
|
[2.9118, 35.2234], [1.9092, 39.1165],
|
|
[1.1508, 42.3227], [0.5833, 44.9694],
|
|
[0.1645, 47.1599], [-0.1391, 48.9776]])
|
|
yout, _t, _xout = lsim(self.mimo_ss1, u, t, x0)
|
|
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
|
|
|
|
def testMargin(self):
|
|
#! TODO: check results to make sure they are OK
|
|
gm, pm, wg, wp = margin(self.siso_tf1);
|
|
gm, pm, wg, wp = margin(self.siso_tf2);
|
|
gm, pm, wg, wp = margin(self.siso_ss1);
|
|
gm, pm, wg, wp = margin(self.siso_ss2);
|
|
gm, pm, wg, wp = margin(self.siso_ss2*self.siso_ss2*2);
|
|
np.testing.assert_array_almost_equal(
|
|
[gm, pm, wg, wp], [1.5451, 75.9933, 1.2720, 0.6559], decimal=3)
|
|
|
|
def testDcgain(self):
|
|
#Create different forms of a SISO system
|
|
A, B, C, D = self.siso_ss1.A, self.siso_ss1.B, self.siso_ss1.C, \
|
|
self.siso_ss1.D
|
|
Z, P, k = sp.signal.ss2zpk(A, B, C, D)
|
|
num, den = sp.signal.ss2tf(A, B, C, D)
|
|
sys_ss = self.siso_ss1
|
|
|
|
#Compute the gain with ``dcgain``
|
|
gain_abcd = dcgain(A, B, C, D)
|
|
gain_zpk = dcgain(Z, P, k)
|
|
gain_numden = dcgain(np.squeeze(num), den)
|
|
gain_sys_ss = dcgain(sys_ss)
|
|
# print('\ngain_abcd:', gain_abcd, 'gain_zpk:', gain_zpk)
|
|
# print('gain_numden:', gain_numden, 'gain_sys_ss:', gain_sys_ss)
|
|
|
|
#Compute the gain with a long simulation
|
|
t = linspace(0, 1000, 1000)
|
|
y, _t = step(sys_ss, t)
|
|
gain_sim = y[-1]
|
|
# print('gain_sim:', gain_sim)
|
|
|
|
#All gain values must be approximately equal to the known gain
|
|
np.testing.assert_array_almost_equal(
|
|
[gain_abcd, gain_zpk, gain_numden, gain_sys_ss,
|
|
gain_sim],
|
|
[59, 59, 59, 59, 59])
|
|
|
|
if slycot_check():
|
|
# Test with MIMO system, which contains ``siso_ss1`` twice
|
|
gain_mimo = dcgain(self.mimo_ss1)
|
|
# print('gain_mimo: \n', gain_mimo)
|
|
np.testing.assert_array_almost_equal(gain_mimo, [[59., 0 ],
|
|
[0, 59.]])
|
|
|
|
def testBode(self):
|
|
bode(self.siso_ss1)
|
|
bode(self.siso_tf1)
|
|
bode(self.siso_tf2)
|
|
(mag, phase, freq) = bode(self.siso_tf2, Plot=False)
|
|
bode(self.siso_tf1, self.siso_tf2)
|
|
w = logspace(-3, 3);
|
|
bode(self.siso_ss1, w)
|
|
bode(self.siso_ss1, self.siso_tf2, w)
|
|
# Not yet implemented
|
|
# bode(self.siso_ss1, '-', self.siso_tf1, 'b--', self.siso_tf2, 'k.')
|
|
|
|
def testRlocus(self):
|
|
rlocus(self.siso_ss1)
|
|
rlocus(self.siso_tf1)
|
|
rlocus(self.siso_tf2)
|
|
klist = [1, 10, 100]
|
|
rlist, klist_out = rlocus(self.siso_tf2, klist, Plot=False)
|
|
np.testing.assert_equal(len(rlist), len(klist))
|
|
np.testing.assert_array_equal(klist, klist_out)
|
|
|
|
def testNyquist(self):
|
|
nyquist(self.siso_ss1)
|
|
nyquist(self.siso_tf1)
|
|
nyquist(self.siso_tf2)
|
|
w = logspace(-3, 3);
|
|
nyquist(self.siso_tf2, w)
|
|
(real, imag, freq) = nyquist(self.siso_tf2, w, Plot=False)
|
|
|
|
def testNichols(self):
|
|
nichols(self.siso_ss1)
|
|
nichols(self.siso_tf1)
|
|
nichols(self.siso_tf2)
|
|
w = logspace(-3, 3);
|
|
nichols(self.siso_tf2, w)
|
|
nichols(self.siso_tf2, grid=False)
|
|
|
|
def testFreqresp(self):
|
|
w = logspace(-3, 3)
|
|
freqresp(self.siso_ss1, w)
|
|
freqresp(self.siso_ss2, w)
|
|
freqresp(self.siso_ss3, w)
|
|
freqresp(self.siso_tf1, w)
|
|
freqresp(self.siso_tf2, w)
|
|
freqresp(self.siso_tf3, w)
|
|
|
|
def testEvalfr(self):
|
|
w = 1j
|
|
np.testing.assert_almost_equal(evalfr(self.siso_ss1, w), 44.8-21.4j)
|
|
evalfr(self.siso_ss2, w)
|
|
evalfr(self.siso_ss3, w)
|
|
evalfr(self.siso_tf1, w)
|
|
evalfr(self.siso_tf2, w)
|
|
evalfr(self.siso_tf3, w)
|
|
if slycot_check():
|
|
np.testing.assert_array_almost_equal(
|
|
evalfr(self.mimo_ss1, w),
|
|
np.array( [[44.8-21.4j, 0.], [0., 44.8-21.4j]]))
|
|
|
|
@unittest.skipIf(not slycot_check(), "slycot not installed")
|
|
def testHsvd(self):
|
|
hsvd(self.siso_ss1)
|
|
hsvd(self.siso_ss2)
|
|
hsvd(self.siso_ss3)
|
|
|
|
@unittest.skipIf(not slycot_check(), "slycot not installed")
|
|
def testBalred(self):
|
|
balred(self.siso_ss1, 1)
|
|
balred(self.siso_ss2, 2)
|
|
balred(self.siso_ss3, [2, 2])
|
|
|
|
@unittest.skipIf(not slycot_check(), "slycot not installed")
|
|
def testModred(self):
|
|
modred(self.siso_ss1, [1])
|
|
modred(self.siso_ss2 * self.siso_ss1, [0, 1])
|
|
modred(self.siso_ss1, [1], 'matchdc')
|
|
modred(self.siso_ss1, [1], 'truncate')
|
|
|
|
@unittest.skipIf(not slycot_check(), "slycot not installed")
|
|
def testPlace_varga(self):
|
|
place_varga(self.siso_ss1.A, self.siso_ss1.B, [-2, -2])
|
|
|
|
def testPlace(self):
|
|
place(self.siso_ss1.A, self.siso_ss1.B, [-2, -2.5])
|
|
|
|
def testAcker(self):
|
|
acker(self.siso_ss1.A, self.siso_ss1.B, [-2, -2.5])
|
|
|
|
|
|
@unittest.skipIf(not slycot_check(), "slycot not installed")
|
|
def testLQR(self):
|
|
(K, S, E) = lqr(self.siso_ss1.A, self.siso_ss1.B, np.eye(2), np.eye(1))
|
|
|
|
# Should work if [Q N;N' R] is positive semi-definite
|
|
(K, S, E) = lqr(self.siso_ss2.A, self.siso_ss2.B, 10*np.eye(3), \
|
|
np.eye(1), [[1], [1], [2]])
|
|
|
|
@unittest.skip("check not yet implemented")
|
|
def testLQR_checks(self):
|
|
# Make sure we get a warning if [Q N;N' R] is not positive semi-definite
|
|
(K, S, E) = lqr(self.siso_ss2.A, self.siso_ss2.B, np.eye(3), \
|
|
np.eye(1), [[1], [1], [2]])
|
|
|
|
def testRss(self):
|
|
rss(1)
|
|
rss(2)
|
|
rss(2, 1, 3)
|
|
|
|
def testDrss(self):
|
|
drss(1)
|
|
drss(2)
|
|
drss(2, 1, 3)
|
|
|
|
def testCtrb(self):
|
|
ctrb(self.siso_ss1.A, self.siso_ss1.B)
|
|
ctrb(self.siso_ss2.A, self.siso_ss2.B)
|
|
|
|
def testObsv(self):
|
|
obsv(self.siso_ss1.A, self.siso_ss1.C)
|
|
obsv(self.siso_ss2.A, self.siso_ss2.C)
|
|
|
|
@unittest.skipIf(not slycot_check(), "slycot not installed")
|
|
def testGram(self):
|
|
gram(self.siso_ss1, 'c')
|
|
gram(self.siso_ss2, 'c')
|
|
gram(self.siso_ss1, 'o')
|
|
gram(self.siso_ss2, 'o')
|
|
|
|
def testPade(self):
|
|
pade(1, 1)
|
|
pade(1, 2)
|
|
pade(5, 4)
|
|
|
|
def testOpers(self):
|
|
self.siso_ss1 + self.siso_ss2
|
|
self.siso_tf1 + self.siso_tf2
|
|
self.siso_ss1 + self.siso_tf2
|
|
self.siso_tf1 + self.siso_ss2
|
|
self.siso_ss1 * self.siso_ss2
|
|
self.siso_tf1 * self.siso_tf2
|
|
self.siso_ss1 * self.siso_tf2
|
|
self.siso_tf1 * self.siso_ss2
|
|
# self.siso_ss1 / self.siso_ss2 not implemented yet
|
|
# self.siso_tf1 / self.siso_tf2
|
|
# self.siso_ss1 / self.siso_tf2
|
|
# self.siso_tf1 / self.siso_ss2
|
|
|
|
def testUnwrap(self):
|
|
phase = np.array(range(1, 100)) / 10.;
|
|
wrapped = phase % (2 * np.pi)
|
|
unwrapped = unwrap(wrapped)
|
|
|
|
def testSISOssdata(self):
|
|
ssdata_1 = ssdata(self.siso_ss2);
|
|
ssdata_2 = ssdata(self.siso_tf2);
|
|
for i in range(len(ssdata_1)):
|
|
np.testing.assert_array_almost_equal(ssdata_1[i], ssdata_2[i])
|
|
|
|
@unittest.skipIf(not slycot_check(), "slycot not installed")
|
|
def testMIMOssdata(self):
|
|
m = (self.mimo_ss1.A, self.mimo_ss1.B, self.mimo_ss1.C, self.mimo_ss1.D)
|
|
ssdata_1 = ssdata(self.mimo_ss1);
|
|
for i in range(len(ssdata_1)):
|
|
np.testing.assert_array_almost_equal(ssdata_1[i], m[i])
|
|
|
|
def testSISOtfdata(self):
|
|
tfdata_1 = tfdata(self.siso_tf2);
|
|
tfdata_2 = tfdata(self.siso_tf2);
|
|
for i in range(len(tfdata_1)):
|
|
np.testing.assert_array_almost_equal(tfdata_1[i], tfdata_2[i])
|
|
|
|
def testDamp(self):
|
|
A = np.mat('''-0.2 0.06 0 -1;
|
|
0 0 1 0;
|
|
-17 0 -3.8 1;
|
|
9.4 0 -0.4 -0.6''')
|
|
B = np.mat('''-0.01 0.06;
|
|
0 0;
|
|
-32 5.4;
|
|
2.6 -7''')
|
|
C = np.eye(4)
|
|
D = np.zeros((4,2))
|
|
sys = ss(A, B, C, D)
|
|
wn, Z, p = damp(sys, False)
|
|
# print (wn)
|
|
np.testing.assert_array_almost_equal(
|
|
wn, np.array([4.07381994, 3.28874827, 3.28874827,
|
|
1.08937685e-03]))
|
|
np.testing.assert_array_almost_equal(
|
|
Z, np.array([1.0, 0.07983139, 0.07983139, 1.0]))
|
|
|
|
def testConnect(self):
|
|
sys1 = ss("1. -2; 3. -4", "5.; 7", "6, 8", "9.")
|
|
sys2 = ss("-1.", "1.", "1.", "0.")
|
|
sys = append(sys1, sys2)
|
|
Q= np.mat([ [ 1, 2], [2, -1] ]) # basically feedback, output 2 in 1
|
|
sysc = connect(sys, Q, [2], [1, 2])
|
|
# print(sysc)
|
|
np.testing.assert_array_almost_equal(
|
|
sysc.A, np.mat('1 -2 5; 3 -4 7; -6 -8 -10'))
|
|
np.testing.assert_array_almost_equal(
|
|
sysc.B, np.mat('0; 0; 1'))
|
|
np.testing.assert_array_almost_equal(
|
|
sysc.C, np.mat('6 8 9; 0 0 1'))
|
|
np.testing.assert_array_almost_equal(
|
|
sysc.D, np.mat('0; 0'))
|
|
|
|
def testConnect2(self):
|
|
sys = append(ss([[-5, -2.25], [4, 0]], [[2], [0]],
|
|
[[0, 1.125]], [[0]]),
|
|
ss([[-1.6667, 0], [1, 0]], [[2], [0]],
|
|
[[0, 3.3333]], [[0]]),
|
|
1)
|
|
Q = [ [ 1, 3], [2, 1], [3, -2]]
|
|
sysc = connect(sys, Q, [3], [3, 1, 2])
|
|
np.testing.assert_array_almost_equal(
|
|
sysc.A, np.mat([[-5, -2.25, 0, -6.6666],
|
|
[4, 0, 0, 0],
|
|
[0, 2.25, -1.6667, 0],
|
|
[0, 0, 1, 0]]))
|
|
np.testing.assert_array_almost_equal(
|
|
sysc.B, np.mat([[2], [0], [0], [0]]))
|
|
np.testing.assert_array_almost_equal(
|
|
sysc.C, np.mat([[0, 0, 0, -3.3333],
|
|
[0, 1.125, 0, 0],
|
|
[0, 0, 0, 3.3333]]))
|
|
np.testing.assert_array_almost_equal(
|
|
sysc.D, np.mat([[1], [0], [0]]))
|
|
|
|
|
|
|
|
def testFRD(self):
|
|
h = tf([1], [1, 2, 2])
|
|
omega = np.logspace(-1, 2, 10)
|
|
frd1 = frd(h, omega)
|
|
assert isinstance(frd1, FRD)
|
|
frd2 = frd(frd1.fresp[0,0,:], omega)
|
|
assert isinstance(frd2, FRD)
|
|
|
|
@unittest.skipIf(not slycot_check(), "slycot not installed")
|
|
def testMinreal(self, verbose=False):
|
|
"""Test a minreal model reduction"""
|
|
#A = [-2, 0.5, 0; 0.5, -0.3, 0; 0, 0, -0.1]
|
|
A = [[-2, 0.5, 0], [0.5, -0.3, 0], [0, 0, -0.1]]
|
|
#B = [0.3, -1.3; 0.1, 0; 1, 0]
|
|
B = [[0.3, -1.3], [0.1, 0.], [1.0, 0.0]]
|
|
#C = [0, 0.1, 0; -0.3, -0.2, 0]
|
|
C = [[0., 0.1, 0.0], [-0.3, -0.2, 0.0]]
|
|
#D = [0 -0.8; -0.3 0]
|
|
D = [[0., -0.8], [-0.3, 0.]]
|
|
# sys = ss(A, B, C, D)
|
|
|
|
sys = ss(A, B, C, D)
|
|
sysr = minreal(sys, verbose=verbose)
|
|
self.assertEqual(sysr.states, 2)
|
|
self.assertEqual(sysr.inputs, sys.inputs)
|
|
self.assertEqual(sysr.outputs, sys.outputs)
|
|
np.testing.assert_array_almost_equal(
|
|
eigvals(sysr.A), [-2.136154, -0.1638459])
|
|
|
|
s = tf([1, 0], [1])
|
|
h = (s+1)*(s+2.00000000001)/(s+2)/(s**2+s+1)
|
|
hm = minreal(h, verbose=verbose)
|
|
hr = (s+1)/(s**2+s+1)
|
|
np.testing.assert_array_almost_equal(hm.num[0][0], hr.num[0][0])
|
|
np.testing.assert_array_almost_equal(hm.den[0][0], hr.den[0][0])
|
|
|
|
def testSS2cont(self):
|
|
sys = ss(
|
|
np.mat("-3 4 2; -1 -3 0; 2 5 3"),
|
|
np.mat("1 4 ; -3 -3; -2 1"),
|
|
np.mat("4 2 -3; 1 4 3"),
|
|
np.mat("-2 4; 0 1"))
|
|
sysd = c2d(sys, 0.1)
|
|
np.testing.assert_array_almost_equal(
|
|
np.mat(
|
|
"""0.742840837331905 0.342242024293711 0.203124211149560;
|
|
-0.074130792143890 0.724553295044645 -0.009143771143630;
|
|
0.180264783290485 0.544385612448419 1.370501013067845"""),
|
|
sysd.A)
|
|
np.testing.assert_array_almost_equal(
|
|
np.mat(""" 0.012362066084719 0.301932197918268;
|
|
-0.260952977031384 -0.274201791021713;
|
|
-0.304617775734327 0.075182622718853"""), sysd.B)
|
|
|
|
def testCombi01(self):
|
|
# test from a "real" case, combines tf, ss, connect and margin
|
|
# this is a type 2 system, with phase starting at -180. The
|
|
# margin command should remove the solution for w = nearly zero
|
|
|
|
# Example is a concocted two-body satellite with flexible link
|
|
Jb = 400;
|
|
Jp = 1000;
|
|
k = 10;
|
|
b = 5;
|
|
|
|
# can now define an "s" variable, to make TF's
|
|
s = tf([1, 0], [1]);
|
|
hb1 = 1/(Jb*s);
|
|
hb2 = 1/s;
|
|
hp1 = 1/(Jp*s);
|
|
hp2 = 1/s;
|
|
|
|
# convert to ss and append
|
|
sat0 = append(ss(hb1), ss(hb2), k, b, ss(hp1), ss(hp2));
|
|
|
|
# connection of the elements with connect call
|
|
Q = [[1, -3, -4], # link moment (spring, damper), feedback to body
|
|
[2, 1, 0], # link integrator to body velocity
|
|
[3, 2, -6], # spring input, th_b - th_p
|
|
[4, 1, -5], # damper input
|
|
[5, 3, 4], # link moment, acting on payload
|
|
[6, 5, 0]]
|
|
inputs = [1];
|
|
outputs = [1, 2, 5, 6];
|
|
sat1 = connect(sat0, Q, inputs, outputs);
|
|
|
|
# matched notch filter
|
|
wno = 0.19
|
|
z1 = 0.05
|
|
z2 = 0.7
|
|
Hno = (1+2*z1/wno*s+s**2/wno**2)/(1+2*z2/wno*s+s**2/wno**2)
|
|
|
|
# the controller, Kp = 1 for now
|
|
Kp = 1.64
|
|
tau_PD = 50.
|
|
Hc = (1 + tau_PD*s)*Kp
|
|
|
|
# start with the basic satellite model sat1, and get the
|
|
# payload attitude response
|
|
Hp = tf(sp.matrix([0, 0, 0, 1])*sat1)
|
|
|
|
# total open loop
|
|
Hol = Hc*Hno*Hp
|
|
|
|
gm, pm, wg, wp = margin(Hol)
|
|
# print("%f %f %f %f" % (gm, pm, wg, wp))
|
|
self.assertAlmostEqual(gm, 3.32065569155)
|
|
self.assertAlmostEqual(pm, 46.9740430224)
|
|
self.assertAlmostEqual(wg, 0.176469728448)
|
|
self.assertAlmostEqual(wp, 0.0616288455466)
|
|
|
|
def test_tf_string_args(self):
|
|
# Make sure that the 's' variable is defined properly
|
|
s = tf('s')
|
|
G = (s + 1)/(s**2 + 2*s + 1)
|
|
np.testing.assert_array_almost_equal(G.num, [[[1, 1]]])
|
|
np.testing.assert_array_almost_equal(G.den, [[[1, 2, 1]]])
|
|
self.assertTrue(isctime(G, strict=True))
|
|
|
|
# Make sure that the 'z' variable is defined properly
|
|
z = tf('z')
|
|
G = (z + 1)/(z**2 + 2*z + 1)
|
|
np.testing.assert_array_almost_equal(G.num, [[[1, 1]]])
|
|
np.testing.assert_array_almost_equal(G.den, [[[1, 2, 1]]])
|
|
self.assertTrue(isdtime(G, strict=True))
|
|
|
|
|
|
#! TODO: not yet implemented
|
|
# def testMIMOtfdata(self):
|
|
# sisotf = ss2tf(self.siso_ss1)
|
|
# tfdata_1 = tfdata(sisotf)
|
|
# tfdata_2 = tfdata(self.mimo_ss1, input=0, output=0)
|
|
# for i in range(len(tfdata)):
|
|
# np.testing.assert_array_almost_equal(tfdata_1[i], tfdata_2[i])
|
|
|
|
def test_suite():
|
|
return unittest.TestLoader().loadTestsFromTestCase(TestMatlab)
|
|
|
|
if __name__ == '__main__':
|
|
unittest.main()
|