<s> """<STR_LIT>""" <EOL> from __future__ import division <EOL> from textwrap import dedent <EOL> import numpy as np <EOL> from . lqcontrol import LQ <EOL> from . quadsums import var_quadratic_sum <EOL> from numpy import dot , log , sqrt , identity , hstack , vstack , trace <EOL> from scipy . linalg import solve , inv , det <EOL> from . matrix_eqn import solve_discrete_lyapunov <EOL> class RBLQ ( object ) : <EOL> r"""<STR_LIT>""" <EOL> def __init__ ( self , Q , R , A , B , C , beta , theta ) : <EOL> A , B , C , Q , R = list ( map ( np . atleast_2d , ( A , B , C , Q , R ) ) ) <EOL> self . A , self . B , self . C , self . Q , self . R = A , B , C , Q , R <EOL> self . k = self . Q . shape [ <NUM_LIT:0> ] <EOL> self . n = self . R . shape [ <NUM_LIT:0> ] <EOL> self . j = self . C . shape [ <NUM_LIT:1> ] <EOL> self . beta , self . theta = beta , theta <EOL> def __repr__ ( self ) : <EOL> return self . __str__ ( ) <EOL> def __str__ ( self ) : <EOL> m = """<STR_LIT>""" <EOL> return dedent ( m . format ( b = self . beta , n = self . n , k = self . k , j = self . j , <EOL> th = self . theta ) ) <EOL> def d_operator ( self , P ) : <EOL> r"""<STR_LIT>""" <EOL> C , theta = self . C , self . theta <EOL> I = np . identity ( self . j ) <EOL> S1 = dot ( P , C ) <EOL> S2 = dot ( C . T , S1 ) <EOL> dP = P + dot ( S1 , solve ( theta * I - S2 , S1 . T ) ) <EOL> return dP <EOL> def b_operator ( self , P ) : <EOL> r"""<STR_LIT>""" <EOL> A , B , Q , R , beta = self . A , self . B , self . Q , self . R , self . beta <EOL> S1 = Q + beta * dot ( B . T , dot ( P , B ) ) <EOL> S2 = beta * dot ( B . T , dot ( P , A ) ) <EOL> S3 = beta * dot ( A . T , dot ( P , A ) ) <EOL> F = solve ( S1 , S2 ) <EOL> new_P = R - dot ( S2 . T , solve ( S1 , S2 ) ) + S3 <EOL> return F , new_P <EOL> def robust_rule ( self ) : <EOL> """<STR_LIT>""" <EOL> A , B , C , Q , R = self . A , self . B , self . C , self . Q , self . R <EOL> beta , theta = self . beta , self . theta <EOL> k , j = self . k , self . j <EOL> I = identity ( j ) <EOL> Z = np . zeros ( ( k , j ) ) <EOL> Ba = hstack ( [ B , C ] ) <EOL> Qa = vstack ( [ hstack ( [ Q , Z ] ) , hstack ( [ Z . T , - beta * I * theta ] ) ] ) <EOL> lq = LQ ( Qa , R , A , Ba , beta = beta ) <EOL> P , f , d = lq . stationary_values ( ) <EOL> F = f [ : k , : ] <EOL> K = - f [ k : f . shape [ <NUM_LIT:0> ] , : ] <EOL> return F , K , P <EOL> def robust_rule_simple ( self , P_init = None , max_iter = <NUM_LIT> , tol = <NUM_LIT> ) : <EOL> """<STR_LIT>""" <EOL> A , B , C , Q , R = self . A , self . B , self . C , self . Q , self . R <EOL> beta , theta = self . beta , self . theta <EOL> P = np . zeros ( ( self . n , self . n ) ) if P_init is None else P_init <EOL> iterate , e = <NUM_LIT:0> , tol + <NUM_LIT:1> <EOL> while iterate < max_iter and e > tol : <EOL> F , new_P = self . b_operator ( self . d_operator ( P ) ) <EOL> e = np . sqrt ( np . sum ( ( new_P - P ) ** <NUM_LIT:2> ) ) <EOL> iterate += <NUM_LIT:1> <EOL> P = new_P <EOL> I = np . identity ( self . j ) <EOL> S1 = P . dot ( C ) <EOL> S2 = C . T . dot ( S1 ) <EOL> K = inv ( theta * I - S2 ) . dot ( S1 . T ) . dot ( A - B . dot ( F ) ) <EOL> return F , K , P <EOL> def F_to_K ( self , F ) : <EOL> """<STR_LIT>""" <EOL> Q2 = self . beta * self . theta <EOL> R2 = - self . R - dot ( F . T , dot ( self . Q , F ) ) <EOL> A2 = self . A - dot ( self . B , F ) <EOL> B2 = self . C <EOL> lq = LQ ( Q2 , R2 , A2 , B2 , beta = self . beta ) <EOL> neg_P , neg_K , d = lq . stationary_values ( ) <EOL> return - neg_K , - neg_P <EOL> def K_to_F ( self , K ) : <EOL> """<STR_LIT>""" <EOL> A1 = self . A + dot ( self . C , K ) <EOL> B1 = self . B <EOL> Q1 = self . Q <EOL> R1 = self . R - self . beta * self . theta * dot ( K . T , K ) <EOL> lq = LQ ( Q1 , R1 , A1 , B1 , beta = self . beta ) <EOL> P , F , d = lq . stationary_values ( ) <EOL> return F , P <EOL> def compute_deterministic_entropy ( self , F , K , x0 ) : <EOL> """<STR_LIT>""" <EOL> H0 = dot ( K . T , K ) <EOL> C0 = np . zeros ( ( self . n , <NUM_LIT:1> ) ) <EOL> A0 = self . A - dot ( self . B , F ) + dot ( self . C , K ) <EOL> e = var_quadratic_sum ( A0 , C0 , H0 , self . beta , x0 ) <EOL> return e <EOL> def evaluate_F ( self , F ) : <EOL> """<STR_LIT>""" <EOL> Q , R , A , B , C = self . Q , self . R , self . A , self . B , self . C <EOL> beta , theta = self . beta , self . theta <EOL> K_F , P_F = self . F_to_K ( F ) <EOL> I = np . identity ( self . j ) <EOL> H = inv ( I - C . T . dot ( P_F . dot ( C ) ) / theta ) <EOL> d_F = log ( det ( H ) ) <EOL> sig = - <NUM_LIT:1.0> / theta <EOL> AO = sqrt ( beta ) * ( A - dot ( B , F ) + dot ( C , K_F ) ) <EOL> O_F = solve_discrete_lyapunov ( AO . T , beta * dot ( K_F . T , K_F ) ) <EOL> ho = ( trace ( H - <NUM_LIT:1> ) - d_F ) / <NUM_LIT> <EOL> tr = trace ( dot ( O_F , C . dot ( H . dot ( C . T ) ) ) ) <EOL> o_F = ( ho + beta * tr ) / ( <NUM_LIT:1> - beta ) <EOL> return K_F , P_F , d_F , O_F , o_F </s>
