Skip to content

Instantly share code, notes, and snippets.

@dmishin dmishin/schreiber.py

Created Nov 18, 2019
Embed
What would you like to do?
Detect optimal step for minimal order Schreiber test signal
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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.