_solvers.py 28 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844
  1. """Matrix equation solver routines"""
  2. # Author: Jeffrey Armstrong <jeff@approximatrix.com>
  3. # February 24, 2012
  4. # Modified: Chad Fulton <ChadFulton@gmail.com>
  5. # June 19, 2014
  6. # Modified: Ilhan Polat <ilhanpolat@gmail.com>
  7. # September 13, 2016
  8. from __future__ import division, print_function, absolute_import
  9. import warnings
  10. import numpy as np
  11. from numpy.linalg import inv, LinAlgError, norm, cond, svd
  12. from .basic import solve, solve_triangular, matrix_balance
  13. from .lapack import get_lapack_funcs
  14. from .decomp_schur import schur
  15. from .decomp_lu import lu
  16. from .decomp_qr import qr
  17. from ._decomp_qz import ordqz
  18. from .decomp import _asarray_validated
  19. from .special_matrices import kron, block_diag
  20. __all__ = ['solve_sylvester',
  21. 'solve_continuous_lyapunov', 'solve_discrete_lyapunov',
  22. 'solve_lyapunov',
  23. 'solve_continuous_are', 'solve_discrete_are']
  24. def solve_sylvester(a, b, q):
  25. """
  26. Computes a solution (X) to the Sylvester equation :math:`AX + XB = Q`.
  27. Parameters
  28. ----------
  29. a : (M, M) array_like
  30. Leading matrix of the Sylvester equation
  31. b : (N, N) array_like
  32. Trailing matrix of the Sylvester equation
  33. q : (M, N) array_like
  34. Right-hand side
  35. Returns
  36. -------
  37. x : (M, N) ndarray
  38. The solution to the Sylvester equation.
  39. Raises
  40. ------
  41. LinAlgError
  42. If solution was not found
  43. Notes
  44. -----
  45. Computes a solution to the Sylvester matrix equation via the Bartels-
  46. Stewart algorithm. The A and B matrices first undergo Schur
  47. decompositions. The resulting matrices are used to construct an
  48. alternative Sylvester equation (``RY + YS^T = F``) where the R and S
  49. matrices are in quasi-triangular form (or, when R, S or F are complex,
  50. triangular form). The simplified equation is then solved using
  51. ``*TRSYL`` from LAPACK directly.
  52. .. versionadded:: 0.11.0
  53. Examples
  54. --------
  55. Given `a`, `b`, and `q` solve for `x`:
  56. >>> from scipy import linalg
  57. >>> a = np.array([[-3, -2, 0], [-1, -1, 3], [3, -5, -1]])
  58. >>> b = np.array([[1]])
  59. >>> q = np.array([[1],[2],[3]])
  60. >>> x = linalg.solve_sylvester(a, b, q)
  61. >>> x
  62. array([[ 0.0625],
  63. [-0.5625],
  64. [ 0.6875]])
  65. >>> np.allclose(a.dot(x) + x.dot(b), q)
  66. True
  67. """
  68. # Compute the Schur decomp form of a
  69. r, u = schur(a, output='real')
  70. # Compute the Schur decomp of b
  71. s, v = schur(b.conj().transpose(), output='real')
  72. # Construct f = u'*q*v
  73. f = np.dot(np.dot(u.conj().transpose(), q), v)
  74. # Call the Sylvester equation solver
  75. trsyl, = get_lapack_funcs(('trsyl',), (r, s, f))
  76. if trsyl is None:
  77. raise RuntimeError('LAPACK implementation does not contain a proper '
  78. 'Sylvester equation solver (TRSYL)')
  79. y, scale, info = trsyl(r, s, f, tranb='C')
  80. y = scale*y
  81. if info < 0:
  82. raise LinAlgError("Illegal value encountered in "
  83. "the %d term" % (-info,))
  84. return np.dot(np.dot(u, y), v.conj().transpose())
  85. def solve_continuous_lyapunov(a, q):
  86. """
  87. Solves the continuous Lyapunov equation :math:`AX + XA^H = Q`.
  88. Uses the Bartels-Stewart algorithm to find :math:`X`.
  89. Parameters
  90. ----------
  91. a : array_like
  92. A square matrix
  93. q : array_like
  94. Right-hand side square matrix
  95. Returns
  96. -------
  97. x : ndarray
  98. Solution to the continuous Lyapunov equation
  99. See Also
  100. --------
  101. solve_discrete_lyapunov : computes the solution to the discrete-time
  102. Lyapunov equation
  103. solve_sylvester : computes the solution to the Sylvester equation
  104. Notes
  105. -----
  106. The continuous Lyapunov equation is a special form of the Sylvester
  107. equation, hence this solver relies on LAPACK routine ?TRSYL.
  108. .. versionadded:: 0.11.0
  109. Examples
  110. --------
  111. Given `a` and `q` solve for `x`:
  112. >>> from scipy import linalg
  113. >>> a = np.array([[-3, -2, 0], [-1, -1, 0], [0, -5, -1]])
  114. >>> b = np.array([2, 4, -1])
  115. >>> q = np.eye(3)
  116. >>> x = linalg.solve_continuous_lyapunov(a, q)
  117. >>> x
  118. array([[ -0.75 , 0.875 , -3.75 ],
  119. [ 0.875 , -1.375 , 5.3125],
  120. [ -3.75 , 5.3125, -27.0625]])
  121. >>> np.allclose(a.dot(x) + x.dot(a.T), q)
  122. True
  123. """
  124. a = np.atleast_2d(_asarray_validated(a, check_finite=True))
  125. q = np.atleast_2d(_asarray_validated(q, check_finite=True))
  126. r_or_c = float
  127. for ind, _ in enumerate((a, q)):
  128. if np.iscomplexobj(_):
  129. r_or_c = complex
  130. if not np.equal(*_.shape):
  131. raise ValueError("Matrix {} should be square.".format("aq"[ind]))
  132. # Shape consistency check
  133. if a.shape != q.shape:
  134. raise ValueError("Matrix a and q should have the same shape.")
  135. # Compute the Schur decomp form of a
  136. r, u = schur(a, output='real')
  137. # Construct f = u'*q*u
  138. f = u.conj().T.dot(q.dot(u))
  139. # Call the Sylvester equation solver
  140. trsyl = get_lapack_funcs('trsyl', (r, f))
  141. dtype_string = 'T' if r_or_c == float else 'C'
  142. y, scale, info = trsyl(r, r, f, tranb=dtype_string)
  143. if info < 0:
  144. raise ValueError('?TRSYL exited with the internal error '
  145. '"illegal value in argument number {}.". See '
  146. 'LAPACK documentation for the ?TRSYL error codes.'
  147. ''.format(-info))
  148. elif info == 1:
  149. warnings.warn('Input "a" has an eigenvalue pair whose sum is '
  150. 'very close to or exactly zero. The solution is '
  151. 'obtained via perturbing the coefficients.',
  152. RuntimeWarning)
  153. y *= scale
  154. return u.dot(y).dot(u.conj().T)
  155. # For backwards compatibility, keep the old name
  156. solve_lyapunov = solve_continuous_lyapunov
  157. def _solve_discrete_lyapunov_direct(a, q):
  158. """
  159. Solves the discrete Lyapunov equation directly.
  160. This function is called by the `solve_discrete_lyapunov` function with
  161. `method=direct`. It is not supposed to be called directly.
  162. """
  163. lhs = kron(a, a.conj())
  164. lhs = np.eye(lhs.shape[0]) - lhs
  165. x = solve(lhs, q.flatten())
  166. return np.reshape(x, q.shape)
  167. def _solve_discrete_lyapunov_bilinear(a, q):
  168. """
  169. Solves the discrete Lyapunov equation using a bilinear transformation.
  170. This function is called by the `solve_discrete_lyapunov` function with
  171. `method=bilinear`. It is not supposed to be called directly.
  172. """
  173. eye = np.eye(a.shape[0])
  174. aH = a.conj().transpose()
  175. aHI_inv = inv(aH + eye)
  176. b = np.dot(aH - eye, aHI_inv)
  177. c = 2*np.dot(np.dot(inv(a + eye), q), aHI_inv)
  178. return solve_lyapunov(b.conj().transpose(), -c)
  179. def solve_discrete_lyapunov(a, q, method=None):
  180. """
  181. Solves the discrete Lyapunov equation :math:`AXA^H - X + Q = 0`.
  182. Parameters
  183. ----------
  184. a, q : (M, M) array_like
  185. Square matrices corresponding to A and Q in the equation
  186. above respectively. Must have the same shape.
  187. method : {'direct', 'bilinear'}, optional
  188. Type of solver.
  189. If not given, chosen to be ``direct`` if ``M`` is less than 10 and
  190. ``bilinear`` otherwise.
  191. Returns
  192. -------
  193. x : ndarray
  194. Solution to the discrete Lyapunov equation
  195. See Also
  196. --------
  197. solve_continuous_lyapunov : computes the solution to the continuous-time
  198. Lyapunov equation
  199. Notes
  200. -----
  201. This section describes the available solvers that can be selected by the
  202. 'method' parameter. The default method is *direct* if ``M`` is less than 10
  203. and ``bilinear`` otherwise.
  204. Method *direct* uses a direct analytical solution to the discrete Lyapunov
  205. equation. The algorithm is given in, for example, [1]_. However it requires
  206. the linear solution of a system with dimension :math:`M^2` so that
  207. performance degrades rapidly for even moderately sized matrices.
  208. Method *bilinear* uses a bilinear transformation to convert the discrete
  209. Lyapunov equation to a continuous Lyapunov equation :math:`(BX+XB'=-C)`
  210. where :math:`B=(A-I)(A+I)^{-1}` and
  211. :math:`C=2(A' + I)^{-1} Q (A + I)^{-1}`. The continuous equation can be
  212. efficiently solved since it is a special case of a Sylvester equation.
  213. The transformation algorithm is from Popov (1964) as described in [2]_.
  214. .. versionadded:: 0.11.0
  215. References
  216. ----------
  217. .. [1] Hamilton, James D. Time Series Analysis, Princeton: Princeton
  218. University Press, 1994. 265. Print.
  219. http://doc1.lbfl.li/aca/FLMF037168.pdf
  220. .. [2] Gajic, Z., and M.T.J. Qureshi. 2008.
  221. Lyapunov Matrix Equation in System Stability and Control.
  222. Dover Books on Engineering Series. Dover Publications.
  223. Examples
  224. --------
  225. Given `a` and `q` solve for `x`:
  226. >>> from scipy import linalg
  227. >>> a = np.array([[0.2, 0.5],[0.7, -0.9]])
  228. >>> q = np.eye(2)
  229. >>> x = linalg.solve_discrete_lyapunov(a, q)
  230. >>> x
  231. array([[ 0.70872893, 1.43518822],
  232. [ 1.43518822, -2.4266315 ]])
  233. >>> np.allclose(a.dot(x).dot(a.T)-x, -q)
  234. True
  235. """
  236. a = np.asarray(a)
  237. q = np.asarray(q)
  238. if method is None:
  239. # Select automatically based on size of matrices
  240. if a.shape[0] >= 10:
  241. method = 'bilinear'
  242. else:
  243. method = 'direct'
  244. meth = method.lower()
  245. if meth == 'direct':
  246. x = _solve_discrete_lyapunov_direct(a, q)
  247. elif meth == 'bilinear':
  248. x = _solve_discrete_lyapunov_bilinear(a, q)
  249. else:
  250. raise ValueError('Unknown solver %s' % method)
  251. return x
  252. def solve_continuous_are(a, b, q, r, e=None, s=None, balanced=True):
  253. r"""
  254. Solves the continuous-time algebraic Riccati equation (CARE).
  255. The CARE is defined as
  256. .. math::
  257. X A + A^H X - X B R^{-1} B^H X + Q = 0
  258. The limitations for a solution to exist are :
  259. * All eigenvalues of :math:`A` on the right half plane, should be
  260. controllable.
  261. * The associated hamiltonian pencil (See Notes), should have
  262. eigenvalues sufficiently away from the imaginary axis.
  263. Moreover, if ``e`` or ``s`` is not precisely ``None``, then the
  264. generalized version of CARE
  265. .. math::
  266. E^HXA + A^HXE - (E^HXB + S) R^{-1} (B^HXE + S^H) + Q = 0
  267. is solved. When omitted, ``e`` is assumed to be the identity and ``s``
  268. is assumed to be the zero matrix with sizes compatible with ``a`` and
  269. ``b`` respectively.
  270. Parameters
  271. ----------
  272. a : (M, M) array_like
  273. Square matrix
  274. b : (M, N) array_like
  275. Input
  276. q : (M, M) array_like
  277. Input
  278. r : (N, N) array_like
  279. Nonsingular square matrix
  280. e : (M, M) array_like, optional
  281. Nonsingular square matrix
  282. s : (M, N) array_like, optional
  283. Input
  284. balanced : bool, optional
  285. The boolean that indicates whether a balancing step is performed
  286. on the data. The default is set to True.
  287. Returns
  288. -------
  289. x : (M, M) ndarray
  290. Solution to the continuous-time algebraic Riccati equation.
  291. Raises
  292. ------
  293. LinAlgError
  294. For cases where the stable subspace of the pencil could not be
  295. isolated. See Notes section and the references for details.
  296. See Also
  297. --------
  298. solve_discrete_are : Solves the discrete-time algebraic Riccati equation
  299. Notes
  300. -----
  301. The equation is solved by forming the extended hamiltonian matrix pencil,
  302. as described in [1]_, :math:`H - \lambda J` given by the block matrices ::
  303. [ A 0 B ] [ E 0 0 ]
  304. [-Q -A^H -S ] - \lambda * [ 0 E^H 0 ]
  305. [ S^H B^H R ] [ 0 0 0 ]
  306. and using a QZ decomposition method.
  307. In this algorithm, the fail conditions are linked to the symmetry
  308. of the product :math:`U_2 U_1^{-1}` and condition number of
  309. :math:`U_1`. Here, :math:`U` is the 2m-by-m matrix that holds the
  310. eigenvectors spanning the stable subspace with 2m rows and partitioned
  311. into two m-row matrices. See [1]_ and [2]_ for more details.
  312. In order to improve the QZ decomposition accuracy, the pencil goes
  313. through a balancing step where the sum of absolute values of
  314. :math:`H` and :math:`J` entries (after removing the diagonal entries of
  315. the sum) is balanced following the recipe given in [3]_.
  316. .. versionadded:: 0.11.0
  317. References
  318. ----------
  319. .. [1] P. van Dooren , "A Generalized Eigenvalue Approach For Solving
  320. Riccati Equations.", SIAM Journal on Scientific and Statistical
  321. Computing, Vol.2(2), DOI: 10.1137/0902010
  322. .. [2] A.J. Laub, "A Schur Method for Solving Algebraic Riccati
  323. Equations.", Massachusetts Institute of Technology. Laboratory for
  324. Information and Decision Systems. LIDS-R ; 859. Available online :
  325. http://hdl.handle.net/1721.1/1301
  326. .. [3] P. Benner, "Symplectic Balancing of Hamiltonian Matrices", 2001,
  327. SIAM J. Sci. Comput., 2001, Vol.22(5), DOI: 10.1137/S1064827500367993
  328. Examples
  329. --------
  330. Given `a`, `b`, `q`, and `r` solve for `x`:
  331. >>> from scipy import linalg
  332. >>> a = np.array([[4, 3], [-4.5, -3.5]])
  333. >>> b = np.array([[1], [-1]])
  334. >>> q = np.array([[9, 6], [6, 4.]])
  335. >>> r = 1
  336. >>> x = linalg.solve_continuous_are(a, b, q, r)
  337. >>> x
  338. array([[ 21.72792206, 14.48528137],
  339. [ 14.48528137, 9.65685425]])
  340. >>> np.allclose(a.T.dot(x) + x.dot(a)-x.dot(b).dot(b.T).dot(x), -q)
  341. True
  342. """
  343. # Validate input arguments
  344. a, b, q, r, e, s, m, n, r_or_c, gen_are = _are_validate_args(
  345. a, b, q, r, e, s, 'care')
  346. H = np.empty((2*m+n, 2*m+n), dtype=r_or_c)
  347. H[:m, :m] = a
  348. H[:m, m:2*m] = 0.
  349. H[:m, 2*m:] = b
  350. H[m:2*m, :m] = -q
  351. H[m:2*m, m:2*m] = -a.conj().T
  352. H[m:2*m, 2*m:] = 0. if s is None else -s
  353. H[2*m:, :m] = 0. if s is None else s.conj().T
  354. H[2*m:, m:2*m] = b.conj().T
  355. H[2*m:, 2*m:] = r
  356. if gen_are and e is not None:
  357. J = block_diag(e, e.conj().T, np.zeros_like(r, dtype=r_or_c))
  358. else:
  359. J = block_diag(np.eye(2*m), np.zeros_like(r, dtype=r_or_c))
  360. if balanced:
  361. # xGEBAL does not remove the diagonals before scaling. Also
  362. # to avoid destroying the Symplectic structure, we follow Ref.3
  363. M = np.abs(H) + np.abs(J)
  364. M[np.diag_indices_from(M)] = 0.
  365. _, (sca, _) = matrix_balance(M, separate=1, permute=0)
  366. # do we need to bother?
  367. if not np.allclose(sca, np.ones_like(sca)):
  368. # Now impose diag(D,inv(D)) from Benner where D is
  369. # square root of s_i/s_(n+i) for i=0,....
  370. sca = np.log2(sca)
  371. # NOTE: Py3 uses "Bankers Rounding: round to the nearest even" !!
  372. s = np.round((sca[m:2*m] - sca[:m])/2)
  373. sca = 2 ** np.r_[s, -s, sca[2*m:]]
  374. # Elementwise multiplication via broadcasting.
  375. elwisescale = sca[:, None] * np.reciprocal(sca)
  376. H *= elwisescale
  377. J *= elwisescale
  378. # Deflate the pencil to 2m x 2m ala Ref.1, eq.(55)
  379. q, r = qr(H[:, -n:])
  380. H = q[:, n:].conj().T.dot(H[:, :2*m])
  381. J = q[:2*m, n:].conj().T.dot(J[:2*m, :2*m])
  382. # Decide on which output type is needed for QZ
  383. out_str = 'real' if r_or_c == float else 'complex'
  384. _, _, _, _, _, u = ordqz(H, J, sort='lhp', overwrite_a=True,
  385. overwrite_b=True, check_finite=False,
  386. output=out_str)
  387. # Get the relevant parts of the stable subspace basis
  388. if e is not None:
  389. u, _ = qr(np.vstack((e.dot(u[:m, :m]), u[m:, :m])))
  390. u00 = u[:m, :m]
  391. u10 = u[m:, :m]
  392. # Solve via back-substituion after checking the condition of u00
  393. up, ul, uu = lu(u00)
  394. if 1/cond(uu) < np.spacing(1.):
  395. raise LinAlgError('Failed to find a finite solution.')
  396. # Exploit the triangular structure
  397. x = solve_triangular(ul.conj().T,
  398. solve_triangular(uu.conj().T,
  399. u10.conj().T,
  400. lower=True),
  401. unit_diagonal=True,
  402. ).conj().T.dot(up.conj().T)
  403. if balanced:
  404. x *= sca[:m, None] * sca[:m]
  405. # Check the deviation from symmetry for lack of success
  406. # See proof of Thm.5 item 3 in [2]
  407. u_sym = u00.conj().T.dot(u10)
  408. n_u_sym = norm(u_sym, 1)
  409. u_sym = u_sym - u_sym.conj().T
  410. sym_threshold = np.max([np.spacing(1000.), 0.1*n_u_sym])
  411. if norm(u_sym, 1) > sym_threshold:
  412. raise LinAlgError('The associated Hamiltonian pencil has eigenvalues '
  413. 'too close to the imaginary axis')
  414. return (x + x.conj().T)/2
  415. def solve_discrete_are(a, b, q, r, e=None, s=None, balanced=True):
  416. r"""
  417. Solves the discrete-time algebraic Riccati equation (DARE).
  418. The DARE is defined as
  419. .. math::
  420. A^HXA - X - (A^HXB) (R + B^HXB)^{-1} (B^HXA) + Q = 0
  421. The limitations for a solution to exist are :
  422. * All eigenvalues of :math:`A` outside the unit disc, should be
  423. controllable.
  424. * The associated symplectic pencil (See Notes), should have
  425. eigenvalues sufficiently away from the unit circle.
  426. Moreover, if ``e`` and ``s`` are not both precisely ``None``, then the
  427. generalized version of DARE
  428. .. math::
  429. A^HXA - E^HXE - (A^HXB+S) (R+B^HXB)^{-1} (B^HXA+S^H) + Q = 0
  430. is solved. When omitted, ``e`` is assumed to be the identity and ``s``
  431. is assumed to be the zero matrix.
  432. Parameters
  433. ----------
  434. a : (M, M) array_like
  435. Square matrix
  436. b : (M, N) array_like
  437. Input
  438. q : (M, M) array_like
  439. Input
  440. r : (N, N) array_like
  441. Square matrix
  442. e : (M, M) array_like, optional
  443. Nonsingular square matrix
  444. s : (M, N) array_like, optional
  445. Input
  446. balanced : bool
  447. The boolean that indicates whether a balancing step is performed
  448. on the data. The default is set to True.
  449. Returns
  450. -------
  451. x : (M, M) ndarray
  452. Solution to the discrete algebraic Riccati equation.
  453. Raises
  454. ------
  455. LinAlgError
  456. For cases where the stable subspace of the pencil could not be
  457. isolated. See Notes section and the references for details.
  458. See Also
  459. --------
  460. solve_continuous_are : Solves the continuous algebraic Riccati equation
  461. Notes
  462. -----
  463. The equation is solved by forming the extended symplectic matrix pencil,
  464. as described in [1]_, :math:`H - \lambda J` given by the block matrices ::
  465. [ A 0 B ] [ E 0 B ]
  466. [ -Q E^H -S ] - \lambda * [ 0 A^H 0 ]
  467. [ S^H 0 R ] [ 0 -B^H 0 ]
  468. and using a QZ decomposition method.
  469. In this algorithm, the fail conditions are linked to the symmetry
  470. of the product :math:`U_2 U_1^{-1}` and condition number of
  471. :math:`U_1`. Here, :math:`U` is the 2m-by-m matrix that holds the
  472. eigenvectors spanning the stable subspace with 2m rows and partitioned
  473. into two m-row matrices. See [1]_ and [2]_ for more details.
  474. In order to improve the QZ decomposition accuracy, the pencil goes
  475. through a balancing step where the sum of absolute values of
  476. :math:`H` and :math:`J` rows/cols (after removing the diagonal entries)
  477. is balanced following the recipe given in [3]_. If the data has small
  478. numerical noise, balancing may amplify their effects and some clean up
  479. is required.
  480. .. versionadded:: 0.11.0
  481. References
  482. ----------
  483. .. [1] P. van Dooren , "A Generalized Eigenvalue Approach For Solving
  484. Riccati Equations.", SIAM Journal on Scientific and Statistical
  485. Computing, Vol.2(2), DOI: 10.1137/0902010
  486. .. [2] A.J. Laub, "A Schur Method for Solving Algebraic Riccati
  487. Equations.", Massachusetts Institute of Technology. Laboratory for
  488. Information and Decision Systems. LIDS-R ; 859. Available online :
  489. http://hdl.handle.net/1721.1/1301
  490. .. [3] P. Benner, "Symplectic Balancing of Hamiltonian Matrices", 2001,
  491. SIAM J. Sci. Comput., 2001, Vol.22(5), DOI: 10.1137/S1064827500367993
  492. Examples
  493. --------
  494. Given `a`, `b`, `q`, and `r` solve for `x`:
  495. >>> from scipy import linalg as la
  496. >>> a = np.array([[0, 1], [0, -1]])
  497. >>> b = np.array([[1, 0], [2, 1]])
  498. >>> q = np.array([[-4, -4], [-4, 7]])
  499. >>> r = np.array([[9, 3], [3, 1]])
  500. >>> x = la.solve_discrete_are(a, b, q, r)
  501. >>> x
  502. array([[-4., -4.],
  503. [-4., 7.]])
  504. >>> R = la.solve(r + b.T.dot(x).dot(b), b.T.dot(x).dot(a))
  505. >>> np.allclose(a.T.dot(x).dot(a) - x - a.T.dot(x).dot(b).dot(R), -q)
  506. True
  507. """
  508. # Validate input arguments
  509. a, b, q, r, e, s, m, n, r_or_c, gen_are = _are_validate_args(
  510. a, b, q, r, e, s, 'dare')
  511. # Form the matrix pencil
  512. H = np.zeros((2*m+n, 2*m+n), dtype=r_or_c)
  513. H[:m, :m] = a
  514. H[:m, 2*m:] = b
  515. H[m:2*m, :m] = -q
  516. H[m:2*m, m:2*m] = np.eye(m) if e is None else e.conj().T
  517. H[m:2*m, 2*m:] = 0. if s is None else -s
  518. H[2*m:, :m] = 0. if s is None else s.conj().T
  519. H[2*m:, 2*m:] = r
  520. J = np.zeros_like(H, dtype=r_or_c)
  521. J[:m, :m] = np.eye(m) if e is None else e
  522. J[m:2*m, m:2*m] = a.conj().T
  523. J[2*m:, m:2*m] = -b.conj().T
  524. if balanced:
  525. # xGEBAL does not remove the diagonals before scaling. Also
  526. # to avoid destroying the Symplectic structure, we follow Ref.3
  527. M = np.abs(H) + np.abs(J)
  528. M[np.diag_indices_from(M)] = 0.
  529. _, (sca, _) = matrix_balance(M, separate=1, permute=0)
  530. # do we need to bother?
  531. if not np.allclose(sca, np.ones_like(sca)):
  532. # Now impose diag(D,inv(D)) from Benner where D is
  533. # square root of s_i/s_(n+i) for i=0,....
  534. sca = np.log2(sca)
  535. # NOTE: Py3 uses "Bankers Rounding: round to the nearest even" !!
  536. s = np.round((sca[m:2*m] - sca[:m])/2)
  537. sca = 2 ** np.r_[s, -s, sca[2*m:]]
  538. # Elementwise multiplication via broadcasting.
  539. elwisescale = sca[:, None] * np.reciprocal(sca)
  540. H *= elwisescale
  541. J *= elwisescale
  542. # Deflate the pencil by the R column ala Ref.1
  543. q_of_qr, _ = qr(H[:, -n:])
  544. H = q_of_qr[:, n:].conj().T.dot(H[:, :2*m])
  545. J = q_of_qr[:, n:].conj().T.dot(J[:, :2*m])
  546. # Decide on which output type is needed for QZ
  547. out_str = 'real' if r_or_c == float else 'complex'
  548. _, _, _, _, _, u = ordqz(H, J, sort='iuc',
  549. overwrite_a=True,
  550. overwrite_b=True,
  551. check_finite=False,
  552. output=out_str)
  553. # Get the relevant parts of the stable subspace basis
  554. if e is not None:
  555. u, _ = qr(np.vstack((e.dot(u[:m, :m]), u[m:, :m])))
  556. u00 = u[:m, :m]
  557. u10 = u[m:, :m]
  558. # Solve via back-substituion after checking the condition of u00
  559. up, ul, uu = lu(u00)
  560. if 1/cond(uu) < np.spacing(1.):
  561. raise LinAlgError('Failed to find a finite solution.')
  562. # Exploit the triangular structure
  563. x = solve_triangular(ul.conj().T,
  564. solve_triangular(uu.conj().T,
  565. u10.conj().T,
  566. lower=True),
  567. unit_diagonal=True,
  568. ).conj().T.dot(up.conj().T)
  569. if balanced:
  570. x *= sca[:m, None] * sca[:m]
  571. # Check the deviation from symmetry for lack of success
  572. # See proof of Thm.5 item 3 in [2]
  573. u_sym = u00.conj().T.dot(u10)
  574. n_u_sym = norm(u_sym, 1)
  575. u_sym = u_sym - u_sym.conj().T
  576. sym_threshold = np.max([np.spacing(1000.), 0.1*n_u_sym])
  577. if norm(u_sym, 1) > sym_threshold:
  578. raise LinAlgError('The associated symplectic pencil has eigenvalues'
  579. 'too close to the unit circle')
  580. return (x + x.conj().T)/2
  581. def _are_validate_args(a, b, q, r, e, s, eq_type='care'):
  582. """
  583. A helper function to validate the arguments supplied to the
  584. Riccati equation solvers. Any discrepancy found in the input
  585. matrices leads to a ``ValueError`` exception.
  586. Essentially, it performs:
  587. - a check whether the input is free of NaN and Infs.
  588. - a pass for the data through ``numpy.atleast_2d()``
  589. - squareness check of the relevant arrays,
  590. - shape consistency check of the arrays,
  591. - singularity check of the relevant arrays,
  592. - symmetricity check of the relevant matrices,
  593. - a check whether the regular or the generalized version is asked.
  594. This function is used by ``solve_continuous_are`` and
  595. ``solve_discrete_are``.
  596. Parameters
  597. ----------
  598. a, b, q, r, e, s : array_like
  599. Input data
  600. eq_type : str
  601. Accepted arguments are 'care' and 'dare'.
  602. Returns
  603. -------
  604. a, b, q, r, e, s : ndarray
  605. Regularized input data
  606. m, n : int
  607. shape of the problem
  608. r_or_c : type
  609. Data type of the problem, returns float or complex
  610. gen_or_not : bool
  611. Type of the equation, True for generalized and False for regular ARE.
  612. """
  613. if not eq_type.lower() in ('dare', 'care'):
  614. raise ValueError("Equation type unknown. "
  615. "Only 'care' and 'dare' is understood")
  616. a = np.atleast_2d(_asarray_validated(a, check_finite=True))
  617. b = np.atleast_2d(_asarray_validated(b, check_finite=True))
  618. q = np.atleast_2d(_asarray_validated(q, check_finite=True))
  619. r = np.atleast_2d(_asarray_validated(r, check_finite=True))
  620. # Get the correct data types otherwise Numpy complains
  621. # about pushing complex numbers into real arrays.
  622. r_or_c = complex if np.iscomplexobj(b) else float
  623. for ind, mat in enumerate((a, q, r)):
  624. if np.iscomplexobj(mat):
  625. r_or_c = complex
  626. if not np.equal(*mat.shape):
  627. raise ValueError("Matrix {} should be square.".format("aqr"[ind]))
  628. # Shape consistency checks
  629. m, n = b.shape
  630. if m != a.shape[0]:
  631. raise ValueError("Matrix a and b should have the same number of rows.")
  632. if m != q.shape[0]:
  633. raise ValueError("Matrix a and q should have the same shape.")
  634. if n != r.shape[0]:
  635. raise ValueError("Matrix b and r should have the same number of cols.")
  636. # Check if the data matrices q, r are (sufficiently) hermitian
  637. for ind, mat in enumerate((q, r)):
  638. if norm(mat - mat.conj().T, 1) > np.spacing(norm(mat, 1))*100:
  639. raise ValueError("Matrix {} should be symmetric/hermitian."
  640. "".format("qr"[ind]))
  641. # Continuous time ARE should have a nonsingular r matrix.
  642. if eq_type == 'care':
  643. min_sv = svd(r, compute_uv=False)[-1]
  644. if min_sv == 0. or min_sv < np.spacing(1.)*norm(r, 1):
  645. raise ValueError('Matrix r is numerically singular.')
  646. # Check if the generalized case is required with omitted arguments
  647. # perform late shape checking etc.
  648. generalized_case = e is not None or s is not None
  649. if generalized_case:
  650. if e is not None:
  651. e = np.atleast_2d(_asarray_validated(e, check_finite=True))
  652. if not np.equal(*e.shape):
  653. raise ValueError("Matrix e should be square.")
  654. if m != e.shape[0]:
  655. raise ValueError("Matrix a and e should have the same shape.")
  656. # numpy.linalg.cond doesn't check for exact zeros and
  657. # emits a runtime warning. Hence the following manual check.
  658. min_sv = svd(e, compute_uv=False)[-1]
  659. if min_sv == 0. or min_sv < np.spacing(1.) * norm(e, 1):
  660. raise ValueError('Matrix e is numerically singular.')
  661. if np.iscomplexobj(e):
  662. r_or_c = complex
  663. if s is not None:
  664. s = np.atleast_2d(_asarray_validated(s, check_finite=True))
  665. if s.shape != b.shape:
  666. raise ValueError("Matrix b and s should have the same shape.")
  667. if np.iscomplexobj(s):
  668. r_or_c = complex
  669. return a, b, q, r, e, s, m, n, r_or_c, generalized_case