def projection(X, Y):
rankX = rank(X)
rankY = rank(Y)
# rank, or dimension, or the original space
rankO = rankX + rankY
# check if two subspaces have the same shapes
if X.shape != Y.shape:
raise Exception('The two subspaces do not have the same shapes')
# check if O is singular
if la.det(np.hstack((X, Y))) == 0:
raise Exception('X + Y is not the direct sum of the original space')
# check whether each subspace is of full column/row rank
if rankX < min(X.shape):
raise Exception('subspace X is not of full rank')
elif rankY < min(Y.shape):
raise Exception('subspace Y is not of full rank')
# X and Y are of full column rank
elif rankX == X.shape[1] & rankY == Y.shape[1]:
return np.hstack((X, np.zeros(
(X.shape[0], rankO - rankX)))).dot(la.inv(np.hstack((X, Y))))
# X and Y are of full row rank
elif rankX == X.shape[0] & rankY == Y.shape[0]:
return np.vstack((X, np.zeros(
(rankO - rankX, X.shape[1])))).dot(la.inv(np.vstack(X, Y)))
def subspace_distance(X, Y, n, method = 'definition'):
'''
compute the distance/gap between subspaces
@param1: X, subspace
@param2: Y, subspace
@param3: n, rank/dimension of the original space
'''
# ask if two subspaces have the same dimension/rank
if rank(X) != rank(Y):
return 1 # the gap/distance between any pair of subspaces with different dimensions is one
# compute distance by its definition
if method == 'definition':
P1 = pro.orthoProjection(X, n)[0]
P2 = pro.orthoProjection(Y, n)[0]
# distance = ||P1 - P2||_2
return la.norm(P1 - P2, 2)
# compute distance by use of completement subspace
else:
# orthogonal projection onto Y's completement
P2 = pro.orthoProjection(Y, n)[1]
# find the completement of Y using orthogonal projection
completement = P2.dot(np.eye(n))
return la.norm(X.conjugate().T.dot(completement), 2)
def do_TS_GMM(self, utu, utr, b, f):
# Unrestricted case where intercepts are included
alpha = b[0:self.N]
gtu = self.set_g(utu, f)
nmom = gtu.shape[0]
d = self.set_d()
m = int(ceil(1.2 * float(self.T) ** (1.0 / 3))) # int(floor(self.T**(1.0/4.0)))
Su = self.set_S(m, gtu)
SIGMAb = self.get_varb(d, Su)
Sigmaalp = SIGMAb[0:self.N,0:self.N]
if rank(Sigmaalp,tol=1e-9)<self.N:
valu = dot(alpha.T,dot(pinv(Sigmaalp),alpha))
else:
valu = dot(alpha.T,solve(Sigmaalp,alpha))
self.TS_GMM_pval_u = squeeze(chi2.sf(valu,self.N - self.k))
# Restricted case with no intercept included
gtr = self.set_g(utr,f)
Sr = self.set_S(m,gtr)
gTr = reshape(mean(gtr,axis=1),(nmom,1))
if rank(Sr,tol=1e-9) < nmom:
valr = self.T*dot(gTr.T,dot(pinv(Sr),gTr))
else:
valr = self.T*dot(gTr.T,solve(Sr,gTr))
self.TS_GMM_pval_r = squeeze(chi2.sf(valr,self.N-self.k))
# GJ test
gTu = reshape(mean(gtu,axis=1),(nmom,1))
val = self.T*(dot(gTr.T,solve(Su,gTr)) - dot(gTu.T,solve(Su,gTu)))
self.TS_GMM_pval_3 = squeeze(chi2.sf(val,self.N-self.k))
return
def matrix_reduction(self, hatR, hatY):
"""
对测量矩阵规约，返回规约的矩阵和对应Y
:return:
"""
dotR = []
dotY = np.empty(shape=[0, 2])
C = np.empty(shape=[0, 1])
while rank(dotR) != rank(hatR):
rank_dotR = rank(dotR)
if rank_dotR == 0:
dotR = np.empty(shape=[0, hatR.shape[1]])
# 取出最小的行和
if C.shape[0] == 0:
index = list(range(hatR.shape[0]))
else:
index = list(
set(range(hatR.shape[0])).difference(set(C.flatten())))
temp_hatR = hatR[index]
k = index[np.random.choice(
np.where(
np.sum(temp_hatR, 1) == np.min(np.sum(temp_hatR, 1)))[0])]
if rank(np.row_stack((dotR, hatR[k]))) > rank_dotR:
dotR = np.row_stack((dotR, hatR[k]))
dotY = np.row_stack((dotY, hatY[k]))
C = np.row_stack((C, k))
# print(dotR, dotY)
return dotR, dotY
def GRS_test(self,alpha,Sigma):
"""
This test should be used for time series regressions
"""
if rank(Sigma,tol=1e-9) < self.N:
print ("Warning Sigma has deficient rank of ", rank(Sigma))
val = (self.T - self.N - self.k)*dot(alpha.T,dot(pinv(Sigma),alpha))/(self.N*(1 + dot(self.fm.T,solve(self.vcvfac,self.fm))))
else:
val = (self.T - self.N - self.k)*dot(alpha.T,solve(Sigma,alpha))/(self.N*(1 + dot(self.fm.T,solve(self.vcvfac,self.fm))))
return squeeze(f.sf(val,self.N,self.T-self.N-self.k))
def get_Jstat(self,alpha,sigma,corr = 1.0):
"""
J test. corr is the prefactor
"""
if rank(sigma,tol=1e-9) < self.N:
print ("Warning Sigma has deficient rank of ", rank(sigma))
val = corr*dot(alpha.T,dot(pinv(sigma),alpha))
else:
val = corr*dot(alpha.T,solve(sigma,alpha))
return chi2.sf(val,self.N-self.k), val
def kernel_distance(X, Y, n):
'''
distance with inner-product
'''
if rank(X) != X.shape[1] & rank(Y) != Y.shape[1]:
raise Exception('Please provide subspaces with full COLUMN rank')
inner = 0
for i in range(X.shape[1]):
for j in range(Y.shape[1]):
inter = inter + np.square(X[:, i].conjugate().T.dot(Y[:, j]))
distance = np.sqrt(inner)
def get_dist_point(self, uv, xy, K=2):
M = xy.shape[0]
x = xy[:, 0].reshape((-1, 1))
y = xy[:, 1].reshape((-1, 1))
tmp1 = np.hstack((x, y, np.ones((M, 1)), np.zeros((M, 1))))
tmp2 = np.hstack((y, -x, np.zeros((M, 1)), np.ones((M, 1))))
X = np.vstack((tmp1, tmp2))
u = uv[:, 0].reshape((-1, 1))
v = uv[:, 1].reshape((-1, 1))
U = np.vstack((u, v))
# X * r = U
if rank(X) >= 2 * K:
r, a, b, c = lstsq(X, U)
r = np.squeeze(r)
else:
raise Exception('cp2tform:twoUniquePointsReq')
sc = r[0]
ss = r[1]
tx = r[2]
ty = r[3]
Tinv = np.array([
[sc, -ss, 0],
[ss, sc, 0],
[tx, ty, 1]
])
T = inv(Tinv)
T[:, 2] = np.array([0, 0, 1])
T = T[:, 0:2].T
return T
def findNonreflectiveSimilarity(uv, xy):
M = xy.shape[0]
x = xy[:, 0].reshape((-1, 1)) # use reshape to keep a column vector
y = xy[:, 1].reshape((-1, 1)) # use reshape to keep a column vector
tmp1 = np.hstack((x, y, np.ones((M, 1)), np.zeros((M, 1))))
tmp2 = np.hstack((y, -x, np.zeros((M, 1)), np.ones((M, 1))))
X = np.vstack((tmp1, tmp2))
u = uv[:, 0].reshape((-1, 1)) # use reshape to keep a column vector
v = uv[:, 1].reshape((-1, 1)) # use reshape to keep a column vector
U = np.vstack((u, v))
if rank(X) >= 4: # fixed 4
r, res, rank_X, sv = lstsq(X, U, rcond=None) # X * r = U
r = np.squeeze(r)
else:
raise Exception('cp2tform:twoUniquePointsReq')
Tinv = np.array([[r[0], -r[1], 0], [r[1], r[0], 0], [r[2], r[3], 1]])
T = inv(Tinv)
T[:, 2] = np.array([0, 0, 1])
return T, Tinv
def _expdes_dist(gen, iterations, lb, ub, int_var):
"""Helper method for picking the best experimental design.
We generate iterations designs and picks the one the maximizes the
minimum distance between points. This isn't a perfect criterion, but
it will help avoid rank-defficient designs such as y=x.
:param lb: Lower bounds
:type lb: numpy.array
:param ub: Upper bounds
:type ub: numpy.array
:param int_var: Indices of integer variables.
:type int_var: numpy.array
:return: Experimental design of size num_pts x dim
:rtype: numpy.ndarray
"""
X = None
best_score = 0
for _ in range(iterations):
cand = gen() # Generate a new design
if all([x is not None for x in [lb, ub]]): # Map and round
cand = round_vars(from_unit_box(cand, lb, ub), int_var, lb, ub)
dists = cdist(cand, cand)
np.fill_diagonal(dists, np.inf) # Since these are zero
score = dists.min().min()
if score > best_score and rank(cand) == cand.shape[1]:
best_score = score
X = cand.copy()
if X is None:
raise ValueError("No valid design found, increase num_pts?")
return X
def findNonreflectiveSimilarity(uv, xy, options=None):
options = {'K': 2}
K = options['K']
M = xy.shape[0]
x = xy[:, 0].reshape((-1, 1)) # use reshape to keep a column vector
y = xy[:, 1].reshape((-1, 1)) # use reshape to keep a column vector
tmp1 = np.hstack((x, y, np.ones((M, 1)), np.zeros((M, 1))))
tmp2 = np.hstack((y, -x, np.zeros((M, 1)), np.ones((M, 1))))
X = np.vstack((tmp1, tmp2))
u = uv[:, 0].reshape((-1, 1)) # use reshape to keep a column vector
v = uv[:, 1].reshape((-1, 1)) # use reshape to keep a column vector
U = np.vstack((u, v))
# We know that X * r = U
if rank(X) >= 2 * K:
r, _, _, _ = lstsq(X, U)
r = np.squeeze(r)
else:
raise Exception('cp2tform:twoUniquePointsReq')
sc = r[0]
ss = r[1]
tx = r[2]
ty = r[3]
Tinv = np.array([[sc, -ss, 0], [ss, sc, 0], [tx, ty, 1]])
T = inv(Tinv)
T[:, 2] = np.array([0, 0, 1])
return T, Tinv
def hausdorff_distance(X, Y, n):
'''
distace between subspaces by Hausdorff's definition
'''
if rank(X) != X.shape[1] & rank(Y) != Y.shape[1]:
raise Exception('Please provide subspaces with full COLUMN rank')
inner = 0
for i in range(X.shape[1]):
for j in range(Y.shape[1]):
inner = inter + np.square(X[:, i].conjugate().T.dot(Y[:, j]))
distance = np.sqrt(np.max(rank(X), rank(Y)) - inner)
return distance
def subspace_intersection(X, Y, n):
'''
return the dimension of the intersection of two subspaces
'''
U = principal_angles(X, Y, n)[1]
V = principal_angles(X, Y, n)[2]
return rank(np.hstack(U, V))
def fmb_pval(self, alpham, vcvalpha):
if rank(vcvalpha, tol=1e-9) < self.N:
self.FMB_JS = dot(alpham.T, dot(pinv(vcvalpha), alpham))
return chi2.sf(dot(alpham.T, dot(pinv(vcvalpha), alpham)), self.N - self.k)
else:
self.FMB_JS = dot(alpham.T, solve(vcvalpha, alpham))
return chi2.sf(dot(alpham.T, solve(vcvalpha, alpham)), self.N - self.k)
def correct_ACCH(self,Lambdas,covLam,covAlp):
"""
Here we correct for the AC and CH.
"""
# Calculate the multiplicative correction factor
if rank(self.vcvfac,tol=1e-9) < self.k:
print ("Warning vcvfac has deficient rank of ", rank(self.vcvfac))
mcor = (1 + dot(Lambdas.T,dot(pinv(self.vcvfac),Lambdas)))
else:
mcor = (1 + dot(Lambdas.T,solve(self.vcvfac,Lambdas)))
# Correct the alpha and Lambda
covLamC = covLam*mcor + self.vcvfac/self.T
covAlpC = covAlp*mcor
return covLamC, covAlpC
def orthoProjection(X, n):
# check if X is a subspace of the original space
if rank(X) < n:
P = X.dot(la.inv(X.conjugate().T.dot(X))).dot(X.conjugate().T)
# return: orthogonal projection onto subspace X, orthogonal projection X's orthogonal complement subspace
return P, np.eye(P.shape[0]) - P
else:
raise Exception('not a subspace')
def evalCriteria(self, cv=3):
for i in range(self.ncut):
varSeli = ~np.isnan(self.criteria[i, :])
xi = self.x[:, varSeli]
if sum(varSeli) < self.ncomp:
regModel = LinearRegression()
else:
regModel = PLSRegression(min([self.ncomp, rank(xi)]))
cvScore = cross_val_score(regModel, xi, self.y, cv=cv)
self.featureR2[i] = np.mean(cvScore)
def lowRank_distance(A, k):
'''
distance between A and any lower rank matrix
'''
if rank(A) >= k:
raise Exception('Please provide a lower rank k')
sigma = la.svdvals(A)
# return the k+1'th singular value
return sigma[k]
def evalCriteria(self, cv=3):
# Note: small P value indicating important feature
self.featureIndex = np.argsort(self.criteria)
for i in range(self.x.shape[1]):
xi = self.x[:, self.featureIndex[:i + 1]]
if i < self.ncomp:
regModel = LinearRegression()
else:
regModel = PLSRegression(min([self.ncomp, rank(xi)]))
cvScore = cross_val_score(regModel, xi, self.y, cv=cv)
self.featureR2[i] = np.mean(cvScore)
def __init__(self, x, y, ncomp=1, nrep=500, testSize=0.2):
self.x = x
self.y = y
# The number of latent components should not be larger than any dimension size of independent matrix
self.ncomp = min([ncomp, rank(x)])
self.nrep = nrep
self.testSize = testSize
self.criteria = None
self.featureIndex = None
self.featureR2 = np.full(self.x.shape[1], np.nan)
self.selFeature = None
def mils(A, B, y, p=1):
"""
x_hat,z_hat = mils(A,B,y,p) produces p pairs of optimal solutions to
the mixed integer least squares problem min_{x,z}||y-Ax-Bz||,
where x and z are real and integer vectors, respectively.
Input arguments:
A - m by k real matrix
B - m by n real matrix
[A,B] has full column rank
y - m-dimensional real vector
p - the number of optimal solutions
Output arguments:
x_hat - k by p real matrix
z_hat - n by p integer matrix (in double precision).
The pair {x_hat(:,j),z_hat(:,j)} is the j-th optimal solution
i.e., its residual is the j-th smallest, so
||y-A*x_hat(:,1)-B*z_hat(:,1)|| <= ...
< =||y-A*x_hat(:,p)-B*z_hat(:,p)||
"""
m, k = A.shape
m2, n = B.shape
if m != m2 or m != len(y) or len(y[1]) != 1:
raise ValueError("Input arguments have a matrix dimension error!")
if rank(A) + rank(B) < k + n:
raise ValueError("hmmm...")
Q, R = qr(A, mode='complete')
Q_A = Q[:, :k]
Q_Abar = Q[:, k:]
R_A = R[:k, :]
# Compute the p optimal integer least squares solutions
z_hat = ils(dot(Q_Abar.T, B), dot(Q_Abar.T, y), p)
# Compute the corresponding real least squares solutions
x_hat = lstsq(R_A, dot(Q_A.T, (dot(y, ones((1, p))) - dot(B, z_hat))))
return x_hat, z_hat
def ils(B, y, p=1):
m, n = B.shape
if rank(B) < n:
raise ValueError("Matrix is rank deficient!")
# Reduction
R, Z_r, y_r = reduction(B.copy(), y.copy())
# Search
_z_hat = search(R.copy(), y_r[:n].copy(), p)
return dot(Z_r, _z_hat)
def find_nonreflective_similarity(uv, xy):
"""
Function:
----------
Find Non-reflective Similarity Transform Matrix 'trans'
Parameters:
----------
- uv: Kx2 np.array
source points each row is a pair of coordinates (x, y)
- xy: Kx2 np.array
each row is a pair of inverse-transformed
Returns:
- trans: 3x3 np.array
transform matrix from uv to xy
- trans_inv: 3x3 np.array
inverse of trans, transform matrix from xy to uv
"""
options = {'K': 2}
K = options['K']
M = xy.shape[0]
# use reshape to keep a column vector
x = xy[:, 0].reshape((-1, 1))
y = xy[:, 1].reshape((-1, 1))
tmp1 = np.hstack((x, y, np.ones((M, 1)), np.zeros((M, 1))))
tmp2 = np.hstack((y, -x, np.zeros((M, 1)), np.ones((M, 1))))
X = np.vstack((tmp1, tmp2))
# use reshape to keep a column vector
u = uv[:, 0].reshape((-1, 1))
v = uv[:, 1].reshape((-1, 1))
U = np.vstack((u, v))
if rank(X) >= 2 * K:
r, _, _, _ = lstsq(X, U)
r = np.squeeze(r)
else:
raise Exception('cp2tform:twoUniquePointsReq')
sc = r[0]
ss = r[1]
tx = r[2]
ty = r[3]
t_inv = np.array([
[sc, -ss, 0],
[ss, sc, 0],
[tx, ty, 1]
])
t = inv(t_inv)
t[:, 2] = np.array([0, 0, 1])
return t, t_inv
def ils(B, y, p=1):
m, n = B.shape
if rank(B) < n:
raise ValueError("Matrix is rank deficient!")
# Reduction
R, Z_r, y_r = reduction(B.copy(), y.copy())
# Search
_z_hat = search(R.copy(), y_r[:n].copy(), p)
return dot(Z_r, _z_hat)
def CS_OLS(self, beta):
"""
This function runs a cross-sectional OLS regression
"""
BB = dot(beta.T, beta)
if rank(BB, tol=1e-9) < self.k:
print ("Warning, B.T B has deficient rank of '", rank(BB))
Binv = pinv(dot(beta.T, beta))
else:
Binv = inv(dot(beta.T, beta))
Lamb = dot(Binv, dot(beta.T, self.ym))
alphas = self.ym - dot(beta, Lamb)
# Calculate covariance matrices
covLamb = dot(Binv, dot(dot(dot(beta.T, self.Sigma), beta), Binv)) / self.T
t1 = eye(self.N) - dot(beta, dot(Binv, beta.T))
covAlp = dot(dot(t1, self.Sigma), t1) / self.T
return Lamb, alphas, covLamb, covAlp
def mils(A, B, y, p=1):
# x_hat,z_hat = mils(A,B,y,p) produces p pairs of optimal solutions to
# the mixed integer least squares problem min_{x,z}||y-Ax-Bz||,
# where x and z are real and integer vectors, respectively.
#
# Input arguments:
# A - m by k real matrix
# B - m by n real matrix
# [A,B] has full column rank
# y - m-dimensional real vector
# p - the number of optimal solutions
#
# Output arguments:
# x_hat - k by p real matrix
# z_hat - n by p integer matrix (in double precision).
# The pair {x_hat(:,j),z_hat(:,j)} is the j-th optimal solution
# i.e., its residual is the j-th smallest, so
# ||y-A*x_hat(:,1)-B*z_hat(:,1)||<=...<=||y-A*x_hat(:,p)-B*z_hat(:,p)||
m, k = A.shape
m2, n = B.shape
if m != m2 or m != len(y) or len(y[1]) != 1:
raise ValueError("Input arguments have a matrix dimension error!")
if rank(A) + rank(B) < k + n:
raise ValueError("hmmm...")
Q, R = qr(A, mode="complete")
Q_A = Q[:, :k]
Q_Abar = Q[:, k:]
R_A = R[:k, :]
# Compute the p optimal integer least squares solutions
z_hat = ils(dot(Q_Abar.T, B), dot(Q_Abar.T, y), p)
# Compute the corresponding real least squares solutions
x_hat = lstsq(R_A, dot(Q_A.T, (dot(y, ones((1, p))) - dot(B, z_hat))))
return x_hat, z_hat
def CS_GLS(self,beta):
"""
This function runs a cross-sectional GLS regression
"""
if rank(self.Sigma,tol=1e-9) < self.N:
BSinv = dot(beta.T,pinv(self.Sigma))
else:
BSinv = dot(beta.T,inv(self.Sigma))
tmp = dot(BSinv,beta)
if rank(tmp,tol=1e-9) < self.k:
print ("Warning tmp has deficient rank of ", rank(tmp))
Binv = pinv(tmp)
else:
Binv = inv(tmp)
Lambdas = dot(Binv,dot(BSinv,self.ym))
alphas = self.ym - dot(beta,Lambdas)
# Calculate covariance matrices
covLam = Binv/self.T
covAlp = (self.Sigma - dot(beta,dot(Binv,beta.T)))/self.T
return Lambdas, alphas, covLam, covAlp
def ls_fast_givens(A, b):
m = A.shape[0]
n = A.shape[1]
if rank(A) < n:
raise Exception('Rank deficient')
S = qr.qr_fast_givens(A)
M^T = np.dot(S, la.inv(A))
b = M^T.dot(b)
x_ls = la.solve(S[:n, :n], b[:n])
return x_ls
def calcCriteria(self):
PLSCoef = np.zeros((self.nrep, self.x.shape[1]))
ss = ShuffleSplit(n_splits=self.nrep, test_size=self.testSize)
step = 0
for train, test in ss.split(self.x, self.y):
xtrain = self.x[train, :]
ytrain = self.y[train]
plsModel = PLSRegression(min([self.ncomp, rank(xtrain)]))
plsModel.fit(xtrain, ytrain)
PLSCoef[step, :] = plsModel.coef_.T
step += 1
meanCoef = np.mean(PLSCoef, axis=0)
stdCoef = np.std(PLSCoef, axis=0)
self.criteria = meanCoef / stdCoef
def findNonreflectiveSimilarity(uv, xy, options=None):
options = {'K': 2}
K = options['K']
M = xy.shape[0]
x = xy[:, 0].reshape((-1, 1)) # use reshape to keep a column vector
y = xy[:, 1].reshape((-1, 1)) # use reshape to keep a column vector
# print('--->x, y:\n', x, y
tmp1 = np.hstack((x, y, np.ones((M, 1)), np.zeros((M, 1))))
tmp2 = np.hstack((y, -x, np.zeros((M, 1)), np.ones((M, 1))))
X = np.vstack((tmp1, tmp2))
# print('--->X.shape: ', X.shape
# print('X:\n', X
u = uv[:, 0].reshape((-1, 1)) # use reshape to keep a column vector
v = uv[:, 1].reshape((-1, 1)) # use reshape to keep a column vector
U = np.vstack((u, v))
# print('--->U.shape: ', U.shape
# print('U:\n', U
# We know that X * r = U
if rank(X) >= 2 * K:
r, _, _, _ = lstsq(X, U, rcond=None)
r = np.squeeze(r)
else:
raise Exception("cp2tform: two Unique Points Req")
# print('--->r:\n', r
sc = r[0]
ss = r[1]
tx = r[2]
ty = r[3]
Tinv = np.array([
[sc, -ss, 0],
[ss, sc, 0],
[tx, ty, 1]
])
# print('--->Tinv:\n', Tinv
T = inv(Tinv)
# print('--->T:\n', T
T[:, 2] = np.array([0, 0, 1])
return T, Tinv
def findNonreflectiveSimilarity(uv, xy, options=None):
options = {'K': 2}
K = options['K']
M = xy.shape[0]
x = xy[:, 0].reshape((-1, 1)) # use reshape to keep a column vector
y = xy[:, 1].reshape((-1, 1)) # use reshape to keep a column vector
# print('--->x, y:\n', x, y
tmp1 = np.hstack((x, y, np.ones((M, 1)), np.zeros((M, 1))))
tmp2 = np.hstack((y, -x, np.zeros((M, 1)), np.ones((M, 1))))
X = np.vstack((tmp1, tmp2))
# print('--->X.shape: ', X.shape
# print('X:\n', X
u = uv[:, 0].reshape((-1, 1)) # use reshape to keep a column vector
v = uv[:, 1].reshape((-1, 1)) # use reshape to keep a column vector
U = np.vstack((u, v))
# print('--->U.shape: ', U.shape
# print('U:\n', U
# We know that X * r = U
if rank(X) >= 2 * K:
r, _, _, _ = lstsq(X, U)
r = np.squeeze(r)
else:
raise Exception("cp2tform: two Unique Points Req")
# print('--->r:\n', r
sc = r[0]
ss = r[1]
tx = r[2]
ty = r[3]
Tinv = np.array([
[sc, -ss, 0],
[ss, sc, 0],
[tx, ty, 1]
])
# print('--->Tinv:\n', Tinv
T = inv(Tinv)
# print('--->T:\n', T
T[:, 2] = np.array([0, 0, 1])
return T, Tinv
def find_non_reflective_similarity(uv, xy):
""" Find Non-reflective Similarity Transform Matrix 'trans':
u = uv[:, 0]
v = uv[:, 1]
x = xy[:, 0]
y = xy[:, 1]
[x, y, 1] = [u, v, 1] * trans
:param uv: Kx2 np.array, source points each row is a coordinate pair (x, y)
:param xy: Kx2 np.array, each row is a pair of inverse-transformed
:returns: trans: transform matrix from uv to xy, 3x3
type: np.array
trans_inv: inverse of trans, transform matrix from xy to uv, 3x3
type: np.array
"""
options = {"K": 2}
n_landmarks = options["K"]
n_pts = xy.shape[0]
x = xy[:, 0].reshape((-1, 1)) # use reshape to keep a column vector
y = xy[:, 1].reshape((-1, 1)) # use reshape to keep a column vector
tmp1 = np.hstack((x, y, np.ones((n_pts, 1)), np.zeros((n_pts, 1))))
tmp2 = np.hstack((y, -x, np.zeros((n_pts, 1)), np.ones((n_pts, 1))))
x_tensor = np.vstack((tmp1, tmp2))
u = uv[:, 0].reshape((-1, 1)) # use reshape to keep a column vector
v = uv[:, 1].reshape((-1, 1)) # use reshape to keep a column vector
u_matrix = np.vstack((u, v))
# We know that X * r = U (u_matrix)
if rank(x_tensor) >= 2 * n_landmarks:
r, _, _, _ = lstsq(x_tensor, u_matrix)
r = np.squeeze(r)
else:
raise Exception("points2transform:twoUniquePointsReq")
sc, ss, tx, ty = r[0], r[1], r[2], r[3]
t_inv = np.array([[sc, -ss, 0], [ss, sc, 0], [tx, ty, 1]])
inverse_tensor = inv(t_inv)
inverse_tensor[:, 2] = np.array([0, 0, 1])
return inverse_tensor, t_inv
def fullrank(X):
'''
return full-rank decomposition of X = FG^T
'''
rankX = rank(X)
U, eigvals, Vh = la.svd(X)
#construct a r-rank sigma-square-root matrix
sigma = np.eye(rankX)
for i in range(sigma.shape[0]):
sigma[i, i] = np.sqrt(eigvals[i])
F = U.dot(np.vstack((sigma, np.zeros((X.shape[0] - rankX, rankX)))))
Gh = np.hstack((sigma, np.zeros((rankX, X.shape[1] - rankX)))).dot(Vh)
return F, Gh
def TLSTDoALocate(self,RN1, RN2, TDoA, TDoAStd):
"""
This applies LS approximation on TDoA to get position P.
Return P
"""
shRN = np.shape(RN1) # shape of RN
RNnum = shRN[1] # Number of reference nodes
c = 3e08 # Speed of light
# Construct the vector K (see theory)
k1 = (np.sum((RN1-RN2)*(RN1-RN2),axis=0)).reshape(RNnum,1) # first half of K
RDoA = c*TDoA # Range of arrival (meters)
RDoA2 = (RDoA*RDoA).reshape(RNnum,1)
k2 = RDoA2 # second half of K
K = k1-k2
# Construct the matrix A (see theory)
A = np.hstack((RN1.T - RN2.T,RDoA))
A2 = np.dot(transpose(A),A)
[U,S,V] = la.svd(A2)
J = 1/S
rA = la.rank(A)
m,n = np.shape(A)
f=0
if np.log10(la.cond(A2))>=c*max(TDoAStd):
f=f+1
for i in range(n-rA):
u = np.where(J==max(J))
J[u] = 0
A2i = np.dot(np.dot(V.T,np.diag(J)),U.T)
# Apply LS operator
Pr = 0.5*np.dot(A2i,np.dot(A.T,K))
P = Pr[:shRN[0],:]
# Return the estimated position
return P
def _expdes_dist(gen, iterations, lb, ub, int_var):
"""Helper method for picking the best experimental design.
We generate iterations designs and picks the one the maximizes the
minimum distance between points. This isn't a perfect criterion, but
it will help avoid rank-defficient designs such as y=x.
:param lb: Lower bounds
:type lb: numpy.array
:param ub: Upper bounds
:type ub: numpy.array
:param int_var: Indices of integer variables.
:type int_var: numpy.array
:return: Experimental design of size num_pts x dim
:rtype: numpy.ndarray
"""
X = None
best_score = 0
for _ in range(iterations):
cand = gen() # Generate a new design
if all([x is not None for x in [lb, ub]]): # Map and round
cand = round_vars(from_unit_box(cand, lb, ub), int_var, lb, ub)
dists = cdist(cand, cand)
np.fill_diagonal(dists, np.inf) # Since these are zero
score = dists.min().min()
if score > best_score and rank(cand) == cand.shape[1]:
best_score = score
X = cand.copy()
if X is None:
raise ValueError("No valid design found, increase num_pts?")
return X
def TWLSRSSLocate(self, RN, PL0, d0, RSS, RSSnp, RSSStd, Rest):
"""
This applies WLS approximation on RSS assuming RSSStd to get position P.
Return P
"""
shRN = np.shape(RN) # shape of RN
RNnum = shRN[1] # Number of reference nodes
# Construct the vector K (see theory)
RN2 = (np.sum(RN*RN,axis=0)).reshape(RNnum,1)
k1 = RN2[1:RNnum,:]-RN2[0,0] # first half of K
RoA = self.getRange(RN, PL0, d0, RSS, RSSnp, RSSStd, Rest) # RSS based Ranges (meters)
RoAStd = self.getRangeStd(RN, PL0, d0, RSS, RSSnp, RSSStd, Rest)
RoA2 = (RoA*RoA).reshape(RNnum,1)
k2 = RoA2[0,0]-RoA2[1:RNnum,:] # second half of K
K = k1+k2
# Construct the matrix A (see theory)
A = RN[:,1:RNnum].T - RN[:,0].reshape(1,shRN[0])
# Construct the Covariance Matrix
C = la.diag((RoAStd[1:RNnum,0])**2)
A2 = np.dot(A.T,np.dot(la.inv(C),A))
[U,S,V] = la.svd(A2)
J = 1/S
rA = la.rank(A)
m,n = np.shape(A)
f=0
if np.log10(cond(A2))>=max(self.getRangeStd(RN, PL0, d0, RSS, RSSnp, RSSStd, Rest)):
f=f+1
for i in range(n-rA):
u = np.where(J==max(J))
J[u] = 0
A2i = np.dot(np.dot(V.T,la.diag(J)),U.T)
P = 0.5*np.dot(A2i,np.dot(np.dot(A.T,la.inv(C)),K))
return P
def kfdemo():
T = 1e-2
A = array([[1, T], [0, 1]])
B = array([(T**2)/2, T])[:, newaxis]
C = array([1, 0])[newaxis, :]
# C = array([[1, 0], [0, 0]])
vara = (1e+2)**2
Q = vara*B.dot(B.T)
R = (1e+2)**2
n = A.shape[0]
m = C.shape[0]
O = vstack([C, C.dot(A)])
print O
print 'Size(O) = %ix%i' % (O.shape[0], O.shape[1])
print 'Rank(O) = %i' % (rank(O))
max_iter = 2e3
t = arange(0,T*max_iter, T)
w = zeros((n, max_iter))
v = zeros((m, max_iter))
x = zeros((n, max_iter+1))
y = zeros((m, max_iter))
xhat = zeros((n, max_iter))
x[:, 0] = [1, 0.1]
Pp = diag([(1e0)**2, (1e2)**2])
xhatp = sqrt(Pp).dot(randn(2,1)) + x[:, 0][:, newaxis];
# import pdb; pdb.set_trace()
for k in range(int(max_iter)):
w[:, k] = (B * sqrt(vara) * randn()).squeeze()
v[:, k] = sqrt(R) * randn()
x[:, k+1] = (A.dot(x[:, k][:, newaxis]) + w[:, k][:, newaxis]).squeeze()
y[:, k] = (C.dot(x[:, k][:, newaxis]) + v[:, k][:, newaxis]).squeeze()
K = Pp.dot(C.T) * 1./(C.dot(Pp).dot(C.T) + R)
xhat[:, k] = (A.dot(xhatp) + K.dot(y[:, k][:, newaxis] - C.dot(xhatp))).squeeze()
import pdb; pdb.set_trace()
P = (eye(n) - K.dot(C)).dot(Pp)
xhatp = A.dot(xhat[:, k][:, newaxis])
Pp = A.dot(P).dot(A.T) + Q
fig = plt.figure()
ax1 = fig.add_subplot(221)
ax1.plot(t,x[0,0:-1], 'r')
ax1.plot(t,xhat[0,:],'b')
# ax1.plot(t,y[0,:],'g')
plt.xlabel('Time')
plt.ylabel('x_1(t) (m)')
plt.title('Red: Actual, Blue: Estimated')
# plt.ylim([0, 3])
ax2 = fig.add_subplot(222)
ax2.plot(t,x[1,0:-1], 'r')
ax2.plot(t,xhat[1,:],'b')
plt.xlabel('Time')
plt.ylabel('x_2(t) (m/s)')
plt.title('Red: Actual, Blue: Estimated')
print xhat.shape, t.shape
ax3 = fig.add_subplot(223)
ax3.plot(t,x[0,0:-1] - xhat[0,:], 'g')
plt.xlabel('Time')
plt.ylabel('$e_1(t)$ (m)')
ax4 = fig.add_subplot(224)
ax4.plot(t,x[1,0:-1] - xhat[1,:], 'g')
plt.xlabel('Time')
plt.ylabel('e_2(t) (m)')
plt.show()