Skip to content

Commit e722b28

Browse files
repaghrepagh
committed
- totally re-vamped margin calculation, works afaict
- FRD and TransferFunction now return array results for omega arrays - corrected rss definitions and calls to use Matlab convention
1 parent 11e0d30 commit e722b28

6 files changed

Lines changed: 172 additions & 142 deletions

File tree

‎src/frdata.py‎

Lines changed: 16 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -340,7 +340,10 @@ def evalfr(self, omega):
340340
"""
341341

342342
# Preallocate the output.
343-
out = empty((self.outputs, self.inputs), dtype=complex)
343+
if getattr(omega, '__iter__', False):
344+
out = empty((self.outputs, self.inputs, len(omega)), dtype=complex)
345+
else:
346+
out = empty((self.outputs, self.inputs), dtype=complex)
344347

345348
if self.ifunc is None:
346349
try:
@@ -350,11 +353,18 @@ def evalfr(self, omega):
350353
"Frequency %f not in frequency list, try an interpolating"
351354
" FRD if you want additional points")
352355
else:
353-
for i in range(self.outputs):
354-
for j in range(self.inputs):
355-
frraw = splev(omega, self.ifunc[i,j], der=0)
356-
out[i,j] = frraw[0] + 1.0j*frraw[1]
357-
356+
if getattr(omega, '__iter__', False):
357+
for i in range(self.outputs):
358+
for j in range(self.inputs):
359+
for k,w in enumerate(omega):
360+
frraw = splev(w, self.ifunc[i,j], der=0)
361+
out[i,j,k] = frraw[0] + 1.0j*frraw[1]
362+
else:
363+
for i in range(self.outputs):
364+
for j in range(self.inputs):
365+
frraw = splev(omega, self.ifunc[i,j], der=0)
366+
out[i,j] = frraw[0] + 1.0j*frraw[1]
367+
358368
return out
359369

360370
# Method for generating the frequency response of the system

‎src/margins.py‎

Lines changed: 147 additions & 130 deletions
Original file line numberDiff line numberDiff line change
@@ -53,145 +53,162 @@
5353
import numpy as np
5454
import control.xferfcn as xferfcn
5555
from control.freqplot import bode
56-
from control.lti import isdtime
56+
from control.lti import isdtime, issiso
57+
import control.frdata as frdata
5758
import scipy as sp
5859

59-
60-
# gain and phase margins
61-
# contributed by Sawyer B. Fuller <minster@caltech.edu>
62-
#! TODO - need to add unit test functions
63-
# def stability_margins(sysdata, deg=True):
64-
# """Calculate gain, phase and stability margins and associated
65-
# crossover frequencies.
66-
67-
# Usage
68-
# -----
69-
# gm, pm, sm, wg, wp, ws = stability_margins(sysdata, deg=True)
60+
# helper functions for stability_margins
61+
def _polyimsplit(pol):
62+
"""split a polynomial with (iw) applied into a real and an
63+
imaginary part with w applied"""
64+
rpencil = np.zeros_like(pol)
65+
ipencil = np.zeros_like(pol)
66+
rpencil[-1::-4] = 1.
67+
rpencil[-3::-4] = -1.
68+
ipencil[-2::-4] = 1.
69+
ipencil[-4::-4] = -1.
70+
return pol * rpencil, pol*ipencil
71+
72+
def _polysqr(pol):
73+
"""return a polynomial squared"""
74+
return np.polymul(pol, pol)
75+
76+
# Took the framework for the old function by
77+
# Sawyer B. Fuller <minster@caltech.edu>, removed a lot of the innards
78+
# and replaced with analytical polynomial functions for Lti systems.
79+
#
80+
# idea for the frequency data solution copied/adapted from
81+
# https://github.com/alchemyst/Skogestad-Python/blob/master/BODE.py
82+
# Rene van Paassen <rene.vanpaassen@gmail.com>
83+
def stability_margins(sysdata, deg=True, returnall=False):
84+
"""Calculate gain, phase and stability margins and associated
85+
crossover frequencies.
7086
71-
# Parameters
72-
# ----------
73-
# sysdata: linsys or (mag, phase, omega) sequence
74-
# sys : linsys
75-
# Linear SISO system
76-
# mag, phase, omega : sequence of array_like
77-
# Input magnitude, phase, and frequencies (rad/sec) sequence from
78-
# bode frequency response data
79-
# deg=True: boolean
80-
# If true, all input and output phases in degrees, else in radians
81-
82-
# Returns
83-
# -------
84-
# gm, pm, sm, wg, wp, ws: float
85-
# Gain margin gm, phase margin pm, stability margin sm, and
86-
# associated crossover
87-
# frequencies wg, wp, and ws of SISO open-loop. If more than
88-
# one crossover frequency is detected, returns the lowest corresponding
89-
# margin.
90-
# """
91-
# #TODO do this precisely without the effects of discretization of frequencies?
92-
# #TODO assumes SISO
93-
# #TODO unit tests, margin plot
94-
95-
# if (not getattr(sysdata, '__iter__', False)):
96-
# sys = sysdata
97-
98-
# # TODO: implement for discrete time systems
99-
# if (isdtime(sys, strict=True)):
100-
# raise NotImplementedError("Function not implemented in discrete time")
101-
102-
# mag, phase, omega = bode(sys, deg=deg, Plot=False)
103-
# elif len(sysdata) == 3:
104-
# # TODO: replace with FRD object type?
105-
# mag, phase, omega = sysdata
106-
# else:
107-
# raise ValueError("Margin sysdata must be either a linear system or a 3-sequence of mag, phase, omega.")
108-
109-
# if deg:
110-
# cycle = 360.
111-
# crossover = 180.
112-
# else:
113-
# cycle = 2 * np.pi
114-
# crossover = np.pi
115-
116-
# wrapped_phase = -np.mod(phase, cycle)
87+
Usage
88+
-----
89+
gm, pm, sm, wg, wp, ws = stability_margins(sysdata, deg=True)
11790
118-
# # phase margin from minimum phase among all gain crossovers
119-
# neg_mag_crossings_i = np.nonzero(np.diff(mag < 1) > 0)[0]
120-
# mag_crossings_p = wrapped_phase[neg_mag_crossings_i]
121-
# if len(neg_mag_crossings_i) == 0:
122-
# if mag[0] < 1: # gain always less than one
123-
# wp = np.nan
124-
# pm = np.inf
125-
# else: # gain always greater than one
126-
# print("margin: no magnitude crossings found")
127-
# wp = np.nan
128-
# pm = np.nan
129-
# else:
130-
# min_mag_crossing_i = neg_mag_crossings_i[np.argmin(mag_crossings_p)]
131-
# wp = omega[min_mag_crossing_i]
132-
# pm = crossover + phase[min_mag_crossing_i]
133-
# if pm < 0:
134-
# print("warning: system unstable: negative phase margin")
91+
Parameters
92+
----------
93+
sysdata: linsys or (mag, phase, omega) sequence
94+
sys : linsys
95+
Linear SISO system
96+
mag, phase, omega : sequence of array_like
97+
Input magnitude, phase, and frequencies (rad/sec) sequence from
98+
bode frequency response data
99+
deg=True: boolean
100+
If true, all input and output phases in degrees, else in radians
101+
returnall=False: boolean
102+
If true, return all margins found. Note that for frequency data or
103+
FRD systems, only one margin is found and returned.
104+
105+
Returns
106+
-------
107+
gm, pm, sm, wg, wp, ws: float or array_like
108+
Gain margin gm, phase margin pm, stability margin sm, and
109+
associated crossover
110+
frequencies wg, wp, and ws of SISO open-loop. If more than
111+
one crossover frequency is detected, returns the lowest corresponding
112+
margin.
113+
When requesting all margins, the return values are array_like,
114+
and all margins are returns for linear systems not equal to FRD
115+
"""
135116

136-
# # gain margin from minimum gain margin among all phase crossovers
137-
# neg_phase_crossings_i = np.nonzero(np.diff(wrapped_phase < -crossover) > 0)[0]
138-
# neg_phase_crossings_g = mag[neg_phase_crossings_i]
139-
# if len(neg_phase_crossings_i) == 0:
140-
# wg = np.nan
141-
# gm = np.inf
142-
# else:
143-
# min_phase_crossing_i = neg_phase_crossings_i[
144-
# np.argmax(neg_phase_crossings_g)]
145-
# wg = omega[min_phase_crossing_i]
146-
# gm = abs(1/mag[min_phase_crossing_i])
147-
# if gm < 1:
148-
# print("warning: system unstable: gain margin < 1")
149-
150-
# # stability margin from minimum abs distance from -1 point
151-
# if deg:
152-
# phase_rad = phase * np.pi / 180.
153-
# else:
154-
# phase_rad = phase
155-
# L = mag * np.exp(1j * phase_rad) # complex loop response to -1 pt
156-
# min_Lplus1_i = np.argmin(np.abs(L + 1))
157-
# sm = np.abs(L[min_Lplus1_i] + 1)
158-
# ws = phase[min_Lplus1_i]
159-
160-
# return gm, pm, sm, wg, wp, ws
161-
162-
# largely copied/adapted from
163-
# https://github.com/alchemyst/Skogestad-Python/blob/master/BODE.py
164-
# by RvP
165-
def stability_margins(sysdata, deg=True):
166-
167-
# calculate gain of system
168-
if (not getattr(sysdata, '__iter__', False)):
169-
sys = sysdata
170-
elif len(sysdata) == 3:
171-
# TODO: replace with FRD object type?
172-
mag, phase, omega = sysdata
173-
sys = FRD(mag*exp((1j/360.)*phase), omega)
174-
else:
117+
try:
118+
if isinstance(sysdata, frdata.FRD):
119+
sys = frdata.FRD(sysdata, smooth=True)
120+
elif isinstance(sysdata, xferfcn.TransferFunction):
121+
sys = sysdata
122+
elif getattr(sysdata, '__iter__', False) and len(sysdata) == 3:
123+
mag, phase, omega = sysdata
124+
sys = frdata.FRD(mag*np.exp((1j/360.)*phase), omega, smooth=True)
125+
else:
126+
sys = xferfcn._convertToTransferFunction(sysdata)
127+
except Exception, e:
128+
print (e)
175129
raise ValueError("Margin sysdata must be either a linear system or "
176130
"a 3-sequence of mag, phase, omega.")
131+
132+
# calculate gain of system
133+
if isinstance(sys, xferfcn.TransferFunction):
177134

178-
def mod(w):
179-
"""to give the function to calculate |G(jw)| = 1"""
180-
print(w)
181-
return np.abs(sys.evalfr(w)) - 1
182-
183-
def arg(w):
184-
"""function to calculate the phase angle at -180 deg"""
185-
return np.angle(sys.evalfr(w)) + np.pi
186-
187-
# how to calculate the frequency at which |G(jw)| = 1
188-
wc = sp.optimize.newton(mod, 0.00001)
189-
w_180 = sp.optimize.newton(arg, 0.00001)
190-
191-
PM = np.angle(Gw(wc), deg=True) + 180
192-
GM = 1/(np.abs(Gw(w_180)))
135+
# check for siso
136+
if not issiso(sys):
137+
raise ValueError("Can only do margins for SISO system")
138+
139+
# real and imaginary part polynomials in omega:
140+
rnum, inum = _polyimsplit(sys.num[0][0])
141+
rden, iden = _polyimsplit(sys.den[0][0])
142+
143+
# test imaginary part of tf == 0, for phase crossover/gain margins
144+
test_w_180 = np.polyadd(np.polymul(inum, rden), np.polymul(rnum, -iden))
145+
w_180 = np.roots(test_w_180)
146+
w_180 = w_180[(np.imag(w_180) == 0) * (w_180 > 0.)]
147+
w_180.sort()
148+
149+
# test magnitude is 1 for gain crossover/phase margins
150+
test_wc = np.polysub(np.polyadd(_polysqr(rnum), _polysqr(inum)),
151+
np.polyadd(_polysqr(rden), _polysqr(iden)))
152+
wc = np.roots(test_wc)
153+
wc = wc[(np.imag(wc) == 0) * (wc > 0.)]
154+
wc.sort()
155+
156+
# stability margin was a bitch to elaborate, relies on magnitude to
157+
# point -1, then take the derivative. Second derivative needs to be >0
158+
# to have a minimum
159+
test_wstabn = np.polyadd(_polysqr(rnum), _polysqr(inum))
160+
test_wstabd = np.polyadd(_polysqr(np.polyadd(rnum,rden)),
161+
_polysqr(np.polyadd(inum,iden)))
162+
test_wstab = np.polysub(
163+
np.polymul(np.polyder(test_wstabn),test_wstabd),
164+
np.polymul(np.polyder(test_wstabd),test_wstabn))
165+
166+
# find the solutions
167+
wstab = np.roots(test_wstab)
168+
169+
# and find the value of the 2nd derivative there, needs to be positive
170+
wstabplus = np.polyval(np.polyder(test_wstab), wstab)
171+
wstab = np.real(wstab[(np.imag(wstab) == 0) * (wstab > 0.) *
172+
(np.abs(wstabplus) > 0.)])
173+
wstab.sort()
174+
175+
else:
176+
# a bit coarse, have the interpolated frd evaluated again
177+
def mod(w):
178+
"""to give the function to calculate |G(jw)| = 1"""
179+
return [np.abs(sys.evalfr(w[0])[0][0]) - 1]
180+
181+
def arg(w):
182+
"""function to calculate the phase angle at -180 deg"""
183+
return [np.angle(sys.evalfr(w[0])[0][0]) + np.pi]
184+
185+
def dstab(w):
186+
"""function to calculate the distance from -1 point"""
187+
return np.abs(sys.evalfr(w[0])[0][0] + 1.)
188+
189+
# how to calculate the frequency at which |G(jw)| = 1
190+
wc = np.array([sp.optimize.fsolve(mod, sys.omega[0])])[0]
191+
w_180 = np.array([sp.optimize.fsolve(arg, sys.omega[0])])[0]
192+
wstab = np.real(
193+
np.array([sp.optimize.fmin(dstab, sys.omega[0], disp=0)])[0])
194+
195+
# margins, as iterables, converted frdata and xferfcn calculations to
196+
# vector for this
197+
PM = np.angle(sys.evalfr(wc)[0][0], deg=True) + 180
198+
GM = 1/(np.abs(sys.evalfr(w_180)[0][0]))
199+
SM = 1/(np.abs(sys.evalfr(wstab)[0][0]))
200+
201+
if returnall:
202+
return GM, PM, SM, wc, w_180, wstab
203+
else:
204+
return (
205+
(GM.shape[0] or None) and GM[0],
206+
(PM.shape[0] or None) and PM[0],
207+
(SM.shape[0] or None) and SM[0],
208+
(wc.shape[0] or None) and wc[0],
209+
(w_180.shape[0] or None) and w_180[0],
210+
(wstab.shape[0] or None) and wstab[0])
193211

194-
return GM, PM, wc, w_180
195212

196213
# Contributed by Steffen Waldherr <waldherr@ist.uni-stuttgart.de>
197214
#! TODO - need to add test functions

‎src/xferfcn.py‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -537,7 +537,10 @@ def horner(self, s):
537537
'''
538538

539539
# Preallocate the output.
540-
out = empty((self.outputs, self.inputs), dtype=complex)
540+
if getattr(s, '__iter__', False):
541+
out = empty((self.outputs, self.inputs, len(s)), dtype=complex)
542+
else:
543+
out = empty((self.outputs, self.inputs), dtype=complex)
541544

542545
for i in range(self.outputs):
543546
for j in range(self.inputs):

‎tests/convert_test.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ def testConvert(self):
5757
for outputs in range(1, self.maxIO):
5858
# start with a random SS system and transform to TF then
5959
# back to SS, check that the matrices are the same.
60-
ssOriginal = matlab.rss(states, inputs, outputs)
60+
ssOriginal = matlab.rss(states, outputs, inputs)
6161
if (verbose):
6262
self.printSys(ssOriginal, 1)
6363

‎tests/matlab_test.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -310,12 +310,12 @@ def testLQR(self):
310310
def testRss(self):
311311
rss(1)
312312
rss(2)
313-
rss(2, 3, 1)
313+
rss(2, 1, 3)
314314

315315
def testDrss(self):
316316
drss(1)
317317
drss(2)
318-
drss(2, 3, 1)
318+
drss(2, 1, 3)
319319

320320
def testCtrb(self):
321321
ctrb(self.siso_ss1.A, self.siso_ss1.B)

‎tests/slycot_convert_test.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,7 @@ def testTF(self, verbose=False):
3737
for inputs in range(1, self.maxI+1):
3838
for outputs in range(1, self.maxO+1):
3939
for testNum in range(self.numTests):
40-
ssOriginal = matlab.rss(states, inputs, outputs)
40+
ssOriginal = matlab.rss(states, outputs, inputs)
4141
if (verbose):
4242
print('====== Original SS ==========')
4343
print(ssOriginal)
@@ -82,7 +82,7 @@ def testFreqResp(self):
8282
for testNum in range(self.numTests):
8383
for inputs in range(1,1):
8484
for outputs in range(1,1):
85-
ssOriginal = matlab.rss(states, inputs, outputs)
85+
ssOriginal = matlab.rss(states, outputs, inputs)
8686

8787
tfOriginal_Actrb, tfOriginal_Bctrb, tfOriginal_Cctrb, tfOrigingal_nctrb, tfOriginal_index,\
8888
tfOriginal_dcoeff, tfOriginal_ucoeff = tb04ad(states,inputs,outputs,\

0 commit comments

Comments
 (0)