8000 add intersection computation + Jupyter notebook, documentation · python-control/python-control@8518b9d · GitHub
[go: up one dir, main page]
Skip to content

Commit 8518b9d

Browse files
murrayrmmurrayrm
committed
add intersection computation + Jupyter notebook, documentation
1 parent a027d3c commit 8518b9d

File tree

4 files changed

+587
-56
lines changed

4 files changed

+587
-56
lines changed

‎control/freqplot.py

Lines changed: 7 additions & 6 deletions
510
Original file line numberDiff line numberDiff line change
@@ -508,7 +508,7 @@ def gen_zero_centered_series(val_min, val_max, period):
508508

509509
def nyquist_plot(syslist, omega=None, plot=True, label_freq=0,
510
arrowhead_length=0.1, arrowhead_width=0.1,
511-
color=None, *args, **kwargs):
511+
mirror='--', color=None, *args, **kwargs):
512512
"""
513513
Nyquist plot for a system
514514
@@ -606,11 +606,12 @@ def nyquist_plot(syslist, omega=None, plot=True, label_freq=0,
606606
head_width=arrowhead_width,
607607
head_length=arrowhead_length)
608608

609-
plt.plot(x, -y, '-', color=c, *args, **kwargs)
610-
ax.arrow(
611-
x[-1], -y[-1], (x[-1]-x[-2])/2, (y[-1]-y[-2])/2,
612-
fc=c, ec=c, head_width=arrowhead_width,
613-
head_length=arrowhead_length)
609+
if mirror is not False:
610+
plt.plot(x, -y, mirror, color=c, *args, **kwargs)
611+
ax.arrow(
612+
x[-1], -y[-1], (x[-1]-x[-2])/2, (y[-1]-y[-2])/2,
613+
fc=c, ec=c, head_width=arrowhead_width,
614+
head_length=arrowhead_length)
614615

615616
# Mark the -1 point
616617
plt.plot([-1], [0], 'r+')

‎control/nltools.py

Lines changed: 139 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
import math
1818
import numpy as np
1919
import matplotlib.pyplot as plt
20+
import scipy
2021
from numpy import where, dstack, diff, meshgrid
2122
from warnings import warn
2223

@@ -28,7 +29,8 @@ def sector_bounds(fcn):
2829
raise NotImplementedError("function not currently implemented")
2930

3031

31-
def describing_function(fcn, amp, num_points=100, zero_check=True):
32+
def describing_function(
33+
fcn, amp, num_points=100, zero_check=True, try_method=True):
3234
"""Numerical compute the describing function of a nonlinear function
3335
3436
The describing function of a static nonlinear function is given by
@@ -47,6 +49,18 @@ def describing_function(fcn, amp, num_points=100, zero_check=True):
4749
amp : float or array
4850
The amplitude(s) at which the describing function should be calculated.
4951
52+
zero_check : bool, optional
53+
If `True` (default) then `amp` is zero, the function will be evaluated
54+
and checked to make sure it is zero. If not, a `TypeError` exception
55+
is raised. If zero_check is `False`, no check is made on the value of
56+
the function at zero.
57+
58+
try_method : bool, optional
59+
If `True` (default), check the `fcn` argument to see if it is an
60+
object with a `describing_function` method and use this to compute the
61+
describing function. See the :class:`NonlienarFunction` class for
62+
more information on the `describing_function` method.
63+
5064
Returns
5165
-------
5266
df : complex or array of complex
@@ -58,6 +72,14 @@ def describing_function(fcn, amp, num_points=100, zero_check=True):
5872
If amp < 0 or if amp = 0 and the function fcn(0) is non-zero.
5973
6074
"""
75+
# If there is an analytical solution, trying using that first
76+
if try_method and hasattr(fcn, 'describing_function'):
77+
# Go through all of the amplitudes we were given
78+
df = []
79+
for a in np.atleast_1d(amp):
80+
df.append(fcn.describing_function(a))
81+
return np.array(df).reshape(np.shape(amp))
82+
6183
#
6284
# The describing function of a nonlinear function F() can be computed by
6385
# evaluating the nonlinearity over a sinusoid. The Fourier series for a
@@ -83,7 +105,7 @@ def describing_function(fcn, amp, num_points=100, zero_check=True):
83105
#
84106
# From these we can compute M1 and \phi.
85107
#
86-
108+
87109
# Evaluate over a full range of angles
88110
theta = np.linspace(0, 2*np.pi, num_points)
89111
dtheta = theta[1] - theta[0]
@@ -122,7 +144,8 @@ def describing_function(fcn, amp, num_points=100, zero_check=True):
122144
return np.array(df).reshape(np.shape(amp))
123145

124146

125-
def describing_function_plot(H, F, a, omega=None):
147+
def describing_function_plot(
148+
H, F, a, omega=None, refine=True, label="%5.2g @ %-5.2g", **kwargs):
126149
"""Plot a Nyquist plot with a describing function for a nonlinear system.
127150
128151
This function generates a Nyquist plot for a closed loop system consisting
@@ -133,24 +156,98 @@ def describing_function_plot(H, F, a, omega=None):
133156
H : LTI system
134157
Linear time-invariant (LTI) system (state space, transfer function, or
135158
FRD)
136-
F : static nonlinear function
159+
F : static nonlinear function
137160
A static nonlinearity, either a scalar function or a single-input,
138161
single-output, static input/output system.
139162
a : list
140163
List of amplitudes to be used for the describing function plot.
141164
omega : list, optional
142165
List of frequences to be used for the linear system Nyquist curve.
143166
167+
Returns
168+
-------
169+
intersection_list : 1D array of 2-tuples
170+
A list of all amplitudes and frequencies in which :math:`H(j\\omega)
171+
N(a) = -1`, where :math:`N(a)` is the describing function associated
172+
with `F`, or `None` if there are no such points. Each pair represents
173+
a potential limit cycle for the closed loop system.
174+
144175
"""
145176
# Start by drawing a Nyquist curve
146-
H_real, H_imag, H_omega = nyquist_plot(H, omega, plot=True)
177+
H_real, H_imag, H_omega = nyquist_plot(H, omega, plot=True, **kwargs)
178+
H_vals = H_real + 1j * H_imag
147179

148180
# Compute the describing function
149181
df = describing_function(F, a)
150-
dfinv = -1/df
151-
152-
# Now add on the describing function
153-
plt.plot(dfinv.real, dfinv.imag)
182+
N_vals = -1/df
183+
184+
# Now add the describing function curve to the plot
185+
plt.plot(N_vals.real, N_vals.imag)
186+
187+
# Look for intersection points
188+
intersections = []
189+
for i in range(N_vals.size - 1):
190+
for j in range(H_vals.size - 1):
191+
intersect = _find_intersection(
192+
N_vals[i], N_vals[i+1], H_vals[j], H_vals[j+1])
193+
if intersect == None:
194+
continue
195+
196+
# Found an intersection, compute a and omega
197+
s_amp, s_omega = intersect
198+
a_guess = (1 - s_amp) * a[i] + s_amp * a[i+1]
199+
omega_guess = (1 - s_omega) * H_omega[j] + s_omega * H_omega[j+1]
200+
201+
# Refine the coarse estimate to get better intersection point
202+
a_final, omega_final = a_guess, omega_guess
203+
if refine:
204+
# Refine the answer to get more accuracy
205+
def _cost(x):
206+
return abs(1 + H(1j * x[1]) *
207+
describing_function(F, x[0]))**2
208+
res = scipy.optimize.minimize(_cost, [a_guess, omega_guess])
209+
210+
if not res.success:
211+
warn("not able to refine result; returning estimate")
212+
else:
213+
a_final, omega_final = res.x[0], res.x[1]
214+
215+
# Add labels to the intersection points
216+
if label:
217+
pos = H(1j * omega_final)
218+
plt.text(pos.real, pos.imag, label % (a_final, omega_final))
219+
220+
# Save the final estimate
221+
intersections.append((a_final, omega_final))
222+
223+
return intersections
224+
225+
# Figure out whether two line segments intersection
226+
def _find_intersection(L1a, L1b, L2a, L2b):
227+
# Compute the tangents for the segments
228+
L1t = L1b - L1a
229+
L2t = L2b - L2a
230+
231+
# Set up components of the solution: b = M s
232+
b = L1a - L2a
233+
detM = L1t.imag * L2t.real - L1t.real * L2t.imag
234+
if abs(detM) < 1e-8: # TODO: fix magic number
235+
return None
236+
237+
# Solve for the intersection points on each line segment
238+
s1 = (L2t.imag * b.real - L2t.real * b.imag) / detM
239+
if s1 < 0 or s1 > 1:
240+
return None
241+
242+
s2 = (L1t.imag * b.real - L1t.real * b.imag) / detM
243+
if s2 < 0 or s2 > 1:
244+
return None
245+
246+
# Debugging test
247+
np.testing.assert_almost_equal(L1a + s1 * L1t, L2a + s2 * L2t)
248+
249+
# Intersection is within segments; return proportional distance
250+
return (s1, s2)
154251

155252

156253
# Class for nonlinear functions
@@ -195,49 +292,38 @@ def describing_function(self, A):
195292
return (math.sin(alpha + beta) * math.cos(alpha - beta) +
196293
(alpha + beta)) / math.pi
197294

198-
199-
# Hysteresis w/ deadzone (#40 in Gelb and Vander Velde, 1968)
200-
class hysteresis_deadzone_nonlinearity(NonlinearFunction):
201-
def __init__(self, delta, D, m):
295+
296+
# Relay with hysteresis (FBS2e, Example 10.12)
297+
class relay_hysteresis_nonlinearity(NonlinearFunction):
298+
def __init__(self, b, c):
202299
# Initialize the state to bottom branch
203300
self.branch = -1 # lower branch
204-
self.delta = delta
205-
self.D = D
206-
self.m = m
301+
self.b = b
302+
self.c = c
207303

208304
def __call__(self, x):
209-
if x > self.delta + self.D / self.m:
210-
y = self.m * (x - self.delta)
305+
if x > self.c:
306+
y = self.b
211307
self.branch = 1
212-
elif x < -self.delta - self.D/self.m:
213-
y = self.m * (x + self.delta)
308+
elif x < -self.c:
309+
y = -self.b
214310
self.branch = -1
215-
elif self.branch == -1 and \
216-
x > -self.delta - self.D / self.m and \
217-
x < self.delta - self.D / self.m:
218-
y = -self.D
219-
elif self.branch == -1 and x >= self.delta - self.D / self.m:
220-
y = self.m * (x - self.delta)
221-
elif self.branch == 1 and \
222-
x > -self.delta + self.D / self.m and \
223-
x < self.delta + self.D / self.m:
224-
y = self.D
225-
elif self.branch == 1 and x <= -self.delta + self.D / self.m:
226-
y = self.m * (x + self.delta)
311+
elif self.branch == -1:
312+
y = -self.b
313+
elif self.branch == 1:
314+
y = self.b
227315
return y
228316

229-
def describing_function(self, A):
317+
def describing_function(self, a):
230318
def f(x):
231319
return math.copysign(1, x) if abs(x) > 1 else \
232320
(math.asin(x) + x * math.sqrt(1 - x**2)) * 2 / math.pi
233321

234-
if A < self.delta + self.D/self.m:
322+
if a < self.c:
235323
return np.nan
236-
237-
df_real = self.m/2 * \
238-
(2 - self._f((self.D/self.m + self.delta)/A) +
239-
self._f((self.D/self.m - self.delta)/A))
240-
df_imag = -4 * self.D * self.delta / (math.pi * A**2)
324+
325+
df_real = 4 * self.b * math.sqrt(1 - (self.c/a)**2) / (a * math.pi)
326+
df_imag = -4 * self.b * self.c / (math.pi * a**2)
241327
return df_real + 1j * df_imag
242328

243329

@@ -248,17 +334,23 @@ def __init__(self, b):
248334
self.center = 0 # current center position
249335

250336
def __call__(self, x):
251-
# If we are outside the backlash, move and shift the center
252-
if x - self.center > self.b/2:
253-
self.center = x - self.b/2
254-
elif x - self.center < -self.b/2:
255-
self.center = x + self.b/2
256-
return self.center
337+
# Convert input to an array
338+
x_array = np.array(x)
339+
340+
y = []
341+
for x in np.atleast_1d(x_array):
342+
# If we are outside the backlash, move and shift the center
343+
if x - self.center > self.b/2:
344+
self.center = x - self.b/2
345+
elif x - self.center < -self.b/2:
346+
self.center = x + self.b/2
347+
y.append(self.center)
348+
return(np.array(y).reshape(x_array.shape))
257349

258350
def describing_function(self, A):
259-
if A < self.b/2:
351+
if A <= self.b/2:
260352
return 0
261-
353+
262354
df_real = (1 + self._f(1 - self.b/A)) / 2
263355
df_imag = -(2 * self.b/A - (self.b/A)**2) / math.pi
264356
return df_real + 1j * df_imag

‎control/tests/nltools_test.py

Lines changed: 40 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -88,19 +88,56 @@ def test_saturation_describing_function(satsys):
8888
np.testing.assert_almost_equal(df_arr, df_anal, decimal=3)
8989

9090
from control.nltools import saturation_nonlinearity, backlash_nonlinearity, \
91-
hysteresis_deadzone_nonlinearity
91+
relay_hysteresis_nonlinearity
9292

9393

9494
@pytest.mark.parametrize("fcn, amin, amax", [
9595
[saturation_nonlinearity(1), 0, 10],
9696
[backlash_nonlinearity(2), 1, 10],
97-
[hysteresis_deadzone_nonlinearity(1, 1, 1), 3, 10],
97+
[relay_hysteresis_nonlinearity(1, 1), 3, 10],
9898
])
9999
def test_describing_function(fcn, amin, amax):
100100
# Store the analytic describing function for comparison
101101
amprange = np.linspace(amin, amax, 100)
102102
df_anal = [fcn.describing_function(a) for a in amprange]
103103

104104
# Compute describing function on an array of values
105-
df_arr = ct.describing_function(fcn, amprange, zero_check=False)
105+
df_arr = ct.describing_function(
106+
fcn, amprange, zero_check=False, try_method=False)
106107
np.testing.assert_almost_equal(df_arr, df_anal, decimal=1)
108+
109+
# Make sure the describing function method also works
110+
df_meth = ct.describing_function(fcn, amprange, zero_check=False)
111+
np.testing.assert_almost_equal(df_meth, df_anal, decimal=1)
112+
113+
def test_describing_function_plot():
114+
# Simple linear system with at most 1 intersection
115+
H_simple = ct.tf([1], [1, 2, 2, 1])
116+
omega = np.logspace(-1, 2, 100)
117+
118+
# Saturation nonlinearity
119+
F_saturation = ct.nltools.saturation_nonlinearity(1)
120+
amp = np.linspace(1, 4, 10)
121+
122+
# No intersection
123+
xsects = ct.describing_function_plot(H_simple, F_saturation, amp, omega)
124+
assert xsects == []
125+
126+
# One intersection
127+
H_larger = H_simple * 8
128+
xsects = ct.describing_function_plot(H_larger, F_saturation, amp, omega)
129+
for a, w in xsects:
130+
np.testing.assert_almost_equal(
131+
H_larger(1j*w),
132+
-1/ct.describing_function(F_saturation, a), decimal=5)
133+
134+
# Multiple intersections
135+
H_multiple = H_simple * ct.tf(*ct.pade(5, 4)) * 4
136+
omega = np.logspace(-1, 3, 50)
137+
F_backlash = ct.nltools.backlash_nonlinearity(1)
138+
amp = np.linspace(0.6, 5, 50)
139+
xsects = ct.describing_function_plot(H_multiple, F_backlash, amp, omega)
140+
for a, w in xsects:
141+
np.testing.assert_almost_equal(
142+
-1/ct.describing_function(F_backlash, a),
143+
H_multiple(1j*w), decimal=5)

0 commit comments

Comments
 (0)
0