def ToStateSpace(M, K, G, T_u, T_w):
shape = np.shape(M)
A = np.block([[np.zeros((shape[0], shape[0])),
np.identity(shape[0])], [-np.inv(M) * K, -np.inv(M) * G]])
B = np.block([[np.zeros((shape[0], 1)),
np.identity(shape[0])], [-np.inv(M) * K, -np.inv(M) * G]])
return (sys)
def _getDirec(P, M, u=1, v=0):
direc0 = np.dot(np.inv(P), [u, v, 1, 1])
direc0 = direc0 / direc0[-1]
orig0 = np.dot(np.inv(P), [u, v, -1, 1])
direc0 = direc0 - orig0
direc0 = direc0 / np.linalg.norm(direc0)
return np.dot(np.inv(M), direc0)
def _getDirec(P, M, u=1, v=0):
direc0 = np.dot(np.inv(P), [u, v, 1, 1])
direc0 = direc0/direc0[-1];
orig0 = np.dot(np.inv(P), [u, v, -1, 1]);
direc0 = direc0-orig0;
direc0 = direc0/np.linalg.norm(direc0)
return np.dot(np.inv(M), direc0)
def polyfitu(x, y, dy, N):
#n = len(y) # number of data points
dysq = dy**2 # Speed up computation
wtsq = 1. / dysq # Speed up computation
# Form the vector of the Sigma
kk = np.linspace(1, N + 1, N + 1).reshape(-1, 1)
Sigma_y = sum(y * x**(kk - 1) * wtsq, 1)
Sigma_xx = np.zeros((N + 1, N + 1))
for k in range(1, N + 1):
for j in range(k, N + 1):
Sigma_xx[j, k] = np.sum(x**(k - 1) * x**(j - 1) * wtsq)
if k == j:
Sigma_xx[k][j] = Sigma_xx[j][k]
Sigma_xx_inv = np.inv(Sigma_xx)
p = Sigma_y * Sigma_xx_inv
xvector = p.zeros((N + 1, n))
xvector[kk, :] = x**(kk - 1) * wtsq
dpdy = Sigma_xx_inv * xvector
CM = npzeros((N + 1, N + 1))
for k in range(1, N + 1):
for j in range(k, N + 1):
CM[j, k] = np.sum(dpdy[k, :] * dpdy[j, :] * dysq)
if k == j:
CM[k, j] = CM[j, k]
dp = np.sqrt(np.diag(CM))
def lfsrsolve(v, n):
"""Given a guess n for the length of the recurrence that generates
the binary vector v, this function returns the coefficients of the
recurrence."""
v = v[:]
vln = len(v)
if (vln < 2 * n):
raise ValueError('The vector v needs to be atleast length 2n')
M = np.array(circmat(v, n))
Mdet = np.det(M)
x = v[n + 2:2 * n]
Minv = np.inv(M)
Minv = np.mod(np.round(Minv * Mdet), 2)
# A note: Technically, the round() function should never show up, but
# since Matlab does double precision arithmetic to calculate the inverse matrix
# we need to bring the result back to integer values so we can perform a meaningful
# mod operation. As long as this routine is not used on huge examples, it should
# be ok
y = np.mod(Minv * x, 2)
y = y[:].T # Convert the output to a row vector
return y
def red_obs(sys, T, poles):
"""Reduced order observer of the system sys
Call:
obs=red_obs(sys,T,poles)
Parameters
----------
sys : System in State Space form
T: Complement matrix
poles: desired observer poles
Returns
-------
obs: ss
Reduced order Observer
"""
if isinstance(sys, TransferFunction):
"System must be in state space form"
return
a = np.mat(sys.A)
b = np.mat(sys.B)
c = np.mat(sys.C)
d = np.mat(sys.D)
T = np.mat(T)
P = np.mat(np.vstack((c, T)))
invP = np.inv(P)
AA = P * a * invP
ny = np.shape(c)[0]
nx = np.shape(a)[0]
nu = np.shape(b)[1]
A11 = AA[0:ny, 0:ny]
A12 = AA[0:ny, ny:nx]
A21 = AA[ny:nx, 0:ny]
A22 = AA[ny:nx, ny:nx]
L1 = place(A22.T, A12.T, poles)
L1 = np.mat(L1).T
nn = nx - ny
tmp1 = np.mat(np.hstack((-L1, np.eye(nn, nn))))
tmp2 = np.mat(np.vstack((np.zeros((ny, nn)), np.eye(nn, nn))))
Ar = tmp1 * P * a * invP * tmp2
tmp3 = np.vstack((np.eye(ny, ny), L1))
tmp3 = np.mat(np.hstack((P * b, P * a * invP * tmp3)))
tmp4 = np.hstack((np.eye(nu, nu), np.zeros((nu, ny))))
tmp5 = np.hstack((-d, np.eye(ny, ny)))
tmp4 = np.mat(np.vstack((tmp4, tmp5)))
Br = tmp1 * tmp3 * tmp4
Cr = invP * tmp2
tmp5 = np.hstack((np.zeros((ny, nu)), np.eye(ny, ny)))
tmp6 = np.hstack((np.zeros((nn, nu)), L1))
tmp5 = np.mat(np.vstack((tmp5, tmp6)))
Dr = invP * tmp5 * tmp4
obs = StateSpace(Ar, Br, Cr, Dr, sys.dt)
return obs
def invMassMatrix(obj):
"""Returns the inverse of obj's generalized mass matrix
[H 0 ]-1
[0 mI]
about the origin."""
try:
import numpy
except ImportError:
raise "invMassMatrix(obj) needs numpy"
Hinv = numpy.zeros((6,6))
if obj == None or isinstance(obj,TerrainModel):
#infinite inertia
return Hinv
if isinstance(obj,RobotModel):
return obj.getMassMatrixInv()
m = obj.getMass()
minv = 1.0/m.mass
Hinv[3,3]=Hinv[4,4]=Hinv[5,5]=minv
#offset the inertia matrix about the COM
H = numpy.array((3,3))
H[0,:] = numpy.array(m.inertia[0:3])
H[1,:] = numpy.array(m.inertia[3:6])
H[2,:] = numpy.array(m.inertia[6:9])
H -= skew(m.com)*skew(m.com)*m.mass
Hinv[0:3,0:3] = numpy.inv(H)
return Hinv
def invMassMatrix(obj):
"""Returns the inverse of obj's generalized mass matrix
[H 0 ]-1
[0 mI]
about the origin."""
try:
import numpy
except ImportError:
raise RuntimeError("invMassMatrix(obj) needs numpy")
Hinv = numpy.zeros((6, 6))
if obj == None or isinstance(obj, TerrainModel):
#infinite inertia
return Hinv
if isinstance(obj, RobotModel):
return obj.getMassMatrixInv()
m = obj.getMass()
minv = 1.0 / m.mass
Hinv[3, 3] = Hinv[4, 4] = Hinv[5, 5] = minv
#offset the inertia matrix about the COM
H = numpy.array((3, 3))
H[0, :] = numpy.array(m.inertia[0:3])
H[1, :] = numpy.array(m.inertia[3:6])
H[2, :] = numpy.array(m.inertia[6:9])
H -= skew(m.com) * skew(m.com) * m.mass
Hinv[0:3, 0:3] = numpy.inv(H)
return Hinv
def computeKalmanGain(self):
# Using Eq (5.15) on Pg 127
P = self.EstimationErrorCovarianceMatrix
H = self.StateToMeasurementMatrix
R = self.MeasurementNoiseCovarianceMatrix
KalmanGain = P @ np.transpose(H) @ np.inv(R)
self.KalmanGain = KalmanGain
def solve(policy):
"""
Given a policy this function evaluates it
"""
sim = Simulator()
pr = sim.get_transition_model()
rew = sim.get_reward_model()
dim = len(rew)
pa0 = np.squeeze( pr[:, 0, :])
pa1 = np.squeeze( pr[:, 1, :])
prob = np.zeros((dim, dim))
for i in range(1, dim+1):
a = policy.select_action(i)[0]
if a == 0:
prob[i,:] = pa0[i, :]
else:
prob[i,:] = pa1[i, :]
v = np.inv(np.eye(dim) - policy.discount*prob).dot(rew)
q1 = rew + policy.discount * pa0.dot(v)
q2 = rew + policy.discount * pa1.dot(v)
q = max(q1, q2)
return v, q1, q2, q
def cramer_rao(model, p0, X, noise, show_plot=False):
"""Calulate inverse of the Fisher information matrix for model
sampled on grid X with parameters p0. Assumes samples are not
correlated and have equal variance noise^2.
Parameters
----------
model : callable
The model function, f(x, ...). It must take the independent
variable as the first argument and the parameters as separate
remaining arguments.
X : array
Grid where model is sampled.
p0 : M-length sequence
Point in parameter space where Fisher information matrix is
evaluated.
"""
labels = model
p0dict = {'a': 2, 'b': 0, 'c': 8} #dict(zip(['a', 'b', 'c'], p0))
D = np.zeros((len(p0), X.size))
for i, argname in enumerate(labels):
D[i, :] = [
derivative(lambda p: model(x, **dict(p0dict, **{argname: p})),
p0dict[argname],
dx=0.0001) for x in X
]
I = 1 / noise**2 * np.einsum('mk,nk', D, D)
iI = np.inv(I)
return iI
def sample_song(self, j):
genres = self.genres[j]
song = self.songs[j]
sgc = self.song_genre_count[j]
cdf = np.zeros(self.K)
pdf = np.zeros(self.K)
for i in xrange(len(song)):
# Hold out this segment
k_old = genres[i]
self.timbre_totals[k_old] -= song[i]
self.genre_count[k_old] -= 1
sgc[k_old] -= 1
# Compute p(k_ji | everything else)
timbre_mu = self.genre_timbre_mean()
pdf = (sgc + self.alpha) / (len(song) + self.K * self.alpha)
(mu, sigma) = self.genre_timbre_posterior()
# TODO: Improve the prior
for k in xrange(self.K):
dev = mu[k] - song[i]
suf_stat = dev * np.inv(sigma) * dev.reshape(self.T, 1)
pdf[k] *= (np.abs(sigma)**-0.5) * np.exp(-suf_stat)
# Sample new genre
np.cumsum(pdf, out=cdf)
k_new = bisect(cdf, np.random.random() * cdf[-1])
self.timbre_totals[k_new] += song[i]
self.genre_count[k_new] += 1
sgc[k_new] += 1
def gauss_pdf(X, M, S):
if M.shape[1] == 1:
DX = X - np.tile(M, X.shape[1])
E = 0.5 * np.sum(DX * (np.dot(np.linalg.inv(S), DX)), axis=0)
E = E + 0.5 * M.shape[0] * np.log(2 * np.pi) + 0.5 * np.log(
np.linalg.det(S))
P = np.exp(-E)
elif X.shape[1] == 1:
DX = np.tile(X, M.shape()[1]) - M
E = 0.5 * np.sum(DX * (np.dot(np.linalg.inv(S), DX)), axis=0)
E = E + 0.5 * M.shape[0] * np.log(2 * np.pi) + 0.5 * np.log(
np.linalg.det(S))
P = np.exp(-E)
else:
DX = X - M
E = 0.5 * np.dot(DX.T, np.dot(np.inv(S), DX))
E = E + 0.5 * M.shape[0] * np.log(2 * np.pi) + 0.5 * np.log(np.det(S))
P = np.exp(-E)
return (P[0], E[0])
def xyz2enu(u,v,w,head,pitch,roll,inverse=False):
"""Transforms volcities in XYZ coordinates to ENU, or vice versa if
inverse=True. Transformation is done according to the Nortek
convention
"""
# convert to radians
hh = np.pi*(head-90)/180
pp = np.pi*pitch/180
rr = np.pi*roll/180
ut = np.NaN(shape(u))
vt = np.NaN(shape(u))
wt = np.NaN(shape(u))
for i in range(len(head)):
# generate heading matrix
H = np.matrix([[cos(hh[i]), sin(hh[i]), 0],[-sin(hh[i]), cos(hh[i]), 0],[0, 0, 1]])
# generate combined pitch and roll matrix
P = [[cos(pp[i]), -sin(pp[i])*sin(rr[i]), -cos(rr[i])*sin(pp[i])],
[0, cos(rr[i]), -sin(rr[i])],
[sin(pp[i]), sin(rr[i])*cos(pp[i]), cos(pp[i])*cos(rr[i])]]
R = H*P
print(R)
if(inverse):
R = np.inv(R)
# do transformation
ut[i] = R[0,0]*u[i] + R[0,1]*v[i] + R[0,2]*w[i];
vt[i] = R[1,0]*u[i] + R[1,1]*v[i] + R[1,2]*w[i];
wt[i] = R[2,0]*u[i] + R[2,1]*v[i] + R[2,2]*w[i];
def sample_song(self, j):
genres = self.genres[j]
song = self.songs[j]
sgc = self.song_genre_count[j]
cdf = np.zeros(self.K)
pdf = np.zeros(self.K)
for i in xrange(len(song)):
# Hold out this segment
k_old = genres[i]
self.timbre_totals[k_old] -= song[i]
self.genre_count[k_old] -= 1
sgc[k_old] -= 1
# Compute p(k_ji | everything else)
timbre_mu = self.genre_timbre_mean()
pdf = (sgc + self.alpha) / (len(song) + self.K * self.alpha)
(mu, sigma) = self.genre_timbre_posterior()
# TODO: Improve the prior
for k in xrange(self.K):
dev = mu[k] - song[i]
suf_stat = dev * np.inv(sigma) * dev.reshape(self.T, 1)
pdf[k] *= (np.abs(sigma) ** -0.5) * np.exp(-suf_stat)
# Sample new genre
np.cumsum(pdf, out=cdf)
k_new = bisect(cdf, np.random.random() * cdf[-1])
self.timbre_totals[k_new] += song[i]
self.genre_count[k_new] += 1
sgc[k_new] += 1
def solveNormal(self):
"""Sets A, b, C, D, and e to the result of a normal autoregression"""
n = len(self.trials[0][0])
mean0 = np.zeros(n)
mean1 = np.zeros(n)
mean2 = np.zeros(n)
cov0 = np.zeros((n, n))
cov = np.zeros((2 * n, 2 * n))
num = 0
for trial in self.trials:
mean0 += trial[0]
for (xi, xn) in zip(trial[:-1], trial[1:]):
mean1 += xi
mean2 += xn
num += 1
mean0 /= len(self.trials)
mean1 /= num
mean2 /= num
for trial in self.trials:
cov0 += np.outer(trial[0] - mean0, trial[0] - mean0)
for (xi, xn) in zip(trial[:-1], trial[1:]):
xin = np.hstack([xi - mean1, xn - mean2])
cov += np.outer(xin, xin)
cov0 /= len(self.trials)
cov /= num
cov11 = cov[0:n, 0:n]
cov12 = cov[0:n, n:2 * n]
cov22 = cov[n:2 * n, n:2 * n]
cov11inv = np.linalg.inv(cov11)
Aall = np.dot(cov12.T, cov11inv)
ball = mean2 - np.dot(Aall, mean1)
#Do schur complement
#[x[t]] = [A11 A12]*[x[t-1]]+[b1]
#[y[t]] [A21 A22] [y[t-1]] [b2]
#A11^-1 (x[t]-b1-A12 y[t-1]) = x[t-1]
#y[t] = A21 x[t-1] + A22 y[t-1] + b2
# = A21 A11^-1 x[t] + (A22 - A21 A11^-1 A12) y[t-1] + A21 A11^-1 b2
xind = self.observedindices
yind = self.unobservedindices
A11inv = np.inv(Aall[xind, xind])
self.A = np.dot(cov0[yobs, xobs], np.inv(cov0[xobs, xobs]))
self.b = mean0[yobs] - np.dot(self.A, mean0[xobs])
self.C = np.dot(Aall[yind, xind], A11inv)
self.D = Aall[yind, yind]
self.D -= np.dot(Aall[yind, xind], np.dot(A11inv, Aall[xind, yind]))
self.e = ball[yind] - np.dot(self.C, ball[xind])
return
def ilc_gary(Freq, dbeta): # ilc recipies used by Gary
Spec_d = fv_d_pl(Freq, dbeta)
Spec_y = fv_y(Freq)
# Spec_y = array([-1.506, -1.037, -0.001, 2.253])
# Spec_y = np.array([-4.031, -2.785, 0.187, 6.205]) / Tcmb
Nf = len(Freq)
Sigma = np.ones(Nf)
Ns = 3
if (Nf < Ns):
print "not enough freedom"
sys.exit()
# Populate the linear system to solve for the coefficients
Matrix = np.zeros((Nf, Nf))
Matrix[0] = Sigma
Matrix[1] = Spec_d
Matrix[2] = Spec_y
RHS = np.zeros(Nf)
RHS[2] = 1.
# Soln = dot(pinv(Matrix), RHS)
U, W, V = np.linalg.svd(Matrix)
V = V.transpose()
WW = np.zeros((Nf, Nf))
for M in range(Nf):
if (W[M] != 0.):
WW[M, M] = 1. / W[M]
Soln = np.dot(np.dot(np.dot(V, WW), U.transpose()), RHS)
# Probe the degeneracy of the system
Nd = Nf - Ns
if (Nd > 0):
Degenerate = np.where(W < 1e-12 * max(abs(W)))[0]
if (len(Degenerate) != Nd):
print "Degenerate direction(s) not properly identified"
sys.exit()
Deg_Soln = V.T[Degenerate]
# Minimize the degenerate solutions relative to the overall sensitivity
if (Nd == 1):
Alpha = -sum(Soln * Deg_Soln * (Sigma**2)) / sum(
(Deg_Soln**2) * (Sigma**2))
Soln1 = Soln + Alpha * Deg_Soln
else:
M_Alpha = np.zeros((Nd, Nd))
RHS_Alpha = np.zeros(Nd)
for i in range(Nd):
RHS_Alpha[i] = -sum(Soln * Deg_Soln[i] * Sigma**2)
for j in range(Nd):
M_Alpha[i, j] = sum(Deg_Soln[i] * Deg_Soln[j] * Sigma**2)
Alpha = np.dot(np.inv(M_Alpha), RHS_Alpha)
Soln_0 = Soln
Soln = Soln_0 + np.dot(Deg_Soln.T, Alpha)
xd100 = h * 100e9 / (k * Tcmb)
Soln /= (xd100 / np.tanh(xd100 / 2.) - 4)
return Soln
def _analyze_results(opti_opts, bpe_results, jacobian, normalized=False):
r"""Analyze the results."""
# hard-coded values
min_eig = 1e-14 # minimum allowed eigenvalue
# alias the log level
log_level = Logger().get_level()
# get the names and number of parameters
num_params = len(bpe_results.param_names)
# update the status
if log_level >= 5:
print('Analyzing final results.')
if log_level >= 8:
print('There were a total of {} function model evaluations.'.format(bpe_results.num_evals))
# exit if nothing else to analyze
if opti_opts.max_iters == 0:
return
# Compute values of un-normalized parameters.
if normalized:
param_typical = OptiParam.get_array(opti_opts.params, type_='typical')
normalize_matrix = np.eye(num_params) @ (1 / param_typical)
else:
normalize_matrix = np.eye(num_params)
# Make information, covariance matrix, compute Singular Value Decomposition (SVD).
try:
# note, python has x = U*S*Vh instead of U*S*V', when V = Vh'
(_, S_jacobian, Vh_jacobian) = np.linalg.svd(jacobian @ normalize_matrix, full_matrices=False)
V_jacobian = Vh_jacobian.T
temp = np.power(S_jacobian, -2, out=np.zeros(S_jacobian.shape), where=S_jacobian > min_eig)
covariance = V_jacobian @ np.diag(temp) @ Vh_jacobian
except MemoryError:
if log_level >= 6:
print('Singular value decomposition of Jacobian failed.')
V_jacobian = np.nan * np.ones((num_params, num_params))
covariance = np.inv(jacobian.T @ jacobian)
param_one_sigmas = np.sqrt(np.diag(covariance))
param_one_sigmas[param_one_sigmas < min_eig] = np.nan
correlation = covariance / (param_one_sigmas[:, np.newaxis] @ param_one_sigmas[np.newaxis, :])
covariance[np.isnan(correlation)] = np.nan
# Update SVD and covariance for the normalized parameters (but correlation remains as calculated above)
if normalized:
try:
(_, S_jacobian, Vh_jacobian) = np.linalg.svd(jacobian, full_matrices=False)
V_jacobian = Vh_jacobian.T
covariance = V_jacobian @ np.diag(S_jacobian**-2) @ Vh_jacobian
except MemoryError:
pass # caught in earlier exception (hopefully?)
# update the results
bpe_results.correlation = correlation
bpe_results.info_svd = V_jacobian.T
bpe_results.covariance = covariance
def utuh(self, u, mlh):
no = u.ndim
n = no - 1
tmp = np.rot90(u, 2)
z = np.fft.ifft(np.vstack(tmp, u[1:n]))
z = 2 * np.real(z[0:no])
ul = np.inv(mlh) * z
return ul
def continuously_updated_gmm():
# First step uses identity weighting matrix
W = lambda b: np.inv(self.Omegahat(b))
bhat = self.minimize(lambda b: self.J(b, W(b)))
return bhat
def inv_matrix(self) -> np.ndarray:
"""
Inverse of lattice matrix.
:return: Inverse of lattice matrix
:rtype: np.ndarray
"""
return np.inv(self.matrix)
def twoPosition(P2x, P2y, theta1, theta2, thetaA, phiPm, betaA, Rho, ThetaBP,
Gamma):
p21 = math.sqrt((P2x)**2 + (P2y)**2)
alpha2 = theta2 - theta1
delta2 = math.atan2(P2y, P2x)
chosenMatrix = [[
math.cos(thetaA + betaA) - math.cos(thetaA),
math.cos(phiPm + alpha2) - math.cos(phiPm)
],
[
math.sin(thetaA + betaA) - math.sin(thetaA),
math.sin(phiPm + alpha2) - math.sin(phiPm)
]]
knownMatrix = [p21 * math.cos(delta2), p21 * math.sin(delta2)]
invA = np.inv(chosenMatrix)
unknown = knownMatrix * invA
wMagnitude = unknown[0]
zMagnitude = unknown[1]
P21 = v.Vector(p21, delta2)
W1 = v.Vector(wMagnitude, thetaA)
W2 = v.Vector(wMagnitude, thetaA + betaA)
Z1 = v.Vector(zMagnitude, phiPm)
Z2 = v.Vector(zMagnitude, phiPm + alpha2)
chosenMatrix = [[
math.cos(Rho + Gamma) - math.cos(Rho),
math.cos(ThetaBP + alpha2) - math.cos(ThetaBP)
],
[
math.sin(Rho + Gamma) - math.sin(Rho),
math.sin(ThetaBP + alpha2) - math.sin(ThetaBP)
]]
knownMatrix = [p21 * math.cos(delta2), p21 * math.sin(delta2)]
invA = np.inv(chosenMatrix)
unknown = knownMatrix * invA
uMagnitude = unknown[0]
sMagnitude = unknown[1]
U1 = v.Vector(uMagnitude, Rho)
U2 = v.Vector(uMagnitude, Rho + Gamma)
S1 = v.Vector(sMagnitude, ThetaBP)
S2 = v.Vector(sMagnitude, ThetaBP + alpha2)
def doSomething(task_num):
#print("executing...", task_num)
for i in range(100000):
A = np.random.normal(0, 1, (1000, 1000))
B = np.inv(A)
return random.randint(1, 10) * random.randint(
1, 500) # real operation, used random to avoid caches and so on...
def projection_gradient(f,
grad_f,
xs,
constraint_fs,
deriv_contrains,
ε1,
ε2=None,
MAX_ITERATIONS=10000):
if ε2 is None:
ε2 = ε1
initial_constr_n = len(constraint_fs)
start_xs = xs
if not all(constr_f(xs) <= 0 for constr_f in constraint_fs):
raise Exception('xStart not in domain')
for it in range(MAX_ITERATIONS):
grad_val = grad_f(xs)
grad_matrix = np.array([grad_val]).transpose()
border_vals = [constr_f(xs) for constr_f in constraint_fs]
passive_constraints = {
i
for i, bv in enumerate(border_vals) if not (ε1 <= bv <= 0)
}
if len(passive_constraints) != initial_constr_n:
matr = np.array([[f_i(xs) for f_i in row]
for i, row in enumerate(deriv_contrains)
if deriv_contrains not in passive_constraints])
m = matr.transpose() @ np.inv(matr @ matr.transpose()) @ matr
p = np.eye(len(m)) - m
dx = (-p @ grad_matrix)[:, 0]
else:
dx = (-grad_matrix)[:, 0]
distamce_to_borders = []
for i, constr_f in enumerate(constraint_fs):
if i in passive_constraints:
continue
zero_val, _, check, _ = fsolve(
lambda a: constr_f(start_xs + a * dx),
np.eye(1),
)
if zero_val >= 0 and check == 1:
distamce_to_borders.append(zero_val[0])
arg_min = optimize.minimize_scalar(lambda a: f(xs + a * dx)).x
arg = min(arg_min, distamce_to_borders)
dx *= arg
xs += dx
if np.linalg.norm(dx) < ε2:
return xs, f(xs)
print('Reached the end before convergence')
return xs, f(xs)
def map_kpoints(other_kpoints, other_lattice, ref_lattice, ref_kpoints, ref_symrecs, has_timrev):
"""
Build mapping between a list of k-points in reduced coordinates (`other_kpoints`)
in the reciprocal lattice `other_lattice` and a list of reference k-points given
in the reciprocal lattice `ref_lattice` with symmetry operations `ref_symrecs`.
Args:
other_kpoints:
other_lattice: matrix whose rows are the reciprocal lattice vectors in cartesian coordinates.
ref_lattice: same meaning as other_lattice.
ref_kpoints:
ref_symrecs: [nsym,3,3] arrays with symmetry operations in the `ref_lattice` reciprocal space.
has_timrev: True if time-reversal can be used.
Returns
(o2r_map, nmissing)
nmissing:
Number of k-points in ref_kpoints that cannot be mapped onto ref_kpoints.
o2r_map[i] gives the mapping between the i-th k-point in other_kpoints and
ref_kpoints. Set to None if the i-th k-point does not have any image in ref.
Each entry is a named tuple with the following attributes:
ik_ref:
tsign:
isym
g0
kpt_other = TS kpt_ref + G0
"""
ref_gprimd_inv = np.inv(np.asarray(ref_lattice).T)
other_gprimd = np.asarray(other_lattice).T
other_kpoints = np.asarray(other_kpoints).reshape((-1, 3))
ref_kpoints = np.asarray(ref_kpoints).reshape((-1, 3))
o2r_map = len(other_kpoints) * [None]
tsigns = (1, -1) if has_timrev else (1,)
kmap = collections.namedtuple("kmap", "ik_ref, tsign, isym, g0")
for ik_oth, okpt in enumerate(other_kpoints):
# Get other k-point in reduced coordinates in the referece lattice.
okpt_red = np.matmul(ref_gprimd_inv, np.matmul(other_gprimd, okpt))
# k_other = TS k_ref + G0
found = False
for ik_ref, kref in enumerate(ref_kpoints):
if found: break
for tsign in tsign:
for isym, symrec in enumerate(ref_symrecs):
krot = tsign * np.matmul(symrec, kref)
if issamek(okpt_red, krot):
g0 = np.rint(okpt_red - krot)
o2r_map[ik_oth] = kmap(ik_ref, tsign, isym, g0)
found = True
break
return o2r_map, o2r_map.count(None)
def update(state, time, cov):
self.health = 50
(X_k, P_k) = prediction(time - self.lasttime)
residual = state - np.dot(Obsticle.H, X_k)
S_k = np.dot(Obsticle.H, np.dot(P_k, Obsticle.H.T)) + np.diag(
[cov, cov, cov, cov])
K_k = np.dot(P_k, np.dot(Obtsicle.H.T, np.inv(S_k)))
self.state = X_k + np.dot(K_k, residual)
self.cov = P_k - np.dot(K_k, np.dot(S_k, K_k.T))
def log_likelihood_basis(transition, p_basis, rate, lagtime):
overlap = calc_overlap_basis(p_basis)
rhs = calc_rhs_diffusionequation_basisfunctions(p_basis, rate)
vals, vecs = np.linalg.eig(np.dot(np.inv(overlap), rhs))
# lagtime:
prop = np.sum(vecs * np.exp(lagtime * vals), axis=X)
pass
def calculate_w(self, grad_h_x: np.array, s: np.array) -> np.array:
# W = P*grad_h_x'*inv(S)
w1 = np.inv(s)
w2 = grad_h_x.transpose()
w2 = np.matmul(self.p, w2)
w = np.matmul(w2, w1)
return w
def log_likelihood_basis(transition,p_basis,rate,lagtime):
overlap = calc_overlap_basis(p_basis)
rhs = calc_rhs_diffusionequation_basisfunctions(p_basis,rate)
vals,vecs = np.linalg.eig(np.dot(np.inv(overlap),rhs))
# lagtime:
prop = np.sum(vecs*np.exp(lagtime*vals),axis=X)
pass
def _computer_delta(self, X, Y, r=0.0):
"""
computer delta of each net, dcorr(X, Y) / dX
PARAMETER:
X, m * n1 matrix, is one kind of input data modality, and H1 is transposed matrix of X
m is number of samples, which also means number of observations
n1 is number of features in modality X, which also means number of variables
Y, m * n2 matrix, is another kind of input data modality, and H2 is transposed matrix of Y
m is number of samples, which also means number of observations
n2 is number of features in modality Y, which also means number of variables
r is regularized parameter
References:
G. Andrew, R. Arora, J. Bilmes, and K. Livescu. Deep canonical correlation analysis. In ICML, 2013.
"""
m1, n1 = X.shape
m2, n2 = Y.shape
assert m1 == m2, 'DCCA computer delta: Input data need to contain same number of samples'
X_mean = np.mean(X, axis=0)
Y_mean = np.mean(X, axis=0)
H_1 = (X - X_mean).T # H1 bar
H_2 = (Y - Y_mean).T # H2 bar
sigma11 = (1 / m1 - 1) * H_1.dot(H_1.T) + r * np.eye(n1)
sigma12 = (1 / m1 - 1) * H_1.dot(H_2.T)
sigma21 = (1 / m1 - 1) * H_2.dot(H_1.T)
sigma22 = (1 / m1 - 1) * H_2.dot(H_2.T) + r * np.eye(n2)
assert np.linalg.det(sigma11) != 0, 'DCCA computer delta: sigma11 is singular, ' \
'please use regularized parameter to adapt this matrix'
assert np.linalg.det(sigma22) != 0, 'DCCA computer delta: sigma22 is singular, ' \
'please use regularized parameter to adapt this matrix'
T1 = np.inv(np.sqrt(sigma11))
T3 = np.inv(np.sqrt(sigma22))
T = T1.dot(sigma12).dot(T3) # T1 dot sigma12 dot T3
U, D, V = np.linalg.svd(T)
delta12 = functools.reduce(lambda x, y: np.dot(x, y), [T1, U, V, T3])
delta11 = -0.5 * functools.reduce(lambda x, y: np.dot(x, y),
[T1, U, D, U.T, T1])
dcorr = (1 / m1 - 1) * (2 * np.dot(delta11, H_1) +
np.dot(delta12, H_2))
return dcorr
def plante(ex, ey, ep, D, eq=None):
Dshape = D.shape
if Dshape[0] != 3:
raise NameError('Wrong constitutive dimension in plante')
if ep[0] == 1:
return tri3e(ex, ey, D, ep[1], eq)
else:
Dinv = np.inv(D)
return tri3e(ex, ey, Dinv, ep[1], eq)
def _computer_delta(self, X, Y, r=0.0):
"""
computer delta of each net, dcorr(X, Y) / dX
PARAMETER:
X, m * n1 matrix, is one kind of input data modality, and H1 is transposed matrix of X
m is number of samples, which also means number of observations
n1 is number of features in modality X, which also means number of variables
Y, m * n2 matrix, is another kind of input data modality, and H2 is transposed matrix of Y
m is number of samples, which also means number of observations
n2 is number of features in modality Y, which also means number of variables
r is regularized parameter
References:
G. Andrew, R. Arora, J. Bilmes, and K. Livescu. Deep canonical correlation analysis. In ICML, 2013.
"""
m1, n1 = X.shape
m2, n2 = Y.shape
assert m1 == m2, 'DCCA computer delta: Input data need to contain same number of samples'
X_mean = np.mean(X, axis=0)
Y_mean = np.mean(X, axis=0)
H_1 = (X - X_mean).T # H1 bar
H_2 = (Y - Y_mean).T # H2 bar
sigma11 = (1 / m1 - 1) * H_1.dot(H_1.T) + r * np.eye(n1)
sigma12 = (1 / m1 - 1) * H_1.dot(H_2.T)
sigma21 = (1 / m1 - 1) * H_2.dot(H_1.T)
sigma22 = (1 / m1 - 1) * H_2.dot(H_2.T) + r * np.eye(n2)
assert np.linalg.det(sigma11) != 0, 'DCCA computer delta: sigma11 is singular, ' \
'please use regularized parameter to adapt this matrix'
assert np.linalg.det(sigma22) != 0, 'DCCA computer delta: sigma22 is singular, ' \
'please use regularized parameter to adapt this matrix'
T1 = np.inv(np.sqrt(sigma11))
T3 = np.inv(np.sqrt(sigma22))
T = T1.dot(sigma12).dot(T3) # T1 dot sigma12 dot T3
U, D, V = np.linalg.svd(T)
delta12 = functools.reduce(lambda x, y: np.dot(x, y), [T1, U, V, T3])
delta11 = -0.5 * functools.reduce(lambda x, y: np.dot(x, y), [T1, U, D, U.T, T1])
dcorr = (1 / m1 - 1) * (2 * np.dot(delta11, H_1) + np.dot(delta12, H_2))
return dcorr
def get_mu(Pmax, Hkk, btmp):
Rmu = np.sqrt(Pmax) / np.norm(Hkk)
Lmu = 0
N = Hkk.shape[0]
Pcomp = np.matmul(
np.matmul(Hkk.T, np.linalg.matrix_power(np.inv(btmp), 2)), Hkk)
if (Pcomp < Pmax):
return Lmu
I = eye(N)
while (Rmu - Lmu > 1e-1):
midmu = (Rmu + Lmu) / 2
Pcomp = np.matmul(
np.matmul(Hkk.T, np.linalg.matrix_power(np.inv(btmp + midmu * I),
2)), Hkk)
if (Pcomp < Pmax):
Rmu = midmu
else:
Lmu = midmu
return Lmu
def gauss_pdf(x, m, s):
"""
Gaussian probability distribution function at point x, mean m, and variance s.
"""
if m.shape[1] == 1:
dx = x - tile(m, x.shape[1])
e = 0.5 * sum(dx * (np.dot(np.inv(s), dx)), axis=0)
e - e + 0.5 * m.shape[0] * log(2 * np.pi) + 0.5 * np.log(np.linalg.det(s))
p = np.exp(-e)
elif x.shape[1] == 1:
dx = tile(x, m.shape[1] - m)
e = 0.5 * sum(dx * (np.dot(np.inv(s), dx)), axis =0)
e = e + 0.5 * m.shape[0] * np.log(2 * np.pi) + 0.5 * np.log(np.linalg.det(s))
p = np.exp(-e)
else:
dx = x - m
e = 0.5 * np.dot(dx.T, np.dot(np.inv(s), dx))
e = e + 0.5 * m.shape[0] * np.log(2 * np.pi) + 0.5 * np.log(np.lingalg.det(s))
p = exp(-e)
return (p[0], e[0]) # p-value and probability
def setup_asr_step_methods(m, vars, additional_stochs=[]):
# groups RE stochastics that are suspected of being dependent
groups = []
fe_group = [n for n in vars.get('beta', []) if isinstance(n, mc.Stochastic)]
ap_group = [n for n in vars.get('gamma', []) if isinstance(n, mc.Stochastic)]
groups += [[g_i, g_j] for g_i, g_j in zip(ap_group[1:], ap_group[:-1])] + [fe_group, ap_group, fe_group+ap_group]
for a in vars.get('hierarchy', []):
group = []
col_map = dict([[key, i] for i,key in enumerate(vars['U'].columns)])
if a in vars['U']:
for b in nx.shortest_path(vars['hierarchy'], 'all', a):
if b in vars['U']:
n = vars['alpha'][col_map[b]]
if isinstance(n, mc.Stochastic):
group.append(n)
groups.append(group)
#if len(group) > 0:
#group += ap_group
#groups.append(group)
#group += fe_group
#groups.append(group)
for stoch in groups:
if len(stoch) > 0 and np.all([isinstance(n, mc.Stochastic) for n in stoch]):
# only step certain stochastics, for understanding convergence
#if 'gamma_i' not in stoch[0].__name__:
# print 'no stepper for', stoch
# m.use_step_method(mc.NoStepper, stoch)
# continue
#print 'finding Normal Approx for', [n.__name__ for n in stoch]
if additional_stochs == []:
vars_to_fit = [vars.get('p_obs'), vars.get('pi_sim'), vars.get('smooth_gamma'), vars.get('parent_similarity'),
vars.get('mu_sim'), vars.get('mu_age_derivative_potential'), vars.get('covariate_constraint')]
else:
vars_to_fit = additional_stochs
try:
raise ValueError
na = mc.NormApprox(vars_to_fit + stoch)
na.fit(method='fmin_powell', verbose=0)
cov = np.array(np.inv(-na.hess), order='F')
#print 'opt:', np.round_([n.value for n in stoch], 2)
#print 'cov:\n', cov.round(4)
if np.all(np.eigvals(cov) >= 0):
m.use_step_method(mc.AdaptiveMetropolis, stoch, cov=cov)
else:
raise ValueError
except ValueError:
#print 'cov matrix is not positive semi-definite'
m.use_step_method(mc.AdaptiveMetropolis, stoch)
def setup_asr_step_methods(m, vars, additional_stochs=[]):
# groups RE stochastics that are suspected of being dependent
groups = []
fe_group = [n for n in vars.get('beta', []) if isinstance(n, mc.Stochastic)]
ap_group = [n for n in vars.get('gamma', []) if isinstance(n, mc.Stochastic)]
groups += [[g_i, g_j] for g_i, g_j in zip(ap_group[1:], ap_group[:-1])] + [fe_group, ap_group, fe_group+ap_group]
for a in vars.get('hierarchy', []):
group = []
col_map = dict([[key, i] for i,key in enumerate(vars['U'].columns)])
if a in vars['U']:
for b in nx.shortest_path(vars['hierarchy'], 'all', a):
if b in vars['U']:
n = vars['alpha'][col_map[b]]
if isinstance(n, mc.Stochastic):
group.append(n)
groups.append(group)
#if len(group) > 0:
#group += ap_group
#groups.append(group)
#group += fe_group
#groups.append(group)
for stoch in groups:
if len(stoch) > 0 and np.all([isinstance(n, mc.Stochastic) for n in stoch]):
# only step certain stochastics, for understanding convergence
#if 'gamma_i' not in stoch[0].__name__:
# print 'no stepper for', stoch
# m.use_step_method(mc.NoStepper, stoch)
# continue
#print 'finding Normal Approx for', [n.__name__ for n in stoch]
if additional_stochs == []:
vars_to_fit = [vars.get('p_obs'), vars.get('pi_sim'), vars.get('smooth_gamma'), vars.get('parent_similarity'),
vars.get('mu_sim'), vars.get('mu_age_derivative_potential'), vars.get('covariate_constraint')]
else:
vars_to_fit = additional_stochs
try:
raise ValueError
na = mc.NormApprox(vars_to_fit + stoch)
na.fit(method='fmin_powell', verbose=0)
cov = np.array(np.inv(-na.hess), order='F')
#print 'opt:', np.round_([n.value for n in stoch], 2)
#print 'cov:\n', cov.round(4)
if np.all(np.eigvals(cov) >= 0):
m.use_step_method(mc.AdaptiveMetropolis, stoch, cov=cov)
else:
raise ValueError
except ValueError:
#print 'cov matrix is not positive semi-definite'
m.use_step_method(mc.AdaptiveMetropolis, stoch)
def apriltag_callback(data):
# use apriltag pose detection to find where is the robot
if len(data.detections)!=0: # check if apriltag is detected
detection = data.detections[0]
print detection.pose
if detection.id == 21: # tag id is the correct one
print detection
# Use the functions in helper.py to do the following
# step 1. convert the pose to poselist Hint: pose data => detection.pose.pose
tf_apriltag_camera = pose2poselist(detection.pose.pose)
# step 2. do the matrix manipulation
tf_base_map = tf_apriltag_map * np.inv (tf_camera_base * tf_apriltag_map)
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check):
# 3.1 Compute Kalman Gain
K_k = p_cov_check@h_jac.T@inv(h_jac@p_cov_check@h_jac.T + np.eye(3)*sensor_var)
# 3.2 Compute error state
delta_x = np.dot(K_k, (y_k - p_check))
# 3.3 Correct predicted state
p_hat = p_check + delta_x[0:3]
v_hat = v_check + delta_x[3:6]
q_hat = Quaternion(axis_angle=delta_x[6:9]).quat_mult_right(q_check)
# 3.4 Compute corrected covariance
p_cov_hat = (np.eye(9) - K_k@h_jac)@p_cov_check
return p_hat, v_hat, q_hat, p_cov_hat
def calcCMat(self, callback=None, progressCallback=None):
nSlopes = self.wfss[0].activeSubaps*2
self.controlShape = (nSlopes, self.simConfig.totalActs)
self.controlMatrix = numpy.zeros((nSlopes, self.simConfig.totalActs))
acts = 0
for dm in xrange(self.simConfig.nDM):
dmIMat = self.dms[dm].iMat
if dmIMat.shape[0]==dmIMat.shape[1]:
dmCMat = numpy.inv(dmIMat)
else:
dmCMat = numpy.linalg.pinv(dmIMat, self.dmConds[dm])
self.controlMatrix[:,acts:acts+self.dms[dm].acts] = dmCMat
acts += self.dms[dm].acts
def calculate_probablity(mean,alpha,covariance,data):
clusters = [[] for i in range(3)]
for point in data:
probablities = []
clusters = [[] for i in range(3)]
for i in range(len(alpha)):
determinant_covariance = np.linalg.det(covariance[i])
prior = 1/(2*pi*(math.sqrt(determinant_covariance)))
exponential_term = math.exp(-
(np.matrix((point-mean[i]))*(np.inv(np.matrix(covariance[i])))*np.matrix((point-mean[i])))/2)
probablities.append(alpha[i]*prior*exponential_term)
for i in range(len(probablities)):
max = probablities[0]
index = 0
if (max > probablities[i]):
max = probablities[i]
index = i
clusters[index] = point
return clusters
def test_set_linear_system_solver(self):
g = ODEFunctionGenerator(self.sys.eom_method.forcing_full,
self.sys.coordinates,
self.sys.speeds,
self.sys.constants_symbols,
mass_matrix=self.sys.eom_method.mass_matrix_full)
assert g._solve_linear_system == np.linalg.solve
g = ODEFunctionGenerator(self.sys.eom_method.forcing_full,
self.sys.coordinates,
self.sys.speeds,
self.sys.constants_symbols,
mass_matrix=self.sys.eom_method.mass_matrix_full,
linear_sys_solver='numpy')
assert g._solve_linear_system == np.linalg.solve
g = ODEFunctionGenerator(self.sys.eom_method.forcing_full,
self.sys.coordinates,
self.sys.speeds,
self.sys.constants_symbols,
mass_matrix=self.sys.eom_method.mass_matrix_full,
linear_sys_solver='scipy')
assert g._solve_linear_system == sp.linalg.solve
solver = lambda A, b: np.dot(np.inv(A), b)
g = ODEFunctionGenerator(self.sys.eom_method.forcing_full,
self.sys.coordinates,
self.sys.speeds,
self.sys.constants_symbols,
mass_matrix=self.sys.eom_method.mass_matrix_full,
linear_sys_solver=solver)
assert g._solve_linear_system == solver
def returnKernel(kernel, var=1):
"""
Utility function to return the correct kernel function given a string
identifying the name of the kernel
"""
kernel = kernel.lower()
if kernel in ['gaussian','normal','l2']:
kern = lambda XX: np.exp(-pdist2(XX,'sqeuclidean')/var)
elif kernel in ['laplace','laplacian','l1']:
kern = lambda XX: np.exp(-pdist2(XX,'minkowski',1)/var)
elif kernel in ['dirac', 'delta','kronecker']:
equals = lambda u,v: 1 - (np.array(u)==np.array(v)).all()
kern = lambda XX: pdist2(XX,equals)
elif kernel in ['mahalanobis']:
VInv = np.inv(var)
kern = lambda XX: np.exp(-np.power(pdist2(XX,'mahalanobis',VInv),2))
elif kernel in ['beta']:
dot = lambda XX: XX.dot(XX.T)
kern = lambda XX: dot(np.matrix(phi_beta(XX,var[0],var[1])))
elif kernel in ['beta_shifted','beta shifted']:
dot = lambda XX: XX.dot(XX.T)
kern = lambda XX: dot(np.matrix(phi_beta_shifted(XX,var[0],var[1])))
return kern
def crossComputeKerns(X,Y,kernel,symmetric,var=1):
"""
Compute pairwise kernel between points in two matrices
Inputs:
X: m x D matrix with m samples of dimension D
kernel: (string) name of kernel being computed
Y: n x D matrix with n samples of dimension D
kernel: (string) name of kernel being computed
k: (int) rank of model (number of hidden states)
symmetric: whether the model has symmetric views or not (should be false
for HMM)
var: for gaussian kernel, the variance (sigma^2)
for laplacian kernel, the bandwidth
for mahalanobis kernel, the covariance matrix
for delta kernel, None
Outputs: tuple (K,L,G), where:
K: cross-kernel matrix c of dimension m x n
"""
kernel = kernel.lower()
if kernel in ['gaussian','normal','l2']:
kern = lambda XX,YY: np.exp(-cdist(XX,YY,'sqeuclidean')/var)
elif kernel in ['laplace','laplacian','l1']:
kern = lambda XX,YY: np.exp(-cdist(XX,YY,'minkowski',1)/var)
elif kernel in ['dirac', 'delta','kronecker']:
equals = lambda u,v: 1 - (np.array(u)==np.array(v)).all()
kern = lambda XX,YY: cdist(XX,YY,equals)
elif kernel in ['mahalanobis']:
VInv = np.inv(var)
kern = lambda XX,YY: np.exp(-np.power(cdist(XX,YY,'mahalanobis',VInv),2))
elif kernel in ['beta','betadot']:
kern = lambda XX,YY: np.matrix(phi_beta(XX,var[0],var[1]).dot(
phi_beta(YY,var[0],var[1]).T))
elif kernel in ['beta_shifted','beta shifted']:
kern = lambda XX,YY: np.matrix(phi_beta_shifted(XX,var[0],var[1]).dot(
phi_beta_shifted(YY,var[0],var[1]).T))
K = kern(X,Y)
return K
A2[indices] = 0.
A2[indices, indices] = 1.
m = N-3
#A2[2*dofs+m,:] = 0.
#A2[2*dofs+m,2*dofs+m] = 1.
#F[2*dofs+m] = 0.
#W=sparse.csr_matrix(W);
A2=sparse.csr_matrix(A2);
print "Time to update", time.time()-t1
print "Starting to solve..."
t1 = time.time()
#p_old = np.copy(pressures)
for k in range(0,Ns_it): #SIMPLE ALGORITHM
velocities = sla.spsolve(A2,F)
pressures = sla.spsolve(B.T*np.inv(D)*B,-B.T*velocities)
velocities = velocities + np.inv(D)*B*pressures
#pressures = pressures #in the SIMPLE algorithm, but not needed
if np.linalg.norm(pressures-p_old)/np.linalg.norm(p_old) < eps:
print "SIMPLE converged!"
break
print "Time to solve: ", time.time()-t1
counter += 1
print "error : ", error
################################
# PLOTTING:
################################
# Making global X and Y coordinates
def kalman_filter(y, A, C, Q, R, init_x, init_V, varargin={}):
# Kalman filter.
# [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, \)
#
# INPUTS:
# y(:,t) - the observation at time t
# A - the system matrix
# C - the observation matrix
# Q - the system covariance
# R - the observation covariance
# init_x - the initial state (column) vector
# init_V - the initial state covariance
#
# OPTIONAL INPUTS (string/value pairs [default in brackets])
# 'model' - model(t)=m means use params from model m at time t [ones(1,T) ]
# In this case, all the above matrices take an additional final dimension,
# i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m).
# However, init_x and init_V are independent of model(1).
# 'u' - u(:,t) the control signal at time t [ [] ]
# 'B' - B(:,:,m) the input regression matrix for model m
#
# OUTPUTS (where X is the hidden state being estimated)
# x(:,t) = E[X(:,t) | y(:,1:t)]
# V(:,:,t) = Cov[X(:,t) | y(:,1:t)]
# VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2
# loglik = sum{t=1}^T log P(y(:,t))
#
# If an input signal is specified, we also condition on it:
# e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)]
# If a model sequence is specified, we also condition on it:
# e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)]
assert type(y) == np.matrixlib.defmatrix.matrix
(os, T) = y.shape
ss = A.shape[0]
# size of state space
# set default params
model = np.ones(T)
u = np.array([])
B = np.array([])
ndx = np.array([])
if varargin:
model = varargin["model"]
u = varargin["u"]
B = varargin["B"]
ndx = varargin.get("ndx", ndx)
x = np.zeros((ss, T))
V = np.zeros((ss, ss, T))
VV = np.zeros((ss, ss, T))
loglik = 0
for t in range(0, T):
if t == 0:
# prevx = init_x(:,m);
# prevV = init_V(:,:,m);
prevx = init_x
prevV = init_V
initial = 1
else:
prevx = x[:, t - 1]
prevV = V[:, :, t - 1]
initial = 0
if u.size == 0:
assert type(y) == np.matrixlib.defmatrix.matrix
(xr, Vr, LL, VVr) = kalman_update(A, C, Q, R, y[:, t], prevx, prevV, {"initial": initial})
x[:, t] = xr
V[:, :, t] = Vr
VV[:, :, t] = VVr
else:
if ndx.size == 0:
(x[:, t], V[:, :, t], LL, VV[:, :, t]) = kalman_update(
A, C, Q, R, y[:, t], prevx, prevV, {"initial": initial, "u": u[:, t], "B": B}
)
else:
i = ndx[t]
# copy over all elements; only some will get updated
x[:, t] = prevx
prevP = np.inv(prevV)
prevPsmall = prevP[i, i]
prevVsmall = np.inv(prevPsmall)
(x[i, t], smallV, LL, VV[i, i, t]) = kalman_update(
A[i, i],
C[:, i],
Q[i, i],
R,
y[:, t],
prevx[i],
prevVsmall,
{"initial": initial, "u": u[:, t], "B": B[i, :]},
)
smallP = np.inv(smallV)
prevP[i, i] = smallP
V[:, :, t] = np.inv(prevP)
loglik = loglik + LL[0, 0]
return (x, V, VV, loglik)
def internal_indices(features,orderings,distance="euclidean"):
clusters={}
indices=[]
for i,x in enumerate(orderings):
if x in clusters.keys():
clusters[x].append(features[i])
else:
clusters[x]=[features[i]]
if distance == "seuclidean":
cmd = "dis.seuclidean(Q,R,variances)"
elif distance == "mahalanobis":
cmd = "dis.mahalanobis(Q,R,inv)"
else:
cmd = "dis."+distance+"(Q,R)"
# 'A'
centroids={}
# 'B'
avgdissim={}
for i in clusters.keys():
centroids[i]=np.mean(clusters[i],axis=0)
sumdist=0
variances=np.var(clusters[i],axis=0) if distance = "seuclidean"
inv = np.inv(np.cov(clusters[i])) if distance = "mahalanobis"
for x in clusters[i]:
third=third+"variances)" if distance="seuclidean"
third=
sumdist+=eval(cmd.replace('Q','x').replace('R','clusters[i]'))
avgdissim[i]=sumdist/len(clusters[i])
maxB=max(avgdissim)
# 'D'
dists={}
for c,i in enumerate(clusters.keys()):
# 'C'
i_to_centroid=[]
for j in np.delete(clusters.keys(),c):
sumdist=0
variances=np.var(clusters[j],axis=0) if distance = "seuclidean"
inv = np.inv(np.cov(clusters[j])) if distance = "mahalanobis"
for x in clusters[i]:
sumdist+=eval(cmd.replace('Q','x').replace('R','centroids[j]'))
i_to_centroid.append(sumdist/len(clusters[i]))
a,b=sorted([i,j])
dists[str(a)+str(b)]=eval(cmd.replace('Q','centroids[i]').replace('R','centroids[j]'))
a,b=avgdissim[i],min(i_to_centroid)
# average Silhouette of cluster
silhouette=(b-a)/max([a,b])
# Davies-Bouldin coefficient
# the average over all clusters would be the davies bouldin index
temp=dict(avgdissim)
del temp[i]
d,e=max(temp.iteritems(),key=operator.itemgetter(1))
a,b=sorted([i,d])
dbc=(avgdissim[i]+e)/dists[str(a)+str(b)]
# Dunn coefficient
# the minimum over all clusters would be the Dunn index
temp=[s for s in dists.keys() if str(i) in s]
temp={k: dists[k] for k in temp}
di=min(temp.values())/maxB
# add indices to the list
a,b=min(temp.values()),max(temp.values())
indices.append([silhouette,dbc,di,avgdissim[i],a,b])
return indices
def read_meas_info(source, tree=None):
"""Read the measurement info
Parameters
----------
source: string or file
If string it is the file name otherwise it's the file descriptor.
If tree is missing, the meas output argument is None
tree: tree
FIF tree structure
Returns
-------
info: dict
Info on dataset
meas: dict
Node in tree that contains the info.
"""
if tree is None:
fid, tree, _ = fiff_open(source)
open_here = True
else:
fid = source
open_here = False
# Find the desired blocks
meas = dir_tree_find(tree, FIFF.FIFFB_MEAS)
if len(meas) == 0:
if open_here:
fid.close()
raise ValueError, 'Could not find measurement data'
if len(meas) > 1:
if open_here:
fid.close()
raise ValueError, 'Cannot read more that 1 measurement data'
meas = meas[0]
meas_info = dir_tree_find(meas, FIFF.FIFFB_MEAS_INFO)
if len(meas_info) == 0:
if open_here:
fid.close()
raise ValueError, 'Could not find measurement info'
if len(meas_info) > 1:
if open_here:
fid.close()
raise ValueError, 'Cannot read more that 1 measurement info'
meas_info = meas_info[0]
# Read measurement info
dev_head_t = None
ctf_head_t = None
meas_date = None
highpass = None
lowpass = None
nchan = None
sfreq = None
chs = []
p = 0
for k in range(meas_info.nent):
kind = meas_info.directory[k].kind
pos = meas_info.directory[k].pos
if kind == FIFF.FIFF_NCHAN:
tag = read_tag(fid, pos)
nchan = int(tag.data)
elif kind == FIFF.FIFF_SFREQ:
tag = read_tag(fid, pos)
sfreq = tag.data
elif kind == FIFF.FIFF_CH_INFO:
tag = read_tag(fid, pos)
chs.append(tag.data)
p += 1
elif kind == FIFF.FIFF_LOWPASS:
tag = read_tag(fid, pos)
lowpass = tag.data
elif kind == FIFF.FIFF_HIGHPASS:
tag = read_tag(fid, pos)
highpass = tag.data
elif kind == FIFF.FIFF_MEAS_DATE:
tag = read_tag(fid, pos)
meas_date = tag.data
elif kind == FIFF.FIFF_COORD_TRANS:
tag = read_tag(fid, pos)
cand = tag.data
if cand['from_'] == FIFF.FIFFV_COORD_DEVICE and \
cand['to'] == FIFF.FIFFV_COORD_HEAD: # XXX : from
dev_head_t = cand
elif cand['from_'] == FIFF.FIFFV_MNE_COORD_CTF_HEAD and \
cand['to'] == FIFF.FIFFV_COORD_HEAD:
ctf_head_t = cand
# Check that we have everything we need
if nchan is None:
if open_here:
fid.close()
raise ValueError, 'Number of channels in not defined'
if sfreq is None:
if open_here:
fid.close()
raise ValueError, 'Sampling frequency is not defined'
if len(chs) == 0:
if open_here:
fid.close()
raise ValueError, 'Channel information not defined'
if len(chs) != nchan:
if open_here:
fid.close()
raise ValueError, 'Incorrect number of channel definitions found'
if dev_head_t is None or ctf_head_t is None:
hpi_result = dir_tree_find(meas_info, FIFF.FIFFB_HPI_RESULT)
if len(hpi_result) == 1:
hpi_result = hpi_result[0]
for k in range(hpi_result.nent):
kind = hpi_result.directory[k].kind
pos = hpi_result.directory[k].pos
if kind == FIFF.FIFF_COORD_TRANS:
tag = read_tag(fid, pos)
cand = tag.data;
if cand.from_ == FIFF.FIFFV_COORD_DEVICE and \
cand.to == FIFF.FIFFV_COORD_HEAD: # XXX: from
dev_head_t = cand;
elif cand.from_ == FIFF.FIFFV_MNE_COORD_CTF_HEAD and \
cand.to == FIFF.FIFFV_COORD_HEAD:
ctf_head_t = cand;
# Locate the Polhemus data
isotrak = dir_tree_find(meas_info,FIFF.FIFFB_ISOTRAK)
if len(isotrak):
isotrak = isotrak[0]
else:
if len(isotrak) == 0:
if open_here:
fid.close()
raise ValueError, 'Isotrak not found'
if len(isotrak) > 1:
if open_here:
fid.close()
raise ValueError, 'Multiple Isotrak found'
dig = []
if len(isotrak) == 1:
for k in range(isotrak.nent):
kind = isotrak.directory[k].kind;
pos = isotrak.directory[k].pos;
if kind == FIFF.FIFF_DIG_POINT:
tag = read_tag(fid,pos);
dig.append(tag.data)
dig[-1]['coord_frame'] = FIFF.FIFFV_COORD_HEAD
# Locate the acquisition information
acqpars = dir_tree_find(meas_info, FIFF.FIFFB_DACQ_PARS);
acq_pars = None
acq_stim = None
if len(acqpars) == 1:
acqpars = acqpars[0]
for k in range(acqpars.nent):
kind = acqpars.directory[k].kind
pos = acqpars.directory[k].pos
if kind == FIFF.FIFF_DACQ_PARS:
tag = read_tag(fid, pos)
acq_pars = tag.data
elif kind == FIFF.FIFF_DACQ_STIM:
tag = read_tag(fid, pos)
acq_stim = tag.data
# Load the SSP data
projs = read_proj(fid, meas_info)
# Load the CTF compensation data
comps = read_ctf_comp(fid, meas_info, chs)
# Load the bad channel list
bads = _read_bad_channels(fid, meas_info)
#
# Put the data together
#
if tree.id is not None:
info = dict(file_id=tree.id)
else:
info = dict(file_id=None)
# Make the most appropriate selection for the measurement id
if meas_info.parent_id is None:
if meas_info.id is None:
if meas.id is None:
if meas.parent_id is None:
info['meas_id'] = info.file_id
else:
info['meas_id'] = meas.parent_id
else:
info['meas_id'] = meas.id
else:
info['meas_id'] = meas_info.id
else:
info['meas_id'] = meas_info.parent_id;
if meas_date is None:
info['meas_date'] = [info['meas_id']['secs'], info['meas_id']['usecs']]
else:
info['meas_date'] = meas_date
info['nchan'] = nchan
info['sfreq'] = sfreq
info['highpass'] = highpass if highpass is not None else 0
info['lowpass'] = lowpass if lowpass is not None else info['sfreq']/2.0
# Add the channel information and make a list of channel names
# for convenience
info['chs'] = chs;
info['ch_names'] = [ch.ch_name for ch in chs]
#
# Add the coordinate transformations
#
info['dev_head_t'] = dev_head_t
info['ctf_head_t'] = ctf_head_t
if dev_head_t is not None and ctf_head_t is not None:
info['dev_ctf_t'] = info['dev_head_t']
info['dev_ctf_t'].to = info['ctf_head_t'].from_ # XXX : see if better name
info['dev_ctf_t'].trans = np.dot(np.inv(ctf_head_t.trans), info.dev_ctf_t.trans)
else:
info['dev_ctf_t'] = []
# All kinds of auxliary stuff
info['dig'] = dig
info['bads'] = bads
info['projs'] = projs
info['comps'] = comps
info['acq_pars'] = acq_pars
info['acq_stim'] = acq_stim
if open_here:
fid.close()
return info, meas
def _getOrig(P, M, u=1, v=0):
orig0 = np.dot(np.inv(P), [u, v, -1, 1])
orig0 = np.dot(np.inv(M), orig0)
orig0 = orig0/orig0[-1]
return orig0
def rollo_ekf(fstate, x_pp, u, Jfunc, Jh, E_pp, hmeas, z, Q, R):
"Calculates the final position accodrding to the odometry model of Rollo"
#function [x_cc, E_cc] = rollo_ekf(fstate, x_pp, u, Jfunc, Jh, E_pp, hmeas, z, Q, R)
#%% PREDICTION UPDATE
# Nonlinear update and linearization at current state
x_cp = fstate(x_pp, u) # state prediction, x_k|k-1
Jf = Jfunc(x_pp, u)
# Partial covariance update
E_cp = Jf * E_pp * np.transpose(Jf) + Q # E_k|k-1
#%% INNOVATION UPDATE
# Nonlinear measurement and linearization
z_estimate = hmeas(x_cp);
P12 = E_cp * np.transpose(Jh) # Cross covariance
S_inv = np.inv(Jh * P12 + R);
# M_inv = matrix3by3_inverse(H*P12+R)
M_inv = np.invert(H * P12 + R)
H = P12 * S_inv # Kalman filter gain, H_k
x_cc = x_cp + H * (z - z_estimate) # State estimate, x_k|k
E_cc = E_cp - H * np.transpose(P12) # State covariance matrix, E_k|k
# if degrees:
# Theta_i = Theta_i / 180.0 * np.pi
# S_L = t * (n_L / 60.0) * 2 * np.pi * r_L # Linear distance traveled by left wheel in meters
# S_R = t * (n_R / 60.0) * 2 * np.pi * r_R # Linear distance traveled by right wheel in meters
# if verbose:
# print('Linear distance traveled by left wheel (S_L [m]): ', S_L)
# print('Linear distance traveled by right wheel (S_R [m]): ', S_R)
# if (S_R > S_L):
## if (np.abs(S_R - S_L) > EPS):
# r = (axle_l / 2.0) * ((S_L + S_R) / (S_R - S_L)) # travel_radius
# beta = S_R / (r + axle_l / 2.0) # travel_angle
# P_f_x = P_i_x - r * (1 - np.cos(beta))
# P_f_y = P_i_y + r * np.sin(beta)
# Theta_f = beta + Theta_i;
# elif (S_R < S_L):
## elif (np.abs(S_R - S_L) < -EPS):
# r = (axle_l / 2.0) * ((S_L + S_R) / (S_L - S_R)) # travel_radius
# beta = S_R / (r - axle_l / 2.0) # travel_angle
# P_f_x = P_i_x + r * (1 - np.cos(beta))
# P_f_y = P_i_y + r * np.sin(beta)
# Theta_f = Theta_i - beta
# elif (S_R == S_L):
## elif (np.abs(S_R - S_L) < EPS):
# r = 0
# beta = 0
# P_f_x = P_i_x + S_L * np.cos(Theta_i)
# P_f_y = P_i_y + S_L * np.sin(Theta_i)
# Theta_f = Theta_i
# else:
# print('Error')
# exit(1);
# if degrees:
# Theta_f = Theta_f * 180 / np.pi
# if verbose:
# print('Travel radius [m]: ', r)
# if degrees:
# print('Travel angle [°]:', beta)
# else:
# print('Travel angle [rad]:', beta)
return [x_cc, E_cc]
alist=map(int,raw_input().split()) from numpy import inv a=np.array([[1,1,1],[4,2,1],[9,3,1],[16,4,1]]) ainv = inv(a) print ainv
def calculate(self, fragments, cupdate=0.05):
nFragBasis = 0
nFragAlpha = 0
nFragBeta = 0
self.fonames = []
unrestricted = ( len(self.data.mocoeffs) == 2 )
self.logger.info("Creating attribute fonames[]")
# Collect basis info on the fragments.
for j in range(len(fragments)):
nFragBasis += fragments[j].nbasis
nFragAlpha += fragments[j].homos[0] + 1
if unrestricted and len(fragments[j].homos) == 1:
nFragBeta += fragments[j].homos[0] + 1 #assume restricted fragment
elif unrestricted and len(fragments[j].homos) == 2:
nFragBeta += fragments[j].homos[1] + 1 #assume unrestricted fragment
#assign fonames based on fragment name and MO number
for i in range(fragments[j].nbasis):
if hasattr(fragments[j],"name"):
self.fonames.append("%s_%i"%(fragments[j].name,i+1))
else:
self.fonames.append("noname%i_%i"%(j,i+1))
nBasis = self.data.nbasis
nAlpha = self.data.homos[0] + 1
if unrestricted:
nBeta = self.data.homos[1] + 1
# Check to make sure calcs have the right properties.
if nBasis != nFragBasis:
self.logger.error("Basis functions don't match")
return False
if nAlpha != nFragAlpha:
self.logger.error("Alpha electrons don't match")
return False
if unrestricted and nBeta != nFragBeta:
self.logger.error("Beta electrons don't match")
return False
if len(self.data.atomcoords) != 1:
self.logger.warning("Molecule calc appears to be an optimization")
for frag in fragments:
if len(frag.atomcoords) != 1:
msg = "One or more fragment appears to be an optimization"
self.logger.warning(msg)
break
last = 0
for frag in fragments:
size = frag.natom
if self.data.atomcoords[0][last:last+size].tolist() != \
frag.atomcoords[0].tolist():
self.logger.error("Atom coordinates aren't aligned")
return False
if self.data.atomnos[last:last+size].tolist() != \
frag.atomnos.tolist():
self.logger.error("Elements don't match")
return False
last += size
# And let's begin!
self.mocoeffs = []
self.logger.info("Creating mocoeffs in new fragment MO basis: mocoeffs[]")
for spin in range(len(self.data.mocoeffs)):
blockMatrix = numpy.zeros((nBasis,nBasis), "d")
pos = 0
# Build up block-diagonal matrix from fragment mocoeffs.
# Need to switch ordering from [mo,ao] to [ao,mo].
for i in range(len(fragments)):
size = fragments[i].nbasis
if len(fragments[i].mocoeffs) == 1:
temp = numpy.transpose(fragments[i].mocoeffs[0])
blockMatrix[pos:pos+size, pos:pos+size] = temp
else:
temp = numpy.transpose(fragments[i].mocoeffs[spin])
blockMatrix[pos:pos+size, pos:pos+size] = temp
pos += size
# Invert and mutliply to result in fragment MOs as basis.
iBlockMatrix = numpy.inv(blockMatrix)
temp = numpy.transpose(self.data.mocoeffs[spin])
results = numpy.transpose(numpy.dot(iBlockMatrix, temp))
self.mocoeffs.append(results)
if hasattr(self.data, "aooverlaps"):
tempMatrix = numpy.dot(self.data.aooverlaps, blockMatrix)
tBlockMatrix = numpy.transpose(blockMatrix)
if spin == 0:
self.fooverlaps = numpy.dot(tBlockMatrix, tempMatrix)
self.logger.info("Creating fooverlaps: array[x,y]")
elif spin == 1:
self.fooverlaps2 = numpy.dot(tBlockMatrix, tempMatrix)
self.logger.info("Creating fooverlaps (beta): array[x,y]")
else:
self.logger.warning("Overlap matrix missing")
self.parsed = True
self.nbasis = nBasis
self.homos = self.data.homos
return True
def update(self):
next_action = self.pdcontroller.get_next_action(self.state., pf_vec)
FSFTsigx = self.F.dot(self.sig0).dot(self.F_trans) + self.sigx
HTBlahInverse = npy.inv(self.H.dot(FSFTsigx).dot(self.H_trans) + self.sigz)
kalman_gain = FSFTsigx.dot(self.H_trans).dot(HTBlahInverse)
def linclosed(TrainingData, y): XtX = np.multiply(np.transpose(TrainingData), TrainingData) theta = np.multiply(np.inv(XtX), np.multiply(np.transpose(TrainingData), y)) return theta
def disclyap(a1, b1, vecflag = False):
# disclyap -- extension of Hansen/Sargent's DOUBLEJ.M
#
# function V = disclyap(a1,b1,vecflag)
# Computes infinite sum V given by
#
# V = SUM (a1^j)*b1*(a1^j)'
#
# where a1 and b1 are each (n X n) matrices with eigenvalues whose moduli are
# bounded by unity, and b1 is an (n X n) matrix.
# The sum goes from j = 0 to j = infinity. V is computed by using
# the following "doubling algorithm". We iterate to convergence on
# V(j) on the following recursions for j = 1, 2, ..., starting from
# V(0) = b1:
#
# a1(j) = a1(j-1)*a1(j-1)
# V(j) = V(j-1) + a1(j-1)*V(j-1)*a1(j-1)'
#
# The limiting value is returned in V.
#
# -----------------------------------------------------------------------------
# EMT added following comments and the vecflag argument (default=false)
# -----------------------------------------------------------------------------
#
# 1) L&S p. 76, write
# X = doublej(A, B * B') solves X = A * X * A' + B * B' where a1 = A and B*B' = b1
#
# 2) Note on algorithm
# * let $V_k \equiv \sum_{j=0}^K A^j C'C A^j'$
# * then $V_{2k} = V_k + A^k V_k A^k'$
# * let $k=2^j$ and rewrite $V_k \equiv V_j$ and $A_j\equiv A^{2^j}$ so that $A_{j+1} = A_j^2$
# * then $V_{j+1} = V_j + A_j^2 V_j A_j^2'$
#
# the doubling uses fewer iterations (namely only $j$ instead of $2^j$ and is thus much faster
#
# 3) Solves "discrete Lyapunov" equation (but can also handle constants in the SS)
#
# 4) new parameter: if vecflag == true: apply vec formula of Hamilton instead of doubling
# will assume invertibility, i.e. no constant in SS
if vecflag:
Np = np.size(a1, 0)
Inp2 = np.eye(Np**2)
vecSIGMA = np.dot(np.dot(np.inv((Inp2 - np.kron(a1, a1))), Inp2), b2[:])
V = np.reshape(vecSIGMA, (Np, Np))
else:
alpha0 = a1
gamma0 = b1
delta = 5
ijk = 1
while delta > np.finfo(float).eps:
alpha1 = alpha0 * alpha0;
gamma1 = gamma0 + alpha0 * gamma0 * alpha0.transpose()
# delta = max(max(abs(gamma1-gamma0)));
delta = np.max(np.abs(gamma1[:] - gamma0[:]))
gamma0 = gamma1
alpha0 = alpha1
ijk = ijk + 1
if ijk > 50:
print('Error: ijk = %d, delta = %f check aopt and c for proper configuration' % (ijk, delta))
V = gamma1
return V
def inverse(hesse):
if type(hesse) == np.array:
return np.inv(hesse)
else:
return 1. / hesse
W11 = W[W11_indices,W11_indices]
s12 = W[col, W11_indices]
#solve using coordinate descent: according to Friedman 2007
beta = sl.linear_model.lasso_path(W11, s12, rho)
#get the last column
beta = beta[:,-1]
W[col, W11_indices] = np.dot(W11, beta)
W[W11_indices, col] = np.transpose(W[col, W11_indices])
#test stop criterion
if np.linalg.norm(W_old-W)<stop_criterion:
break;
W_old = W
#final result:
Theta = np.inv(W)
def d(g, G, A):
""" Calculates
"""
G_inv = np.inv(G.T * G)
d = g.T * G_inv * A * np.inv(A.T * G_inv * A) * A.T * G_inv * g
return d
def label_propagation(G, Label_Matrix, aff_mat, label_mapping, num_changed,
repeat, alpha, eps, max_iters, tol):
"""Performs iterative weighted average algorithm to propagate labels to unlabeled nodes.
Features: Hard label clamps, deterministic solution.
See: Zhu and Ghahramani, 2002.
"""
restore = max_iters
n = aff_mat.shape[0]
# Construct Diagonal Degree Matrix
diag_degree = compute_diagonal_degree_matrix(aff_mat, inverse=True)
# Variables necessary for the algorithm:
mu = alpha / (1 - alpha + eps)
A = sparse.lil_matrix((n, n))
A = A.tocsr()
A.setdiag(1 / (1 + mu*diag_degree + mu*eps))
np.identity(n) + mu*diag_degree + mu*eps
W = W.tocsr()
if no_inverse == False:
ddmi.setdiag(1 / W.sum(axis=1))
else:
ddmi.setdiag(W.sum(axis=1))
ddmi = ddmi.tocsr()
# Allow for the option to iteratively do the algorithm:
# i.e. it will rerun if the number of unlabeled_nodes exceeds a threshold
threshold_unlabeled = 100
number_unlabeled = num_changed
while (number_unlabeled > threshold_unlabeled) and (repeat == 1):
# In one vs. all fashion, iteratively process the weighted average of neighbors.
column_index = 0
for column in Label_Matrix.T:
mat_column = np.mat(column).T
labeled_indices = np.nonzero(mat_column != 0)[0]
num_members = (np.nonzero(mat_column == 1))[0].shape[1]
print 'Initial number of members in class {0}: {1}'.format(column_index, num_members)
Y_hat_now = mat_column
Y_hat_next = np.inv(A) * (mu * W * Y_hat_now + column)
while not_converged(Y_hat_next, Y_hat_now, tol) and max_iters > 0:
Y_hat_now = Y_hat_next
Y_hat_next = diag_degree_inv * aff_mat * Y_hat_now
Y_hat_next[labeled_indices] = mat_column[labeled_indices]
max_iters -= 1
class_members = np.nonzero(Y_hat_next>0)[0]
print 'Current number of members in class {0}: {1}'.format(column_index, class_members.shape[1])
Label_Matrix[class_members, :] = -1
Label_Matrix[class_members, column_index] = 1
column_index += 1
max_iters = restore
# Account for the case where an unlabeled node does not get labeled to any class:
number_unlabeled = 0
for line in Label_Matrix:
if 0 in line:
number_unlabeled += 1
print "number of nodes still unlabeled:", number_unlabeled
repeat = 0
Graph_Final = label_nodes(G, Label_Matrix, label_mapping)
return Graph_Final
