diff --git a/control/statesp.py b/control/statesp.py index 522d187a9..296aa709a 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -930,6 +930,66 @@ def dcgain(self): gain = np.tile(np.nan, (self.outputs, self.inputs)) return np.squeeze(gain) + def clean(self, precision=10): + """ + Rounds the A, B, C and D matrices of a StateSpace system, removing + small non-zero numbers that are, for instance, a result of computational + errors and that can otherwise lead to problems in future computations + and analyses + + Parameters + ---------- + sys : LTI (StateSpace) + LTI system whose data will be returned + precision : integer + Number of decimals to be left unchanged in rounding + + Returns + ------- + out : StateSpace + LTI StateSpace rounded to the desired precision + """ + return ss(np.round(self.A, precision), np.round(self.B, precision), np.round(self.C, precision), + np.round(self.D, precision)) + + def fival(self, forcing="step", input=1, output=1, stabilityCheck=False, precision=10): + """ + Returns the final value of the StateSpace system. It is based on + the mathematical final value theorem and is thus faster and avoids + unnecessarily large time vectors. For MIMO systems one can prescribe + which input and output are desired. + + Parameters + ---------- + sys : LTI (StateSpace, or TransferFunction) + LTI system whose data will be returned + forcing: A SISO TransferFunction + Laplace transform of the forcingunction describing the input + input : integer + Refers to the requested input (starting from 1), 1 by default + output : integer + Refers to the requested output (starting from 1), 1 by default + stabilityCheck : bool + If True it is checked if the system is stable. The final value theorem gives + erroneous results for unstable systems + precision : integer + Number of decimals to be left unchanged in rounding + + Returns + ------- + out : float, inf or None + The final value of the system. inf has the correct sign. None is returned + if stabilityCheck=True and the system is unstable + + Raises + ------ + TypeError + if forcing is not a TransferFunction object + """ + from .timeresp import fival + return fival(self, forcing = forcing, input = input, output = output,\ + stabilityCheck = stabilityCheck, precision = precision) + # TODO: add discrete time check def _convertToStateSpace(sys, **kw): @@ -1503,3 +1563,36 @@ def ssdata(sys): """ ss = _convertToStateSpace(sys) return ss.A, ss.B, ss.C, ss.D + + +def clean_ss(sys, precision=10): + """ + Rounds the A, B, C and D matrices of a StateSpace system, removing small + non-zero numbers that are, for instance, a result of computational errors + and that can otherwise lead to problems in future computations and analyses. + If sys is a TransferFunction it is first converted to a StateSpace + + Parameters + ---------- + sys : LTI (StateSpace, or TransferFunction) + LTI system whose data will be returned + precision : integer + Number of decimals to be left unchanged in rounding + + Returns + ------- + out : StateSpace + LTI StateSpace rounded to the desired precision + + Raises + ------ + TypeError + if sys is not a StateSpace or TransferFunction object + """ + from .xferfcn import TransferFunction + if (type(sys) is not TransferFunction) and (type(sys) is not StateSpace): + raise TypeError("Neither a StateSpace nor TransferFunction was passed to 'sys'") + if (type(sys) is not StateSpace): + sys = ss(sys) + return ss(np.round(sys.A, precision), np.round(sys.B, precision), np.round(sys.C, precision), + np.round(sys.D, precision)) \ No newline at end of file diff --git a/control/timeresp.py b/control/timeresp.py index 8670c180d..c16a8df24 100644 --- a/control/timeresp.py +++ b/control/timeresp.py @@ -74,7 +74,7 @@ import warnings from .lti import LTI # base class of StateSpace, TransferFunction from .statesp import _convertToStateSpace, _mimo2simo, _mimo2siso, ssdata -from .lti import isdtime, isctime +from .lti import isdtime, isctime, pole __all__ = ['forced_response', 'step_response', 'step_info', 'initial_response', 'impulse_response'] @@ -882,4 +882,61 @@ def _default_time_vector(sys, N=None, tfinal=None): if N is None: N = int(np.clip(tfinal/ideal_dt, N_min_ct, N_max)) # N<-[N_min, N_max] - return np.linspace(0, tfinal, N, endpoint=False) \ No newline at end of file + return np.linspace(0, tfinal, N, endpoint=False) + + +def fival(sys, forcing="step", input=1, output=1, stabilityCheck=False, precision=10): + """ + Returns the final value of a state space or transfer function. + It is based on the mathematical final value theorem and is thus + faster and avoids unnecessarily large time vectors. For MIMO + systems one can prescribe which input and output are desired. + + Parameters + ---------- + sys : LTI (StateSpace, or TransferFunction) + LTI system whose data will be returned + forcing: A SISO TransferFunction + Laplace transform of the forcingunction describing the input + input : integer + Refers to the requested input (starting from 1), 1 by default + output : integer + Refers to the requested output (starting from 1), 1 by default + stabilityCheck : bool + If True it is checked if the system is stable. The final value theorem gives + erroneous results for unstable systems + precision : integer + Number of decimals to be left unchanged in rounding + + Returns + ------- + out : float, inf or None + The final value of the system. inf has the correct sign. None is returned + if stabilityCheck=True and the system is unstable + + Raises + ------ + TypeError + if sys is not a StateSpace or TransferFunction object + TypeError + if forcing is not a TransferFunction object + """ + from .statesp import StateSpace + from .xferfcn import TransferFunction, tf, clean_tf + if (type(sys) is not TransferFunction) and (type(sys) is not StateSpace): + raise TypeError("Neither a StateSpace nor TransferFunction was passed to 'sys'") + if forcing != "step" and (type(forcing) is not TransferFunction): + raise TypeError("The input passed to 'forcing' is not a TransferFunction") + + cleaned_sys = clean_tf(sys, input=input, output=output, precision=precision) + if stabilityCheck: + p = np.real(pole(cleaned_sys)) + if len(p[p > 0]) != 0: + return print("Unstable system due to ", len(p[p > 0]), " pole(s) in the right half of the complex plane") + if len(p[p == 0]) > 1: + return print("Unstable system due to ", len(p[p == 0]), " poles at the origin") + if forcing != "step": + cleaned_sys = cleaned_sys * forcing * tf([1, 0], [1]) + if cleaned_sys.den[0][0][-1] == 0: + return np.inf * np.sign(cleaned_sys.num[0][0][-1]) + return cleaned_sys.num[0][0][-1] / cleaned_sys.den[0][0][-1] \ No newline at end of file diff --git a/control/xferfcn.py b/control/xferfcn.py index f50d5141d..7490bf63b 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -1091,6 +1091,88 @@ def _dcgain_cont(self): gain[i][j] = np.nan return np.squeeze(gain) + def clean(self, input=None, output=None, precision=10): + """ + Rounds the elements of the transfer function. This removes + small non-zero numbers that, for instance, are a result of + computational errors and can otherwise lead to problems. The + function also removes cancelling poles and zeros, similar to + the control.minreal() function + + Parameters + ---------- + sys : LTI (StateSpace, or TransferFunction) + LTI system whose data will be returned + input : None + optionally an integer referring to the requested input (starting from 1) + output : None + optionally an integer referring to the requested output (starting from 1) + precision : integer + Number of decimals to be left unchanged in rounding + + Returns + ------- + out : TransferFunctino + LTI TransferFunction rounded to the desired precision + """ + sys = self.minreal() + if input is not None and output is not None: + sys = sys[output - 1, input - 1] + elif output is not None: + sys = sys[output - 1, :] + elif input is not None: + sys = sys[:, input - 1] + num = np.round(sys.num, precision) + den = np.round(sys.den, precision) + for i in range(sys.outputs): + for j in range(sys.inputs): + test = True + while test: + if num[i][j][-1] == den[i][j][-1] == 0: + num[i][j] = np.insert(num[i][j][:-1], 0, 0) + den[i][j] = np.insert(den[i][j][:-1], 0, 0) + else: + test = False + return tf(num, den) + + def fival(self, forcing="step", input=1, output=1, stabilityCheck=False, precision=10): + """ + Returns the final value of the TransferFunction. It is + based on the mathematical final value theorem and is thus + faster and avoids unnecessarily large time vectors. For MIMO + systems one can prescribe which input and output are desired. + + Parameters + ---------- + sys : LTI (StateSpace, or TransferFunction) + LTI system whose data will be returned + forcing: A SISO TransferFunction + Laplace transform of the forcingunction describing the input + input : integer + Refers to the requested input (starting from 1), 1 by default + output : integer + Refers to the requested output (starting from 1), 1 by default + stabilityCheck : bool + If True it is checked if the system is stable. The final value theorem gives + erroneous results for unstable systems + precision : integer + Number of decimals to be left unchanged in rounding + + Returns + ------- + out : float, inf or None + The final value of the system. inf has the correct sign. None is returned + if stabilityCheck=True and the system is unstable + + Raises + ------ + TypeError + if forcing is not a TransferFunction object + """ + from .timeresp import fival + return fival(self, forcing = forcing, input = input, output = output,\ + stabilityCheck = stabilityCheck, precision = precision) + # c2d function contributed by Benjamin White, Oct 2012 def _c2d_matched(sysC, Ts): @@ -1565,6 +1647,43 @@ def _clean_part(data): return data +def clean_tf(sys, input=None, output=None, precision=10): + """ + Rounds the elements of the desired transfer function (which + is created if sys is a StateSpace). This removes small non-zero + numbers that, for instance, are a result of computational errors + and can otherwise lead to problems. The function also removes + cancelling poles and zeros, similar to the control.minreal() function + + Parameters + ---------- + sys : LTI (StateSpace, or TransferFunction) + LTI system whose data will be returned + input : None + optionally an integer referring to the requested input (starting from 1) + output : None + optionally an integer referring to the requested output (starting from 1) + precision : integer + Number of decimals to be left unchanged in rounding + + Returns + ------- + out : TransferFunctino + LTI TransferFunction rounded to the desired precision + + Raises + ------ + TypeError + if sys is not a StateSpace or TransferFunction object + """ + from .statesp import StateSpace + if (type(sys) is not TransferFunction) and (type(sys) is not StateSpace): + raise TypeError("Neither a StateSpace nor TransferFunction was passed to 'sys'") + if (type(sys) is not TransferFunction): + sys = tf(sys).minreal() + return sys.clean(input = input, output = output, precision = precision) + + # Define constants to represent differentiation, unit delay TransferFunction.s = TransferFunction([1, 0], [1], 0) TransferFunction.z = TransferFunction([1, 0], [1], True) diff --git a/tester.py b/tester.py new file mode 100644 index 000000000..8fd6441b7 --- /dev/null +++ b/tester.py @@ -0,0 +1,37 @@ +import control as c +from control.xferfcn import clean_tf +from control.statesp import clean_ss +from control.timeresp import fival +import numpy as np + +ci = 2 / np.sqrt(13) +w = np.sqrt(13) +Kq = -24 +T02 = 1.4 +V = 160 +s = c.tf([1, 0], [1]) +Hq = Kq * (1 + T02 * s) / (s ** 2 + 2 * ci * w * s + w ** 2) +Htheta = Hq / s +Hgamma = Kq / s / (s ** 2 + 2 * ci * w * s + w ** 2) +Hh = Hgamma * V / s +H = c.tf([[Hq.num[0][0], Htheta.num[0][0]], [Hgamma.num[0][0], Hh.num[0][0]]], + [[Hq.den[0][0], Htheta.den[0][0]], [Hgamma.den[0][0], Hh.den[0][0]]]) +sys1 = c.ss(H) +sys1.D = np.array([[1, 2], [3e-13, 4]]) # Changes it to a non-zero input matrix D + +H = c.tf(sys1) # Gives a tf with nice unrounded residual components + +H2 = c.tf([1, -3, 0, 0], [1, 1e-13, 7, 0, 0, 0]) # to test minreal things + +print(H.clean()) +#print(sys1) +#print(H.clean(input = 1, output = 2, precision = 9)) +#print(sys1.clean(precision = 12)) +#print(clean_tf(sys1, input = 1, output = 2, precision = 1)) +#print(clean_tf(H, input = 2, output = 2, precision = 8)) +#print(clean_ss(sys1, precision = 2)) +#print(clean_ss(H, precision = 12)) +print(H.fival(forcing="step", input=1, output=1, stabilityCheck=True)) +print(sys1.fival(forcing="step", input=2, stabilityCheck=True, precision=17)) +print(fival(H, forcing="step", output=2, stabilityCheck=True, precision=3)) +print(fival(sys1, forcing="step", input=2, output=2, precision=10)) \ No newline at end of file