Skip to content

Fixes to margin calculation #68

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 8 commits into from
May 30, 2016
Merged

Fixes to margin calculation #68

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
6523270
Keep the "smooth" parameter when combining FRD functions with other s…
repagh Jul 7, 2015
ae09104
Fix the margin calculation for FRD/data based margins.
repagh Jul 7, 2015
7eefe8b
corrected the stability margin calculations
repagh Jul 8, 2015
f309232
still imperfect
repagh Jul 8, 2015
815ca2d
further attempts at fixing
repagh Jul 9, 2015
cd30a00
corrected margin calculations; extended test to do all-margin calcula…
repagh Jul 11, 2015
04ed40d
Merge remote-tracking branch 'upstream/master'
repagh Jul 11, 2015
ebde9e4
need small fix to doc in stability_margins; needed 1e-8 as cutoff for
repagh Jul 11, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 24 additions & 13 deletions control/frdata.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -220,7 +220,8 @@ def __mul__(self, other):

# Convert the second argument to a transfer function.
if isinstance(other, (int, float, complex)):
return FRD(self.fresp * other, self.omega)
return FRD(self.fresp * other, self.omega,
smooth=(self.ifunc is not None))
else:
other = _convertToFRD(other, omega=self.omega)

Expand All @@ -236,14 +237,17 @@ def __mul__(self, other):
dtype=self.fresp.dtype)
for i in range(len(self.omega)):
fresp[:,:,i] = dot(self.fresp[:,:,i], other.fresp[:,:,i])
return FRD(fresp, self.omega)
return FRD(fresp, self.omega,
smooth=(self.ifunc is not None) and
(other.ifunc is not None))

def __rmul__(self, other):
"""Right Multiply two LTI objects (serial connection)."""

# Convert the second argument to an frd function.
if isinstance(other, (int, float, complex)):
return FRD(self.fresp * other, self.omega)
return FRD(self.fresp * other, self.omega,
smooth=(self.ifunc is not None))
else:
other = _convertToFRD(other, omega=self.omega)

Expand All @@ -260,14 +264,17 @@ def __rmul__(self, other):
dtype=self.fresp.dtype)
for i in range(len(self.omega)):
fresp[:,:,i] = dot(other.fresp[:,:,i], self.fresp[:,:,i])
return FRD(fresp, self.omega)
return FRD(fresp, self.omega,
smooth=(self.ifunc is not None) and
(other.ifunc is not None))

# TODO: Division of MIMO transfer function objects is not written yet.
def __truediv__(self, other):
"""Divide two LTI objects."""

if isinstance(other, (int, float, complex)):
return FRD(self.fresp * (1/other), self.omega)
return FRD(self.fresp * (1/other), self.omega,
smooth=(self.ifunc is not None))
else:
other = _convertToFRD(other, omega=self.omega)

Expand All @@ -277,7 +284,9 @@ def __truediv__(self, other):
raise NotImplementedError(
"FRD.__truediv__ is currently implemented only for SISO systems.")

return FRD(self.fresp/other.fresp, self.omega)
return FRD(self.fresp/other.fresp, self.omega,
smooth=(self.ifunc is not None) and
(other.ifunc is not None))

# TODO: Remove when transition to python3 complete
def __div__(self, other):
Expand All @@ -287,7 +296,8 @@ def __div__(self, other):
def __rtruediv__(self, other):
"""Right divide two LTI objects."""
if isinstance(other, (int, float, complex)):
return FRD(other / self.fresp, self.omega)
return FRD(other / self.fresp, self.omega,
smooth=(self.ifunc is not None))
else:
other = _convertToFRD(other, omega=self.omega)

Expand All @@ -306,7 +316,8 @@ def __pow__(self,other):
if not type(other) == int:
raise ValueError("Exponent must be an integer")
if other == 0:
return FRD(ones(self.fresp.shape),self.omega) #unity
return FRD(ones(self.fresp.shape),self.omega,
smooth=(self.ifunc is not None)) #unity
if other > 0:
return self * (self**(other-1))
if other < 0:
Expand Down Expand Up @@ -338,7 +349,7 @@ def evalfr(self, omega):
except:
raise ValueError(
"Frequency %f not in frequency list, try an interpolating"
" FRD if you want additional points")
" FRD if you want additional points" % omega)
else:
if getattr(omega, '__iter__', False):
for i in range(self.outputs):
Expand Down Expand Up @@ -401,7 +412,7 @@ def feedback(self, other=1, sign=-1):
self.fresp[:, :, k].view(type=matrix),
eye(self.inputs))

return FRD(fresp, other.omega)
return FRD(fresp, other.omega, smooth=(self.ifunc is not None))

def _convertToFRD(sys, omega, inputs=1, outputs=1):
"""Convert a system to frequency response data form (if needed).
Expand Down Expand Up @@ -436,11 +447,11 @@ def _convertToFRD(sys, omega, inputs=1, outputs=1):
for k, w in enumerate(omega):
fresp[:, :, k] = sys.evalfr(w)

return FRD(fresp, omega)
return FRD(fresp, omega, smooth=True)

elif isinstance(sys, (int, float, complex)):
fresp = ones((outputs, inputs, len(omega)), dtype=float)*sys
return FRD(fresp, omega)
return FRD(fresp, omega, smooth=True)

# try converting constant matrices
try:
Expand All @@ -450,7 +461,7 @@ def _convertToFRD(sys, omega, inputs=1, outputs=1):
for i in range(outputs):
for j in range(inputs):
fresp[i,j,:] = sys[i,j]
return FRD(fresp, omega)
return FRD(fresp, omega, smooth=True)
except:
pass

Expand Down
107 changes: 74 additions & 33 deletions control/margins.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,10 @@ def _polysqr(pol):
#
# RvP, July 8, 2014, corrected to exclude phase=0 crossing for the gain
# margin polynomial
def stability_margins(sysdata, returnall=False, epsw=1e-10):
# RvP, July 8, 2015, augmented to calculate all phase/gain crossings with
# frd data. Correct to return smallest phase
# margin, smallest gain margin and their frequencies
def stability_margins(sysdata, returnall=False, epsw=1e-8):
"""Calculate stability margins and associated crossover frequencies.

Parameters
Expand All @@ -101,7 +104,7 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10):
minimum stability margins. For frequency data or FRD systems, only one
margin is found and returned.
epsw: float, optional
Frequencies below this value (default 1e-10) are considered static gain,
Frequencies below this value (default 1e-8) are considered static gain,
and not returned as margin.

Returns
Expand All @@ -127,8 +130,8 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10):
sys = sysdata
elif getattr(sysdata, '__iter__', False) and len(sysdata) == 3:
mag, phase, omega = sysdata
sys = frdata.FRD(mag * np.exp(1j * phase * np.pi/180), omega,
smooth=True)
sys = frdata.FRD(mag * np.exp(1j * phase * np.pi/180),
omega, smooth=True)
else:
sys = xferfcn._convertToTransferFunction(sysdata)
except Exception as e:
Expand All @@ -147,23 +150,27 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10):
rnum, inum = _polyimsplit(sys.num[0][0])
rden, iden = _polyimsplit(sys.den[0][0])

# test imaginary part of tf == 0, for phase crossover/gain margins
# test (imaginary part of tf) == 0, for phase crossover/gain margins
test_w_180 = np.polyadd(np.polymul(inum, rden), np.polymul(rnum, -iden))
w_180 = np.roots(test_w_180)
#print ('1:w_180', w_180)

# first remove imaginary and negative frequencies, epsw removes the
# "0" frequency for type-2 systems
w_180 = np.real(w_180[(np.imag(w_180) == 0) * (w_180 >= epsw)])
#print ('2:w_180', w_180)

# evaluate response at remaining frequencies, to test for phase 180 vs 0
resp_w_180 = np.real(np.polyval(sys.num[0][0], 1.j*w_180) /
np.polyval(sys.den[0][0], 1.j*w_180))
#print ('resp_w_180', resp_w_180)

# only keep frequencies where the negative real axis is crossed
w_180 = w_180[(resp_w_180 < 0.0)]
w_180 = w_180[np.real(resp_w_180) < 0.0]

# and sort
w_180.sort()
#print ('3:w_180', w_180)

# test magnitude is 1 for gain crossover/phase margins
test_wc = np.polysub(np.polyadd(_polysqr(rnum), _polysqr(inum)),
Expand All @@ -175,58 +182,91 @@ def stability_margins(sysdata, returnall=False, epsw=1e-10):
# stability margin was a bitch to elaborate, relies on magnitude to
# point -1, then take the derivative. Second derivative needs to be >0
# to have a minimum
test_wstabn = np.polyadd(_polysqr(rnum), _polysqr(inum))
test_wstabd = np.polyadd(_polysqr(np.polyadd(rnum,rden)),
test_wstabd = np.polyadd(_polysqr(rden), _polysqr(iden))
test_wstabn = np.polyadd(_polysqr(np.polyadd(rnum,rden)),
_polysqr(np.polyadd(inum,iden)))
test_wstab = np.polysub(
np.polymul(np.polyder(test_wstabn),test_wstabd),
np.polymul(np.polyder(test_wstabd),test_wstabn))

# find the solutions
# find the solutions, for positive omega, and only real ones
wstab = np.roots(test_wstab)
#print('wstabr', wstab)
wstab = np.real(wstab[(np.imag(wstab) == 0) *
(np.real(wstab) >= 0)])
#print('wstab', wstab)

# and find the value of the 2nd derivative there, needs to be positive
wstabplus = np.polyval(np.polyder(test_wstab), wstab)
#print('wstabplus', wstabplus)
wstab = np.real(wstab[(np.imag(wstab) == 0) * (wstab > epsw) *
(np.abs(wstabplus) > 0.)])
(wstabplus > 0.)])
#print('wstab', wstab)
wstab.sort()

else:
# a bit coarse, have the interpolated frd evaluated again
def mod(w):
"""to give the function to calculate |G(jw)| = 1"""
return [np.abs(sys.evalfr(w[0])[0][0]) - 1]
return np.abs(sys.evalfr(w)[0][0]) - 1

def arg(w):
"""function to calculate the phase angle at -180 deg"""
return [np.angle(sys.evalfr(w[0])[0][0]) + np.pi]
return np.angle(-sys.evalfr(w)[0][0])

def dstab(w):
"""function to calculate the distance from -1 point"""
return np.abs(sys.evalfr(w[0])[0][0] + 1.)

# how to calculate the frequency at which |G(jw)| = 1
wc = np.array([sp.optimize.fsolve(mod, sys.omega[0])])[0]
w_180 = np.array([sp.optimize.fsolve(arg, sys.omega[0])])[0]
wstab = np.real(
np.array([sp.optimize.fmin(dstab, sys.omega[0], disp=0)])[0])
return np.abs(sys.evalfr(w)[0][0] + 1.)

# Find all crossings, note that this depends on omega having
# a correct range
widx = np.where(np.diff(np.sign(mod(sys.omega))))[0]
wc = np.array(
[ sp.optimize.brentq(mod, sys.omega[i], sys.omega[i+1])
for i in widx if i+1 < len(sys.omega)])

# find the phase crossings ang(H(jw) == -180
widx = np.where(np.diff(np.sign(arg(sys.omega))))[0]
#print('widx (180)', widx, sys.omega[widx])
#print('x', sys.evalfr(sys.omega[widx])[0][0])
widx = widx[np.real(sys.evalfr(sys.omega[widx])[0][0]) <= 0]
#print('widx (180,2)', widx)
w_180 = np.array(
[ sp.optimize.brentq(arg, sys.omega[i], sys.omega[i+1])
for i in widx if i+1 < len(sys.omega) ])
#print('x', sys.evalfr(w_180)[0][0])
#print('w_180', w_180)

# find all stab margins?
widx = np.where(np.diff(np.sign(np.diff(dstab(sys.omega)))))[0]
#print('widx', widx)
#print('wstabx', sys.omega[widx])
wstab = np.array([ sp.optimize.minimize_scalar(
dstab, bracket=(sys.omega[i], sys.omega[i+1])).x
for i in widx if i+1 < len(sys.omega) and
np.diff(np.diff(dstab(sys.omega[i-1:i+2])))[0] > 0 ])
#print('wstabf0', wstab)
wstab = wstab[(wstab >= sys.omega[0]) *
(wstab <= sys.omega[-1])]
#print ('wstabf', wstab)


# margins, as iterables, converted frdata and xferfcn calculations to
# vector for this
PM = np.angle(sys.evalfr(wc)[0][0], deg=True) + 180
GM = 1/(np.abs(sys.evalfr(w_180)[0][0]))
GM = 1/np.abs(sys.evalfr(w_180)[0][0])
SM = np.abs(sys.evalfr(wstab)[0][0]+1)

PM = np.angle(sys.evalfr(wc)[0][0], deg=True) + 180

if returnall:
return GM, PM, SM, w_180, wc, wstab
else:
return (
(GM.shape[0] or None) and GM[0],
(PM.shape[0] or None) and PM[0],
(SM.shape[0] or None) and SM[0],
(w_180.shape[0] or None) and w_180[0],
(wc.shape[0] or None) and wc[0],
(wstab.shape[0] or None) and wstab[0])
(GM.shape[0] or None) and np.amin(GM),
(PM.shape[0] or None) and np.amin(PM),
(SM.shape[0] or None) and np.amin(SM),
(w_180.shape[0] or None) and w_180[GM==np.amin(GM)][0],
(wc.shape[0] or None) and wc[PM==np.amin(PM)][0],
(wstab.shape[0] or None) and wstab[SM==np.amin(SM)][0])


# Contributed by Steffen Waldherr <waldherr@ist.uni-stuttgart.de>
Expand Down Expand Up @@ -291,11 +331,12 @@ def margin(*args):
Returns
-------
gm, pm, Wcg, Wcp : float
Gain margin gm, phase margin pm (in deg), gain crossover frequency
(corresponding to phase margin) and phase crossover frequency
(corresponding to gain margin), in rad/sec of SISO open-loop.
If more than one crossover frequency is detected, returns the lowest
corresponding margin.
Gain margin gm, phase margin pm (in deg), phase crossover frequency
(corresponding to gain margin, where phase=-180) and gain crossover
frequency (corresponding to phase margin, where gain is 0dB),
in rad/sec of SISO open-loop.
If more than one crossover frequency is detected for gain or phase,
this returns one with the lowest corresponding margin.

Examples
--------
Expand Down
Loading