Detect optimal step for minimal order Schreiber test signal

schreiber.py

import numpy as np
import scipy as sp
import scipy.linalg
from math import floor, pi

#Detect optimal step for minimal order Schreiber test signal

#Set to False to disable printing
_verbose = True

################################ Various utilities #########################
def _group_by(values, key_function, tol):
    cur_value = None
    cur_group = []
    groups = []
    for v in sorted(values, key=key_function):
        key = key_function(v)
        if cur_value is None:
            cur_value = key
        elif abs(cur_value - key) > tol:
            groups.append((cur_value, cur_group))
            cur_group = []
            cur_value = key
        cur_group.append(v)
    if cur_group:
        groups.append((cur_value, cur_group))
    return groups

def _ratapprox(x, tol):
    """Searches for the nearest rational within given tolerance, using chain fraction decomposition. x must be positive"""
    if x < tol:
        return (1,0)
    xint = floor(x)
    xfloat = x - xint
    den, num = _ratapprox(1.0/xfloat, tol/xfloat**2)
    return (round(xint)*den + num, den)
    
def _is_ratio_rational(x, y, max_den, tol):
    x=abs(x)
    y=abs(y)
    #assume 0/x and x/0 rational
    if x < tol or y < tol: return True    
    num, den = _ratapprox(x/y, tol)
    return den <= max_den and num <= max_den

def _group_by_relation(values, relation):
    """group values by an equivalence relation, represented by a function (v,v)->boolean"""
    values = list(values)
    groups = []
    while values:
        vbase = values.pop()
        cur_group = [vbase]
        rest_values = []
        for vother in values:
            if relation(vbase, vother):
                cur_group.append(vother)
            else:
                rest_values.append(vother)
        groups.append(cur_group)
        values = rest_values
    return groups

def _cont2discrete(A,b,h):
    """Convert A,b matrices of a continuous system to the matrices of discrete system,
    assuming piecewise-constant input signal interpolation"""
    #todo: use scipy.signal.cont2discrete ?

    Phi = sp.linalg.expm(A*h)

    if _verbose: print("========== step=",h)
    if _verbose: print("Phi=",Phi)
    #(e^(Ah) - I)A^(-1)b
    p = np.dot( Phi - np.eye(A.shape[0]),
                np.dot( np.linalg.pinv(A), b) )
    if _verbose: print("p'=",p.T)
    if _verbose: print("--------------")
    return Phi, p

def _ctrb(A,p,n=None):
    if n is None:
        n, _ = A.shape
    
    stack = [p]
    for _ in range(n-1):
        p = np.dot(A,p)
        stack.append(p)
        
    return np.hstack(stack)
    
def _minpoly_order(A, p, tol):
    R = _ctrb(A,p)
    if _verbose: print("R=",R)
    return np.linalg.matrix_rank(R, tol=tol)
    
################################ Main procedures #########################

def optimal_step(A,b, tol=1e-5, max_den=100):
    """find optimal Schreiber step"""
    #group roots by real value
    root_groups = _group_by(np.linalg.eigvals(A), lambda r: r.real, tol=tol)
    #it is a list of pairs: (real value, (list of roots with this real value))
    if _verbose: print(root_groups)

    possible_steps = []
    #now for each group,
    for alpha, roots in root_groups:
        if _verbose: print("Root for Re(z)=",alpha,":",roots)

        #List L from the algo
        root_pairs = [(z1,z2)
                      for i1,z1 in enumerate(roots)
                      for z2 in roots[i1+1:]]
        
        #groups = _group_by_relation(roots, lambda z1,z2: _is_rational(z1.imag, z2.imag, max_den, tol))
        if _verbose: print("Pairs:",root_pairs)

        def pairs_are_compatible(z12, w12):
            z1,z2 = z12
            w1,w2 = w12
            return _is_ratio_rational(z1.imag - z2.imag, w1.imag- w2.imag,
                                      max_den, tol)

        groups = _group_by_relation(root_pairs, pairs_are_compatible)
        if _verbose: print("Found {} mutually rational groups:".format(len(groups)))
        for grp in groups:
            h = _nullifying_multiplier([abs(z1.imag - z2.imag) for (z1, z2) in grp],
                                       tol)
            if h is not None:
                possible_steps.append(h)
    if _verbose: print("Detected these possible steps:", possible_steps)
    possible_steps = [h for (h,_) in _group_by(possible_steps, lambda x:x, tol=tol)]
    if _verbose: print("After removing near duplicates:", possible_steps)

    #Calculate minimal polynomial rank for every possible step
    
    possible_steps_with_rank = [(_minpoly_order(*_cont2discrete(A,b,h), tol=tol), h)
                                for h in possible_steps]
    possible_steps_with_rank.sort()

    if _verbose: print("Step to rank, from minimal rank:")
    for rank, h in possible_steps_with_rank:
        if _verbose: print( "    h=",h,"order=",rank)

    if possible_steps_with_rank:
        return possible_steps_with_rank[0]

def optimal_test_signal(A,b,h,rank):
    """Generate test signal of given rank, for the given step"""
    Phi, p = _cont2discrete(A,b,h)
    #construct controllability matrix only for the required rank
    if _verbose: print("rank",rank)
    R = _ctrb(Phi, p, rank+1)

    #find null space
    ns = sp.linalg.null_space(R)
    #must be an n x 1 matrix
    return ns[::-1,0].T
    
def _nullifying_multiplier( values, tol ):
    """Find step h such that all values in the list are multiples of 2Pi.
    Values must be nonnegative"""
    #remove values that are close to 0
    values = [v for v in values
              if abs(v) > tol]
    if not values:
        if _verbose: print("Weird things happened, differences are zero. Any step is good then")
        return None
    #unique list of (p/q) pairs for each pair.
    #Using set, because numerator and denominator are integers
    rational_ratios = set(_ratapprox(vi/values[0], tol) for vi in values[1:])
    if _verbose: print("Rational ratios:", rational_ratios)
    if rational_ratios:
        N = np.gcd.reduce([num for num, _ in rational_ratios])
    else:
        N = 1
    return 2.0*pi/values[0]*N
    

############################ Run a test ################################
if __name__=="__main__":
    A = np.array([[-0.1, 1,0,0],
                  [-1, -0.1, 0, 1],
                  [0,0,0,4],
                  [0,0,-4,0]])
    b = np.array([[0.0,1,0,1]]).T

    rank, h = optimal_step(A,b)

    signal = optimal_test_signal(A,b,h,rank)

    print("Optimal step:",h)
    print("Optimal signal:", signal.T)