def KalmanBasisNew( check_A, check_B, check_C ): ''' This produces the Kalman Basis needed for a kalman decomposition of the system, which is used for a minimal realization of the state space +---------------------+---------------+-------------------+-----------+--------------+ | Conditions | T1 | T2 | T3 | T4 | +---------------------+---------------+-------------------+-----------+--------------+ | R, !N | R( C ) | * | ~R( C ) | * | | ( R N ) full rank | R( C ) | * | * | N( O ) | | ( R N ) low rank | R( C ) | * | ~( R N ) | N( O ) | | Kalman | R( C ) - T2 | R( C ) int N( O ) | ~( R N ) | N( O ) - T2 | +---------------------+---------------+-------------------+-----------+--------------+ ''' check_cont , bool_cont = controlability( check_B , check_A ) check_obs, bool_obs = observability( check_C, check_A ) range_cont = columspace( check_cont ) null_obs = nullspace( check_obs ) # The system has R( Cont ) but has not N( Obs ) => Fully observable but not controllable. Similarity Transform into Cont basis can finish the job. if not bool_obs : interBasis = intersection( range_cont , null_obs ) else : print "Completely Observable system + Uncontrollable system" new_A , new_B, new_C, KBasis = ContSpace( check_A, check_B, check_C , False ) return KBasis, matrix_rank( check_cont ) , 0,0, matrix_rank( check_obs ) - matrix_rank( check_cont ) dim_int = min(shape(interBasis)) dim_C = min( shape( range_cont ) ) dim_N = min( shape( null_obs ) ) T2 = interBasis[: , 0:dim_int] if min( shape( T2 ) ) == 0: T1 = range_cont T4 = null_obs if matrix_rank( nC( (T1,T4), 1 ) ) == max( shape( check_A ) ): return nC( ( T1, T4 ) ,1 ), min( shape( T1 ) ), 0, 0, min(shape(T4) ) else : T3 = nullspace ( ( nC( ( T1,T4) , 1 ) ).T ) return nC( (T1,T3,T4) , 1 ) , min( shape( T1 ) ), 0 ,min( shape( T3 ) ),min( shape( T4 ) ) else : # The system has R( Cont ) and N( Obs ) => Usual Kalman Decomposition can take place. T1 = removeBasis( range_cont, T2 ) T4 = removeBasis( null_obs , T2 ) T3 = nullspace( ( numpy.concatenate( (T1, T2,T4 ), 1 ) ).T) KBasis = numpy.concatenate( (T1,T2,T3,T4) , 1 ) return KBasis, min( shape( T1 ) ) , min( shape( T2 ) ), min( shape( T3 ) ), min( shape( T4 ) )
def KalmanBasisNew(check_A, check_B, check_C):
''' This produces the Kalman Basis needed for a kalman decomposition of the system, which is used for a minimal realization of the state space
+---------------------+---------------+-------------------+-----------+--------------+
| Conditions | T1 | T2 | T3 | T4 |
+---------------------+---------------+-------------------+-----------+--------------+
| R, !N | R( C ) | * | ~R( C ) | * |
| ( R N ) full rank | R( C ) | * | * | N( O ) |
| ( R N ) low rank | R( C ) | * | ~( R N ) | N( O ) |
| Kalman | R( C ) - T2 | R( C ) int N( O ) | ~( R N ) | N( O ) - T2 |
+---------------------+---------------+-------------------+-----------+--------------+ '''
check_cont, bool_cont = controlability(check_B, check_A)
check_obs, bool_obs = observability(check_C, check_A)
range_cont = columspace(check_cont)
null_obs = nullspace(check_obs)
# The system has R( Cont ) but has not N( Obs ) => Fully observable but not controllable. Similarity Transform into Cont basis can finish the job.
if not bool_obs:
interBasis = intersection(range_cont, null_obs)
else:
print "Completely Observable system + Uncontrollable system"
new_A, new_B, new_C, KBasis = ContSpace(check_A, check_B, check_C,
False)
return KBasis, matrix_rank(
check_cont), 0, 0, matrix_rank(check_obs) - matrix_rank(check_cont)
dim_int = min(shape(interBasis))
dim_C = min(shape(range_cont))
dim_N = min(shape(null_obs))
T2 = interBasis[:, 0:dim_int]
if min(shape(T2)) == 0:
T1 = range_cont
T4 = null_obs
if matrix_rank(nC((T1, T4), 1)) == max(shape(check_A)):
return nC((T1, T4), 1), min(shape(T1)), 0, 0, min(shape(T4))
else:
T3 = nullspace((nC((T1, T4), 1)).T)
return nC((T1, T3, T4),
1), min(shape(T1)), 0, min(shape(T3)), min(shape(T4))
else:
# The system has R( Cont ) and N( Obs ) => Usual Kalman Decomposition can take place.
T1 = removeBasis(range_cont, T2)
T4 = removeBasis(null_obs, T2)
T3 = nullspace((numpy.concatenate((T1, T2, T4), 1)).T)
KBasis = numpy.concatenate((T1, T2, T3, T4), 1)
return KBasis, min(shape(T1)), min(shape(T2)), min(shape(T3)), min(
shape(T4))
def controlability( B, A ):
''' Returns the controllability matrix and also checks its rank
returns
False : if rank deficient
True : if full rank '''
start = [ B ]
order = (shape( A ))[ 0 ]
for i in range( order -1 ):
start.append( nD( A, start[ -1 ] ) )
cont = nC( start,1 )
if ( matrix_rank( cont ) < order ):
return cont, False
else :
return cont, True
def controlability(B, A):
''' Returns the controllability matrix and also checks its rank
returns
False : if rank deficient
True : if full rank '''
start = [B]
order = (shape(A))[0]
for i in range(order - 1):
start.append(nD(A, start[-1]))
cont = nC(start, 1)
if (matrix_rank(cont) < order):
return cont, False
else:
return cont, True
def observability( C , A ): ''' Checks for the observability of the system. Returns: Observability matrix True : if full rank False : if rank deficient ''' start = [ C ] order = (shape( A ))[ 0 ] for i in range( order - 1 ) : start.append( nD( start[ -1 ], A ) ) obs = nC( start , 0 ) if( matrix_rank( obs) < order ) : return obs, False else: return obs, True
def observability(C, A):
''' Checks for the observability of the system.
Returns:
Observability matrix
True : if full rank
False : if rank deficient '''
start = [C]
order = (shape(A))[0]
for i in range(order - 1):
start.append(nD(start[-1], A))
obs = nC(start, 0)
if (matrix_rank(obs) < order):
return obs, False
else:
return obs, True
def KBasisContObs( check_A, check_B, check_C ): ''' This produces the Kalman Basis needed for a kalman decomposition of the system, which is used for a minimal realization of the state space Using a different algorithm than the one previously mentioned. This algorithm checks for controllability and then moves over to observability ''' check_cont , bool_cont = controlability( check_B , check_A ) check_obs, bool_obs = observability( check_C, check_A ) range_cont = columspace( check_cont ) null_obs = nullspace( check_obs ) if bool_obs and bool_cont: print " Minimal realization already" return numpy.eye( max( shape( check_A ) ) ), max( shape( check_A ) ) , max( shape( check_A ) ),max( shape( check_A ) ),max( shape( check_A ) ) else: Cont_A, Cont_B , Cont_C , Cont_col = ContSpace( check_A, check_B, check_C,False) #print Cont_A, Cont_B , Cont_C # Stage I : Checking for Controllablity #------------------------------------- C = matrix_rank( range_cont ) ACont,BCont, CCont = Cont_A[ :C , :C] , Cont_B[ :C , : ],Cont_C[ : , :C ] AUncont, BUncont, CUncont = Cont_A[ C: , C: ] ,Cont_B[ C: , : ], Cont_C[ :, C: ] # print AUncont, BUncont, CUncont # Stage II : checking for Observability #------------------------------------- # Stage II ( a ) : Checking for Observablity of Controllable states #-------------------------------------------------------------------- check_obs , bool_obs = observability ( CCont, ACont ) ObsACont, ObsBCont, ObsCCont , ObsCont_row = ObsSpace( ACont, BCont , CCont ,False ) # Observable from Controllable #print ObsACont, ObsBCont, ObsCCont O = matrix_rank( check_obs ) AObsCont,BObsCont, CObsCont = ObsACont[ :O , :O],ObsBCont[ :O , : ] , ObsCCont[ : , :O ] # Stage II ( b ) : Checking for Observability of Uncontrollable states #--------------------------------------------------------------------- check_obs , bool_obs = observability ( CUncont, AUncont ) ObsAUncont, ObsBUncont, ObsCUncont , ObsUncont_row = ObsSpace( AUncont, BUncont , CUncont ,False ) # Observable from Uncontrollable # print ObsAUncont, ObsBUncont, ObsCUncont O = matrix_rank( check_obs ) AObsUncont, BOBsUncont, CObsUncont = ObsAUncont[ :O , :O] ,ObsBUncont[ :O , : ] , ObsCUncont[ : , :O ] # Stage III : Putting it all together, finding the basis of transformation #--------------------------------------------------------------------------- Z1 = numpy.matrix( numpy.zeros( ( shape( nI(ObsCont_row) ) [ 0 ] , shape( nI(ObsUncont_row) ) [ 1 ]) ) ) Z2 = numpy.matrix( numpy.zeros( ( shape( nI(ObsUncont_row) ) [ 0 ] , shape( nI(ObsCont_row) ) [ 1 ]) ) ) PO = nC( ( nC( (nI(ObsCont_row),Z1) ,1 ) , nC( (Z2,nI(ObsUncont_row)) ,1 ) ),0 ) Kbasis = nD( PO , nI(Cont_col) ) # Truncate output : AObsCont, BObsCont, CObsCont return Kbasis.I , max( shape( AObsCont ) ) , max( shape(BObsCont) ), max( shape( CObsCont ) )
def KBasisContObs(check_A, check_B, check_C):
''' This produces the Kalman Basis needed for a kalman decomposition of the system, which is used for a minimal realization of the state space Using a different algorithm than the one previously mentioned. This algorithm checks for controllability and then moves over to observability '''
check_cont, bool_cont = controlability(check_B, check_A)
check_obs, bool_obs = observability(check_C, check_A)
range_cont = columspace(check_cont)
null_obs = nullspace(check_obs)
if bool_obs and bool_cont:
print " Minimal realization already"
return numpy.eye(max(shape(check_A))), max(shape(check_A)), max(
shape(check_A)), max(shape(check_A)), max(shape(check_A))
else:
Cont_A, Cont_B, Cont_C, Cont_col = ContSpace(check_A, check_B, check_C,
False)
#print Cont_A, Cont_B , Cont_C
# Stage I : Checking for Controllablity
#-------------------------------------
C = matrix_rank(range_cont)
ACont, BCont, CCont = Cont_A[:C, :C], Cont_B[:C, :], Cont_C[:, :C]
AUncont, BUncont, CUncont = Cont_A[C:, C:], Cont_B[C:, :], Cont_C[:, C:]
# print AUncont, BUncont, CUncont
# Stage II : checking for Observability
#-------------------------------------
# Stage II ( a ) : Checking for Observablity of Controllable states
#--------------------------------------------------------------------
check_obs, bool_obs = observability(CCont, ACont)
ObsACont, ObsBCont, ObsCCont, ObsCont_row = ObsSpace(
ACont, BCont, CCont, False) # Observable from Controllable
#print ObsACont, ObsBCont, ObsCCont
O = matrix_rank(check_obs)
AObsCont, BObsCont, CObsCont = ObsACont[:O, :
O], ObsBCont[:O, :], ObsCCont[:, :
O]
# Stage II ( b ) : Checking for Observability of Uncontrollable states
#---------------------------------------------------------------------
check_obs, bool_obs = observability(CUncont, AUncont)
ObsAUncont, ObsBUncont, ObsCUncont, ObsUncont_row = ObsSpace(
AUncont, BUncont, CUncont, False) # Observable from Uncontrollable
# print ObsAUncont, ObsBUncont, ObsCUncont
O = matrix_rank(check_obs)
AObsUncont, BOBsUncont, CObsUncont = ObsAUncont[:O, :
O], ObsBUncont[:
O, :], ObsCUncont[:, :
O]
# Stage III : Putting it all together, finding the basis of transformation
#---------------------------------------------------------------------------
Z1 = numpy.matrix(
numpy.zeros((shape(nI(ObsCont_row))[0], shape(nI(ObsUncont_row))[1])))
Z2 = numpy.matrix(
numpy.zeros((shape(nI(ObsUncont_row))[0], shape(nI(ObsCont_row))[1])))
PO = nC((nC((nI(ObsCont_row), Z1), 1), nC((Z2, nI(ObsUncont_row)), 1)), 0)
Kbasis = nD(PO, nI(Cont_col))
# Truncate output : AObsCont, BObsCont, CObsCont
return Kbasis.I, max(shape(AObsCont)), max(shape(BObsCont)), max(
shape(CObsCont))
