Skip to content

Update find_eqpts to handle discrete time systems #824

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 2 commits into from
Dec 28, 2022
Merged

Update find_eqpts to handle discrete time systems #824

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5d0b3d0
add code + tests for discrete time find_eqpt
murrayrm Dec 24, 2022
0bde571
add documentation for continuous vs discrete find_eqpt computation
murrayrm Dec 28, 2022
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
46 changes: 29 additions & 17 deletions control/iosys.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1994,6 +1994,14 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None,
If `return_result` is True, returns the `result` from the
:func:`scipy.optimize.root` function.

Notes
-----
For continuous time systems, equilibrium points are defined as points for
which the right hand side of the differential equation is zero:
:math:`f(t, x_e, u_e) = 0`. For discrete time systems, equilibrium points
are defined as points for which the right hand side of the difference
equation returns the current state: :math:`f(t, x_e, u_e) = x_e`.

"""
from scipy.optimize import root

Expand All @@ -2010,11 +2018,6 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None,
if np.isscalar(y0):
y0 = np.ones((ninputs,)) * y0

# Discrete-time not yet supported
if isdtime(sys, strict=True):
raise NotImplementedError(
"Discrete time systems are not yet supported.")

# Make sure the input arguments match the sizes of the system
if len(x0) != nstates or \
(u0 is not None and len(u0) != ninputs) or \
Expand All @@ -2030,18 +2033,28 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None,
# Special cases: either inputs or outputs are constrained
if y0 is None:
# Take u0 as fixed and minimize over x
# TODO: update to allow discrete time systems
def ode_rhs(z): return sys._rhs(t, z, u0)
result = root(ode_rhs, x0)
if sys.isdtime(strict=True):
def state_rhs(z): return sys._rhs(t, z, u0) - z
else:
def state_rhs(z): return sys._rhs(t, z, u0)

result = root(state_rhs, x0)
z = (result.x, u0, sys._out(t, result.x, u0))

else:
# Take y0 as fixed and minimize over x and u
def rootfun(z):
# Split z into x and u
x, u = np.split(z, [nstates])
# TODO: update to allow discrete time systems
return np.concatenate(
(sys._rhs(t, x, u), sys._out(t, x, u) - y0), axis=0)
if sys.isdtime(strict=True):
def rootfun(z):
x, u = np.split(z, [nstates])
return np.concatenate(
(sys._rhs(t, x, u) - x, sys._out(t, x, u) - y0),
axis=0)
else:
def rootfun(z):
x, u = np.split(z, [nstates])
return np.concatenate(
(sys._rhs(t, x, u), sys._out(t, x, u) - y0), axis=0)

z0 = np.concatenate((x0, u0), axis=0) # Put variables together
result = root(rootfun, z0) # Find the eq point
x, u = np.split(result.x, [nstates]) # Split result back in two
Expand Down Expand Up @@ -2135,7 +2148,6 @@ def rootfun(z):

# Keep track of the number of states in the set of free variables
nstate_vars = len(state_vars)
dtime = isdtime(sys, strict=True)

def rootfun(z):
# Map the vector of values into the states and inputs
Expand All @@ -2144,8 +2156,8 @@ def rootfun(z):

# Compute the update and output maps
dx = sys._rhs(t, x, u) - dx0
if dtime:
dx -= x # TODO: check
if sys.isdtime(strict=True):
dx -= x

# If no y0 is given, don't evaluate the output function
if y0 is None:
Expand Down
87 changes: 79 additions & 8 deletions control/tests/iosys_test.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,10 @@

import re
import warnings
import pytest

import numpy as np
import pytest
from math import sqrt

import control as ct
from control import iosys as ios
Expand Down Expand Up @@ -238,7 +239,7 @@ def test_linearize_named_signals(self, kincar):
assert lin_nocopy.find_state('x') is None

# if signal names are provided, they should override those of kincar
linearized_newnames = kincar.linearize([0, 0, 0], [0, 0],
linearized_newnames = kincar.linearize([0, 0, 0], [0, 0],
name='linearized',
copy_names=True, inputs=['v2', 'phi2'], outputs=['x2','y2'])
assert linearized_newnames.name == 'linearized'
Expand Down Expand Up @@ -766,8 +767,8 @@ def nlsys_output(t, x, u, params):
np.testing.assert_allclose(ios_t, lin_t,atol=0.002,rtol=0.)
np.testing.assert_allclose(ios_y, lin_y,atol=0.002,rtol=0.)

def test_find_eqpts(self, tsys):
"""Test find_eqpt function"""
def test_find_eqpts_dfan(self, tsys):
"""Test find_eqpt function on dfan example"""
# Simple equilibrium point with no inputs
nlsys = ios.NonlinearIOSystem(predprey)
xeq, ueq, result = ios.find_eqpt(
Expand Down Expand Up @@ -836,7 +837,7 @@ def test_find_eqpts(self, tsys):
np.testing.assert_array_almost_equal(
nlsys_full._rhs(0, xeq, ueq)[-4:], np.zeros((4,)), decimal=5)

# The same test as previous, but now all constraints are in the state vector
# Same test as before, but now all constraints are in the state vector
nlsys_full = ios.NonlinearIOSystem(pvtol_full, None)
xeq, ueq, result = ios.find_eqpt(
nlsys_full, [0, 0, 0.1, 0.1, 0, 0], [0.01, 4*9.8],
Expand Down Expand Up @@ -1482,7 +1483,7 @@ def test_linear_interconnection():
tf_siso = ct.tf(1, [0.1, 1])
ss_siso = ct.ss(1, 2, 1, 1)
nl_siso = ios.NonlinearIOSystem(
lambda t, x, u, params: x*x,
lambda t, x, u, params: x*x,
lambda t, x, u, params: u*x, states=1, inputs=1, outputs=1)

# Create a "regular" InterconnectedSystem
Expand Down Expand Up @@ -1530,7 +1531,7 @@ def test_linear_interconnection():
np.testing.assert_array_almost_equal(io_connect.C, ss_connect.C)
np.testing.assert_array_almost_equal(io_connect.D, ss_connect.D)

# make sure interconnections of linear systems are linear and
# make sure interconnections of linear systems are linear and
# if a nonlinear system is included then system is nonlinear
assert isinstance(ss_siso*ss_siso, ios.LinearIOSystem)
assert isinstance(tf_siso*ss_siso, ios.LinearIOSystem)
Expand All @@ -1541,7 +1542,7 @@ def test_linear_interconnection():
assert ~isinstance(tf_siso*nl_siso, ios.LinearIOSystem)
assert ~isinstance(nl_siso*tf_siso, ios.LinearIOSystem)
assert ~isinstance(nl_siso*nl_siso, ios.LinearIOSystem)


def predprey(t, x, u, params={}):
"""Predator prey dynamics"""
Expand Down Expand Up @@ -1898,3 +1899,73 @@ def test_rss():
with pytest.warns(UserWarning, match="may be interpreted as continuous"):
sys = ct.drss(2, 1, 1, dt=None)
assert np.all(np.abs(sys.poles()) < 1)


def eqpt_rhs(t, x, u, params):
return np.array([x[0]/2 + u[0], x[0] - x[1]**2 + u[1], x[1] - x[2]])

def eqpt_out(t, x, u, params):
return np.array([x[0], x[1] + u[1]])

@pytest.mark.parametrize(
"x0, ix, u0, iu, y0, iy, dx0, idx, dt, x_expect, u_expect", [
# Equilibrium points with input given
(0, None, 0, None, None, None, None, None, 0, [0, 0, 0], [0, 0]),
(0, None, 0, None, None, None, None, None, None, [0, 0, 0], [0, 0]),
([0.9, 0.9, 0.9], None, [-1, 0], None, None, None, None, None, 0,
[2, sqrt(2), sqrt(2)], [-1, 0]),
([0.9, -0.9, 0.9], None, [-1, 0], None, None, None, None, None, 0,
[2, -sqrt(2), -sqrt(2)], [-1, 0]), # same input, different eqpt
(0, None, 0, None, None, None, None, None, 1, [0, 0, 0], [0, 0]), #DT
(0, None, [-1, 0], None, None, None, None, None, 1, None, None), #DT
([0, -0.1, 0], None, [0, -0.25], None, None, None, None, None, 1, #DT
[0, -0.5, -0.25], [0, -0.25]),

# Equilibrium points with output given
([0.9, 0.9, 0.9], None, [-0.9, 0], None, [2, sqrt(2)], None, None,
None, 0, [2, sqrt(2), sqrt(2)], [-1, 0]),
(0, None, [0, -0.25], None, [0, -0.75], None, None, None, 1, #DT
[0, -0.5, -0.25], [0, -0.25]),

# Equilibrium points with mixture of inputs and outputs given
([0.9, 0.9, 0.9], None, [-1, 0], [0], [2, sqrt(2)], [1], None,
None, 0, [2, sqrt(2), sqrt(2)], [-1, 0]),
(0, None, [0, -0.22], [0], [0, -0.75], [1], None, None, 1, #DT
[0, -0.5, -0.25], [0, -0.25]),
])

def test_find_eqpt(x0, ix, u0, iu, y0, iy, dx0, idx, dt, x_expect, u_expect):
sys = ct.NonlinearIOSystem(
eqpt_rhs, eqpt_out, dt=dt, states=3, inputs=2, outputs=2)

xeq, ueq = ct.find_eqpt(
sys, x0, u0, y0, ix=ix, iu=iu, iy=iy, dx0=dx0, idx=idx)

# If no equilibrium points, skip remaining tests
if x_expect is None:
assert xeq is None
assert ueq is None
return

# Make sure we are at an appropriate equilibrium point
if dt is None or dt == 0:
# Continuous time system
np.testing.assert_allclose(eqpt_rhs(0, xeq, ueq, {}), 0, atol=1e-6)
if y0 is not None:
y0 = np.array(y0)
iy = np.s_[:] if iy is None else np.array(iy)
np.testing.assert_allclose(
eqpt_out(0, xeq, ueq, {})[iy], y0[iy], atol=1e-6)

else:
# Discrete time system
np.testing.assert_allclose(eqpt_rhs(0, xeq, ueq, {}), xeq, atol=1e-6)
if y0 is not None:
y0 = np.array(y0)
iy = np.s_[:] if iy is None else np.array(iy)
np.testing.assert_allclose(
eqpt_out(0, xeq, ueq, {})[iy], y0[iy], atol=1e-6)

# Check that we got the expected result as well
np.testing.assert_allclose(np.array(xeq), x_expect, atol=1e-6)
np.testing.assert_allclose(np.array(ueq), u_expect, atol=1e-6)