Skip to content

Commit 775b03f

Browse files
murrayrmmurrayrm
committed
implementation of initial_phase, wrap_phase keywords for bode()
1 parent 383e7a4 commit 775b03f

File tree

2 files changed

+139
-13
lines changed

2 files changed

+139
-13
lines changed

‎control/freqplot.py

Lines changed: 72 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -78,6 +78,7 @@
7878
'bode.deg': True, # Plot phase in degrees
7979
'bode.Hz': False, # Plot frequency in Hertz
8080
'bode.grid': True, # Turn on grid for gain and phase
81+
'bode.wrap_phase': False, # Wrap the phase plot at a given value
8182
}
8283

8384

@@ -131,7 +132,18 @@ def bode_plot(syslist, omega=None,
131132
grid : bool
132133
If True, plot grid lines on gain and phase plots. Default is set by
133134
`config.defaults['bode.grid']`.
134-
135+
initial_phase : float
136+
Set the reference phase to use for the lowest frequency. If set, the
137+
initial phase of the Bode plot will be set to the value closest to the
138+
value specified. Default is 180 if wrap_phase is False, 0 if
139+
wrap_phase is True.
140+
wrap_phase : bool or float
141+
If wrap_phase is `False`, then the phase will be unwrapped so that it
142+
is continuously increasing or decreasing. If wrap_phase is `True` the
143+
phase will be restricted to the range [-180, 180) (or [:math:`-\\pi`,
144+
:math:`\\pi`) radians). If `wrap_phase` is specified as a float, the
145+
phase will be offset by 360 degrees if it falls below the specified
146+
value. Default to `False`, set by config.defaults['bode.wrap_phase'].
135147
136148
The default values for Bode plot configuration parameters can be reset
137149
using the `config.defaults` dictionary, with module name 'bode'.
@@ -171,6 +183,10 @@ def bode_plot(syslist, omega=None,
171183
grid = config._get_param('bode', 'grid', kwargs, _bode_defaults, pop=True)
172184
plot = config._get_param('bode', 'grid', plot, True)
173185
margins = config._get_param('bode', 'margins', margins, False)
186+
wrap_phase = config._get_param(
187+
'bode', 'wrap_phase', kwargs, _bode_defaults, pop=True)
188+
initial_phase = config._get_param(
189+
'bode', 'initial_phase', kwargs, None, pop=True)
174190

175191
# If argument was a singleton, turn it into a list
176192
if not getattr(syslist, '__iter__', False):
@@ -209,11 +225,47 @@ def bode_plot(syslist, omega=None,
209225
# TODO: What distance to the Nyquist frequency is appropriate?
210226
else:
211227
nyquistfrq = None
228+
212229
# Get the magnitude and phase of the system
213230
mag_tmp, phase_tmp, omega_sys = sys.freqresp(omega_sys)
214231
mag = np.atleast_1d(np.squeeze(mag_tmp))
215232
phase = np.atleast_1d(np.squeeze(phase_tmp))
216-
phase = unwrap(phase)
233+
234+
#
235+
# Post-process the phase to handle initial value and wrapping
236+
#
237+
238+
if initial_phase is None:
239+
# Start phase in the range 0 to -360 w/ initial phase = -180
240+
# If wrap_phase is true, use 0 instead (phase \in (-pi, pi])
241+
initial_phase = -math.pi if wrap_phase is not True else 0
242+
elif isinstance(initial_phase, (int, float)):
243+
# Allow the user to override the default calculation
244+
if deg:
245+
initial_phase = initial_phase/180. * math.pi
246+
else:
247+
raise ValueError("initial_phase must be a number.")
248+
249+
# Shift the phase if needed
250+
if abs(phase[0] - initial_phase) > math.pi:
251+
phase -= 2*math.pi * \
252+
round((phase[0] - initial_phase) / (2*math.pi))
253+
254+
# Phase wrapping
255+
if wrap_phase is False:
256+
phase = unwrap(phase) # unwrap the phase
257+
elif wrap_phase is True:
258+
pass # default calculation OK
259+
elif isinstance(wrap_phase, (int, float)):
260+
phase = unwrap(phase) # unwrap the phase first
261+
if deg:
262+
wrap_phase *= math.pi/180.
263+
264+
# Shift the phase if it is below the wrap_phase
265+
phase += 2*math.pi * np.maximum(
266+
0, np.ceil((wrap_phase - phase)/(2*math.pi)))
267+
else:
268+
raise ValueError("wrap_phase must be bool or float.")
217269

218270
mags.append(mag)
219271
phases.append(phase)
@@ -270,7 +322,9 @@ def bode_plot(syslist, omega=None,
270322
label='control-bode-phase',
271323
sharex=ax_mag)
272324

325+
#
273326
# Magnitude plot
327+
#
274328
if dB:
275329
pltline = ax_mag.semilogx(omega_plot, 20 * np.log10(mag),
276330
*args, **kwargs)
@@ -285,19 +339,22 @@ def bode_plot(syslist, omega=None,
285339
ax_mag.grid(grid and not margins, which='both')
286340
ax_mag.set_ylabel("Magnitude (dB)" if dB else "Magnitude")
287341

342+
#
288343
# Phase plot
289-
if deg:
290-
phase_plot = phase * 180. / math.pi
291-
else:
292-
phase_plot = phase
344+
#
345+
phase_plot = phase * 180. / math.pi if deg else phase
346+
347+
# Plot the data
293348
ax_phase.semilogx(omega_plot, phase_plot, *args, **kwargs)
294349

295350
# Show the phase and gain margins in the plot
296351
if margins:
352+
# Compute stability margins for the system
297353
margin = stability_margins(sys)
298-
gm, pm, Wcg, Wcp = \
299-
margin[0], margin[1], margin[3], margin[4]
300-
# TODO: add some documentation describing why this is here
354+
gm, pm, Wcg, Wcp = (margin[i] for i in (0, 1, 3, 4))
355+
356+
# Figure out sign of the phase at the first gain crossing
357+
# (needed if phase_wrap is True)
301358
phase_at_cp = phases[0][(np.abs(omegas[0] - Wcp)).argmin()]
302359
if phase_at_cp >= 0.:
303360
phase_limit = 180.
@@ -307,6 +364,7 @@ def bode_plot(syslist, omega=None,
307364
if Hz:
308365
Wcg, Wcp = Wcg/(2*math.pi), Wcp/(2*math.pi)
309366

367+
# Draw lines at gain and phase limits
310368
ax_mag.axhline(y=0 if dB else 1, color='k', linestyle=':',
311369
zorder=-20)
312370
ax_phase.axhline(y=phase_limit if deg else
@@ -315,6 +373,7 @@ def bode_plot(syslist, omega=None,
315373
mag_ylim = ax_mag.get_ylim()
316374
phase_ylim = ax_phase.get_ylim()
317375

376+
# Annotate the phase margin (if it exists)
318377
if pm != float('inf') and Wcp != float('nan'):
319378
if dB:
320379
ax_mag.semilogx(
@@ -327,7 +386,7 @@ def bode_plot(syslist, omega=None,
327386

328387
if deg:
329388
ax_phase.semilogx(
330-
[Wcp, Wcp], [1e5, phase_limit+pm],
389+
[Wcp, Wcp], [1e5, phase_limit + pm],
331390
color='k', linestyle=':', zorder=-20)
332391
ax_phase.semilogx(
333392
[Wcp, Wcp], [phase_limit + pm, phase_limit],
@@ -343,6 +402,7 @@ def bode_plot(syslist, omega=None,
343402
math.radians(phase_limit)],
344403
color='k', zorder=-20)
345404

405+
# Annotate the gain margin (if it exists)
346406
if gm != float('inf') and Wcg != float('nan'):
347407
if dB:
348408
ax_mag.semilogx(
@@ -360,11 +420,11 @@ def bode_plot(syslist, omega=None,
360420

361421
if deg:
362422
ax_phase.semilogx(
363-
[Wcg, Wcg], [1e-8, phase_limit],
423+
[Wcg, Wcg], [0, phase_limit],
364424
color='k', linestyle=':', zorder=-20)
365425
else:
366426
ax_phase.semilogx(
367-
[Wcg, Wcg], [1e-8, math.radians(phase_limit)],
427+
[Wcg, Wcg], [0, math.radians(phase_limit)],
368428
color='k', linestyle=':', zorder=-20)
369429

370430
ax_mag.set_ylim(mag_ylim)

‎control/tests/freqresp_test.py

Lines changed: 67 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
import matplotlib.pyplot as plt
1010
import numpy as np
1111
from numpy.testing import assert_allclose
12+
import math
1213
import pytest
1314

1415
import control as ctrl
@@ -173,7 +174,7 @@ def test_bode_margin(dB, maginfty1, maginfty2, gminv,
173174
rtol=1e-5)
174175

175176
phase_to_infinity = (np.array([Wcg, Wcg]),
176-
np.array([1e-8, p0]))
177+
np.array([0, p0]))
177178
assert_allclose(phase_to_infinity, allaxes[1].lines[4].get_data(),
178179
rtol=1e-5)
179180

@@ -271,3 +272,68 @@ def test_options(editsdefaults):
271272
# Make sure we got the right number of points
272273
assert numpoints1 != numpoints3
273274
assert numpoints3 == 13
275+
276+
@pytest.mark.parametrize(
277+
"TF, initial_phase, default_phase, expected_phase",
278+
[pytest.param(ctrl.tf([1], [1, 0]),
279+
None, -math.pi/2, -math.pi/2, id="order1, default"),
280+
pytest.param(ctrl.tf([1], [1, 0]),
281+
180, -math.pi/2, 3*math.pi/2, id="order1, 180"),
282+
pytest.param(ctrl.tf([1], [1, 0, 0]),
283+
None, -math.pi, -math.pi, id="order2, default"),
284+
pytest.param(ctrl.tf([1], [1, 0, 0]),
285+
180, -math.pi, math.pi, id="order2, 180"),
286+
pytest.param(ctrl.tf([1], [1, 0, 0, 0]),
287+
None, -3*math.pi/2, -3*math.pi/2, id="order2, default"),
288+
pytest.param(ctrl.tf([1], [1, 0, 0, 0]),
289+
180, -3*math.pi/2, math.pi/2, id="order2, 180"),
290+
pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0]),
291+
None, 0, 0, id="order4, default"),
292+
pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0]),
293+
180, 0, 0, id="order4, 180"),
294+
pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0]),
295+
-360, 0, -2*math.pi, id="order4, -360"),
296+
])
297+
def test_initial_phase(TF, initial_phase, default_phase, expected_phase):
298+
# Check initial phase of standard transfer functions
299+
mag, phase, omega = ctrl.bode(TF)
300+
assert(abs(phase[0] - default_phase) < 0.1)
301+
302+
# Now reset the initial phase to +180 and see if things work
303+
mag, phase, omega = ctrl.bode(TF, initial_phase=initial_phase)
304+
assert(abs(phase[0] - expected_phase) < 0.1)
305+
306+
# Make sure everything works in rad/sec as well
307+
# Turn off plotting since that seems to slow things down a lot (?)
308+
if initial_phase:
309+
mag, phase, omega = ctrl.bode(
310+
TF, initial_phase=initial_phase/180. * math.pi, deg=False,
311+
plot=False)
312+
assert(abs(phase[0] - expected_phase) < 0.1)
313+
314+
315+
@pytest.mark.parametrize(
316+
"TF, wrap_phase, min_phase, max_phase",
317+
[pytest.param(ctrl.tf([1], [1, 0]),
318+
None, -math.pi/2, 0, id="order1, default"),
319+
pytest.param(ctrl.tf([1], [1, 0]),
320+
True, -math.pi, math.pi, id="order1, True"),
321+
pytest.param(ctrl.tf([1], [1, 0]),
322+
-270, -3*math.pi/2, math.pi/2, id="order1, -270"),
323+
pytest.param(ctrl.tf([1], [1, 0, 0, 0]),
324+
None, -3*math.pi/2, 0, id="order3, default"),
325+
pytest.param(ctrl.tf([1], [1, 0, 0, 0]),
326+
True, -math.pi, math.pi, id="order3, True"),
327+
pytest.param(ctrl.tf([1], [1, 0, 0, 0]),
328+
-270, -3*math.pi/2, math.pi/2, id="order3, -270"),
329+
pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0, 0]),
330+
True, -3*math.pi/2, 0, id="order5, default"),
331+
pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0, 0]),
332+
True, -math.pi, math.pi, id="order5, True"),
333+
pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0, 0]),
334+
-270, -3*math.pi/2, math.pi/2, id="order5, -270"),
335+
])
336+
def test_phase_wrap(TF, wrap_phase, min_phase, max_phase):
337+
mag, phase, omega = ctrl.bode(TF, wrap_phase=wrap_phase)
338+
assert(min(phase) >= min_phase)
339+
assert(max(phase) <= max_phase)

0 commit comments

Comments
 (0)