diff --git a/ChangeLog b/ChangeLog index fc171331f..d6c51ab20 100644 --- a/ChangeLog +++ b/ChangeLog @@ -148,7 +148,7 @@ 2012-11-03 Richard Murray * src/rlocus.py (_RLSortRoots): convert output of range() to - explicit list for python 3 compatability + explicit list for python 3 compatibility * tests/modelsimp_test.py, tests/slycot_convert_test.py, tests/mateqn_test.py, tests/statefbk_test.py: updated test suites to @@ -604,7 +604,7 @@ initial_response, impulse_response and step_response. * src/rlocus.py: changed RootLocus to root_locus for better - compatability with PEP 8. Also updated unit tests and examples. + compatibility with PEP 8. Also updated unit tests and examples. 2011-07-25 Richard Murray @@ -994,7 +994,7 @@ 2010-09-02 Richard Murray * src/statefbk.py (place): Use np.size() instead of len() for - finding length of placed_eigs for better compatability with + finding length of placed_eigs for better compatibility with different python versions [courtesy of Roberto Bucher] * src/delay.py (pade): New file for delay-based computations + diff --git a/control/canonical.py b/control/canonical.py index d148ca411..5eacce372 100644 --- a/control/canonical.py +++ b/control/canonical.py @@ -7,7 +7,7 @@ from .statefbk import ctrb, obsv from numpy import zeros, shape, poly -from numpy.linalg import inv +from numpy.linalg import solve, matrix_rank __all__ = ['canonical_form', 'reachable_form', 'observable_form'] @@ -68,23 +68,29 @@ def reachable_form(xsys): # Generate the system matrices for the desired canonical form zsys.B = zeros(shape(xsys.B)) - zsys.B[0, 0] = 1 + zsys.B[0, 0] = 1.0 zsys.A = zeros(shape(xsys.A)) Apoly = poly(xsys.A) # characteristic polynomial for i in range(0, xsys.states): zsys.A[0, i] = -Apoly[i+1] / Apoly[0] if (i+1 < xsys.states): - zsys.A[i+1, i] = 1 + zsys.A[i+1, i] = 1.0 # Compute the reachability matrices for each set of states Wrx = ctrb(xsys.A, xsys.B) Wrz = ctrb(zsys.A, zsys.B) + if matrix_rank(Wrx) != xsys.states: + raise ValueError("System not controllable to working precision.") + # Transformation from one form to another - Tzx = Wrz * inv(Wrx) + Tzx = solve(Wrx.T, Wrz.T).T # matrix right division, Tzx = Wrz * inv(Wrx) + + if matrix_rank(Tzx) != xsys.states: + raise ValueError("Transformation matrix singular to working precision.") # Finally, compute the output matrix - zsys.C = xsys.C * inv(Tzx) + zsys.C = solve(Tzx.T, xsys.C.T).T # matrix right division, zsys.C = xsys.C * inv(Tzx) return zsys, Tzx diff --git a/control/margins.py b/control/margins.py index 5fb5a8b40..5bb7f09ee 100644 --- a/control/margins.py +++ b/control/margins.py @@ -8,7 +8,7 @@ margin.phase_crossover_frequencies """ -# Python 3 compatability (needs to go here) +# Python 3 compatibility (needs to go here) from __future__ import print_function """Copyright (c) 2011 by California Institute of Technology diff --git a/control/mateqn.py b/control/mateqn.py index d463769c7..e5f7ba4f6 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -5,7 +5,7 @@ Implementation of the functions lyap, dlyap, care and dare for solution of Lyapunov and Riccati equations. """ -# Python 3 compatability (needs to go here) +# Python 3 compatibility (needs to go here) from __future__ import print_function """Copyright (c) 2011, All rights reserved. @@ -41,9 +41,8 @@ Author: Bjorn Olofsson """ -from numpy.linalg import inv from scipy import shape, size, asarray, asmatrix, copy, zeros, eye, dot -from scipy.linalg import eigvals, solve_discrete_are +from scipy.linalg import eigvals, solve_discrete_are, solve from .exception import ControlSlycot, ControlArgument __all__ = ['lyap', 'dlyap', 'dare', 'care'] @@ -557,9 +556,9 @@ def care(A,B,Q,R=None,S=None,E=None): # Calculate the gain matrix G if size(R_b) == 1: - G = dot(dot(1/(R_ba),asarray(B_ba).T) , X) + G = dot(dot(1/(R_ba), asarray(B_ba).T), X) else: - G = dot(dot(inv(R_ba),asarray(B_ba).T) , X) + G = dot(solve(R_ba, asarray(B_ba).T), X) # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G @@ -660,9 +659,9 @@ def care(A,B,Q,R=None,S=None,E=None): # Calculate the gain matrix G if size(R_b) == 1: - G = dot(1/(R_b),dot(asarray(B_b).T,dot(X,E_b))+asarray(S_b).T) + G = dot(1/(R_b), dot(asarray(B_b).T, dot(X,E_b)) + asarray(S_b).T) else: - G = dot(inv(R_b),dot(asarray(B_b).T,dot(X,E_b))+asarray(S_b).T) + G = solve(R_b, dot(asarray(B_b).T, dot(X, E_b)) + asarray(S_b).T) # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G @@ -699,7 +698,7 @@ def dare(A,B,Q,R,S=None,E=None): Rmat = asmatrix(R) Qmat = asmatrix(Q) X = solve_discrete_are(A, B, Qmat, Rmat) - G = inv(B.T.dot(X).dot(B) + Rmat) * B.T.dot(X).dot(A) + G = solve(B.T.dot(X).dot(B) + Rmat, B.T.dot(X).dot(A)) L = eigvals(A - B.dot(G)) return X, L, G @@ -825,11 +824,11 @@ def dare_old(A,B,Q,R,S=None,E=None): # Calculate the gain matrix G if size(R_b) == 1: - G = dot( 1/(dot(asarray(B_ba).T,dot(X,B_ba))+R_ba) , \ - dot(asarray(B_ba).T,dot(X,A_ba)) ) + G = dot(1/(dot(asarray(B_ba).T, dot(X, B_ba)) + R_ba), \ + dot(asarray(B_ba).T, dot(X, A_ba))) else: - G = dot( inv(dot(asarray(B_ba).T,dot(X,B_ba))+R_ba) , \ - dot(asarray(B_ba).T,dot(X,A_ba)) ) + G = solve(dot(asarray(B_ba).T, dot(X, B_ba)) + R_ba, \ + dot(asarray(B_ba).T, dot(X, A_ba))) # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G @@ -930,11 +929,11 @@ def dare_old(A,B,Q,R,S=None,E=None): # Calculate the gain matrix G if size(R_b) == 1: - G = dot( 1/(dot(asarray(B_b).T,dot(X,B_b))+R_b) , \ - dot(asarray(B_b).T,dot(X,A_b)) + asarray(S_b).T) + G = dot(1/(dot(asarray(B_b).T, dot(X,B_b)) + R_b), \ + dot(asarray(B_b).T, dot(X,A_b)) + asarray(S_b).T) else: - G = dot( inv(dot(asarray(B_b).T,dot(X,B_b))+R_b) , \ - dot(asarray(B_b).T,dot(X,A_b)) + asarray(S_b).T) + G = solve(dot(asarray(B_b).T, dot(X,B_b)) + R_b, \ + dot(asarray(B_b).T, dot(X,A_b)) + asarray(S_b).T) # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G diff --git a/control/modelsimp.py b/control/modelsimp.py index 1bdeb7e99..c58f5f287 100644 --- a/control/modelsimp.py +++ b/control/modelsimp.py @@ -40,7 +40,7 @@ # # $Id$ -# Python 3 compatability +# Python 3 compatibility from __future__ import print_function # External packages and modules @@ -149,16 +149,12 @@ def modred(sys, ELIM, method='matchdc'): #Check system is stable - D,V = np.linalg.eig(sys.A) - for e in D: - if e.real >= 0: - raise ValueError("Oops, the system is unstable!") + if np.any(np.linalg.eigvals(sys.A).real >= 0.0): + raise ValueError("Oops, the system is unstable!") + ELIM = np.sort(ELIM) - NELIM = [] # Create list of elements not to eliminate (NELIM) - for i in range(0,len(sys.A)): - if i not in ELIM: - NELIM.append(i) + NELIM = [i for i in range(len(sys.A)) if i not in ELIM] # A1 is a matrix of all columns of sys.A not to eliminate A1 = sys.A[:,NELIM[0]] for i in NELIM[1:]: @@ -177,14 +173,26 @@ def modred(sys, ELIM, method='matchdc'): B1 = sys.B[NELIM,:] B2 = sys.B[ELIM,:] - A22I = np.linalg.inv(A22) - if method=='matchdc': # if matchdc, residualize - Ar = A11 - A12*A22.I*A21 - Br = B1 - A12*A22.I*B2 - Cr = C1 - C2*A22.I*A21 - Dr = sys.D - C2*A22.I*B2 + + # Check if the matrix A22 is invertible + if np.linalg.matrix_rank(A22) != len(ELIM): + raise ValueError("Matrix A22 is singular to working precision.") + + # Now precompute A22\A21 and A22\B2 (A22I = inv(A22)) + # We can solve two linear systems in one pass, since the + # coefficients matrix A22 is the same. Thus, we perform the LU + # decomposition (cubic runtime complexity) of A22 only once! + # The remaining back substitutions are only quadratic in runtime. + A22I_A21_B2 = np.linalg.solve(A22, np.concatenate((A21, B2), axis=1)) + A22I_A21 = A22I_A21_B2[:, :A21.shape[1]] + A22I_B2 = A22I_A21_B2[:, A21.shape[1]:] + + Ar = A11 - A12*A22I_A21 + Br = B1 - A12*A22I_B2 + Cr = C1 - C2*A22I_A21 + Dr = sys.D - C2*A22I_B2 elif method=='truncate': # if truncate, simply discard state x2 Ar = A11 @@ -243,12 +251,8 @@ def balred(sys, orders, method='truncate'): dico = 'C' #Check system is stable - D,V = np.linalg.eig(sys.A) - # print D.shape - # print D - for e in D: - if e.real >= 0: - raise ValueError("Oops, the system is unstable!") + if np.any(np.linalg.eigvals(sys.A).real >= 0.0): + raise ValueError("Oops, the system is unstable!") if method=='matchdc': raise ValueError ("MatchDC not yet supported!") @@ -373,8 +377,6 @@ def markov(Y, U, M): UU = np.hstack((UU, Ulast)) # Invert and solve for Markov parameters - H = UU.I - H = np.dot(H, Y) + H = np.linalg.lstsq(UU, Y)[0] return H - diff --git a/control/phaseplot.py b/control/phaseplot.py index 2108d99e6..ffb3588bb 100644 --- a/control/phaseplot.py +++ b/control/phaseplot.py @@ -34,7 +34,7 @@ # IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -# Python 3 compatability +# Python 3 compatibility from __future__ import print_function import numpy as np diff --git a/control/statefbk.py b/control/statefbk.py index 26778960e..48584ce97 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -127,7 +127,7 @@ def acker(A, B, poles): # Make sure the system is controllable ct = ctrb(A, B) - if sp.linalg.det(ct) == 0: + if np.linalg.matrix_rank(ct) != a.shape[0]: raise ValueError("System not reachable; pole placement invalid") # Compute the desired characteristic polynomial @@ -138,7 +138,7 @@ def acker(A, B, poles): pmat = p[n-1]*a**0 for i in np.arange(1,n): pmat = pmat + p[n-i-1]*a**i - K = sp.linalg.inv(ct) * pmat + K = np.linalg.solve(ct, pmat) K = K[-1][:] # Extract the last row return K @@ -242,7 +242,8 @@ def lqr(*args, **keywords): X,rcond,w,S,U,A_inv = sb02md(nstates, A_b, G, Q_b, 'C') # Now compute the return value - K = np.dot(np.linalg.inv(R), (np.dot(B.T, X) + N.T)); + # We assume that R is positive definite and, hence, invertible + K = np.linalg.solve(R, np.dot(B.T, X) + N.T); S = X; E = w[0:nstates]; @@ -354,10 +355,9 @@ def gram(sys,type): #TODO: Check system is stable, perhaps a utility in ctrlutil.py # or a method of the StateSpace class? - D,V = np.linalg.eig(sys.A) - for e in D: - if e.real >= 0: - raise ValueError("Oops, the system is unstable!") + if np.any(np.linalg.eigvals(sys.A).real >= 0.0): + raise ValueError("Oops, the system is unstable!") + if type=='c': tra = 'T' C = -np.dot(sys.B,sys.B.transpose()) @@ -379,4 +379,3 @@ def gram(sys,type): X,scale,sep,ferr,w = sb03md(n, C, A, U, dico, job='X', fact='N', trana=tra) gram = X return gram - diff --git a/control/statesp.py b/control/statesp.py index 80da2a5f8..253245d37 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -8,7 +8,7 @@ """ -# Python 3 compatability (needs to go here) +# Python 3 compatibility (needs to go here) from __future__ import print_function """Copyright (c) 2010 by California Institute of Technology @@ -124,12 +124,7 @@ def __init__(self, *args): # Here we're going to convert inputs to matrices, if the user gave a # non-matrix type. - #! TODO: [A, B, C, D] = map(matrix, [A, B, C, D])? - matrices = [A, B, C, D] - for i in range(len(matrices)): - # Convert to matrix first, if necessary. - matrices[i] = matrix(matrices[i]) - [A, B, C, D] = matrices + A, B, C, D = map(matrix, [A, B, C, D]) LTI.__init__(self, B.shape[1], C.shape[0], dt) self.A = A @@ -453,11 +448,16 @@ def feedback(self, other=1, sign=-1): F = eye(self.inputs) - sign * D2 * D1 if matrix_rank(F) != self.inputs: - raise ValueError("I - sign * D2 * D1 is singular.") + raise ValueError("I - sign * D2 * D1 is singular to working precision.") # Precompute F\D2 and F\C2 (E = inv(F)) - E_D2 = solve(F, D2) - E_C2 = solve(F, C2) + # We can solve two linear systems in one pass, since the + # coefficients matrix F is the same. Thus, we perform the LU + # decomposition (cubic runtime complexity) of F only once! + # The remaining back substitutions are only quadratic in runtime. + E_D2_C2 = solve(F, concatenate((D2, C2), axis=1)) + E_D2 = E_D2_C2[:, :other.inputs] + E_C2 = E_D2_C2[:, other.inputs:] T1 = eye(self.outputs) + sign * D1 * E_D2 T2 = eye(self.inputs) + sign * E_D2 * D1 diff --git a/control/tests/canonical_test.py b/control/tests/canonical_test.py new file mode 100644 index 000000000..f5908a8f4 --- /dev/null +++ b/control/tests/canonical_test.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python + +import unittest +import numpy as np +from control import ss +from control.canonical import canonical_form + + +class TestCanonical(unittest.TestCase): + """Tests for the canonical forms class""" + + def test_reachable_form(self): + """Test the reachable canonical form""" + + # Create a system in the reachable canonical form + coeffs = [1.0, 2.0, 3.0, 4.0, 1.0] + A_true = np.polynomial.polynomial.polycompanion(coeffs) + A_true = np.fliplr(np.rot90(A_true)) + B_true = np.matrix("1.0 0.0 0.0 0.0").T + C_true = np.matrix("1.0 1.0 1.0 1.0") + D_true = 42.0 + + # Perform a coordinate transform with a random invertible matrix + T_true = np.matrix([[-0.27144004, -0.39933167, 0.75634684, 0.44135471], + [-0.74855725, -0.39136285, -0.18142339, -0.50356997], + [-0.40688007, 0.81416369, 0.38002113, -0.16483334], + [-0.44769516, 0.15654653, -0.50060858, 0.72419146]]) + A = np.linalg.solve(T_true, A_true)*T_true + B = np.linalg.solve(T_true, B_true) + C = C_true*T_true + D = D_true + + # Create a state space system and convert it to the reachable canonical form + sys_check, T_check = canonical_form(ss(A, B, C, D), "reachable") + + # Check against the true values + np.testing.assert_array_almost_equal(sys_check.A, A_true) + np.testing.assert_array_almost_equal(sys_check.B, B_true) + np.testing.assert_array_almost_equal(sys_check.C, C_true) + np.testing.assert_array_almost_equal(sys_check.D, D_true) + np.testing.assert_array_almost_equal(T_check, T_true) + + def test_unreachable_system(self): + """Test reachable canonical form with an unreachable system""" + + # Create an unreachable system + A = np.matrix("1.0 2.0 2.0; 4.0 5.0 5.0; 7.0 8.0 8.0") + B = np.matrix("1.0 1.0 1.0").T + C = np.matrix("1.0 1.0 1.0") + D = 42.0 + sys = ss(A, B, C, D) + + # Check if an exception is raised + np.testing.assert_raises(ValueError, canonical_form, sys, "reachable") diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index 82d84d713..5eadcfefa 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -46,7 +46,7 @@ from numpy import matrix from numpy.testing import assert_array_almost_equal, assert_array_less # need scipy version of eigvals for generalized eigenvalue problem -from scipy.linalg import inv, eigvals +from scipy.linalg import eigvals, solve from scipy import zeros,dot from control.mateqn import lyap,dlyap,care,dare from control.exception import slycot_check @@ -150,8 +150,8 @@ def test_care_g(self): # print("The solution obtained is", X) assert_array_almost_equal( A.T * X * E + E.T * X * A - - (E.T * X * B + S) * inv(R) * (B.T * X * E + S.T) + Q, zeros((2,2))) - assert_array_almost_equal(inv(R) * (B.T * X * E + S.T), G) + (E.T * X * B + S) * solve(R, B.T * X * E + S.T) + Q, zeros((2,2))) + assert_array_almost_equal(solve(R, B.T * X * E + S.T), G) A = matrix([[-2, -1],[-1, -1]]) Q = matrix([[0, 0],[0, 1]]) @@ -177,8 +177,8 @@ def test_dare(self): # print("The solution obtained is", X) assert_array_almost_equal( A.T * X * A - X - - A.T * X * B * inv(B.T * X * B + R) * B.T * X * A + Q, zeros((2,2))) - assert_array_almost_equal(inv(B.T * X * B + R) * B.T * X * A, G) + A.T * X * B * solve(B.T * X * B + R, B.T * X * A) + Q, zeros((2,2))) + assert_array_almost_equal(solve(B.T * X * B + R, B.T * X * A), G) # check for stable closed loop lam = eigvals(A - B * G) assert_array_less(abs(lam), 1.0) @@ -192,7 +192,7 @@ def test_dare(self): # print("The solution obtained is", X) assert_array_almost_equal( A.T * X * A - X - - A.T * X * B * inv(B.T * X * B + R) * B.T * X * A + Q, zeros((2,2))) + A.T * X * B * solve(B.T * X * B + R, B.T * X * A) + Q, zeros((2,2))) assert_array_almost_equal(B.T * X * A / (B.T * X * B + R), G) # check for stable closed loop lam = eigvals(A - B * G) @@ -210,9 +210,9 @@ def test_dare_g(self): # print("The solution obtained is", X) assert_array_almost_equal( A.T * X * A - E.T * X * E - - (A.T * X * B + S) * inv(B.T * X * B + R) * (B.T * X * A + S.T) + Q, + (A.T * X * B + S) * solve(B.T * X * B + R, B.T * X * A + S.T) + Q, zeros((2,2)) ) - assert_array_almost_equal(inv(B.T * X * B + R) * (B.T * X * A + S.T), G) + assert_array_almost_equal(solve(B.T * X * B + R, B.T * X * A + S.T), G) # check for stable closed loop lam = eigvals(A - B * G, E) assert_array_less(abs(lam), 1.0) @@ -228,7 +228,7 @@ def test_dare_g(self): # print("The solution obtained is", X) assert_array_almost_equal( A.T * X * A - E.T * X * E - - (A.T * X * B + S) * inv(B.T * X * B + R) * (B.T * X * A + S.T) + Q, + (A.T * X * B + S) * solve(B.T * X * B + R, B.T * X * A + S.T) + Q, zeros((2,2)) ) assert_array_almost_equal((B.T * X * A + S.T) / (B.T * X * B + R), G) # check for stable closed loop diff --git a/control/tests/matlab_test.py b/control/tests/matlab_test.py index e1bc43bcc..552950ef9 100644 --- a/control/tests/matlab_test.py +++ b/control/tests/matlab_test.py @@ -386,7 +386,7 @@ def testBalred(self): def testModred(self): modred(self.siso_ss1, [1]) - modred(self.siso_ss2 * self.siso_ss3, [1, 2]) + modred(self.siso_ss2 * self.siso_ss3, [0, 1]) modred(self.siso_ss3, [1], 'matchdc') modred(self.siso_ss3, [1], 'truncate') diff --git a/control/tests/modelsimp_test.py b/control/tests/modelsimp_test.py index 8db556045..146c94b77 100644 --- a/control/tests/modelsimp_test.py +++ b/control/tests/modelsimp_test.py @@ -50,6 +50,18 @@ def testModredMatchDC(self): np.testing.assert_array_almost_equal(rsys.C, Crtrue,decimal=3) np.testing.assert_array_almost_equal(rsys.D, Drtrue,decimal=2) + def testModredUnstable(self): + # Check if an error is thrown when an unstable system is given + A = np.matrix('4.5418, 3.3999, 5.0342, 4.3808; \ + 0.3890, 0.3599, 0.4195, 0.1760; \ + -4.2117, -3.2395, -4.6760, -4.2180; \ + 0.0052, 0.0429, 0.0155, 0.2743') + B = np.matrix('1.0, 1.0; 2.0, 2.0; 3.0, 3.0; 4.0, 4.0') + C = np.matrix('1.0, 2.0, 3.0, 4.0; 1.0, 2.0, 3.0, 4.0') + D = np.matrix('0.0, 0.0; 0.0, 0.0') + sys = ss(A,B,C,D) + np.testing.assert_raises(ValueError, modred, sys, [2, 3]) + def testModredTruncate(self): #balanced realization computed in matlab for the transfer function: # num = [1 11 45 32], den = [1 15 60 200 60] diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index e94ba0d0e..5499148fb 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -111,8 +111,7 @@ def testAcker(self): # Make sure the system is not degenerate Cmat = ctrb(sys.A, sys.B) - if (np.linalg.matrix_rank(Cmat) != states or - abs(np.linalg.det(Cmat)) < 1e-5): + if np.linalg.matrix_rank(Cmat) != states: if (self.debug): print(" skipping (not reachable or ill conditioned)") continue diff --git a/control/xferfcn.py b/control/xferfcn.py index 7556d9049..02c5cc71e 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -7,7 +7,7 @@ for the python-control library. """ -# Python 3 compatability (needs to go here) +# Python 3 compatibility (needs to go here) from __future__ import print_function from __future__ import division