def cca1(self, X, Y):
'''
正準相関分析
http://en.wikipedia.org/wiki/Canonical_correlation
'''
X = np.array(X)
Y = np.array(Y)
n, p = X.shape
n, q = Y.shape
# zero mean
X = X - X.mean(axis=0)
Y = Y - Y.mean(axis=0)
# covariances
S = np.cov(X.T, Y.T)
# S = np.corrcoef(X.T, Y.T)
SXX = S[:p,:p]
SYY = S[p:,p:]
SXY = S[:p,p:]
SYX = S[p:,:p]
#
sqx = SLA.sqrtm(SLA.inv(SXX)) # SXX^(-1/2)
sqy = SLA.sqrtm(SLA.inv(SYY)) # SYY^(-1/2)
M = np.dot(np.dot(sqx, SXY), sqy.T) # SXX^(-1/2) * SXY * SYY^(-T/2)
A, s, Bh = SLA.svd(M, full_matrices=False)
B = Bh.T
#print np.dot(np.dot(A[:,0].T,SXX),A[:,0])
return s, A, B
def get_precision(self):
"""Compute data precision matrix with the FactorAnalysis model.
Returns
-------
precision : array, shape (n_features, n_features)
Estimated precision of data.
"""
check_is_fitted(self, 'components_')
n_features = self.components_.shape[1]
# handle corner cases first
if self.n_components == 0:
return np.diag(1. / self.noise_variance_)
if self.n_components == n_features:
return linalg.inv(self.get_covariance())
# Get precision using matrix inversion lemma
components_ = self.components_
precision = np.dot(components_ / self.noise_variance_, components_.T)
precision.flat[::len(precision) + 1] += 1.
precision = np.dot(components_.T,
np.dot(linalg.inv(precision), components_))
precision /= self.noise_variance_[:, np.newaxis]
precision /= -self.noise_variance_[np.newaxis, :]
precision.flat[::len(precision) + 1] += 1. / self.noise_variance_
return precision
def build_bilinear(self, Pibra, Piket):
r"""Convert the oscillator :math:`-\overline{g_k} + g_l`
occuring in the integral :math:`\langle\phi_k\left[\Pi_k\right] | \phi_l\left[\Pi_l\right]\rangle`
into a bilinear form
:math:`g(x) = \underline{x}^{\mathrm{H}} \mathbf{A} \underline{x} + \underline{b}^{\mathrm{T}} \underline{x} + c`.
:param Pibra: The parameters :math:`\Pi_k = (\underline{q_k}, \underline{p_k}, \mathbf{Q_k}, \mathbf{P_k})` from the 'bra' packet.
:param Piket: The parameters :math:`\Pi_l = (\underline{q_l}, \underline{p_l}, \mathbf{Q_l}, \mathbf{P_l})` from the 'ket' packet.
:return: Three arrays: a matrix :math:`\mathbf{A}` of shape :math:`D \times D`,
a vector :math:`\underline{b}` of shape :math:`D \times 1` and a scalar value :math:`c`.
"""
qr, pr, Qr, Pr = Pibra[:4]
qc, pc, Qc, Pc = Piket[:4]
Gr = dot(Pr, inv(Qr))
Gc = dot(Pc, inv(Qc))
# Merge into a single oscillator
A = 0.5 * (Gc - conjugate(transpose(Gr)))
b = (0.5 * ( dot(Gr, qr)
- dot(conjugate(transpose(Gc)), qc)
+ dot(transpose(Gr), conjugate(qr))
- dot(conjugate(Gc), conjugate(qc))
)
+ (pc - conjugate(pr))
)
b = conjugate(b)
c = (0.5 * ( dot(conjugate(transpose(qc)), dot(Gc, qc))
- dot(conjugate(transpose(qr)), dot(conjugate(transpose(Gr)), qr)))
+ (dot(conjugate(transpose(qr)), pr) - dot(conjugate(transpose(pc)), qc))
)
return A, b, c
def sample_transition_within_subspace(model, state, hyperparams):
"""
MCMC iteration (Gibbs sampling) for transition matrix and covariance
within the constrained subspace
"""
# Calculate sufficient statistics
suffStats = smp.evaluate_transition_sufficient_statistics(state)
# Convert to Givens factorisation form
U,D = model.convert_to_givens_form()
# Sample a new projected transition matrix and transition covariance
rank = model.parameters['rank'][0]
nu0 = rank
Psi0 = rank*hyperparams['rPsi0']
nu,Psi,M,V = smp.hyperparam_update_degenerate_mniw_transition(
suffStats, U,
nu0,
Psi0,
hyperparams['M0'],
hyperparams['V0'])
D = la.inv(smp.sample_wishart(nu, la.inv(Psi)))
FU = smp.sample_matrix_normal(M, D, V)
# Project out
Fold = model.parameters['F']
F = smp.project_degenerate_transition_matrix(Fold, FU, U)
model.parameters['F'] = F
# Convert back to eigen-decomposition form
model.update_from_givens_form(U, D)
return model
def get_precision(self):
"""Compute data precision matrix with the generative model.
Equals the inverse of the covariance but computed with
the matrix inversion lemma for efficiency.
Returns
-------
precision : array, shape=(n_features, n_features)
Estimated precision of data.
"""
n_features = self.components_.shape[1]
# handle corner cases first
if self.n_components_ == 0:
return np.eye(n_features) / self.noise_variance_
if self.n_components_ == n_features:
return linalg.inv(self.get_covariance())
# Get precision using matrix inversion lemma
components_ = self.components_
exp_var = self.explained_variance_
if self.whiten:
components_ = components_ * np.sqrt(exp_var[:, np.newaxis])
exp_var_diff = np.maximum(exp_var - self.noise_variance_, 0.)
precision = np.dot(components_, components_.T) / self.noise_variance_
precision.flat[::len(precision) + 1] += 1. / exp_var_diff
precision = np.dot(components_.T,
np.dot(linalg.inv(precision), components_))
precision /= -(self.noise_variance_ ** 2)
precision.flat[::len(precision) + 1] += 1. / self.noise_variance_
return precision
def test_orthogonal_procrustes():
np.random.seed(1234)
for m, n in ((6, 4), (4, 4), (4, 6)):
# Sample a random target matrix.
B = np.random.randn(m, n)
# Sample a random orthogonal matrix
# by computing eigh of a sampled symmetric matrix.
X = np.random.randn(n, n)
w, V = eigh(X.T + X)
assert_allclose(inv(V), V.T)
# Compute a matrix with a known orthogonal transformation that gives B.
A = np.dot(B, V.T)
# Check that an orthogonal transformation from A to B can be recovered.
R, s = orthogonal_procrustes(A, B)
assert_allclose(inv(R), R.T)
assert_allclose(A.dot(R), B)
# Create a perturbed input matrix.
A_perturbed = A + 1e-2 * np.random.randn(m, n)
# Check that the orthogonal procrustes function can find an orthogonal
# transformation that is better than the orthogonal transformation
# computed from the original input matrix.
R_prime, s = orthogonal_procrustes(A_perturbed, B)
assert_allclose(inv(R_prime), R_prime.T)
# Compute the naive and optimal transformations of the perturbed input.
naive_approx = A_perturbed.dot(R)
optim_approx = A_perturbed.dot(R_prime)
# Compute the Frobenius norm errors of the matrix approximations.
naive_approx_error = norm(naive_approx - B, ord='fro')
optim_approx_error = norm(optim_approx - B, ord='fro')
# Check that the orthogonal Procrustes approximation is better.
assert_array_less(optim_approx_error, naive_approx_error)
def mix_parameters(self, Pibra, Piket):
r"""Mix the two parameter sets :math:`\Pi_i` and :math:`\Pi_j`
from the 'bra' and the 'ket' wavepackets :math:`\Phi\left[\Pi_i\right]`
and :math:`\Phi^\prime\left[\Pi_j\right]`.
:param Pibra: The parameter set :math:`\Pi_i` from the bra part wavepacket.
:param Piket: The parameter set :math:`\Pi_j` from the ket part wavepacket.
:return: The mixed parameters :math:`q_0` and :math:`Q_S`. (See the theory for details.)
"""
# <Pibra | ... | Piket>
qr, pr, Qr, Pr = Pibra
qc, pc, Qc, Pc = Piket
# Mix the parameters
Gr = dot(Pr, inv(Qr))
Gc = dot(Pc, inv(Qc))
r = imag(Gc - conjugate(Gr.T))
s = imag(dot(Gc, qc) - dot(conjugate(Gr.T), qr))
q0 = dot(inv(r), s)
Q0 = 0.5 * r
# Here we can not avoid the matrix root by using svd
Qs = inv(sqrtm(Q0))
return (q0, Qs)
def sample_transition_full_rank(model, state, hyperparams, pseudo_dof=None, pseudo_sd=None):
"""
MCMC iteration (Gibbs sampling) for transition matrix and covariance
with full rank model
"""
Q = model.transition_covariance()
# Sampke a pseudo-observation to constrain the size of move
if pseudo_dof is not None:
extra_nu = pseudo_dof
extra_Psi = smp.sample_wishart(pseudo_dof, Q)
else:
extra_nu = 0
extra_Psi = 0
# Calculate sufficient statistics
suffStats = smp.evaluate_transition_sufficient_statistics(state)
# Sample a new projected transition matrix and transition covariance
nu0 = model.ds + extra_nu
Psi0 = model.ds*hyperparams['rPsi0'] + extra_Psi
nu,Psi,M,V = smp.hyperparam_update_basic_mniw_transition(
suffStats,
nu0,
Psi0,
hyperparams['M0'],
hyperparams['V0'])
Q = la.inv(smp.sample_wishart(nu, la.inv(Psi)))
F = smp.sample_matrix_normal(M, Q, V)
model.parameters['F'] = F
model.parameters['Q']= Q
return model
def RImat(WI, mx):
""" R matrix
Parameters
----------
WI : numpy 3d array
array of inverted interaction arrays, as returned from WImat
mx: int
matching point in inward and outward solutions (bound states only)
Returns
-------
RI : numpy 3d array
R matrix of the Johnson method
"""
oo, n, m = WI.shape
I = np.identity(n)
RI = np.zeros_like(WI)
U = 12*WI-I*10
for i in range(1, mx+1):
RI[i] = linalg.inv(U[i]-RI[i-1])
for i in range(oo-2, mx, -1):
RI[i] = linalg.inv(U[i]-RI[i+1])
return RI
def oom_spectral_analysis(oom_dict):
nstates=oom_dict['Xi_set'].shape[0]
order=oom_dict['sigma'].shape[0]
Xi_0=oom_dict['Xi_set'].sum(0)
v,w=linalg.eig(Xi_0)
v=np.real(v)
w=np.real(w)
assert(np.linalg.matrix_rank(Xi_0) == order), 'The sum of all observable operators is singular.'
assert(np.allclose(w.dot(np.diag(v)).dot(linalg.inv(w)),Xi_0)), 'The sum of all observable operators is not diagonalizable.'
inv_V=linalg.inv(np.diag(v))
inv_w=linalg.inv(w)
tmp_Q_1=np.empty((nstates,order))
tmp_Q_2=np.empty((nstates,order))
for k in range(nstates):
tmp_Q_1[k]=(oom_dict['sigma'].T.dot(oom_dict['Xi_set'][k].T).dot(inv_w.T).dot(inv_V)).reshape(-1)
tmp_Q_2[k]=(oom_dict['omega'].dot(oom_dict['Xi_set'][k]).dot(w)).reshape(-1)
eigen_vectors=0.5*(np.sign(tmp_Q_1)+np.sign(tmp_Q_2))*np.sqrt(np.maximum(tmp_Q_1*tmp_Q_2,0.0));
idx=(-v).argsort()
eigen_values=v[idx]
eigen_vectors=eigen_vectors[:,idx]
if eigen_vectors[:,0].sum()<0:
eigen_vectors[:,0]=-eigen_vectors[:,0]
return eigen_values,eigen_vectors
def sample_transition_covariance_within_subspace(model, state, hyperparams, pseudo_dof=None):
"""
MCMC iteration (Gibbs sampling) for transition matrix and covariance
within the constrained subspace
"""
# Calculate sufficient statistics
suffStats = smp.evaluate_transition_sufficient_statistics(state)
# Convert to Givens factorisation form
U,D = model.convert_to_givens_form()
# Sampke a pseudo-observation to constrain the size of move
if pseudo_dof is not None:
extra_nu = pseudo_dof
extra_Psi = smp.sample_wishart(pseudo_dof, D)
else:
extra_nu = 0
extra_Psi = 0
# Sample a new projected transition matrix and transition covariance
rank = model.parameters['rank'][0]
nu0 = rank + extra_nu
Psi0 = rank*hyperparams['rPsi0'] + np.dot(U, np.dot(extra_Psi, U.T))
nu,Psi = smp.hyperparam_update_degenerate_iw_transition_covariance(
suffStats, U,
model.parameters['F'],
nu0,
Psi0)
D = la.inv(smp.sample_wishart(nu, la.inv(Psi)))
# Convert back to eigen-decomposition form
model.update_from_givens_form(U, D)
return model
def geigen(Amat, Bmat, Cmat):
"""
generalized eigenvalue problem of the form
max tr L'AM / sqrt(tr L'BL tr M'CM) w.r.t. L and M
:param Amat numpy ndarray of shape (M,N)
:param Bmat numpy ndarray of shape (M,N)
:param Bmat numpy ndarray of shape (M,N)
:rtype: numpy ndarray
:return values: eigenvalues
:return Lmat: left eigenvectors
:return Mmat: right eigenvectors
"""
if Bmat.shape[0] != Bmat.shape[1]:
print("BMAT is not square.\n")
sys.exit(1)
if Cmat.shape[0] != Cmat.shape[1]:
print("CMAT is not square.\n")
sys.exit(1)
p = Bmat.shape[0]
q = Cmat.shape[0]
s = min(p, q)
tmp = fabs(Bmat - Bmat.transpose())
tmp1 = fabs(Bmat)
if tmp.max() / tmp1.max() > 1e-10:
print("BMAT not symmetric..\n")
sys.exit(1)
tmp = fabs(Cmat - Cmat.transpose())
tmp1 = fabs(Cmat)
if tmp.max() / tmp1.max() > 1e-10:
print("CMAT not symmetric..\n")
sys.exit(1)
Bmat = (Bmat + Bmat.transpose()) / 2.
Cmat = (Cmat + Cmat.transpose()) / 2.
Bfac = cholesky(Bmat)
Cfac = cholesky(Cmat)
Bfacinv = inv(Bfac)
Bfacinvt = Bfacinv.transpose()
Cfacinv = inv(Cfac)
Dmat = Bfacinvt.dot(Amat).dot(Cfacinv)
if p >= q:
u, d, v = svd(Dmat)
values = d
Lmat = Bfacinv.dot(u)
Mmat = Cfacinv.dot(v.transpose())
else:
u, d, v = svd(Dmat.transpose())
values = d
Lmat = Bfacinv.dot(u)
Mmat = Cfacinv.dot(v.transpose())
return values, Lmat, Mmat
def write_ctf_comp(fid, comps):
"""Write the CTF compensation data into a fif file
Parameters
----------
fid: file
The open FIF file descriptor
comps: list
The compensation data to write
"""
if len(comps) <= 0:
return
# This is very simple in fact
start_block(fid, FIFF.FIFFB_MNE_CTF_COMP)
for comp in comps:
start_block(fid, FIFF.FIFFB_MNE_CTF_COMP_DATA)
# Write the compensation kind
write_int(fid, FIFF.FIFF_MNE_CTF_COMP_KIND, comp['ctfkind'])
write_int(fid, FIFF.FIFF_MNE_CTF_COMP_CALIBRATED,
comp['save_calibrated'])
# Write an uncalibrated or calibrated matrix
import pdb; pdb.set_trace()
comp['data']['data'] = linalg.inv(
np.dot(np.diag(comp['rowcals'].ravel())),
np.dot(comp.data.data,
linalg.inv(np.diag(comp.colcals.ravel()))))
write_named_matrix(fid, FIFF.FIFF_MNE_CTF_COMP_DATA, comp['data'])
end_block(fid, FIFF.FIFFB_MNE_CTF_COMP_DATA)
end_block(fid, FIFF.FIFFB_MNE_CTF_COMP)
def __init__(self, pos, neg, k=5):
self.k = k
pos = pos.T
neg = neg.T
totals = pos.sum(axis=1)
popular = numpy.argsort(totals, axis=0)[::-1, :][1 : 1 + k, :]
popular = numpy.array(popular.T)[0]
self.popular = popular
pos = pos[popular, :].todense()
neg = neg[popular, :].todense()
self.posmu = pos.mean(axis=1)
self.negmu = neg.mean(axis=1)
p = pos - self.posmu
n = neg - self.negmu
self.pcov = p * p.T / p.shape[1]
self.ncov = n * n.T / n.shape[1]
self.pdet = dlinalg.det(self.pcov)
self.ndet = dlinalg.det(self.ncov)
assert self.pdet != 0
assert self.ndet != 0
self.picov = dlinalg.inv(self.pcov)
self.nicov = dlinalg.inv(self.ncov)
def dataNorm(self):
SXX = np.cov(self.X)
U, l, Ut = LA.svd(SXX, full_matrices=True)
H = np.dot(LA.sqrtm(LA.inv(np.diag(l))),Ut)
self.nX = np.dot(H,self.X)
#print np.cov(self.nX)
#print "mean:"
#print np.mean(self.nX)
SYY = np.cov(self.Y)
U, l, Ut = LA.svd(SYY, full_matrices=True)
H = np.dot(LA.sqrtm(LA.inv(np.diag(l))),Ut)
#print "H"
#print H
self.nY = np.dot(H,self.Y)
#print np.cov(self.nY)
print "dataNorm_X:"
for i in range(len(self.nX)):
print(self.nX[i])
print("---")
print "dataNorm_Y:"
for i in range(len(self.nY)):
print(self.nY[i])
print("---")
def cohenpid(A,B,C,kp):
m_t = 0.2
Astate = [[B,A,1],[-1,0,0],[0,-1,0]]
Avar = [[-C,0,0],[0,-1,0],[0,0,-1]]
forcingfunc = [[kp],[0],[0]]
Amat = np.dot(linalg.inv(Astate),Avar)
Bmat = np.dot(linalg.inv(Astate),forcingfunc)
Xo = -1*np.dot(linalg.inv(Amat),Bmat*m_t)
tfinal = 10# simulation period
dt = 0.005
t = np.arange(0, tfinal, dt)
entries = len(t)
x = np.zeros((entries,1))
Astep = m_t
Bv = kp*Astep
for i in range(0,entries):
X = np.dot(1 - linalg.expm2(Amat*t[i]), Xo)
x[i] = X[0]
t0 = 0
for j in range(0,entries-1):
if np.sign(0-x[j])==0:
t0 = t[j]
if np.sign((0.5*Bv) - x[j]) != np.sign((0.5*Bv) -x[j+1]):
t2 = np.interp(0.5*Bv,[x[j][0],x[j+1][0]],[t[j],t[j+1]])
if np.sign((0.632*Bv) - x[j]) != np.sign((0.632*Bv) -x[j+1]):
t3 = np.interp(0.632*Bv,[x[j][0],x[j+1][0]],[t[j],t[j+1]])
t1 = (t2 - (np.log(2))*t3)/(1 - np.log(2))
tau = t3-t1
tdel = t1- t0
K= Bv/Astep
r = tdel/tau
kccc = (1/(r*K))*(0.9 + (r/12))
ticc = tdel*(30 + (3*r))/(9 + (20*r))
return kccc,ticc
def __init__(self, rng, n_samples=500, n_components=2, n_features=2,
scale=50):
self.n_samples = n_samples
self.n_components = n_components
self.n_features = n_features
self.weights = rng.rand(n_components)
self.weights = self.weights / self.weights.sum()
self.means = rng.rand(n_components, n_features) * scale
self.covariances = {
'spherical': .5 + rng.rand(n_components),
'diag': (.5 + rng.rand(n_components, n_features)) ** 2,
'tied': make_spd_matrix(n_features, random_state=rng),
'full': np.array([
make_spd_matrix(n_features, random_state=rng) * .5
for _ in range(n_components)])}
self.precisions = {
'spherical': 1. / self.covariances['spherical'],
'diag': 1. / self.covariances['diag'],
'tied': linalg.inv(self.covariances['tied']),
'full': np.array([linalg.inv(covariance)
for covariance in self.covariances['full']])}
self.X = dict(zip(COVARIANCE_TYPE, [generate_data(
n_samples, n_features, self.weights, self.means, self.covariances,
covar_type) for covar_type in COVARIANCE_TYPE]))
self.Y = np.hstack([np.full(int(np.round(w * n_samples)), k,
dtype=np.int)
for k, w in enumerate(self.weights)])
def simulate_matrices(A, B, C, kp, kc, ti):
firstrow =[(ti/(kp*kc)*(C + (kp*kc))), (ti/(kp*kc))*B, (ti/(kp*kc))*A, (ti/(kp*kc))]
mat = [firstrow,[-1 ,0,0,0],[0,-1,0,0],[0, 0 ,-1,0]]
Amat = -1*linalg.inv(mat)
Bmat = np.dot(linalg.inv(mat),[[1],[0] ,[0],[0]])
return Amat, Bmat
def CalcXY2GPSParam_2p(x1,x2,g1,g2,K=[0,0]): # Kx = dLng/dx; Ky = dlat/dy; # In China: # Kx = (133.4-1.2*lat)*1e3 # Ky = (110.2+0.002*lat)*1e3 X1 = array(x1) Y1 = array(g1) X2 = array(x2) Y2 = array(g2) detX = X2-X1 detY = Y2-Y1 lat = Y1[1] if K[0] == 0: Kx = (133.4-1.2*lat)*1e3 Ky = (110.2+0.002*lat)*1e3 K = array([Kx,Ky]) else: Kx = K[0] Ky = K[1] detKY = detY*K alpha = myArctan(detX[0],detX[1]) - myArctan(detKY[0],detKY[1]) A = array([[sp.cos(alpha),sp.sin(alpha)],[-sp.sin(alpha),sp.cos(alpha)]]) X01 = X1 - dot(linalg.inv(A),Y1*K) X02 = X2 - dot(linalg.inv(A),Y2*K) X0 = (X01+X02) /2 return A,X0,K
def test_simple(self):
a = [[1, 2], [3, 4]]
a_inv = inv(a)
assert_array_almost_equal(dot(a, a_inv), [[1, 0], [0, 1]])
a = [[1, 2, 3], [4, 5, 6], [7, 8, 10]]
a_inv = inv(a)
assert_array_almost_equal(dot(a, a_inv), [[1, 0, 0], [0, 1, 0], [0, 0, 1]])
def update(self):
"""Update basis and mixture matrix based on Euclidean distance multiplicative update rules."""
# self.H = multiply(
# self.H, elop(dot(self.W.T, self.V), dot(self.W.T, dot(self.W, self.H)), div))
# self.W = multiply(
# self.W, elop(dot(self.V, self.H.T), dot(self.W, dot(self.H, self.H.T)), div))
rank = self.H.shape[0]
self.W = self.W.tolil()
for row, cols in self.row_to_cols.items():
Hsub = self.H.tocsc()[:,cols]
E = sp.identity(rank, dtype=np.float64)
A = dot(Hsub, Hsub.T) + self.lambda_var * len(cols) * E
Ainv = sp.csr_matrix(li.inv(A.toarray()), dtype=np.float64)
Vsub = self.V.tocsr()[[row],:].tocsc()[:,cols]
self.W[row,:] = dot(dot(Ainv, Hsub), Vsub.T).T
self.W = self.W.tocsr()
self.H = self.H.tolil()
for col, rows in self.col_to_rows.items():
Wsub = self.W.tocsr()[rows,:]
E = sp.identity(rank, dtype=np.float64)
A = dot(Wsub.T, Wsub) + self.lambda_var * len(rows) * E
Ainv = sp.csr_matrix(li.inv(A.toarray()), dtype=np.float64)
Vsub = self.V.tocsr()[rows,:].tocsc()[:,[col]]
self.H[:,col] = dot(dot(Ainv, Wsub.T), Vsub)
self.H = self.H.tocsr()
def computeNHomographies(L, refIndex, blurDescriptor=0.5, radiusDescriptor=4):
"""H_list: a list of Homorgraphy relative to L[refIndex]"""
number_of_images = len(L)
homographies = []
for i in xrange(number_of_images - 1):
features1 = computeFeatures(L[i], HarrisCorners(L[i]), blurDescriptor, radiusDescriptor)
features2 = computeFeatures(L[i + 1], HarrisCorners(L[i + 1]), blurDescriptor, radiusDescriptor)
correspondences = findCorrespondences(features1, features2)
homographies.append(RANSAC(correspondences)[0])
compoundHomographies = [np.zeros([3, 3])] * number_of_images
for i in xrange(number_of_images):
if i < refIndex:
compound = np.dot(np.identity(3), homographies[i])
for j in xrange(i + 1, refIndex):
compound = np.dot(compound, homographies[j])
compoundHomographies[i] = compound
elif i > refIndex:
compound = np.dot(linalg.inv(np.identity(3)), linalg.inv(homographies[i - 1]))
for j in xrange(i - 2, refIndex - 1, -1):
compound = np.dot(compound, linalg.inv(homographies[j]))
compoundHomographies[i] = compound
else:
compoundHomographies[i] = np.identity(3)
return compoundHomographies
def update_matrices(self):
if hasattr(self,"dataImg"):
mScale = self._stack_scale_mat()
invM = inv(np.dot(self.modelView,mScale))
self.dev.writeBuffer(self.invMBuf,invM.flatten().astype(np.float32))
invP = inv(self.projection)
self.dev.writeBuffer(self.invPBuf,invP.flatten().astype(np.float32))
def kcca(self, X, Y, kernel_x=gaussian_kernel, kernel_y=gaussian_kernel, eta=1.0):
n, p = X.shape
n, q = Y.shape
Kx = DIST.squareform(DIST.pdist(X, kernel_x))
Ky = DIST.squareform(DIST.pdist(Y, kernel_y))
J = np.eye(n) - np.ones((n, n)) / n
M = np.dot(np.dot(Kx.T, J), Ky) / n
L = np.dot(np.dot(Kx.T, J), Kx) / n + eta * Kx
N = np.dot(np.dot(Ky.T, J), Ky) / n + eta * Ky
sqx = SLA.sqrtm(SLA.inv(L))
sqy = SLA.sqrtm(SLA.inv(N))
a = np.dot(np.dot(sqx, M), sqy.T)
A, s, Bh = SLA.svd(a, full_matrices=False)
B = Bh.T
# U = np.dot(np.dot(A.T, sqx), X).T
# V = np.dot(np.dot(B.T, sqy), Y).T
print s.shape
print A.shape
print B.shape
return s, A, B
def get_corr_pred(self, sctx, eps_app_eng, d_eps, tn, tn1):
'''
Corrector predictor computation.
@param eps_app_eng input variable - engineering strain
'''
delta_gamma = 0.
if sctx.update_state_on:
# print "in us"
eps_n = eps_app_eng - d_eps
sigma, f_trial, epsilon_p, q_1, q_2 = self._get_state_variables(
sctx, eps_n)
sctx.mats_state_array[:3] = epsilon_p
sctx.mats_state_array[3] = q_1
sctx.mats_state_array[4:] = q_2
diff1s = zeros([3])
sigma, f_trial, epsilon_p, q_1, q_2 = self._get_state_variables(
sctx, eps_app_eng)
# Note: the state variables are not needed here, just gamma
diff2ss = self.yf.get_diff2ss(eps_app_eng, self.E, self.nu, sctx)
Xi_mtx = inv(inv(self.D_el) + delta_gamma * diff2ss * f_trial)
N_mtx_denom = sqrt(dot(dot(diff1s, Xi_mtx), diff1s))
if N_mtx_denom == 0.:
N_mtx = zeros(3)
else:
N_mtx = dot(Xi_mtx, self.diff1s) / N_mtx_denom
D_mtx = Xi_mtx - vdot(N_mtx, N_mtx)
# print "sigma ",sigma
# print "D_mtx ",D_mtx
return sigma, D_mtx
def propose(self):
ee = np.asarray(self.Y.value) - np.asarray(self.muY.value)
H = (np.exp(self.LH.value)**(-0.5))[1:]
K = np.asarray(self.Y.value).shape[1]
b_new = np.empty_like(self.stochastic.value)
# auxiliary variables to pick the right subvector/submatrix for the equations
lb = 0
ub = 1
for j in range(1, K):
z = np.expand_dims(H[:, j], 1)*np.expand_dims(ee[:, j], 1) # LHS variable in the regression
Z = np.expand_dims(-H[:, j], 1)*ee[:, :j] # RHS variables in the regression
b_prior = np.asarray([self.b_bar[lb:ub]])
Vinv_prior = inv(self.Pb_bar[lb:ub, lb:ub])
V_post = inv(Vinv_prior + Z.T @ Z)
b_post = V_post @ (Vinv_prior @ b_prior.T + Z.T @ z)
b_new[lb:ub] = pm.rmv_normal_cov(b_post.ravel(), V_post)
lb = ub
ub += j+1
self.stochastic.value = b_new
def __init__(self,data,cov_matrix=False,loc=None):
"""Parameters
----------
data : array of data, shape=(number points,number dim)
If cov_matrix is True then data is the covariance matrix (see below)
Keywords
--------
cov_matrix : bool (optional)
If True data is treated as a covariance matrix with shape=(number dim, number dim)
loc : the mean of the data if a covarinace matrix is given, shape=(number dim)
"""
if cov_matrix:
self.dim=data.shape[0]
self.n=None
self.data_t=None
self.mu=loc
self.evec,eval,V=sl.svd(data,full_matrices=False)
self.sigma=sqrt(eval)
self.Sigma=diag(1./self.sigma)
self.B=dot(self.evec,self.Sigma)
self.Binv=sl.inv(self.B)
else:
self.n,self.dim=data.shape #the shape of input data
self.mu=data.mean(axis=0) #the mean of the data
self.data_t=data-self.mu #remove the mean
self.evec,eval,V=sl.svd(self.data_t.T,full_matrices=False) #get the eigenvectors (axes of the ellipsoid)
data_p=dot(self.data_t,self.evec) #project the data onto the eigenvectors
self.sigma=data_p.std(axis=0) #get the spread of the distribution (the axis ratos for the ellipsoid)
self.Sigma=diag(1./self.sigma) #the eigenvalue matrix for the ellipsoid equation
self.B=dot(self.evec,self.Sigma) #used in the ellipsoid equation
self.Binv=sl.inv(self.B) #also useful to have around
def gp_likelihood_cplt2(self, target, D_nm, D_mm, logtheta, logdelta, rglr):
y = target
Len = len(y)
Len_rr = len(D_mm[0])
num_of_feature = len(logdelta)
theta = np.exp(logtheta)
delta = []
for i in range(0, num_of_feature):
delta.append(np.exp(logdelta[i]))
K_nm = GaussOperation.kernel_gauss_gp_cplt(D_nm, theta, delta)
K_mm = GaussOperation.kernel_gauss_gp_cplt(D_mm, theta, delta)
I = np.identity(Len, dtype=np.float)
I_rr = np.identity(Len_rr, dtype=np.float)
L = cholesky(K_mm + rglr * I_rr)
inv_L = inv(L)
Q = np.dot(K_nm, np.transpose(inv_L))
inv_Q = np.real(inv(rglr * I_rr + np.dot(np.transpose(Q), Q)))
inv_K = 1 / rglr * I - 1 / rglr * np.dot(Q, np.dot(inv_Q, np.transpose(Q)))
part1 = 0.5 * np.dot(np.transpose(y), np.dot(inv_K, y))
part2 = 0.5 * Len * np.log(rglr) + 0.5 * np.log(det(I_rr + 1 / rglr * np.dot(np.transpose(Q), Q)))
part3 = Len / 2 * np.log(2 * pi)
LLH = part1 + part2 + part3
return LLH
def kernel_embedding_model(self, yita, alpha, Beta, K_tp, K_ts, K_tt, A, R, logtheta, logeta, Len_sr, lda):
I_sr = np.identity(Len_sr, dtype = float)
L = cholesky((K_tt + lda * I_sr))
inv_L = inv(L).transpose()
Q = np.dot(np.transpose(K_ts), inv_L)
Pi = inv( np.dot(np.transpose(Q), Q) + lda * I_sr)
H = np.dot( K_tp, Q)
BLK = np.dot(K_tp,Beta)-K_ts
Delta = R + np.dot(np.transpose(BLK),alpha)
W = np.exp(Delta/yita)
C = sum(W[:,0])/len(W[:,0])
W = W/C
Lambda = np.diag(W[:,0])
#Lambda = np.identity(len(W[:,0]),dtype = float)
Gamma = np.dot( np.dot(K_ts, Lambda), np.transpose(K_ts))
X = inv(Gamma + lda * K_tt + lda**2 * I_sr)
Y = np.dot(K_ts, A)
M = np.dot(X, Y)
return inv_L, Q, Pi, H, Lambda, Gamma, X, Y, M, C
def _B2formula(Ac, t1, t2, B2):
if t1 == 0 and t2 == 0:
term = B2
return term
n = Ac.shape[0]
tmp = np.eye(n) - expm(-Ac)
if cond(tmp) < 1000000.0:
term = np.dot(((expm(-Ac * t1) - expm(-Ac * t2)) * inv(tmp)), B2)
return term
# Numerical trouble. Perturb slightly and check the result
ntry = 0
k = np.sqrt(eps)
Ac0 = Ac
while ntry < 2:
Ac = Ac0 + k * rand(n, n)
tmp = np.eye(n) - expm(-Ac)
if cond(tmp) < 1 / np.sqrt(eps):
ntry = ntry + 1
if ntry == 1:
term = np.dot(np.dot(expm(-Ac * t1) - expm(-Ac * t2), inv(tmp)), B2)
else:
term1 = np.dot(np.dot(expm(-Ac * t1) - expm(-Ac * t2), inv(tmp)), B2)
k = k * np.sqrt(2)
if norm(term1 - term) > 0.001:
warn("Inaccurate calculation in mapCtoD.")
return term
def update(self, z, R=None, H=None):
"""
Add a new measurement (z) to the Kalman filter. If z is None, nothing
is changed.
Parameters
----------
z : np.array
measurement for this update. z can be a scalar if dim_z is 1,
otherwise it must be a column vector.
R : np.array, scalar, or None
Optionally provide R to override the measurement noise for this
one call, otherwise self.R will be used.
H : np.array, or None
Optionally provide H to override the measurement function for this
one call, otherwise self.H will be used.
"""
if z is None:
return
if R is None:
R = self.R
elif isscalar(R):
R = eye(self.dim_z) * R
# rename for readability and a tiny extra bit of speed
if H is None:
H = self.H
P = self.P
x = self.x
# handle special case: if z is in form [[z]] but x is not a column
# vector dimensions will not match
if x.ndim == 1 and shape(z) == (1, 1):
z = z[0]
if shape(z) == (): # is it scalar, e.g. z=3 or z=np.array(3)
z = np.asarray([z])
# y = z - Hx
# error (residual) between measurement and prediction
self.y = z - dot(H, x)
# common subexpression for speed
PHT = dot(P, H.T)
# S = HPH' + R
# project system uncertainty into measurement space
self.S = dot(H, PHT) + R
# K = PH'inv(S)
# map system uncertainty into kalman gain
self.K = PHT.dot(linalg.inv(self.S))
# x = x + Ky
# predict new x with residual scaled by the kalman gain
self.x = x + dot(self.K, self.y)
# P = (I-KH)P(I-KH)' + KRK'
I_KH = self.I - dot(self.K, H)
self.P = dot(I_KH, P).dot(I_KH.T) + dot(self.K, R).dot(self.K.T)
def runSampler(X, Y, A, C, yerr, nburn, nsamples, parsigma, mbrange):
'''Runs the MCMC sampler, and returns the summary quantities that will
be plotted:
'''
#Now compute the best fit and the uncertainties
bestfit = sc.dot(linalg.inv(C), Y.T)
bestfit = sc.dot(A.T, bestfit)
bestfitvar = sc.dot(linalg.inv(C), A)
bestfitvar = sc.dot(A.T, bestfitvar)
bestfitvar = linalg.inv(bestfitvar)
bestfit = sc.dot(bestfitvar, bestfit)
initialguess = sc.array(
[bestfit[0], bestfit[1], 0.,
sc.mean(Y), m.log(sc.var(Y))]) #(m,b,Pb,Yb,Vb)
#With this initial guess start off the sampling procedure
initialX = objective(initialguess, X, Y, yerr)
currentX = initialX
bestX = initialX
bestfit = initialguess
currentguess = initialguess
naccept = 0
samples = []
samples.append(currentguess)
for jj in range(nburn + nsamples):
#Draw a sample from the proposal distribution
newsample = sc.zeros(5)
newsample[0] = currentguess[0] + stats.norm.rvs() * parsigma[0]
newsample[1] = currentguess[1] + stats.norm.rvs() * parsigma[1]
#newsample[2]= stats.uniform.rvs()#Sample from prior
newsample[2] = currentguess[2] + stats.norm.rvs() * parsigma[2]
newsample[3] = currentguess[3] + stats.norm.rvs() * parsigma[3]
newsample[4] = currentguess[4] + stats.norm.rvs() * parsigma[4]
#Calculate the objective function for the newsample
newX = objective(newsample, X, Y, yerr)
#Accept or reject
#Reject with the appropriate probability
u = stats.uniform.rvs()
if u < m.exp(newX - currentX):
#Accept
currentX = newX
currentguess = newsample
naccept = naccept + 1
if currentX > bestX:
bestfit = currentguess
bestX = currentX
samples.append(currentguess)
if double(naccept) / (nburn + nsamples) < .5 or double(naccept) / (
nburn + nsamples) > .8:
print "Acceptance ratio was " + str(
double(naccept) / (nburn + nsamples))
samples = sc.array(samples).T[:, nburn:-1]
print "Best-fit, overall"
print bestfit, sc.mean(samples[2, :]), sc.median(samples[2, :])
histmb, edges = sc.histogramdd(samples.T[:, 0:2],
bins=round(sc.sqrt(nsamples) / 5.),
range=mbrange)
mbsamples = []
for ii in range(10):
#Random sample
ransample = sc.floor((stats.uniform.rvs() * nsamples))
ransample = samples.T[ransample, 0:2]
bestb = ransample[0]
bestm = ransample[1]
mbsamples.append((bestm, bestb))
(pbhist, pbedges) = histogram(samples[2, :],
bins=round(sc.sqrt(nsamples) / 5.),
range=[0, 1])
return (histmb, edges, mbsamples, pbhist, pbedges)
p[:, (m * i) + 1],
p[:, (m * i) + 2],
c=agentcolor[i])
ax.scatter(p[0, m * i],
p[0, (m * i) + 1],
p[0, (m * i) + 2],
c=agentcolor[i],
marker='x')
ax.scatter(p[-1, m * i],
p[-1, (m * i) + 1],
p[-1, (m * i) + 2],
c=agentcolor[i])
plt.show()
z_star = Bb.T.dot(p_star)
z_tilde_star = la.inv(Bb.T.dot(Dx).dot(Bb)).dot(Bb.T).dot(Bb).dot(z_star)
M_breve = -(Dx.dot(Bb) -
Bb.dot(la.inv(Bb.T.dot(Bb))).dot(Bb.T).dot(Dx).dot(Bb))
M_breve_zstar = Bb - Dx.dot(Bb).dot(la.inv(Bb.T.dot(Dx).dot(Bb))).dot(
Bb.T).dot(Bb)
v_star = M_breve_zstar.dot(z_star)
e_distortion = np.zeros(len(t))
v_distortion = np.zeros(len(t))
for i, time in enumerate(t):
e_distortion[i] = la.norm(z_tilde_star - Bb.T.dot(p[i, :]))
v_distortion[i] = la.norm(v_star -
(-(Dx.dot(Lb)).dot(p[i, :]) + Lb.dot(p_star)))
plt.figure(1)
plt.plot(t, e_distortion, label='$||z(t) - \\tilde z||$')
X_train = np.dot(base_X_train, coloring_matrix)
X_test = np.dot(base_X_test, coloring_matrix)
###############################################################################
# Compute the likelihood on test data
# spanning a range of possible shrinkage coefficient values
shrinkages = np.logspace(-2, 0, 30)
negative_logliks = [-ShrunkCovariance(shrinkage=s).fit(X_train).score(X_test)
for s in shrinkages]
# under the ground-truth model, which we would not have access to in real
# settings
real_cov = np.dot(coloring_matrix.T, coloring_matrix)
emp_cov = empirical_covariance(X_train)
loglik_real = -log_likelihood(emp_cov, linalg.inv(real_cov))
###############################################################################
# Compare different approaches to setting the parameter
# GridSearch for an optimal shrinkage coefficient
tuned_parameters = [{'shrinkage': shrinkages}]
cv = GridSearchCV(ShrunkCovariance(), tuned_parameters)
cv.fit(X_train)
# Ledoit-Wolf optimal shrinkage coefficient estimate
lw = LedoitWolf()
loglik_lw = lw.fit(X_train).score(X_test)
# OAS coefficient estimate
oa = OAS()
def update(x, P, z, R, H=None, return_all=False):
"""
Add a new measurement (z) to the Kalman filter. If z is None, nothing
is changed.
This can handle either the multidimensional or unidimensional case. If
all parameters are floats instead of arrays the filter will still work,
and return floats for x, P as the result.
update(1, 2, 1, 1, 1) # univariate
update(x, P, 1
Parameters
----------
x : numpy.array(dim_x, 1), or float
State estimate vector
P : numpy.array(dim_x, dim_x), or float
Covariance matrix
z : numpy.array(dim_z, 1), or float
measurement for this update.
R : numpy.array(dim_z, dim_z), or float
Measurement noise matrix
H : numpy.array(dim_x, dim_x), or float, optional
Measurement function. If not provided, a value of 1 is assumed.
return_all : bool, default False
If true, y, K, S, and log_likelihood are returned, otherwise
only x and P are returned.
Returns
-------
x : numpy.array
Posterior state estimate vector
P : numpy.array
Posterior covariance matrix
y : numpy.array or scalar
Residua. Difference between measurement and state in measurement space
K : numpy.array
Kalman gain
S : numpy.array
System uncertainty in measurement space
log_likelihood : float
log likelihood of the measurement
"""
if z is None:
if return_all:
return x, P, None, None, None, None
else:
return x, P
if H is None:
H = np.array([1])
if np.isscalar(H):
H = np.array([H])
if not np.isscalar(x):
# handle special case: if z is in form [[z]] but x is not a column
# vector dimensions will not match
if x.ndim == 1 and shape(z) == (1, 1):
z = z[0]
if shape(z) == (): # is it scalar, e.g. z=3 or z=np.array(3)
z = np.asarray([z])
# error (residual) between measurement and prediction
y = z - dot(H, x)
# project system uncertainty into measurement space
S = dot(H, P).dot(H.T) + R
# map system uncertainty into kalman gain
try:
K = dot(P, H.T).dot(linalg.inv(S))
except:
K = dot(P, H.T).dot(1 / S)
# predict new x with residual scaled by the kalman gain
x = x + dot(K, y)
# P = (I-KH)P(I-KH)' + KRK'
KH = dot(K, H)
try:
I_KH = np.eye(KH.shape[0]) - KH
except:
I_KH = np.array(1 - KH)
P = dot(I_KH, P).dot(I_KH.T) + dot(K, R).dot(K.T)
if return_all:
# compute log likelihood
log_likelihood = logpdf(z, dot(H, x), S)
return x, P, y, K, S, log_likelihood
else:
return x, P
def rts_smoother(self, Xs, Ps, Fs=None, Qs=None):
""" Runs the Rauch-Tung-Striebal Kalman smoother on a set of
means and covariances computed by a Kalman filter. The usual input
would come from the output of `KalmanFilter.batch_filter()`.
Parameters
----------
Xs : numpy.array
array of the means (state variable x) of the output of a Kalman
filter.
Ps : numpy.array
array of the covariances of the output of a kalman filter.
Fs : list-like collection of numpy.array, optional
State transition matrix of the Kalman filter at each time step.
Optional, if not provided the filter's self.F will be used
Qs : list-like collection of numpy.array, optional
Process noise of the Kalman filter at each time step. Optional,
if not provided the filter's self.Q will be used
Returns
-------
'x' : numpy.ndarray
smoothed means
'P' : numpy.ndarray
smoothed state covariances
'K' : numpy.ndarray
smoother gain at each step
'Pp' : numpy.ndarray
Predicted state covariances
Examples
--------
.. code-block:: Python
zs = [t + random.randn()*4 for t in range (40)]
(mu, cov, _, _) = kalman.batch_filter(zs)
(x, P, K, Pp) = rts_smoother(mu, cov, kf.F, kf.Q)
"""
assert len(Xs) == len(Ps)
shape = Xs.shape
n = shape[0]
dim_x = shape[1]
if Fs is None:
Fs = [self.F] * n
if Qs is None:
Qs = [self.Q] * n
# smoother gain
K = zeros((n, dim_x, dim_x))
x, P, Pp = Xs.copy(), Ps.copy(), Ps.copy()
for k in range(n - 2, -1, -1):
Pp[k] = dot(Fs[k + 1], P[k]).dot(Fs[k + 1].T) + Qs[k + 1]
K[k] = dot(P[k], Fs[k + 1].T).dot(linalg.inv(Pp[k]))
x[k] += dot(K[k], x[k + 1] - dot(Fs[k + 1], x[k]))
P[k] += dot(K[k], P[k + 1] - Pp[k]).dot(K[k].T)
return (x, P, K, Pp)
def update_correlated(self, z, R=None, H=None):
""" Add a new measurement (z) to the Kalman filter assuming that
process noise and measurement noise are correlated as defined in
the `self.M` matrix.
If z is None, nothing is changed.
Parameters
----------
z : np.array
measurement for this update.
R : np.array, scalar, or None
Optionally provide R to override the measurement noise for this
one call, otherwise self.R will be used.
H : np.array, or None
Optionally provide H to override the measurement function for this
one call, otherwise self.H will be used.
"""
if z is None:
return
if R is None:
R = self.R
elif isscalar(R):
R = eye(self.dim_z) * R
# rename for readability and a tiny extra bit of speed
if H is None:
H = self.H
x = self.x
P = self.P
M = self.M
# handle special case: if z is in form [[z]] but x is not a column
# vector dimensions will not match
if x.ndim == 1 and shape(z) == (1, 1):
z = z[0]
if shape(z) == (): # is it scalar, e.g. z=3 or z=np.array(3)
z = np.asarray([z])
# y = z - Hx
# error (residual) between measurement and prediction
self.y = z - dot(H, x)
# common subexpression for speed
PHT = dot(P, H.T)
# project system uncertainty into measurement space
self.S = dot(H, PHT) + dot(H, M) + dot(M.T, H.T) + R
# K = PH'inv(S)
# map system uncertainty into kalman gain
self.K = dot(PHT + M, linalg.inv(self.S))
# x = x + Ky
# predict new x with residual scaled by the kalman gain
self.x = x + dot(self.K, self.y)
self.P = P - dot(self.K, dot(H, P) + M.T)
def randorth(shape):
from scipy.linalg import sqrtm, inv
assert len(shape) == 2
w = np.random.normal(0, size=shape)
w = w.dot(inv(sqrtm(w.T.dot(w))))
return G.sharedf(w)
# build the nasty two-electron repulsion integral Vee = asarray(mints.ao_eri()) # Construct AO orthogonalization matrix A # this is the Psi4 way, which is for symmetric orthog # A = mints.ao_overlap() # A.power(-0.5, 1.0e-16) # A = asarray(A) # get nuclear repulsion energy from Psi4 E_nuc = mol.nuclear_repulsion_energy() # we'll keep our way in here, it works the same u, V = eigh(Smat) U = sqrt(inv(u * eye(len(u)))) # A = dot(V.T, dot(U, V)) A = dot(V, dot(U, V.T)) # maximum scf iterations maxiter = 40 # energy convergence criterion E_conv = 1.0e-6 D_conv = 1.0e-4 # pre-iteration step # scf & previous energy SCF_E = 0.0 E_old = 0.0 # form core guess
def rts_smoother(Xs, Ps, Fs, Qs):
""" Runs the Rauch-Tung-Striebal Kalman smoother on a set of
means and covariances computed by a Kalman filter. The usual input
would come from the output of `KalmanFilter.batch_filter()`.
Parameters
----------
Xs : numpy.array
array of the means (state variable x) of the output of a Kalman
filter.
Ps : numpy.array
array of the covariances of the output of a kalman filter.
Fs : list-like collection of numpy.array
State transition matrix of the Kalman filter at each time step.
Qs : list-like collection of numpy.array, optional
Process noise of the Kalman filter at each time step.
Returns
-------
'x' : numpy.ndarray
smoothed means
'P' : numpy.ndarray
smoothed state covariances
'K' : numpy.ndarray
smoother gain at each step
'pP' : numpy.ndarray
predicted state covariances
Examples
--------
.. code-block:: Python
zs = [t + random.randn()*4 for t in range (40)]
(mu, cov, _, _) = kalman.batch_filter(zs)
(x, P, K, pP) = rts_smoother(mu, cov, kf.F, kf.Q)
"""
assert len(Xs) == len(Ps)
n = Xs.shape[0]
dim_x = Xs.shape[1]
# smoother gain
K = zeros((n, dim_x, dim_x))
x, P, pP = Xs.copy(), Ps.copy(), Ps.copy()
for k in range(n - 2, -1, -1):
pP[k] = dot(Fs[k], P[k]).dot(Fs[k].T) + Qs[k]
K[k] = dot(P[k], Fs[k].T).dot(linalg.inv(pP[k]))
x[k] += dot(K[k], x[k + 1] - dot(Fs[k], x[k]))
P[k] += dot(K[k], P[k + 1] - pP[k]).dot(K[k].T)
return (x, P, K, pP)
def detrendn(da, axes=None):
"""
Detrend by subtracting out the least-square plane or least-square cubic fit
depending on the number of axis.
Parameters
----------
da : `dask.array`
The data to be detrended
Returns
-------
da : `numpy.array`
The detrended input data
"""
N = [da.shape[n] for n in axes]
M = []
for n in range(da.ndim):
if n not in axes:
M.append(da.shape[n])
if len(N) == 2:
G = np.ones((N[0] * N[1], 3))
for i in range(N[0]):
G[N[1] * i:N[1] * i + N[1], 1] = i + 1
G[N[1] * i:N[1] * i + N[1], 2] = np.arange(1, N[1] + 1)
if type(da) == xr.DataArray:
d_obs = np.reshape(da.copy().values, (N[0] * N[1], 1))
else:
d_obs = np.reshape(da.copy(), (N[0] * N[1], 1))
elif len(N) == 3:
if type(da) == xr.DataArray:
if da.ndim > 3:
raise NotImplementedError(
"Cubic detrend is not implemented "
"for 4-dimensional `xarray.DataArray`."
" We suggest converting it to "
"`dask.array`.")
else:
d_obs = np.reshape(da.copy().values, (N[0] * N[1] * N[2], 1))
else:
d_obs = np.reshape(da.copy(), (N[0] * N[1] * N[2], 1))
G = np.ones((N[0] * N[1] * N[2], 4))
G[:, 3] = np.tile(np.arange(1, N[2] + 1), N[0] * N[1])
ys = np.zeros(N[1] * N[2])
for i in range(N[1]):
ys[N[2] * i:N[2] * i + N[2]] = i + 1
G[:, 2] = np.tile(ys, N[0])
for i in range(N[0]):
G[len(ys) * i:len(ys) * i + len(ys), 1] = i + 1
else:
raise NotImplementedError("Detrending over more than 4 axes is "
"not implemented.")
m_est = np.dot(np.dot(spl.inv(np.dot(G.T, G)), G.T), d_obs)
d_est = np.dot(G, m_est)
lin_trend = np.reshape(d_est, da.shape)
return da - lin_trend
def calculateRegCoef(indVar, depVar):
xTxI = linalg.inv(np.dot(indVar.T, indVar))
xTy = np.dot(indVar.T, depVar)
weights = np.dot(xTxI, xTy)
return np.dot(indVar, weights)
def linear_regression(X, y):
''' Given a set of predictor values X and target values y,
computes the vector of weights that determines a linear model of best fit.'''
weights = np.matmul(np.matmul(la.inv(np.matmul(X.T, X)), X.T), y)
return weights
np.arange(0, (np.shape(V)[1] - 2 * bounds + 1),
(np.shape(V)[1] - 2 * bounds) / (2 * int(xf))),
np.arange(-int(xf), 2 * int(xf) + 1))
plt.yticks(np.arange(0,
np.shape(V)[0],
np.shape(V)[0] / 4.0), np.linspace(0, 1, 5))
plt.colorbar()
plt.show()
for i in range(points_t):
psipres = psigrid[i]
#T=-0.5*Nderivat2(psipres,x)
Vc = np.diag(V[i, :])
Hpsi = T + Vc
matt = onesies - 1j * Hpsi * dt
mattinv = la.inv(onesies + 1j * Hpsi * dt)
psinew = (mattinv @ (matt @ psipres))
psigrid.append(psinew)
'''
for i in range(2*itertevol+points_t,2*itertevol+points_t+5*itertevol):
psipres=psigrid[i]
#T=-0.5*Nderivat2(psipres,x)
Vc=np.diag(V[-1,:])
Hpsi=T+Vc
matt=onesies-1j*Hpsi*dt
mattinv=la.inv( onesies +1j*Hpsi*dt)
psinew=(mattinv@(matt@psipres))
psigrid.append(psinew)
'''
import math
expk = exp(1j * 2 * pi * dot(kps[ik], R)) * wgh[ik]
#print R, kps[ik], dot(kps[ik],R)*5
tHr += expk * Hk[ik]
dsum += expk
print 'R=', R, 'exp=', abs(dsum)
Hr.append(tHr)
# Printing of the Hamiltonian
fh = open('hamiltonian.dat', 'w')
print >> fh, '# Tighbinding Hamiltonian with entries: ', inl.legends
print >> fh
print >> fh, "lattice_unit=[", tuple(DV[0]), ',', tuple(DV[1]), ',', tuple(
DV[2]), "]"
print >> fh, "orbital_positions=["
for atm in inl.siginds.keys():
for i in range(len(inl.siginds[atm])):
print >> fh, pos[atm - 1].tolist(), ','
print >> fh, ']\n'
DVinv = linalg.inv(DV)
print >> fh, 'Hopping={}'
for ir, R in enumerate(Rs):
Print(fh, R, Hr[ir], DVinv)
print >> fh
fh.close()
def update(self, z, R=None, UT=None, hx_args=()):
""" Update the UKF with the given measurements. On return,
self.x and self.P contain the new mean and covariance of the filter.
Parameters
----------
z : numpy.array of shape (dim_z)
measurement vector
R : numpy.array((dim_z, dim_z)), optional
Measurement noise. If provided, overrides self.R for
this function call.
UT : function(sigmas, Wm, Wc, noise_cov), optional
Optional function to compute the unscented transform for the sigma
points passed through hx. Typically the default function will
work - you can use x_mean_fn and z_mean_fn to alter the behavior
of the unscented transform.
hx_args : tuple, optional, default (,)
arguments to be passed into Hx function after the required state
variable.
"""
if z is None:
return
if not isinstance(hx_args, tuple):
hx_args = (hx_args, )
if UT is None:
UT = unscented_transform
if R is None:
R = self.R
elif isscalar(R):
R = eye(self._dim_z) * R
diag_R = np.diagonal(R)
if self._dim_z != len(diag_R):
# checks to see if observation covariance matrix dimsenion is a multiple of the sigmas, so the sizing changes
# with the number of available observables (for a specific type!)
temp = np.zeros([self._dim_z, self._dim_z])
_len = len(np.diagonal(R))
for i in range(0, self._dim_z, _len):
temp[i:i + _len, i:i + _len] = self.R_store
R = temp
self.sigmas_h = zeros((self._num_sigmas, self._dim_z))
for i in range(self._num_sigmas):
self.sigmas_h[i] = self.hx(self.sigmas_f[i], *hx_args)
# mean and covariance of prediction passed through unscented transform
zp, Pz = UT(self.sigmas_h, self.Wm, self.Wc, R, self.z_mean,
self.residual_z)
# compute cross variance of the state and the measurements
Pxz = zeros((self._dim_x, self._dim_z))
for i in range(self._num_sigmas):
dx = self.residual_x(self.sigmas_f[i], self.x)
dz = self.residual_z(self.sigmas_h[i], zp)
Pxz += self.Wc[i] * outer(dx, dz)
self.K = dot(Pxz, inv(Pz)) # Kalman gain
self.y = self.residual_z(z, zp) # residual
self.x += dot(self.K, self.y)
self.P = np.subtract(self.P, dot(self.K, Pz).dot(self.K.T))
#print("%f\t%f\t%f\n" % (self.P[0][0], self.P[1][1], self.P[2][2]))
self.log_likelihood = logpdf(self.y, np.zeros(len(self.y)), Pz)
def reciprocal_lattice(self):
return type(self)(matrix=2 * np.pi * la.inv(self.matrix).T)
(data[7]-data[5])/(x[7]-x[5]),
(data[8]-data[6])/(x[8]-x[6]),
(data[9]-data[7])/(x[9]-x[7]),
(data[2]-data[0])/(x[2]-x[0]),
(data[3]-data[1])/(x[3]-x[1]),
(data[4]-data[2])/(x[4]-x[2]),
(data[5]-data[3])/(x[5]-x[3]),
(data[6]-data[4])/(x[6]-x[4]),
(data[7]-data[5])/(x[7]-x[5]),
(data[8]-data[6])/(x[8]-x[6]),
(data[9]-data[7])/(x[9]-x[7]),
0,
0],
dtype=np.float64)
inv_const_mat = inv(const_mat)
coeffs = inv_const_mat.dot(const_vec)
def P(i):
if 0 <= i <= 8:
return lambda a: coeffs[4*i]*a*a*a + coeffs[4*i+1]*a*a + coeffs[4*i+2]*a + coeffs[4*i+3]
else:
raise ValueError("i must be between 0 and 8 included")
interp_data = np.zeros((30,), dtype=np.float64)
ind = 0
for i in range(30):
while xsamples[i] > x[ind+1]:
ind += 1
interp_data[i] = P(ind)(xsamples[i])
#線形代数用のライブラリ
import scipy.linalg as linalg
#最適化計算（最小値）用の関数
from scipy.optimize import minimize_scalar
from six import X
matrix = np.array([[1,-1,-1],[-1,1,-1],[-1,-1,1]])
#行列式
print('行列式')
print(linalg.det(matrix))
#逆行列
print('逆行列')
print(linalg.inv(matrix))
#固有値と固有ベクトル
eig_value, eig_vector = linalg.eig(matrix)
#固有値と固有ベクトル
print('固有値')
print(eig_value)
print('固有ベクトル')
print(eig_vector)
# 関数の定義
def my_function(x):
return(x**2 + 2*x + 1)
def V_MAP(self, pars, option=2, plot_eig=False):
def _ybar(sbar, ibar, **kwargs):
sbar_arr = sbar * np.ones(self.N)
ibar_arr = ibar * np.ones(self.N)
return np.concatenate((sbar_arr, ibar_arr))
def _Crr(sig_r, sig_s, sig_i, rho1, rho2, rho3, **kwargs):
cov_FP = get_cov_FP2(sig_r, sig_s, sig_i, rho1, rho2, rho3) # 3x3
return np.diag(self.N*[cov_FP[0,0]])
def _Cry(sig_r, sig_s, sig_i, rho1, rho2, rho3, **kwargs):
cov_FP = get_cov_FP2(sig_r, sig_s, sig_i, rho1, rho2, rho3) # 3x3
v = cov_FP[1:3,0].reshape(-1, 1) # shape = (2,1)
return linalg.kron(v, np.eye(self.N))
def _Cyy(sig_r, sig_s, sig_i, rho1, rho2, rho3, **kwargs):
cov_FP = get_cov_FP2(sig_r, sig_s, sig_i, rho1, rho2, rho3) # 3x3
return linalg.kron(cov_FP[1:3,1:3], np.eye(self.N))
def _A_inv():
A = np.zeros(self.N)
for i,z in enumerate(self.z):
kappa = cosmo_model.kappa_v(z, v=1.0) # only need factor so set v=1
A[i] = -kappa/np.log(10.0)
return np.diag(1./A)
def _R_fast(sig_star=362.8, b1=1., **kwargs):
"""
To create the covariance this method instead pushes the loops down
to cython code. The covariance is written as the sum of two terms,
each made up of a matrix product consisting of the angular part and
correlation part.
By default this function uses interpolation for correlation functions
and also the low-z approximation to compute comoving distance
"""
chi_arr = cosmo_model.chi_lowz(self.z) * 1e-3 # units Mpc/h
r_arr = np.zeros((self.N,self.N))
cos1cos2_arr = np.zeros((self.N,self.N))
sin1sin2_arr = np.zeros((self.N,self.N))
r12, C12, S12 = cy_pairs(chi_arr, self.n_hats, r_arr, cos1cos2_arr, sin1sin2_arr)
if cosmo_model.log_xi_perp_interpolator is None:
cosmo_model.init_xiV_interpolation()
iu = np.triu_indices(self.N, k=1)
r12_offdiag_flat = r12[iu] # 1d array
xi_perp_flat = 10**cosmo_model.log_xi_perp_interpolator(r12_offdiag_flat)
xi_para_flat = cosmo_model.xi_para_interpolator(r12_offdiag_flat)
xi_perp = np.zeros((self.N,self.N))
xi_para = np.zeros((self.N,self.N))
xi_perp[iu] = xi_perp_flat
xi_para[iu] = xi_para_flat
R = (S12 * xi_perp) + (C12 * xi_para) # elementwise multiplication
R = R + R.T
np.fill_diagonal(R, cosmo_model.sigmav**2 + sig_star**2)
# R *= b1**2 # linear bias
return R
pd = get_param_dict(self.par_names, pars)
cosmo_model = cosmology.cosmo(**pd)
cosmo_model.init_xiV_interpolation()
Ainv = _A_inv()
R = _R_fast()
Crr = _Crr(**pd)
Cry = _Cry(**pd)
Cyy = _Cyy(**pd)
Cyy_tot_inv = linalg.inv(Cyy + self.cov)
try:
# Cyy_tot_inv = linalg.inv(Cyy + self.cov)
Cyy_tot_cholfac = linalg.cho_factor(Cyy+self.cov, overwrite_a=True, lower=True)
except:
raise ValueError
Crr_prime = Crr - np.matmul(Cry.T, linalg.cho_solve(Cyy_tot_cholfac, Cry)) # Sig_0
Noise = np.matmul(Ainv, np.matmul(Crr_prime, Ainv))
W = np.matmul(R, linalg.inv(R+Noise)) # Wiener filter
cosmo_model = cosmology.cosmo(**pd)
cosmo_model.init_xiV_interpolation()
dA = np.zeros(self.N)
for i,z in enumerate(self.z):
dA[i] = cosmo_model.dA(z, use_lowz=True)
ldA = np.log10(dA)
dy = _ybar(**pd) - self.y
rbar_shift = pd['rbar'] - np.matmul(Cry.T, linalg.cho_solve(Cyy_tot_cholfac, dy))
Delta = rbar_shift - (self.ltheta + ldA)
V = np.matmul(W, np.matmul(Ainv,Delta))
Hess = linalg.inv(R) + linalg.inv(Noise)
Cov_V = linalg.inv(Hess)
if plot_eig:
u, s, _ = np.linalg.svd(W)
idx = [np.argmax(np.abs(u[:,i])) for i in range(self.N)]
vals = [u[j,i] for (j,i) in zip(idx,range(self.N))]
print(vals)
print(np.sort(idx))
print(u[:,0])
import matplotlib.pyplot as plt
plt.plot(s, c='k', ls='None', marker='.')
# plt.savefig('test.pdf')
plt.show()
if option == 1: # LSS correlations by highest to lowest
rho_V = R / cosmo_model.sigmav**2
B = rho_V
elif option == 2: # total covariance
B = Cov_V
elif option == 3: # total correlations
rho_V = np.zeros_like(Cov_V)
for i in range(self.N):
for j in range(i,self.N):
if i == j:
rho_V[i,i] = 1.
else:
rho_V[i,j] = Cov_V[i,j] / np.sqrt(Cov_V[i,i] * Cov_V[j,j])
rho_V[j,i] = rho_V[i,j]
B = rho_V
elif option == 4:
B = Hess
r_, c_ = np.triu_indices(B.shape[1], 1) # row, column indices of upper triangle
idx = B[r_,c_].argsort()[::-1] # high to low
r,c = r_[idx], c_[idx]
out = zip(r,c,B[r,c])
return V, Cov_V, B, out
def calibrate():
width = 20.4 / BOARD_W
heigth = 14.6 / BOARD_H
WRL = np.zeros((48, 2))
i = 0
for y in range(BOARD_H):
for x in range(BOARD_W):
WRL[i] = ((x * width), (y * heigth))
i += 1
fs = cv2.FileStorage('intrinsics.xml', cv2.FILE_STORAGE_READ)
intrinsics = fs.getNode('floatdata').mat()
fs.release()
fs = cv2.FileStorage('distortion.xml', cv2.FILE_STORAGE_READ)
distortion = fs.getNode('floatdata').mat()
fs.release()
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
cap = cv2.VideoCapture(0)
ret, frame = cap.read()
h, w = frame.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(intrinsics, distortion,
(w, h), 1, (w, h))
mapx, mapy = cv2.initUndistortRectifyMap(intrinsics, distortion, None,
newcameramtx, (w, h), 5)
cv2.namedWindow(UNDISTORT_WIN) # Create a named window
cv2.moveWindow(UNDISTORT_WIN, 10, 10)
cv2.namedWindow(RAW_WIN) # Create a named window
cv2.moveWindow(RAW_WIN, 650, 10)
found = False
exit = False
start = False
print("setup camera and chessboard and press s to start...")
print("press q to quit without finding chessboard...")
while (not found) and (not exit):
# Capture frame-by-frame
ret, frame = cap.read()
if ret:
#frame = cv2.flip(f, 1)
frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
undist = cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR)
undist_gray = cv2.cvtColor(undist, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(undist_gray,
(BOARD_W, BOARD_H), None)
if start and ret and (len(corners) == BOARD_W * BOARD_H):
found = True
CAM = cv2.cornerSubPix(undist_gray, corners, (11, 11),
(-1, -1), criteria)
# Draw and display the corners
undist = cv2.drawChessboardCorners(undist, (BOARD_W, BOARD_H),
CAM, ret)
# Display the resulting frame
cv2.imshow(UNDISTORT_WIN,
undist) # Se achou mostra snapshot colorido
else:
cv2.imshow(UNDISTORT_WIN,
undist_gray) # Se nao achou mostra snapshot p&b
# Display the resulting frame
cv2.imshow(RAW_WIN, frame)
key = cv2.waitKey(3)
if key & 0xFF == ord('s'):
start = True
if key & 0xFF == ord('q'):
return
CAM = CAM.reshape(48, 2)
A = np.zeros((2 * BOARD_W * BOARD_H, 9))
for i in range(BOARD_W * BOARD_H):
A[i * 2] = (
WRL[i][0], # X
WRL[i][1], # Y
1, # 1
0, # 0
0, # 0
0, # 0
-CAM[i][0] * WRL[i][0], # -xX
-CAM[i][0] * WRL[i][1], # -xY
-CAM[i][0] # x
)
A[(i * 2) + 1] = (
0, # 0
0, # 0
0, # 0
WRL[i][0], # X
WRL[i][1], # Y
1, # 1
-CAM[i][1] * WRL[i][0], # -yX
-CAM[i][1] * WRL[i][1], # -yY
-CAM[i][1] # y
)
#np.set_printoptions(linewidth=160)
U, s, Vh = linalg.svd(A)
V = np.transpose(Vh)
P = np.array([row[8] for row in V]).reshape((3, 3))
p = P.reshape(9)
X = np.array([row[0] for row in WRL])
Y = np.array([row[1] for row in WRL])
x = np.array([row[0] for row in CAM])
y = np.array([row[1] for row in CAM])
def func(p, x, y, X, Y):
proj_x = (p[0] * X + p[1] * Y + p[2]) / (p[6] * X + p[7] * Y + p[8])
proj_y = (p[3] * X + p[4] * Y + p[5]) / (p[6] * X + p[7] * Y + p[8])
r = (x - proj_x)**2 + (y - proj_y)**2
return r
res = least_squares(func,
p,
args=(np.array(x), np.array(y), np.array(X),
np.array(Y)),
max_nfev=9000,
verbose=0)
new_P = res.x.reshape((3, 3))
H = np.array(np.matmul(linalg.inv(intrinsics), new_P)) #@
h1 = np.array([row[0] for row in H])
h2 = np.array([row[1] for row in H])
_R = (2 / (linalg.norm(h1) + linalg.norm(h2))) * H
t = np.array([row[2] for row in _R])
r1 = np.array([row[0] for row in _R])
r2 = np.array([row[1] for row in _R])
r3 = r1 * r2
_R = np.append(np.append(r1, r2), r3).reshape((3, 3))
U, s, Vh = linalg.svd(_R)
R = np.matmul(U, Vh) # @
r = np.array([row[:2] for row in R])
print("\nr (rotation matrix:")
print(r)
print("\nt (translation vector:")
print(t)
print("\n||t||:", linalg.norm(t))
fs = cv2.FileStorage('rotation.xml', cv2.FILE_STORAGE_WRITE)
fs.write("floatdata", r)
fs.release()
fs = cv2.FileStorage('translation.xml', cv2.FILE_STORAGE_WRITE)
fs.write("floatdata", t)
fs.release()
# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()
cv2.namedWindow(UNDISTORT_WIN) # Create a named window
cv2.moveWindow(UNDISTORT_WIN, 10, 10)
cv2.namedWindow(RAW_WIN) # Create a named window
cv2.moveWindow(RAW_WIN, 650, 10)
def main(f=1.0, m1=0.4, m2=1.0, **kwargs):
'''
main(f=1.0, m1=0.4, m2=1.0, **kwargs)
Plot frequencies of four eigenmodes of a linear chain as a function of the wave number (q).
Parameters
----------
f : float
Spring constant
m1, m2: float
masses of the atoms
Returns
-------
eigensys: array
The eigenvalues and eigenmodes
'''
print "\n Oscillator Chain \n"
F = lambda q: np.array([[2 * f, -f, 0, -f * np.exp(-1j * q)],
[-f, 2 * f, -f, 0], [0, -f, 2 * f, -f],
[-f * np.exp(1j * q), 0, -f, 2 * f]])
# will cyclic rearrangements of m1 and m2 below change the results? Try it
''' No, the results were not changed since we have a periodic repetition
of the linear chains so a cylic rearrangement would not the dynamics'''
# what will be the dimensions/shape of massmat?
'''Massmat is a 4x4 diagonal matrix'''
massmat = np.diag([m1, m1, m1, m2])
# what happens if inv(massmat) * mat1(q) is used instead of
# inv(massmat).dot(mat1(q))?
'''They will multiplied as arrays, element by element, instead of
matrix multiplication.'''
mat2 = lambda q: inv(massmat).dot(F(q))
# what is the python type of kwargs?
'''Kwargs is a dictionary'''
plot_step = kwargs.get('plot_step', np.pi / 50)
# complete the following line. you should use plot_step
x_axis = np.arange(-np.pi, np.pi + plot_step, plot_step)
# what is the difference between eigvals() and eig()?
'''eigvals() returns the eigenvalues while eig() returns
both the eigenvalues and the correspnding eigenvectors'''
# replace the following line to use eig() instead of eigvals()
eigenlist = [eig(mat2(x))[0] for x in x_axis]
# complete the following lines:
plt.plot(x_axis, np.sqrt(eigenlist), '.')
plt.xticks(
np.arange(-np.pi, np.pi + np.pi / 2, np.pi / 2),
[r"$\pi$", r"$\frac{\pi}{2}$", "0", r"$\frac{\pi}{2}$", r"$\pi$"])
plt.xlim([-np.pi, np.pi])
plt.xlabel('q', fontsize='x-large')
plt.ylabel('$\omega$', fontsize='x-large')
plt.tick_params('x', labelsize='x-large')
plt.show()
# why is the argument of mat2(), 0.0?
'''We only need to consider the unit cell.'''
# what does the parameter of mat2() mean?
'''The parameter of mat2() is q.'''
eigensys = eig(mat2(0.0))
return eigensys
# additional:
# * rename mat1 and mat2 to be more descriptive
'''In the book, mat1 is named as F. mat2 is the product of the inverse of the matrix massmat
and the matrix F)'''
# * describe the result if the values of m1 and m2 are interchanged
'''When the values of m1 and m2 are interchanged, the bands are lower.'''
# * describe the effect of different values of f
'''As f (spring constant) increases, the angular frequency omega increases.'''
def compute_TL(t,Fd):
(P,G) = matrix_nullspace()
TL_min = (P.T @ inv(P @ P.T) @ Fd).reshape(-1,n_cables,n_dims,1)
TL_opt = empty_like(TL_min)
for i, ti in enumerate(t):
Fdi = Fd[i,...]
TL_mini = TL_min[i].reshape(n_cables*n_dims,1)
### Setup optimization
Q = eye(TL_len)
# minimum angle with inertial/body z-axis
theta = theta_min_for_optimization
A = P
b = Fdi.ravel()
# TL numbering starts with 1 and is clockwize from x-axis
#
# TL_1 (NE)
# TL_2 (NW)
# TL_3 (SW)
# TL_4 (SE)
# Inner products with these vectors with TL must be positive for
# some combinations of vector/TL
#
# Vectors are in the body frame as when the box rotates
# the cones should also rotate
v_N = [ cos(theta), 0, sin(theta)] # E
v_E = [ 0, cos(theta), sin(theta)] # N
v_S = [-cos(theta), 0, sin(theta)] # W
v_W = [ 0, -cos(theta), sin(theta)] # S
# Match "rho" in SCORE lab
Ai = vstack([
concatenate([v_N , zeros(9)]),
concatenate([v_W, zeros(9)]),
concatenate([zeros(3) , v_N, zeros(6)]),
concatenate([zeros(3) , v_E , zeros(6)]),
concatenate([zeros(6) , v_S, zeros(3)]),
concatenate([zeros(6) , v_W, zeros(3)]),
concatenate([zeros(9) , v_S]),
concatenate([zeros(9) , v_E]),
])
## Comment to choose -> theta angle with inertial z-axis
## Uncomment to choose -> theta angle with body z-axis
# Ai = (R_mult.T @ Ai.T).T
bi = zeros(Ai.shape[0])
### Run optimization
# use solution from previous timestep
try:
x0 = compute_TL.x0
except AttributeError:
x0 = random.randn(TL_len)
def loss(x, sign=1.):
return sign * (0.5 * x.T @ Q @ x)
def jac(x, sign=1.):
return sign * (x.T @ Q)
cons = ({'type':'eq',
'fun':lambda x: A @ x - b,
'jac':lambda x: A},
# {'type':'ineq',
# 'fun':lambda x: Ai @ x - bi,
# 'jac':lambda x: Ai}
)
options = {'disp':False}
res = optimize.minimize(loss,
x0,
jac=jac,
constraints=cons,
options=options,
tol=1e-9)
if(res.success):
# print(res)
TL_opt[i] = res.x.reshape(n_cables,n_dims,1)
compute_TL.x0 = res.x
else:
print(f't = {ti:0.3} \t No solution:', res.message)
# TL_opt[i] = zeros_like(res.x)
return (TL_min, TL_opt)
def em_vmat22(y,
xmat,
zmat_lst,
gmat_lst,
init=None,
maxiter=100,
cc_par=1.0e-8):
"""
Estimate variance parameters with vmat based on em
:param y: Phenotypic vector
:param xmat: the design matrix for fixed effect
:param zmat_lst: A list of design matrix for random effects
:param gmat_lst: A list for relationship matrix
:param init: Default is None. A list for initial values of variance components
:param maxiter: Default is 100. The maximum number of iteration times.
:param cc_par: Convergence criteria for update vector.
:return: The estimated variances.
"""
num_var = len(gmat_lst) + 1
var_com = np.ones(num_var)
if init is not None:
var_com = np.array(init)
var_com_new = np.array(var_com) * 100
y = np.array(y).reshape(-1, 1)
n = y.shape[0]
xmat = np.array(xmat).reshape(n, -1)
zgzmat_lst = []
for val in range(num_var - 1):
zgzmat_lst.append(zmat_lst[val].dot(
(zmat_lst[val].dot(gmat_lst[val])).T))
h_mat = np.eye(n) - reduce(
np.dot, [xmat, linalg.inv(np.dot(xmat.T, xmat)), xmat.T])
logging.info('Initial variances: ' +
' '.join(list(np.array(var_com, dtype=str))))
logging.info("#####Start the iteration#####\n\n")
iter = 0
cc_par_val = 1000.0
while iter < maxiter:
iter += 1
logging.info('###Round: ' + str(iter) + '###')
# V, the inverse of V and P
vmat = np.diag([var_com[-1]] * n)
for val in range(len(zgzmat_lst)):
vmat += zgzmat_lst[val] * var_com[val]
zgzmat = vmat - np.diag([var_com[-1]] * n)
vmat = linalg.inv(vmat)
vxmat = np.dot(vmat, xmat)
xvxmat = np.dot(xmat.T, vxmat)
xvxmat = linalg.inv(xvxmat)
vxxvxxv_mat = reduce(np.dot, [vxmat, xvxmat, vxmat.T])
pmat = vmat - vxxvxxv_mat
pymat = np.dot(pmat, y)
# Updated variances
trace_term1 = reduce(np.dot, [xmat, xvxmat, xmat.T])
trace_term2 = reduce(np.dot, [xmat, xvxmat, vxmat.T]) * var_com[-1]
trace_term = trace_term1 - trace_term2 + np.diag([var_com[-1]] * n) - vmat*var_com[-1]*var_com[-1] - \
trace_term2.T + vxxvxxv_mat * var_com[-1] * var_com[-1]
trace_term = np.sum(trace_term * h_mat)
y_zu = y - np.dot(zgzmat, pymat)
var_com_new[-1] = trace_term + np.sum(
reduce(np.dot, [y_zu.T, h_mat, y_zu]))
var_com_new[-1] /= n - xmat.shape[1]
for k in range(num_var - 1):
var_com_new[k] = -np.sum(zgzmat_lst[k] * pmat) + np.sum(
reduce(np.dot, [pymat.T, zgzmat_lst[k], pymat]))
var_com_new[k] = var_com[k] + var_com[k] * var_com[k] / gmat_lst[
k].shape[0] * var_com_new[k]
delta = var_com_new - var_com
cc_par_val = np.sum(delta * delta) / np.sum(var_com_new * var_com_new)
cc_par_val = np.sqrt(cc_par_val)
var_com = np.array(var_com_new)
logging.info('Norm of update vector: ' + str(cc_par_val))
logging.info('Updated variances: ' +
' '.join(list(np.array(var_com_new, dtype=str))))
if cc_par_val < cc_par:
break
if cc_par_val < cc_par:
logging.info('Variances converged.')
else:
logging.info('Variances not converged.')
return var_com
pairwise_dists = squareform(pdist(Image_Data, 'euclidean'))
W = sp.exp(pairwise_dists**2 / sigma**2)
#compute the combinatorial Laplacian
d_i = W.sum(axis=1)
D = np.diag(d_i)
Delta = D - W
#computing the harmonic function - without any acquisitions yet
Delta_ll = Delta[0:X_train.shape[0], 0:X_train.shape[0]]
Delta_ul = Delta[X_train.shape[0]:, 0:X_train.shape[0]]
Delta_lu = Delta[0:X_train.shape[0], X_train.shape[0]:]
Delta_uu = Delta[X_train.shape[0]:, X_train.shape[0]:]
inv_Delta_uu = linalg.inv(Delta_uu)
Original_f_L = y_train
Delta_mult = np.dot(inv_Delta_uu, Delta_ul)
Original_f_U = -np.dot(Delta_mult, Original_f_L)
#f_I is the entire harmonic function over all the data points (U + L)
Original_f_I = np.concatenate((Original_f_L, Original_f_U), axis=0)
print(
'Compute Expected Bayes Risk for ALL Pool Points in Acquisition Iteration ',
i)
Bayes_Risk = np.zeros(shape=(X_Pool.shape[0]))
#use a subset of the pool points - for Q=10, use 500 pool point subset
Pool_Subset = 50
def process_state(t,y):
state = SimpleNamespace()
state.t = t
(p,v,R,o,delta_TLd,q,oq,qR,qo,V) = unpack_solution(y)
state.p = p
state.v = v
state.delta_TLd = delta_TLd
state.q = q
state.oq = oq
state.qR = qR
state.qo = qo
state.V = V
state.pd = pr(t).reshape(shape(p))
state.dpd = dpr(t).reshape(shape(p))
state.d2pd = d2pr(t).reshape(shape(p))
state.d3pd = d3pr(t).reshape(shape(p))
state.d4pd = d4pr(t).reshape(shape(p))
# state.d5pd = d5pr(t).reshape(shape(p))
# Load R(3) controller
Fd = F_feedback(state)
state.Fd = Fd
# Retrieve TLd - Cable tensions
(P, G) = matrix_nullspace()
# Minimum norm TLd
TLd_min = (P.T @ inv(P @ P.T) @ Fd).reshape(shape(delta_TLd))
state.TLd_min = TLd_min
# Repulsion for TL/cables not to be too close to each other
ddelta_TLd = TL_repulsive(TLd_min + delta_TLd) - TL_damping * delta_TLd
if(isnan(ddelta_TLd).any()):
print('NaN @ t = ', t)
# Project dTLd_star on null space of P
ddelta_TLd = (G @ G.T @ ddelta_TLd.reshape(-1,n_cables*n_dims,1)).reshape(shape(delta_TLd))
state.ddelta_TLd = ddelta_TLd
# Compute desired cable directions
# TODO: Include TLd derivatives
(TLd) = cable_tensions(state)
# TODO / DEBUG: Change for other method
(TLd_min, TLd_opt) = compute_TL(atleast_1d(t),Fd)
TLd = TLd_min
state.TLd = TLd
state.I_TLd = TLd
(qd) = q_desired_direction(state)
state.qd = qd
# Auxiliar variable - mu is projection of mud on current cable directions
mud = qd @ mt(qd) @ TLd
state.mud = mud
mu = q @ mt(q) @ TLd
state.mu = mu
# Load dynamics
dp = v
dR = 0*R
# Ideal actuation (actuation on "cable" level)
dv = Fd / mL + g*e3
do = 0*o
# Real actuation - Actuation is total force at quadrotor position
F = sum(mu, axis=-3, keepdims=True)
state.F = F
dv = F / mL + g*e3
do = 0*o
state.dv = dv
(Fd,dFd) = F_feedback(state)
state.dFd = dFd
dTLd_min = (P.T @ inv(P @ P.T) @ dFd).reshape(shape(delta_TLd))
state.dTLd_min = dTLd_min
(_, dTLd) = cable_tensions(state)
state.dTLd = dTLd
state.dI_TLd = dTLd
(_,dqd,oqd) = q_desired_direction(state)
state.dqd = dqd
state.oqd = oqd
dq = skew(oq) @ q
state.dq = dq
# Auxiliar variable - mu is projection of mud on current cable directions
dmud = (dqd @ mt(qd) @ TLd
+ qd @ mt(dqd) @ TLd
+ qd @ mt(qd) @ dTLd)
state.dmud = dmud
dmu = (dq @ mt(q) @ TLd
+ q @ mt(dq) @ TLd
+ q @ mt(q) @ dTLd)
state.dmu = dmu
dF = sum(dmu, axis=-3, keepdims=True)
state.dF = dF
d2v = dF / mL
state.d2v = d2v
(Fd,dFd,d2Fd) = F_feedback(state)
state.d2Fd = d2Fd
# TODO
d2delta_TLd = 0*ddelta_TLd
state.d2delta_TLd = d2delta_TLd
d2TLd_min = (P.T @ inv(P @ P.T) @ d2Fd).reshape(shape(delta_TLd))
state.d2TLd_min = d2TLd_min
(_,_,d2TLd) = cable_tensions(state)
state.d2TLd = d2TLd
state.d2I_TLd = d2TLd
(_,_,_,d2qd,doqd) = q_desired_direction(state)
state.d2qd = d2qd
state.doqd = doqd
state.dp = dp
state.dv = dv
state.dR = dR
state.do = do
# Cable dynamics
(q_actuation, e_q, e_oq) = q_feedback(q,oq,qd,oqd,doqd)
state.q_actuation = q_actuation
state.e_q = e_q
state.e_oq = e_oq
a = dv - g*e3
u_parallel = mu + mQ * lq * norm(oq, axis=-2, keepdims=True)**2 * q + mQ * q @ mt(q) @ a
# Actuation without cross-terms
u_perp = mQ * lq * skew(q) @ q_actuation - mQ * skew(q)@skew(q) @ a
pd = pr(t)
dpd = dpr(t)
d2pd = d2pr(t)
d3pd = d3pr(t)
pe = p-pd
ve = v-dpd
ae = dv-d2pd
zoq = (
- oq
- skew(q) @ skew(q) @ oqd
- 1 / L_q * (mt(qd) @ TLd) * skew(q) @ (
1/mL * (x_pv * pe + ve))
)
state.zoq = zoq
u_perp_d = (mQ * lq) * skew(q) @ (
- 1 / lq * skew(q) @ a
- skew(dq) @ skew(q) @ oqd
- skew(q) @ skew(dq) @ oqd
- skew(q) @ skew(q) @ doqd
- 1 / L_q * (
+ (mt(dqd) @ TLd) * skew(q)
+ (mt(qd) @ dTLd) * skew(q)
+ (mt(qd) @ TLd) * skew(dq)
) @ (
1/mL * (x_pv * pe + ve)
)
- 1 / L_q * (mt(qd) @ TLd) * skew(q) @ (
1/mL * (x_pv * ve + ae)
)
+ koq / L_oq * zoq
+ L_q / L_oq * skew(q) @ qd
)
# Actuation with cross-terms
if not NO_U_PERP_CROSS_TERMS:
u_perp = u_perp_d
state.u_perp_d = u_perp_d
state.a = a
state.u_parallel = u_parallel
state.u_perp = u_perp
u = u_perp + u_parallel
state.u = u
doq = 1 / lq * skew(q) @ a - 1 / (mQ * lq) * skew(q) @ u_perp
state.doq = doq
# Quadrotor dynamics
# DEBUG: Independent Rd control
(qRd,qod,qtaud) = qR_desired(t,u)
qtau = tau_feedback(qR,qo,qRd,qod,qtaud)
dqR = qR @ skew(qo)
dqo = inv_J @ qtau - inv_J @ skew(qo)@J@qo
state.qRd = qRd
state.qod = qod
state.qtaud = qtaud
state.qtau = qtau
state.dqR = dqR
state.dqo = dqo
qFd = u
r3 = qR @ e3
qT = - mt(r3) @ qFd
state.qFd = qFd
state.qT = qT
state.qF = qT * - r3
# Lyapunov verification
de_q = cross(dqd,q,axis=-2) + cross(qd,dq,axis=-2)
state.de_q = de_q
dV = (
- kp*x_pv * mt(pe) @ (pe)
- kv*x_pv * mt(pe) @ (ve)
- (kv - x_pv) * mt(ve) @ ve
- koq * sum( mt( zoq ) @ ( zoq )
, axis=-3, keepdims=True)
)
state.dV = dV
# Output time derivative as 1-d array and not a 2D nx1 array
# print(state.dp.shape)
# print(state.dv.shape)
# print(state.dR.shape)
# print(state.do.shape)
# print(state.ddelta_TLd.shape)
# print(state.dq.shape)
# print(state.doq.shape)
# print(state.dqR.shape)
# print(state.dqo.shape)
# print(state.dV.shape)
dy = pack_solution(state.dp,
state.dv,
state.dR,
state.do,
state.ddelta_TLd,
state.dq,
state.doq,
state.dqR,
state.dqo,
state.dV,
).ravel()
# if atleast_1d(t % 1 < 1e-3).all():
# print(f"t = {t}")
return dy, state
for m in range(2):
M_loc[l][m] = (h[k] / 2) * np.sum(
psi_q[l, :] * psi_q[m, :] * gaussW)
K_loc[l][m] = 2 * (2 / h[k]) * dpsi[l] * dpsi[m]
# combine local terms to get global matrix
for l in range(2):
global_node = loc2glob_map[k][l]
for m in range(2):
global_node2 = loc2glob_map[k][m]
K[global_node][global_node2] += K_loc[l][m]
M[global_node][global_node2] += M_loc[l][m]
# calculate inverses
B = (1 / dt) * M + K
B_inv = inv(B)
B_inv_copy = np.copy(B_inv)
B_inv_M = B_inv @ M
B_inv_M_copy = np.copy(B_inv_M)
# initialize u by evaluating it at boundary condition
u = evaluate(u_0, local_dict={'x': x}, global_dict=global_expr)
for t in time:
F = np.zeros(numN)
for k in range(numE):
x_quad = (h[k] / 2) * (gaussP +
1) + x[k] # map quadrature points back to x
for l in range(2):
F_quad = evaluate(f,
kqR = 100 kqo = 10 # DEBUG TL_repulsive_gain = 0.0 TL_damping = 0.0 mL = 0.3 mass_quad = 1.50 mQ = mass_quad * ones([n_cables,1,1]) g = 9.8 # inertia matrix for the quadrotors J = diag([0.21, 0.22, 0.23]) inv_J = inv(J) J = stack(n_cables*[J]) inv_J = stack(n_cables*[inv_J]) e1 = array([[1,0,0]]).T e2 = array([[0,1,0]]).T e3 = array([[0,0,1]]).T # Minimum angle with vertical of z-body direction when optimizing theta_min_for_optimization = deg2rad(20) # ## Load trajectory # In[ ]:
matriz[i][j] = int(
input("Introduce la cantidad en la ubicacion (%d,%d): " %
(i + 1, j + 1)))
else:
for i in range(3):
for j in range(3):
matriz[i][j] = random.randint(1, 100)
m = np.array(matriz)
print("Mostrando la matriz ingresada: ")
print(m)
print("")
try:
i = linalg.inv(matriz)
print("Mostrando la matriz inversa")
print("")
print(i)
print("")
I = m.dot(i)
print("Comprobando los resultados")
print("")
print(I)
print("")
print(
"Si gusta comprobar la matriz multiplicandola, le dejo una pagina web muy buena"
)
print("https://matrix.reshish.com/es/inverse.php")
str(abs(best_det)),
end="\r",
flush=True)
return best_i, best_j, best_k, best_det
# %%
best_i, best_j, best_k, best_det = find_linear_independent_initial()
# %%
states = np.array(
[histories[best_i][:NS], histories[best_j][:NS], histories[best_k][:NS]])
#assert(np.round(abs(det(states)), decimals=2) == 1.0)
# Find what linear combination would bring to the desired initial state psi_0_n
coeffs = inv(states.T) @ psi_0_n
# %%
history = coeffs.dot(
np.array([histories[best_i], histories[best_j], histories[best_k]]))
# %%
# 3D parametric plot
times_discrete = np.diag(T)
psi = history.reshape((-1, NS)).T
for (vertical_angle, horizontal_angle, height,
width) in (10, -120, 15, 13), (60, -45, 13, 13):
fig = plt.figure(figsize=(width, height))
