318 lines
12 KiB
Python
318 lines
12 KiB
Python
#!/usr/bin/env python
|
|
#
|
|
# margin_test.py - test suit for stability margin commands
|
|
# RMM, 15 Jul 2011
|
|
|
|
from __future__ import print_function
|
|
import unittest
|
|
import numpy as np
|
|
from control.xferfcn import TransferFunction
|
|
from control.frdata import FRD
|
|
from control.statesp import StateSpace
|
|
from control.margins import *
|
|
|
|
def assert_array_almost_equal(x, y, ndigit=4):
|
|
|
|
x = np.array(x)
|
|
y = np.array(y)
|
|
try:
|
|
if np.isfinite(x).any() and \
|
|
np.equal(np.isfinite(x), np.isfinite(y)).all() and \
|
|
np.equal(np.isnan(x), np.isnan(y)).all():
|
|
np.testing.assert_array_almost_equal(
|
|
x[np.isfinite(x)], y[np.isfinite(y)], ndigit)
|
|
return
|
|
except TypeError as e:
|
|
print("Error", e, "with", x, "and", y)
|
|
#raise e
|
|
np.testing.assert_array_almost_equal(x, y, ndigit)
|
|
|
|
class TestMargin(unittest.TestCase):
|
|
"""These are tests for the margin commands in margin.py."""
|
|
|
|
def setUp(self):
|
|
# system, gain margin, gm freq, phase margin, pm freq
|
|
s = TransferFunction([1, 0], [1])
|
|
self.tsys = (
|
|
(TransferFunction([1, 2], [1, 2, 3]),
|
|
[], [], [], []),
|
|
(TransferFunction([1], [1, 2, 3, 4]),
|
|
[2.001], [1.7321], [], []),
|
|
(StateSpace([[1., 4.], [3., 2.]], [[1.], [-4.]],
|
|
[[1., 0.]], [[0.]]),
|
|
[], [], [147.0743], [2.5483]),
|
|
((8.75*(4*s**2+0.4*s+1))/((100*s+1)*(s**2+0.22*s+1)) *
|
|
1./(s**2/(10.**2)+2*0.04*s/10.+1),
|
|
[2.2716], [10.0053], [97.5941, -157.7904, 134.7359],
|
|
[0.0850, 0.9373, 1.0919]))
|
|
|
|
|
|
"""
|
|
sys1 = tf([1, 2], [1, 2, 3]);
|
|
sys2 = tf([1], [1, 2, 3, 4]);
|
|
sys3 = ss([1, 4; 3, 2], [1; -4], ...
|
|
[1, 0], [0])
|
|
s = tf('s')
|
|
sys4 = (8.75*(4*s^2+0.4*s+1))/((100*s+1)*(s^2+0.22*s+1)) * ...
|
|
1.0/(s^2/(10.0^2)+2*0.04*s/10.0+1);
|
|
"""
|
|
|
|
self.sys1 = TransferFunction([1, 2], [1, 2, 3])
|
|
# alternative
|
|
# sys1 = tf([1, 2], [1, 2, 3])
|
|
self.sys2 = TransferFunction([1], [1, 2, 3, 4])
|
|
self.sys3 = StateSpace([[1., 4.], [3., 2.]], [[1.], [-4.]],
|
|
[[1., 0.]], [[0.]])
|
|
s = TransferFunction([1, 0], [1])
|
|
self.sys4 = (8.75*(4*s**2+0.4*s+1))/((100*s+1)*(s**2+0.22*s+1)) * \
|
|
1./(s**2/(10.**2)+2*0.04*s/10.+1)
|
|
self.stability_margins4 = \
|
|
[2.2716, 97.5941, 0.5591, 10.0053, 0.0850, 9.9918]
|
|
|
|
"""
|
|
hm1 = s/(s+1);
|
|
h0 = 1/(s+1)^3;
|
|
h1 = (s + 0.1)/s/(s+1);
|
|
h2 = (s + 0.1)/s^2/(s+1);
|
|
h3 = (s + 0.1)*(s+0.1)/s^3/(s+1);
|
|
"""
|
|
self.types = {
|
|
'typem1': s/(s+1),
|
|
'type0': 1/(s+1)**3,
|
|
'type1': (s + 0.1)/s/(s+1),
|
|
'type2': (s + 0.1)/s**2/(s+1),
|
|
'type3': (s + 0.1)*(s+0.1)/s**3/(s+1) }
|
|
self.tmargin = ( self.types,
|
|
dict(sys='typem1', K=2.0, digits=3, result=(
|
|
float('Inf'), -120.0007, float('NaN'), 0.5774)),
|
|
dict(sys='type0', K = 0.8, digits=3, result=(
|
|
10.0014, float('inf'), 1.7322, float('nan'))),
|
|
dict(sys='type0', K = 2.0, digits=2, result=(
|
|
4.000, 67.6058, 1.7322, 0.7663)),
|
|
dict(sys='type1', K=1.0, digits=4, result=(
|
|
float('Inf'), 144.9032, float('NaN'), 0.3162)),
|
|
dict(sys='type2', K=1.0, digits=4, result=(
|
|
float('Inf'), 44.4594, float('NaN'), 0.7907)),
|
|
dict(sys='type3', K=1.0, digits=3, result=(
|
|
0.0626, 37.1748, 0.1119, 0.7951)),
|
|
)
|
|
|
|
|
|
# from "A note on the Gain and Phase Margin Concepts
|
|
# Journal of Control and Systems Engineering, Yazdan Bavafi-Toosi,
|
|
# Dec 2015, vol 3 iss 1, pp 51-59
|
|
#
|
|
# A cornucopia of tricky systems for phase / gain margin
|
|
# Still have to convert more to tests + fix margin to handle
|
|
# also these torture cases
|
|
"""
|
|
% matlab compatible
|
|
s = tf('s');
|
|
h21 = 0.002*(s+0.02)*(s+0.05)*(s+5)*(s+10)/( ...
|
|
(s-0.0005)*(s+0.0001)*(s+0.01)*(s+0.2)*(s+1)*(s+100)^2 );
|
|
h23 = ((s+0.1)^2 + 1)*(s-0.1)/( ...
|
|
((s+0.1)^2+4)*(s+1) );
|
|
h25a = s/(s^2+2*s+2)^4; h25b = h25a*100;
|
|
h26a = ((s-0.1)^2 + 1)/( ...
|
|
(s + 0.1)*((s-0.2)^2 + 4) ) ;
|
|
h26b = ((s-0.1)^2 + 1)/( ...
|
|
(s - 0.3)*((s-0.2)^2 + 4) );
|
|
"""
|
|
self.yazdan = {
|
|
'example21' :
|
|
0.002*(s+0.02)*(s+0.05)*(s+5)*(s+10)/(
|
|
(s-0.0005)*(s+0.0001)*(s+0.01)*(s+0.2)*(s+1)*(s+100)**2 ),
|
|
|
|
'example23' :
|
|
((s+0.1)**2 + 1)*(s-0.1)/(
|
|
((s+0.1)**2+4)*(s+1) ),
|
|
|
|
'example25a' :
|
|
s/(s**2+2*s+2)**4,
|
|
|
|
'example26a' :
|
|
((s-0.1)**2 + 1)/(
|
|
(s + 0.1)*((s-0.2)**2 + 4) ),
|
|
|
|
'example26b': ((s-0.1)**2 + 1)/(
|
|
(s - 0.3)*((s-0.2)**2 + 4) )
|
|
}
|
|
self.yazdan['example24'] = self.yazdan['example21']*20000
|
|
self.yazdan['example25b'] = self.yazdan['example25a']*100
|
|
self.yazdan['example22'] = self.yazdan['example21']*(s**2 - 2*s + 401)
|
|
self.ymargin = (
|
|
dict(sys='example21', K=1.0, digits=2, result=(
|
|
0.0100, -14.5640, 0, 0.0022)),
|
|
dict(sys='example21', K=1000.0, digits=2, result=(
|
|
0.1793, 22.5215, 0.0243, 0.0630)),
|
|
dict(sys='example21', K=5000.0, digits=4, result=(
|
|
4.5596, 21.2101, 0.4385, 0.1868)),
|
|
)
|
|
|
|
self.yallmargin = (
|
|
dict(sys='example21', K=1.0, result=(
|
|
[0.01, 179.2931, 2.2798e+4, 1.5946e+07, 7.2477e+08],
|
|
[0, 0.0243, 0.4385, 6.8640, 84.9323],
|
|
[-14.5640],
|
|
[0.0022]))
|
|
)
|
|
|
|
|
|
def test_stability_margins(self):
|
|
omega = np.logspace(-2, 2, 2000)
|
|
for sys,rgm,rwgm,rpm,rwpm in self.tsys:
|
|
print(sys)
|
|
out = np.array(stability_margins(sys))
|
|
gm, pm, sm, wg, wp, ws = out
|
|
outf = np.array(stability_margins(FRD(sys, omega)))
|
|
print(out,'\n', outf)
|
|
#print(out != np.array(None))
|
|
assert_array_almost_equal(
|
|
out, outf, 2)
|
|
# final one with fixed values
|
|
assert_array_almost_equal(
|
|
[gm, pm, sm, wg, wp, ws],
|
|
self.stability_margins4, 3)
|
|
|
|
def test_margin(self):
|
|
gm, pm, wg, wp = margin(self.sys4)
|
|
assert_array_almost_equal(
|
|
[gm, pm, wg, wp],
|
|
self.stability_margins4[:2] + self.stability_margins4[3:5], 3)
|
|
|
|
|
|
def test_stability_margins_all(self):
|
|
for sys,rgm,rwgm,rpm,rwpm in self.tsys:
|
|
out = stability_margins(sys, returnall=True)
|
|
gm, pm, sm, wg, wp, ws = out
|
|
print(sys)
|
|
for res,comp in zip(out, (rgm,rpm,[],rwgm,rwpm,[])):
|
|
if comp:
|
|
print(res, '\n', comp)
|
|
assert_array_almost_equal(
|
|
res, comp, 2)
|
|
|
|
def test_phase_crossover_frequencies(self):
|
|
omega, gain = phase_crossover_frequencies(self.sys2)
|
|
assert_array_almost_equal(omega, [1.73205, 0.])
|
|
assert_array_almost_equal(gain, [-0.5, 0.25])
|
|
|
|
tf = TransferFunction([1],[1,1])
|
|
omega, gain = phase_crossover_frequencies(tf)
|
|
assert_array_almost_equal(omega, [0.])
|
|
assert_array_almost_equal(gain, [1.])
|
|
|
|
# testing MIMO, only (0,0) element is considered
|
|
tf = TransferFunction([[[1],[2]],[[3],[4]]],
|
|
[[[1, 2, 3, 4],[1,1]],[[1,1],[1,1]]])
|
|
omega, gain = phase_crossover_frequencies(tf)
|
|
assert_array_almost_equal(omega, [1.73205081, 0.])
|
|
assert_array_almost_equal(gain, [-0.5, 0.25])
|
|
|
|
def test_mag_phase_omega(self):
|
|
# test for bug reported in gh-58
|
|
sys = TransferFunction(15, [1, 6, 11, 6])
|
|
out = stability_margins(sys)
|
|
omega = np.logspace(-2,2,1000)
|
|
mag, phase, omega = sys.freqresp(omega)
|
|
#print( mag, phase, omega)
|
|
out2 = stability_margins((mag, phase*180/np.pi, omega))
|
|
ind = [0,1,3,4] # indices of gm, pm, wg, wp -- ignore sm
|
|
marg1 = np.array(out)[ind]
|
|
marg2 = np.array(out2)[ind]
|
|
assert_array_almost_equal(marg1, marg2, 4)
|
|
|
|
def test_frd(self):
|
|
f = np.array([0.005, 0.010, 0.020, 0.030, 0.040,
|
|
0.050, 0.060, 0.070, 0.080, 0.090,
|
|
0.100, 0.200, 0.300, 0.400, 0.500,
|
|
0.750, 1.000, 1.250, 1.500, 1.750,
|
|
2.000, 2.250, 2.500, 2.750, 3.000,
|
|
3.250, 3.500, 3.750, 4.000, 4.250,
|
|
4.500, 4.750, 5.000, 6.000, 7.000,
|
|
8.000, 9.000, 10.000 ])
|
|
gain = np.array([ 0.0, 0.0, 0.0, 0.0, 0.0,
|
|
0.0, 0.0, 0.0, 0.0, 0.0,
|
|
0.0, 0.1, 0.2, 0.3, 0.5,
|
|
0.5, -0.4, -2.3, -4.8, -7.3,
|
|
-9.6, -11.7, -13.6, -15.3, -16.9,
|
|
-18.3, -19.6, -20.8, -22.0, -23.1,
|
|
-24.1, -25.0, -25.9, -29.1, -31.9,
|
|
-34.2, -36.2, -38.1 ])
|
|
phase = np.array([ 0, -1, -2, -3, -4,
|
|
-5, -6, -7, -8, -9,
|
|
-10, -19, -29, -40, -51,
|
|
-81, -114, -144, -168, -187,
|
|
-202, -214, -224, -233, -240,
|
|
-247, -253, -259, -264, -269,
|
|
-273, -277, -280, -292, -301,
|
|
-307, -313, -317 ])
|
|
# calculate response as complex number
|
|
resp = 10**(gain / 20) * np.exp(1j * phase / (180./np.pi))
|
|
# frequency response data
|
|
fresp = FRD(resp, f*2*np.pi, smooth=True)
|
|
s=TransferFunction([1,0],[1])
|
|
G=1./(s**2)
|
|
K=1.
|
|
C=K*(1+1.9*s)
|
|
TFopen=fresp*C*G
|
|
gm, pm, sm, wg, wp, ws = stability_margins(TFopen)
|
|
assert_array_almost_equal(
|
|
[pm], [44.55], 2)
|
|
|
|
def test_nocross(self):
|
|
# what happens when no gain/phase crossover?
|
|
s = TransferFunction([1, 0], [1])
|
|
h1 = 1/(1+s)
|
|
h2 = 3*(10+s)/(2+s)
|
|
h3 = 0.01*(10-s)/(2+s)/(1+s)
|
|
gm, pm, wm, wg, wp, ws = stability_margins(h1)
|
|
assert_array_almost_equal(
|
|
[gm, pm, wg, wp],
|
|
[float('Inf'), float('Inf'), float('NaN'), float('NaN')])
|
|
gm, pm, wm, wg, wp, ws = stability_margins(h2)
|
|
self.assertEqual(pm, float('Inf'))
|
|
gm, pm, wm, wg, wp, ws = stability_margins(h3)
|
|
self.assertTrue(np.isnan(wp))
|
|
omega = np.logspace(-2,2, 100)
|
|
out1b = stability_margins(FRD(h1, omega))
|
|
out2b = stability_margins(FRD(h2, omega))
|
|
out3b = stability_margins(FRD(h3, omega))
|
|
|
|
def test_zmore_margin(self):
|
|
print("""
|
|
warning, Matlab gives different values (0 and 0) for gain
|
|
margin of the following system:
|
|
{type2!s}
|
|
python-control gives inf
|
|
difficult to argue which is right? Special case or different
|
|
approach?
|
|
|
|
edge cases, like
|
|
{type0!s}
|
|
which approaches a gain of 1 for w -> 0, are also not identically
|
|
indicated, Matlab gives phase margin -180, at w = 0. for higher or
|
|
lower gains, results match
|
|
""".format(**self.types))
|
|
|
|
sdict = self.tmargin[0]
|
|
for test in self.tmargin[1:]:
|
|
res = margin(sdict[test['sys']]*test['K'])
|
|
print("more margin {}\n".format(sdict[test['sys']]),
|
|
res, '\n', test['result'])
|
|
assert_array_almost_equal(
|
|
res, test['result'], test['digits'])
|
|
sdict = self.yazdan
|
|
for test in self.ymargin:
|
|
res = margin(sdict[test['sys']]*test['K'])
|
|
print("more margin {}\n".format(sdict[test['sys']]),
|
|
res, '\n', test['result'])
|
|
assert_array_almost_equal(
|
|
res, test['result'], test['digits'])
|
|
|
|
def test_suite():
|
|
return unittest.TestLoader().loadTestsFromTestCase(TestMargin)
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|