代数Riccati方程的数值算法

 

# -*- coding: utf-8 -*-
"""
Created on Sat Jul 28 17:26:44 2012

@author: FL
"""

import Parameters as P
import numpy as np
from scipy.linalg.decomp_schur import schur
from numpy.linalg import inv, LinAlgError

def solve_care(a, b, q, r):
    """Solves the continuous algebraic Riccati equation, or CARE, defined
    as (A'X + XA - XBR^-1B'X+Q=0) directly using a Schur decomposition
    method.	
    Parameters
   ----------
+    a : array_like
+        m x m square matrix
+    b : array_like
+        m x n matrix
+    q : array_like
+        m x m square matrix
+    r : array_like
+        Non-singular n x n square matrix
+    Returns
+    -------
+    x : array_like
+        Solution (m x m) to the continuous algebraic Riccati equation
+    Notes
+    Method taken from:
+    Laub, "A Schur Method for Solving Algebraic Riccati Equations."
+    U.S. Energy Research and Development Agency under contract
+    ERDA-E(49-18)-2087.
+    http://dspace.mit.edu/bitstream/handle/1721.1/1301/R-0859-05666488.pdf
+    """
    try:
        g = inv(r)
    except LinAlgError:
        raise ValueError('Matrix R in the algebraic Riccati equation solver is ill-conditioned')
        
    g = np.dot(np.dot(b, g), b.conj().transpose())
    z11 = a
    z12 = -1.0*g
    z21 = -1.0*q
    z22 = -1.0*a.conj().transpose()
    z = np.vstack((np.hstack((z11, z12)), np.hstack((z21, z22))))
    # Note: we need to sort the upper left of s to have negative real parts,
    #       while the lower right is positive real components (Laub, p. 7)
    [s, u, sorted] = schur(z, sort='lhp')
    (m, n) = u.shape
    u11 = u[0:m/2, 0:n/2]
    u12 = u[0:m/2, n/2:n]
    u21 = u[m/2:m, 0:n/2]
    u22 = u[m/2:m, n/2:n]
    u11i = inv(u11)
    
    return np.dot(u21, u11i)