LSR/env/lib/python3.6/site-packages/control/tests/margin_test.py
2020-06-04 17:24:47 +02:00

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()