Skip to content

Commit 11e0d30

Browse files
repaghrepagh
committed
trying to fix margin calculation
1 parent 38f6c80 commit 11e0d30

4 files changed

Lines changed: 184 additions & 97 deletions

File tree

‎src/margins.py‎

Lines changed: 122 additions & 86 deletions
Original file line numberDiff line numberDiff line change
@@ -54,108 +54,144 @@
5454
import control.xferfcn as xferfcn
5555
from control.freqplot import bode
5656
from control.lti import isdtime
57+
import scipy as sp
58+
5759

5860
# gain and phase margins
5961
# contributed by Sawyer B. Fuller <minster@caltech.edu>
6062
#! TODO - need to add unit test functions
61-
def stability_margins(sysdata, deg=True):
62-
"""Calculate gain, phase and stability margins and associated
63-
crossover frequencies.
63+
# def stability_margins(sysdata, deg=True):
64+
# """Calculate gain, phase and stability margins and associated
65+
# crossover frequencies.
6466

65-
Usage
66-
-----
67-
gm, pm, sm, wg, wp, ws = stability_margins(sysdata, deg=True)
67+
# Usage
68+
# -----
69+
# gm, pm, sm, wg, wp, ws = stability_margins(sysdata, deg=True)
6870

69-
Parameters
70-
----------
71-
sysdata: linsys or (mag, phase, omega) sequence
72-
sys : linsys
73-
Linear SISO system
74-
mag, phase, omega : sequence of array_like
75-
Input magnitude, phase, and frequencies (rad/sec) sequence from
76-
bode frequency response data
77-
deg=True: boolean
78-
If true, all input and output phases in degrees, else in radians
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
7981

80-
Returns
81-
-------
82-
gm, pm, sm, wg, wp, ws: float
83-
Gain margin gm, phase margin pm, stability margin sm, and
84-
associated crossover
85-
frequencies wg, wp, and ws of SISO open-loop. If more than
86-
one crossover frequency is detected, returns the lowest corresponding
87-
margin.
88-
"""
89-
#TODO do this precisely without the effects of discretization of frequencies?
90-
#TODO assumes SISO
91-
#TODO unit tests, margin plot
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)
117+
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")
135+
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):
92166

167+
# calculate gain of system
93168
if (not getattr(sysdata, '__iter__', False)):
94169
sys = sysdata
95-
96-
# TODO: implement for discrete time systems
97-
if (isdtime(sys, strict=True)):
98-
raise NotImplementedError("Function not implemented in discrete time")
99-
100-
mag, phase, omega = bode(sys, deg=deg, Plot=False)
101170
elif len(sysdata) == 3:
102171
# TODO: replace with FRD object type?
103172
mag, phase, omega = sysdata
173+
sys = FRD(mag*exp((1j/360.)*phase), omega)
104174
else:
105-
raise ValueError("Margin sysdata must be either a linear system or a 3-sequence of mag, phase, omega.")
175+
raise ValueError("Margin sysdata must be either a linear system or "
176+
"a 3-sequence of mag, phase, omega.")
106177

107-
if deg:
108-
cycle = 360.
109-
crossover = 180.
110-
else:
111-
cycle = 2 * np.pi
112-
crossover = np.pi
113-
114-
wrapped_phase = -np.mod(phase, cycle)
115-
116-
# phase margin from minimum phase among all gain crossovers
117-
neg_mag_crossings_i = np.nonzero(np.diff(mag < 1) > 0)[0]
118-
mag_crossings_p = wrapped_phase[neg_mag_crossings_i]
119-
if len(neg_mag_crossings_i) == 0:
120-
if mag[0] < 1: # gain always less than one
121-
wp = np.nan
122-
pm = np.inf
123-
else: # gain always greater than one
124-
print("margin: no magnitude crossings found")
125-
wp = np.nan
126-
pm = np.nan
127-
else:
128-
min_mag_crossing_i = neg_mag_crossings_i[np.argmin(mag_crossings_p)]
129-
wp = omega[min_mag_crossing_i]
130-
pm = crossover + phase[min_mag_crossing_i]
131-
if pm < 0:
132-
print("warning: system unstable: negative phase margin")
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)
133190

134-
# gain margin from minimum gain margin among all phase crossovers
135-
neg_phase_crossings_i = np.nonzero(np.diff(wrapped_phase < -crossover) > 0)[0]
136-
neg_phase_crossings_g = mag[neg_phase_crossings_i]
137-
if len(neg_phase_crossings_i) == 0:
138-
wg = np.nan
139-
gm = np.inf
140-
else:
141-
min_phase_crossing_i = neg_phase_crossings_i[
142-
np.argmax(neg_phase_crossings_g)]
143-
wg = omega[min_phase_crossing_i]
144-
gm = abs(1/mag[min_phase_crossing_i])
145-
if gm < 1:
146-
print("warning: system unstable: gain margin < 1")
147-
148-
# stability margin from minimum abs distance from -1 point
149-
if deg:
150-
phase_rad = phase * np.pi / 180.
151-
else:
152-
phase_rad = phase
153-
L = mag * np.exp(1j * phase_rad) # complex loop response to -1 pt
154-
min_Lplus1_i = np.argmin(np.abs(L + 1))
155-
sm = np.abs(L[min_Lplus1_i] + 1)
156-
ws = phase[min_Lplus1_i]
157-
158-
return gm, pm, sm, wg, wp, ws
191+
PM = np.angle(Gw(wc), deg=True) + 180
192+
GM = 1/(np.abs(Gw(w_180)))
193+
194+
return GM, PM, wc, w_180
159195

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

‎src/matlab.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1217,7 +1217,7 @@ def damp(sys, doprint=True):
12171217
# Simulation routines
12181218
# Call corresponding functions in timeresp, with arguments transposed
12191219

1220-
def step(sys, T=None, X0=0., input=0, output=0, **keywords):
1220+
def step(sys, T=None, X0=0., input=0, output=None, **keywords):
12211221
'''
12221222
Step response of a linear system
12231223

‎src/statesp.py‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -815,3 +815,50 @@ def _mimo2siso(sys, input, output, warn_conversion=False):
815815

816816
return sys
817817

818+
def _mimo2simo(sys, input, warn_conversion=False):
819+
#pylint: disable=W0622
820+
"""
821+
Convert a MIMO system to a SISO system. (Convert a system with multiple
822+
inputs and/or outputs, to a system with a single input and output.)
823+
824+
The input and output that are used in the SISO system can be selected
825+
with the parameters ``input`` and ``output``. All other inputs are set
826+
to 0, all other outputs are ignored.
827+
828+
If ``sys`` is already a SIMO system, it will be returned unaltered.
829+
830+
Parameters
831+
----------
832+
sys: StateSpace
833+
Linear (MIMO) system that should be converted.
834+
input: int
835+
Index of the input that will become the SISO system's only input.
836+
warn_conversion: bool
837+
If True: print a warning message when sys is a MIMO system.
838+
Warn that a conversion will take place.
839+
840+
Returns:
841+
842+
sys: StateSpace
843+
The converted (SIMO) system.
844+
"""
845+
if not (isinstance(input, int)):
846+
raise TypeError("Parameter ``input`` be an integer number.")
847+
if not (0 <= input < sys.inputs):
848+
raise ValueError("Selected input does not exist. "
849+
"Selected input: {sel}, "
850+
"number of system inputs: {ext}."
851+
.format(sel=input, ext=sys.inputs))
852+
#Convert sys to SISO if necessary
853+
if sys.inputs > 1:
854+
if warn_conversion:
855+
warnings.warn("Converting MIMO system to SISO system. "
856+
"Only input {i} and output {o} are used."
857+
.format(i=input, o=output))
858+
# $X = A*X + B*U
859+
# Y = C*X + D*U
860+
new_B = sys.B[:, input]
861+
new_D = sys.D[:, input]
862+
sys = StateSpace(sys.A, new_B, sys.C, new_D, sys.dt)
863+
864+
return sys

‎src/timeresp.py‎

Lines changed: 14 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -121,7 +121,7 @@
121121
from copy import deepcopy
122122
import warnings
123123
from control.lti import Lti # base class of StateSpace, TransferFunction
124-
from control. statesp import StateSpace, _rss_generate, _convertToStateSpace, _mimo2siso
124+
from control. statesp import StateSpace, _rss_generate, _convertToStateSpace, _mimo2simo, _mimo2siso
125125
from control.lti import isdtime, isctime
126126

127127
# Helper function for checking array-like parameters
@@ -386,15 +386,15 @@ def f_dot(x, t):
386386

387387
return T, yout, xout
388388

389-
def step_response(sys, T=None, X0=0., input=0, output=0, \
390-
transpose = False, **keywords):
389+
def step_response(sys, T=None, X0=0., input=0, output=None, \
390+
transpose = False, **keywords):
391391
#pylint: disable=W0622
392392
"""Step response of a linear system
393393
394-
If the system has multiple inputs or outputs (MIMO), one input and one
395-
output have to be selected for the simulation. The parameters `input`
396-
and `output` do this. All other inputs are set to 0, all other outputs
397-
are ignored.
394+
If the system has multiple inputs or outputs (MIMO), one input has
395+
to be selected for the simulation. Optionally, one output may be
396+
selected. The parameters `input` and `output` do this. All other
397+
inputs are set to 0, all other outputs are ignored.
398398
399399
For information on the **shape** of parameters `T`, `X0` and
400400
return values `T`, `yout` see: :ref:`time-series-convention`
@@ -416,7 +416,8 @@ def step_response(sys, T=None, X0=0., input=0, output=0, \
416416
Index of the input that will be used in this simulation.
417417
418418
output: int
419-
Index of the output that will be used in this simulation.
419+
Index of the output that will be used in this simulation. Set to None
420+
to not trim outputs
420421
421422
transpose: bool
422423
If True, transpose all input and output arrays (for backward
@@ -447,7 +448,10 @@ def step_response(sys, T=None, X0=0., input=0, output=0, \
447448
>>> T, yout = step_response(sys, T, X0)
448449
"""
449450
sys = _convertToStateSpace(sys)
450-
sys = _mimo2siso(sys, input, output, warn_conversion=True)
451+
if output == None:
452+
sys = _mimo2simo(sys, input, warn_conversion=True)
453+
else:
454+
sys = _mimo2siso(sys, input, output, warn_conversion=True)
451455
if T is None:
452456
if isctime(sys):
453457
T = _default_response_times(sys.A, 100)
@@ -459,7 +463,7 @@ def step_response(sys, T=None, X0=0., input=0, output=0, \
459463
U = np.ones_like(T)
460464

461465
T, yout, _xout = forced_response(sys, T, U, X0,
462-
transpose=transpose, **keywords)
466+
transpose=transpose, **keywords)
463467

464468
return T, yout
465469

0 commit comments

Comments
 (0)