def compute_integrator_dynamics(K):
''' Compute the matrices associated to an n-th order continuous time integrator.
The form of the dynamics is:
dx = A*x + B*u
The control law is a linear state feedback:
u = -K*x
Input parameters:
K : n-dimensional vector of feedback gains
Returns a tuple containing the following elements:
H: closed-loop dynamics matrix (A-B*K)
A: state transition matrix
B: control input matrix
'''
m = K.shape[0] # size of pos vector
n = K.shape[1] / m # integrator order
H = matlib.zeros((m * n, m * n))
A = matlib.zeros((m * n, m * n))
B = matlib.zeros((m * n, m))
H[-m:, :] = -K
I = matlib.eye(m)
B[-m:, :] = I
for i in range(n - 1):
H[m * i:m * (i + 1), m * (i + 1):m * (i + 2)] = I
A[m * i:m * (i + 1), m * (i + 1):m * (i + 2)] = I
return (H, A, B)
def cost_func(B,model,profile,profiler):
cost = ml.zeros([model.T,1])
U = ml.zeros([model.uDim,model.T])
T = model.T
for t in range(T-1):
U[:,t] = B[0:model.xDim,t+1]-B[0:model.xDim,t];
B[:,t+1] = belief.belief_dynamics(B[:,t],U[:,t],None,model);
if max(U[:,t])> 1:
cost[t] = 1000
elif abs(B[0,t]) > model.xMax[0]:
cost[t] = 1000
elif abs(B[1,t]) > model.xMax[1]:
cost[t] = 1000
else:
null, s = belief.decompose_belief(B[:,t], model)
cost[t] = model.alpha_belief*ml.trace(s*s)+model.alpha_control*ml.sum(U[:,t].T*U[:,t])
x, s = belief.decompose_belief(B[:,T-1], model)
cost[T-1] = model.alpha_final_belief*ml.trace(s*s)
return cost, B, U
def __init__(self):
# model dimensions
self.xDim = 2 # state space dimension
self.uDim = 2 # control input dimension
self.qDim = 2 # dynamics noise dimension
self.zDim = 2 # observation dimension
self.rDim = 2 # observtion noise dimension
# belief space dimension
# note that we only store the lower (or upper) triangular portion
# of the covariance matrix to eliminate redundancy
self.bDim = int(self.xDim + self.xDim*((self.xDim+1)/2.))
self.dT = 1. # time step for dynamics function
self.T = 15 # number of time steps in trajectory
self.alpha_belief = 10. # weighting factor for penalizing uncertainty at intermediate time steps
self.alpha_final_belief = 10. # weighting factor for penalizing uncertainty at final time step
self.alpha_control = 1. # weighting factor for penalizing control cost
self.xMin = ml.vstack([-5,-3]) # minimum limits on state (xMin <= x)
self.xMax = ml.vstack([5,3]) # maximum limits on state (x <= xMax)
self.uMin = ml.vstack([-1,-1]) # minimum limits on control (uMin <= u)
self.uMax = ml.vstack([1,1]) # maximum limits on control (u <= uMax)
self.Q = ml.eye(self.qDim) # dynamics noise variance
self.R = ml.eye(self.rDim) # observation noise variance
self.start = ml.zeros([self.xDim,1]) # start state, OVERRIDE
self.goal = ml.zeros([self.xDim,1]) # end state, OVERRIDE
self.sqpParams = LightDarkSqpParams()
def rpca(self, data):
def shrink(T, zeta):
return np.matrix(
np.maximum(np.zeros(T.shape),
np.array(np.absolute(T)) - zeta) *
np.array(np.sign(T)))
def SVD_thresh(X, tau):
U, s, V = svd(X, full_matrices=False)
s_thresh = np.array(
[max(abs(sig) - tau, 0.0) * np.sign(sig) for sig in s])
return U * np.matrix(np.diag(s_thresh)) * V
D = np.transpose(np.matrix(data))
d, n = data.shape
k = 0
D_norm_fro = norm(D, 'fro')
D_norm_2 = norm(D, 2)
D_norm_inf = norm(D, np.inf)
mu = self._mu if self._mu != -1 else 1.25 / D_norm_2
rho = self._rho
lamb = self._lamb if self._lamb != -1 else 1.0 / np.sqrt(d)
Y = D / max(D_norm_2, D_norm_inf / lamb)
E = ml.zeros(D.shape)
A = ml.zeros(D.shape)
while k < self._max_iter and norm(D - A - E,
'fro') > self._epsilon * D_norm_fro:
A = SVD_thresh(D - E + Y / mu, 1 / mu)
E = shrink(D - A + Y / mu, lamb / mu)
Y = Y + mu * (D - A - E)
mu = rho * mu
k += 1
W = np.array(A.T)
return W
def belief_dynamics(b_t, u_t, z_tp1, model, profiler=None, profile=False):
dynamics_func = model.dynamics_func
obs_func = model.obs_func
qDim = model.qDim
rDim = model.rDim
x_t, SqrtSigma_t = decompose_belief(b_t, model)
Sigma_t = SqrtSigma_t * SqrtSigma_t
if z_tp1 is None:
# Maximum likelihood observation assumption
z_tp1 = obs_func(dynamics_func(x_t, u_t, ml.zeros([qDim, 1])),
ml.zeros([rDim, 1]))
if profile: profiler.start('ekf')
x_tp1, Sigma_tp1 = ekf(x_t, Sigma_t, u_t, z_tp1, model)
if profile: profiler.stop('ekf')
# Compute square root for storage
# Several different choices available -- we use the principal square root
if profile: profiler.start('eigh')
D_diag, V = ml.linalg.eigh(Sigma_tp1)
if profile: profiler.stop('eigh')
SqrtSigma_tp1 = V * np.sqrt(ml.diag(D_diag)) * V.T
b_tp1 = compose_belief(x_tp1, SqrtSigma_tp1, model)
return b_tp1
def trefftz_plane_velocities(self,
pnts: MatrixVector,
betx: float = 1.0,
bety: float = 1.0,
betz: float = 1.0):
# Trailing Vortex A
agcs = self.relative_mach(pnts,
self.grda,
betx=betx,
bety=bety,
betz=betz)
dirxa = -self.diro
dirza = self.nrm
dirya = dirza**dirxa
alcs = MatrixVector(agcs * dirxa, agcs * dirya, agcs * dirza)
alcs.x = zeros(alcs.shape, dtype=float)
axx = MatrixVector(alcs.x, -alcs.z, alcs.y)
am2 = square(alcs.y) + square(alcs.z)
chkam2 = absolute(am2) < tol
am2r = zeros(pnts.shape, dtype=float)
reciprocal(am2, where=logical_not(chkam2), out=am2r)
faca = -1.0
veldl = elementwise_multiply(axx, am2r) * faca
veldl.x[chkam2] = 0.0
veldl.y[chkam2] = 0.0
veldl.z[chkam2] = 0.0
dirxi = Vector(dirxa.x, dirya.x, dirza.x)
diryi = Vector(dirxa.y, dirya.y, dirza.y)
dirzi = Vector(dirxa.z, dirya.z, dirza.z)
velda = MatrixVector(veldl * dirxi, veldl * diryi,
veldl * dirzi) * faca
# Trailing Vortex B
bgcs = self.relative_mach(pnts,
self.grdb,
betx=betx,
bety=bety,
betz=betz)
dirxb = self.diro
dirzb = self.nrm
diryb = dirzb**dirxb
blcs = MatrixVector(bgcs * dirxb, bgcs * diryb, bgcs * dirzb)
blcs.x = zeros(blcs.shape, dtype=float)
bxx = MatrixVector(blcs.x, -blcs.z, blcs.y)
bm2 = square(blcs.y) + square(blcs.z)
chkbm2 = absolute(bm2) < tol
bm2r = zeros(pnts.shape, dtype=float)
reciprocal(bm2, where=logical_not(chkbm2), out=bm2r)
facb = 1.0
veldl = elementwise_multiply(bxx, bm2r) * facb
veldl.x[chkbm2] = 0.0
veldl.y[chkbm2] = 0.0
veldl.z[chkbm2] = 0.0
dirxi = Vector(dirxb.x, diryb.x, dirzb.x)
diryi = Vector(dirxb.y, diryb.y, dirzb.y)
dirzi = Vector(dirxb.z, diryb.z, dirzb.z)
veldb = MatrixVector(veldl * dirxi, veldl * diryi,
veldl * dirzi) * facb
# Add Together
veld = velda + veldb
return veld / twoPi
def createBarsDataSet(dim=10, numTrain=10000, numTest=5000, nonlinear=True):
# CREATEBARSDATASET create a set of square images stored row-wise in a matrix
# [Xtrain Xtest] = createBarsDataSet(dim, numTrain, numTest, nonlinear)
# dim - image width = height = dim
# numTrain - number of training images
# numTest - number of test images
# nonlinear - if this flag is true/1, pixel intensities at crossing points of bars are not added up
imgdim = dim * dim
X = zeros((numTrain+numTest, imgdim))
for k in range(numTrain+numTest):
x = zeros((dim, dim))
for z in range(2):
i = np.random.permutation(range(dim))
j = np.random.permutation(range(dim))
if nonlinear is True:
x[i[z], :] = 1.0
x[:, j[z]] = 1.0
else:
x[i[z], :] += 1.0
x[:, j[z]] += 1.0
if nonlinear is False:
x = x / 4
X[k, :] = x.reshape(1, imgdim)
Xtrain = X[:numTrain, :]
Xtest = X[numTrain+1:, :]
return Xtrain, Xtest
def fetch_pids_ttol(pnts: MatrixVector, psys: PanelSystem, ztol: float=0.01, ttol: float=0.1):
shp = pnts.shape
pnts = pnts.reshape((-1, 1))
numpnt = pnts.shape[0]
numpnl = len(psys.pnls)
pidm = zeros((1, numpnl), dtype=int)
wintm = zeros((numpnt, numpnl), dtype=bool)
abszm = zeros((numpnt, numpnl), dtype=float)
for pnl in psys.pnls.values():
pidm[0, pnl.ind] = pnl.pid
wintm[:, pnl.ind], abszm[:, pnl.ind] = pnl.within_and_absz_ttol(pnts[:, 0], ttol=ttol)
abszm[wintm is False] = float('inf')
minm = argmin(abszm, axis=1)
minm = array(minm).flatten()
pidm = array(pidm).flatten()
pids = pidm[minm]
pids = matrix([pids], dtype=int).transpose()
indp = arange(numpnt)
minz = array(abszm[indp, minm]).flatten()
minz = matrix([minz], dtype=float).transpose()
chkz = minz < ztol
pids[logical_not(chkz)] = 0
pids = pids.reshape(shp)
chkz = chkz.reshape(shp)
return pids, chkz
def kernel_matrix(X, kernel, n1, n2):
(n, d) = X.shape
assert n == n1 + n2
K = mat.zeros((n, n))
for i in xrange(n):
for j in xrange(i + 1):
K[i, j] = kernel(X[i, :], X[j, :])
K[j, i] = K[i, j]
U1 = mat.sum(K[0:n1, :], 0) / n1
U2 = mat.sum(K[n1:n, :], 0) / n2
U1m = mat.tile(U1, (n1, 1))
U2m = mat.tile(U2, (n2, 1))
U = mat.bmat('U1m; U2m')
m1m1 = mat.sum(K[0:n1, 0:n1]) / (n1 * n1)
m1m2 = mat.sum(K[0:n1, n1:n]) / (n1 * n2)
m2m2 = mat.sum(K[n1:n, n1:n]) / (n2 * n2)
mumu = mat.zeros((n, n))
mumu[0:n1, 0:n1] = m1m1
mumu[0:n1, n1:n] = m1m2
mumu[n1:n, 0:n1] = m1m2
mumu[n1:n, n1:n] = m2m2
Kcu = K - U
Kuc = Kcu.T
N = mat.ones((n, n)) / n
Kc = K - U - U.T + mumu
return (K, Kuc, Kc)
def cost_func(B, model, profile, profiler):
cost = ml.zeros([model.T, 1])
U = ml.zeros([model.uDim, model.T])
T = model.T
for t in range(T - 1):
U[:, t] = B[0:model.xDim, t + 1] - B[0:model.xDim, t]
B[:, t + 1] = belief.belief_dynamics(B[:, t], U[:, t], None, model)
if max(U[:, t]) > 1:
cost[t] = 1000
elif abs(B[0, t]) > model.xMax[0]:
cost[t] = 1000
elif abs(B[1, t]) > model.xMax[1]:
cost[t] = 1000
else:
null, s = belief.decompose_belief(B[:, t], model)
cost[t] = model.alpha_belief * ml.trace(
s * s) + model.alpha_control * ml.sum(U[:, t].T * U[:, t])
x, s = belief.decompose_belief(B[:, T - 1], model)
cost[T - 1] = model.alpha_final_belief * ml.trace(s * s)
return cost, B, U
def play(self,
q,
dt,
slow_down_factor=1,
print_time_every=-1.0,
robotName='robot1'):
self.addSphere('com', self.COM_SPHERE_RADIUS, zeros((3, 1)),
zeros((3, 1)), self.COM_SPHERE_COLOR, 'OFF')
if (ENABLE_VIEWER):
trajRate = 1.0 / dt
rate = max(
1, int(slow_down_factor * trajRate / self.PLAYER_FRAME_RATE))
lastRefreshTime = time()
timePeriod = 1.0 / self.PLAYER_FRAME_RATE
for t in range(0, q.shape[1], rate):
self.updateRobotConfig(q[:, t], robotName, refresh=False)
com = self.robots[robotName].com(q[:, t])
self.updateObjectConfig('com',
(com[0, 0], com[1, 0], 0, 0, 0, 0, 1))
timeLeft = timePeriod - (time() - lastRefreshTime)
if (timeLeft > 0.0):
sleep(timeLeft)
self.robot.viewer.gui.refresh()
lastRefreshTime = time()
if (print_time_every > 0.0
and t * dt % print_time_every == 0.0):
print "%.1f" % (t * dt)
def MakeWind(speedmean, speedstd, dirmean, dirstd, n):
"""
Uses auto-regressive process to compute a set
of wind x/y velocities. Returns a 2-item list where each
item is a list of all the x/y velocities respectively.
"""
N = 10
phispeed = matlib.ones((1, N)) / N * 0.99
phidir = matlib.ones((1, N)) / N * 0.99
s0 = speedmean
d0 = dirmean
speeds = [s0]
dirs = [d0]
xs = []
ys = []
espeed = lambda: random.normal(speedmean, speedstd)
edir = lambda: random.normal(dirmean, dirstd)
for ii in range(1, n+1):
Xspeed = matlib.zeros(phispeed.shape).T
Xdir = matlib.zeros(phidir.shape).T
for jj in range(N):
idx = max(ii + jj - N, 0)
Xspeed[jj, 0] = speeds[idx] - speedmean
Xdir[jj, 0] = dirs[idx] - dirmean
speeds.append(float(phispeed * Xspeed + espeed()))
dirs.append(float(phidir * Xdir + edir()))
xs.append(speeds[-1] * np.cos(dirs[-1]))
ys.append(speeds[-1] * np.sin(dirs[-1]))
return [xs, ys]
def test_random_matrix():
n = 2
k = 10
A = -matlib.rand(n, n)
x0 = 1e3 * matlib.rand(n).T
a = matlib.zeros(n).T
T = 1e-2
for i in range(n):
A[i, i] *= k
print("A\n", A)
x = compute_x_T(A, a, x0, T)
print('x(0)= ', x0.T)
print('x= ', x.T)
# compute approximate x
x_a = matlib.zeros(n).T
for i in range(n):
Ai = A[i:i + 1, i:i + 1]
x_a[i, 0] = expm(T * Ai) * x0[i, 0]
print('approximate x=', x_a.T)
a = A * x0
for i in range(n):
Ai = A[i:i + 1, i:i + 1]
ai = a[i, 0] - Ai * x0[i, 0]
xi = compute_x_T(Ai, ai, x0[i, 0], T)
x_a[i, 0] = xi[0, 0]
print('approximate x=', x_a.T)
def __init__(self):
# model dimensions
self.xDim = 2 # state space dimension
self.uDim = 2 # control input dimension
self.qDim = 2 # dynamics noise dimension
self.zDim = 2 # observation dimension
self.rDim = 2 # observtion noise dimension
# belief space dimension
# note that we only store the lower (or upper) triangular portion
# of the covariance matrix to eliminate redundancy
self.bDim = int(self.xDim + self.xDim*((self.xDim+1)/2.))
self.dT = 1. # time step for dynamics function
self.T = 15 # number of time steps in trajectory
self.alpha_belief = 10. # weighting factor for penalizing uncertainty at intermediate time steps
self.alpha_final_belief = 10. # weighting factor for penalizing uncertainty at final time step
self.alpha_control = 1. # weighting factor for penalizing control cost
self.xMin = ml.vstack([-5,-3]) # minimum limits on state (xMin <= x)
self.xMax = ml.vstack([5,3]) # maximum limits on state (x <= xMax)
self.uMin = ml.vstack([-1,-1]) # minimum limits on control (uMin <= u)
self.uMax = ml.vstack([1,1]) # maximum limits on control (u <= uMax)
self.Q = ml.eye(self.qDim) # dynamics noise variance
self.R = ml.eye(self.rDim) # observation noise variance
self.start = ml.zeros([self.xDim,1]) # start state, OVERRIDE
self.goal = ml.zeros([self.xDim,1]) # end state, OVERRIDE
self.sqpParams = LightDarkSqpParams()
def compute_x_T(A, a, x0, T, dt=None, invertible_A=True):
if (dt is not None):
N = int(T / dt)
x = matlib.copy(x0)
for i in range(N):
dx = A.dot(x) + a
x += dt * dx
return x
if (invertible_A):
e_TA = expm(T * A)
A_inv_a = solve(A, a)
return e_TA * (x0 + A_inv_a) - A_inv_a
n = A.shape[0]
C = matlib.zeros((n + 1, n + 1))
C[0:n, 0:n] = A
C[0:n, n] = a
z0 = matlib.zeros((n + 1, 1))
z0[:n, 0] = x0
z0[-1, 0] = 1.0
e_TC = expm(T * C)
z = e_TC * z0
x_T = z[:n, 0]
return x_T
def assemble_panels_full(self, time: bool=True, mach: float=0.0):
if time:
start = perf_counter()
shp = (self.numpnl, self.numpnl)
apd = zeros(shp, dtype=float)
aps = zeros(shp, dtype=float)
avd = zero_matrix_vector(shp, dtype=float)
avs = zero_matrix_vector(shp, dtype=float)
betm = betm_from_mach(mach)
for pnl in self.pnls.values():
ind = pnl.ind
apd[:, ind], aps[:, ind], avd[:, ind], avs[:, ind] = pnl.influence_coefficients(self.pnts, betx=betm)
if self._apd is None:
self._apd = {}
self._apd[mach] = apd
if self._aps is None:
self._aps = {}
self._aps[mach] = aps
if self._avd is None:
self._avd = {}
self._avd[mach] = avd
if self._avs is None:
self._avs = {}
self._avs[mach] = avs
if time:
finish = perf_counter()
elapsed = finish - start
print(f'Full panel assembly time is {elapsed:.3f} seconds.')
def __init__(self, inpDim, hidDim):
self.inpDim = inpDim #number of input neurons (and output neurons)
self.hidDim = hidDim #number of hidden neurons
self.inp = zeros((self.inpDim, 1)) #vector holding current input
self.out = zeros((self.hidDim, 1)) #output neurons
self.g = zeros((self.hidDim, 1)) #neural activity before non-linearity
self.h = zeros((self.hidDim, 1)) #hidden neuron activation
self.a = ones((self.hidDim, 1)) #slopes of activation functions
self.b = -3 * ones((self.hidDim, 1)) #biases of activation functions
scale = 0.025
self.W = scale * (
2 * rand((self.inpDim, self.hidDim)) - 0.5 * ones(
(self.inpDim, self.hidDim))
) + scale #shared network weights, i.e. used to compute hidden layer activations and estimated outputs
print self.W.shape
self.lrateRO = 0.01
#learning rate for synaptic plasticity of read-out layer (RO)
self.regRO = 0.0002
#numerical regularization constant
self.decayP = 0
#decay factor for positive weights [0..1]
self.decayN = 1
#decay factor for negative weights [0..1]
self.lrateIP = 0.001
#learning rate for intrinsic plasticity (IP)
self.meanIP = 0.2
def SimilarityMatrixForVectors (vecA, vecB):
"""
Returns the similarity transformation matrix that converts
a given column vector to an other column vector. It works
even with zero entries.
Parameters
----------
vecA : column vector, shape(M,1)
The original column vector
vecB : column vector, shape(M,1)
The target column vector
Returns
-------
B : matrix, shape(M,M)
The matrix by which `B\cdot vecA = vecB` holds
"""
# construct permutation matrix to move at least one non-zero element to the first position
# to acchieve it, the code below sorts it in a reverse order
m = vecA.shape[0]
ix = np.argsort(-np.array(vecA).flatten())
P=ml.zeros((m,m))
for i in range(m):
P[i,ix[i]] = 1.0
cp = P*vecA
# construct transformation matrix B for which B*rp=1 holds
B = ml.zeros((m,m))
for i in range(m):
B[i,0:i+1] = vecB.flat[i] / np.sum(cp[0:i+1,0])
# construct matrix Bp for which Bp*r=1 holds
return B*P
def inf(self, x, meanonly=False):
x = np.asmatrix(x)
if x.shape[1] != self.d:
if x.shape[0] == self.d:
x = x.T
else:
raise Exception('Invalid test-set dimension -- '
'expected d = ' + str(self.d) + '.')
n = x.shape[0]
# Handle empty test set
if n == 0:
return (np.zeros((0, 1)), np.zeros((0, 1)))
ms = self.kernel.mean*np.ones((n, 1))
Kbb = self.kernel(x, diag=True)
# Handle empty training set
if len(self) == 0:
return (ms, np.asmatrix(Kbb))
Kba = self.kernel(x, self.x)
m = self.kernel.mean*np.ones((len(self), 1))
fm = ms + Kba*scipy.linalg.cho_solve((self.L, True), self.y - m,
overwrite_b=True)
if meanonly:
return fm
else:
W = scipy.linalg.cho_solve((self.L, True), Kba.T)
fv = np.asmatrix(Kbb - np.sum(np.multiply(Kba.T, W), axis=0).T)
# W = np.asmatrix(scipy.linalg.solve(self.L, Kba.T, lower=True))
# fv = np.asmatrix(Kbb - np.sum(np.power(W, 2), axis=0).T)
return (fm, fv)
def LagkJointMomentsFromMRAP(H, K=0, L=1):
"""
Returns the lag-L joint moments of a marked rational
arrival process.
Parameters
----------
H : list/cell of matrices of shape(M,M), length(N)
The H0...HN matrices of the MRAP to check
K : int, optional
The dimension of the matrix of joint moments to
compute. If K=0, the MxM joint moments will be
computed. The default value is 0
L : int, optional
The lag at which the joint moments are computed.
The default value is 1
prec : double, optional
Numerical precision to check if the input is valid.
The default value is 1e-14
Returns
-------
Nm : list/cell of matrices of shape(K+1,K+1), length(N)
The matrices containing the lag-L joint moments,
starting from moment 0.
"""
if butools.checkInput and not CheckMRAPRepresentation(H):
raise Exception(
"LagkJointMomentsFromMRAP: Input is not a valid MRAP representation!"
)
if K == 0:
K = H[0].shape[0] - 1
M = len(H) - 1
H0 = H[0]
sumH = ml.zeros(H[0].shape)
for i in range(M):
sumH += H[i + 1]
H0i = la.inv(-H0)
P = H0i * sumH
pi = DRPSolve(P)
Pw = ml.eye(H0.shape[0])
H0p = [ml.matrix(Pw)]
for i in range(1, K + 1):
Pw *= i * H0i
H0p.append(ml.matrix(Pw))
Pl = la.matrix_power(P, L - 1)
Nm = []
for m in range(M):
Nmm = ml.zeros((K + 1, K + 1))
for i in range(K + 1):
for j in range(K + 1):
Nmm[i, j] = np.sum(pi * H0p[i] * H0i * H[m + 1] * Pl * H0p[j])
Nm.append(ml.matrix(Nmm))
return Nm
def __init__(self, **kwargs):
super(Sensor, self).__init__( **kwargs )
self.properties.update( measurementList = kwargs.get('measurementList', self.measurementList ),
stateList = kwargs.get('stateList',self.stateList ) )
for attribute in self.measurementList:
setattr(self, attribute, kwargs.get(attribute, 0.) )
for key in self.stateList:
attribute = key.split('.')[1]
setattr( self, attribute, kwargs.get(attribute, 0.) )
self.properties.update(
Z = kwargs.get('Z', matlib.zeros( [len(self), 1] )),
StateMap = kwargs.get('StateMap', matlib.zeros( [len(self) , 1] )),
MeasurementJacobian = kwargs.get('MeasurementJacobian', matlib.eye( len(self) ) ),
MeasurementCovariance = kwargs.get('MeasurementCovariance', 0.3 * matlib.eye( len(self) )),
)
# self.Z = kwargs.get('Z', matlib.zeros( [len(self), 1] ))
# self.StateMap = kwargs.get('StateMap', matlib.zeros( [len(self) , 1] ))
# self.MeasurementJacobian = kwargs.get('MeasurementJacobian', matlib.eye( len(self) ) )
# self.MeasurementCovariance = kwargs.get('MeasurementCovariance', 0.3 * matlib.eye( len(self) ))
self.set_Z()
def sde_coupled(x0, y0, t0, b, sigma, N, h, delta=None):
if delta is None:
delta = h
# t = np.arange(T[0], T[1]+h/2, h)
t = np.arange(t0, t0 + N * h, h)
t.shape = (t.shape[0], 1)
# N = t.size
d = x0.shape[0]
dB = np.mat(np.random.normal(0, np.sqrt(h), (d, N)))
dB[:, 0] = 0
X = mat.zeros((d, N))
X[:, 0] = x0
Y = mat.zeros((d, N))
Y[:, 0] = y0
Id = mat.eye(d)
dBhat = mat.zeros((d, N))
for i in range(0, N - 1):
dBhat[:,
i + 1] = (Id - 2 * proj(sigma, X[:, i] - Y[:, i])) * dB[:, i + 1]
X[:, i + 1] = X[:, i] + h * b(t[i], X[:, i]) + sigma * dB[:, i + 1]
Y[:, i + 1] = Y[:, i] + h * b(t[i], Y[:, i]) + sigma * dBhat[:, i + 1]
if la.norm(X[:, i + 1] - Y[:, i + 1]) < delta:
Y[:, i + 1] = X[:, i + 1]
if d == 1 and (X[:, i] - Y[:, i]) * (X[:, i + 1] - Y[:, i + 1]) < 0:
Y[:, i + 1] = X[:, i + 1]
B = np.cumsum(dB, axis=1)
Bhat = np.cumsum(dBhat, axis=1)
return (X, Y, B, Bhat, t.T)
def SimilarityMatrixForVectors(vecA, vecB):
"""
Returns the similarity transformation matrix that converts
a given column vector to an other column vector. It works
even with zero entries.
Parameters
----------
vecA : column vector, shape(M,1)
The original column vector
vecB : column vector, shape(M,1)
The target column vector
Returns
-------
B : matrix, shape(M,M)
The matrix by which `B\cdot vecA = vecB` holds
"""
# construct permutation matrix to move at least one non-zero element to the first position
# to acchieve it, the code below sorts it in a reverse order
m = vecA.shape[0]
ix = np.argsort(-np.array(vecA).flatten())
P = ml.zeros((m, m))
for i in range(m):
P[i, ix[i]] = 1.0
cp = P * vecA
# construct transformation matrix B for which B*rp=1 holds
B = ml.zeros((m, m))
for i in range(m):
B[i, 0:i + 1] = vecB.flat[i] / np.sum(cp[0:i + 1, 0])
# construct matrix Bp for which Bp*r=1 holds
return B * P
def lookahead(self, x0, Yd, Q, R, N):
Abar = matlib.zeros((self.p * N, self.n))
Bbar = matlib.zeros((self.p * N, self.m * N))
Qbar = matlib.zeros((self.p * N, self.p * N))
Rbar = matlib.zeros((self.m * N, self.m * N))
# TODO right now we don't include a separate QN for terminal condition
# Construct matrices
for k in range(N):
l = k * self.p
u = (k + 1) * self.p
Abar[k * self.p:(k + 1) * self.p, :] = self.C * self.A**(k + 1)
Qbar[k * self.p:(k + 1) * self.p, k * self.p:(k + 1) * self.p] = Q
Rbar[k * self.m:(k + 1) * self.m, k * self.m:(k + 1) * self.m] = R
for j in range(k + 1):
Bbar[k * self.p:(k + 1) * self.p, j * self.m:(j + 1) *
self.m] = self.C * self.A**(k - j - 1) * self.B
# TODO note H is independt of x0 and Yd, and is thus constant at each
# step
H = Rbar + Bbar.T * Qbar * Bbar
g = (Abar * x0 - Yd).T * Qbar * Bbar
return np.array(H), np.array(g).flatten()
def compute_integral_x_T(A, a, x0, T, dt=None, invertible_A=True):
if (dt is not None):
N = int(T / dt)
int_x = dt * x0
for i in range(1, N):
x = compute_x_T(A, a, x0, i * dt)
int_x += dt * x
return int_x
if (invertible_A):
e_TA = expm(T * A)
Ainv_a = solve(A, a)
Ainv_x0_plus_Ainv_a = solve(A, x0 + Ainv_a)
I = matlib.eye(A.shape[0])
return (e_TA - I) * Ainv_x0_plus_Ainv_a - T * Ainv_a
n = A.shape[0]
C = matlib.zeros((n + 2, n + 2))
C[0:n, 0:n] = A
C[0:n, n] = a
C[0:n, n + 1] = x0
C[n:n + 1, n + 1:] = 1.0
z0 = matlib.zeros((n + 2, 1))
z0[-1, 0] = 1.0
e_TC = expm(T * C)
z = e_TC * z0
int_x = z[:n, 0]
return int_x
def belief_dynamics(b_t, u_t, z_tp1, model,profiler=None,profile=False):
dynamics_func = model.dynamics_func
obs_func = model.obs_func
qDim = model.qDim
rDim = model.rDim
x_t, SqrtSigma_t = decompose_belief(b_t, model)
Sigma_t = SqrtSigma_t*SqrtSigma_t
if z_tp1 is None:
# Maximum likelihood observation assumption
z_tp1 = obs_func(dynamics_func(x_t, u_t, ml.zeros([qDim,1])), ml.zeros([rDim,1]))
if profile: profiler.start('ekf')
x_tp1, Sigma_tp1 = ekf(x_t, Sigma_t, u_t, z_tp1, model)
if profile: profiler.stop('ekf')
# Compute square root for storage
# Several different choices available -- we use the principal square root
if profile: profiler.start('eigh')
D_diag, V = ml.linalg.eigh(Sigma_tp1)
if profile: profiler.stop('eigh')
SqrtSigma_tp1 = V*np.sqrt(ml.diag(D_diag))*V.T
b_tp1 = compose_belief(x_tp1, SqrtSigma_tp1, model)
return b_tp1
def getData(fid=None,
lbins=None,
ubins=None,
nbins=None,
VRval=None,
nSP=None):
# Get data
temp = np.genfromtxt(fid,
dtype={
'names': ('counts', 'runNum'),
'formats': ('f8', 'i8')
},
invalid_raise=False)
counts = temp[:]['counts']
#runNum = temp[:]['nPart']
bins = np.linspace(float(lbins), float(ubins), num=nbins)
nParts = np.size(counts)
val = zeros((nbins - 1, 1))
eDep = zeros((nbins - 1, 1))
for ii in range(0, nParts):
if (counts[ii] > 0):
eDeptemp = counts[ii]
eDep[:][(bins[:-1] <= counts[ii])
& (bins[1:] > counts[ii])] += counts[ii] * VRval / nSP
val[:][(bins[:-1] <= counts[ii])
& (bins[1:] > counts[ii])] += VRval / nSP
return bins, val, eDep
def kernel_matrix(X, kernel, n1, n2):
(n, d) = X.shape
assert n == n1 + n2
K = mat.zeros((n,n))
for i in xrange(n):
for j in xrange(i+1):
K[i,j] = kernel(X[i,:], X[j,:])
K[j,i] = K[i,j]
U1 = mat.sum(K[0:n1,:],0) / n1
U2 = mat.sum(K[n1:n,:],0) / n2
U1m = mat.tile(U1, (n1,1))
U2m = mat.tile(U2, (n2,1))
U = mat.bmat('U1m; U2m')
m1m1 = mat.sum(K[0:n1, 0:n1]) / (n1*n1)
m1m2 = mat.sum(K[0:n1, n1:n]) / (n1*n2)
m2m2 = mat.sum(K[n1:n, n1:n]) / (n2*n2)
mumu = mat.zeros((n,n))
mumu[0:n1, 0:n1] = m1m1
mumu[0:n1, n1:n] = m1m2
mumu[n1:n, 0:n1] = m1m2
mumu[n1:n, n1:n] = m2m2
Kcu = K - U
Kuc = Kcu.T
N = mat.ones((n,n))/n
Kc = K - U - U.T + mumu
return (K, Kuc, Kc)
def bcv(self):
if self._bcv is None:
num = self.opt.sys.nums
if self.param == 'L':
self._bcv = zeros((num, 1))
for i in self.strplst:
self._bcv[i, 0] += self.rhov * self.opt.sys.blg[i, 0]
elif self.param == 'Y':
self._bcv = zeros((num, 1))
for i in self.strplst:
self._bcv[i, 0] += self.rhov * self.opt.sys.bdg[i, 0]
elif self.param == 'l':
self._bcv = zeros((num, 1))
for i in self.strplst:
strp = self.opt.sys.strps[i]
bli = self.opt.sys.blg[i, 0]
byi = self.opt.sys.byg[i, 0]
ryi = strp.pnti.y - self.point.y
rzi = strp.pnti.z - self.point.z
self._bcv[i, 0] += self.rhov * (ryi * bli - rzi * byi)
elif self.param == 'm':
self._bcv = zeros((num, 1))
for i in self.strplst:
strp = self.opt.sys.strps[i]
bli = self.opt.sys.blg[i, 0]
rxi = strp.pnti.x - self.point.x
self._bcv[i, 0] -= self.rhov * rxi * bli
elif self.param == 'n':
self._bcv = zeros((num, 1))
for i in self.strplst:
strp = self.opt.sys.strps[i]
byi = self.opt.sys.byg[i, 0]
rxi = strp.pnti.x - self.point.x
self._bcv[i, 0] += self.rhov * rxi * byi
return self._bcv
def compute_x_T_and_two_integrals(A, a, x0, T):
'''
Define a new variable:
z(t) = (1, x(t), w(t), y(t))
where:
w(t) = int_0^t x(s) ds
y(t) = int_0^t w(s) ds
and then the dynamic of z(t) is:
d1 = 0
dx = a*1 + A*x
dw = x
dy = w
which we can re-write in matrix form as:
(0 0 0 0)
(a A 0 0)
dz = (0 I 0 0) z
(0 0 I 0)
dz = C z
So we can compute x(t), w(t), y(t) by computing
z(t) = e^{tC} z(0)
with z(0) = (1, x(0), 0, 0)
'''
n = A.shape[0]
C = matlib.zeros((3 * n + 1, 3 * n + 1))
C[1:1 + n, 0] = a
C[1:1 + n, 1:1 + n] = A
C[1 + n:1 + 2 * n, 1:1 + n] = matlib.eye(n)
C[1 + 2 * n:, 1 + n:1 + 2 * n] = matlib.eye(n)
z0 = np.vstack((1, x0, matlib.zeros((2 * n, 1))))
e_TC = expm(T * C)
z = e_TC * z0
x = z[1:1 + n, 0]
int_x = z[1 + n:2 * n + 1, 0]
int2_x = z[1 + 2 * n:, 0]
return x, int_x, int2_x
def __init__(self, **kwargs):
super(IMU_Kalman, self).__init__(**kwargs)
# self.Ts = kwargs.get('Ts', 0.010)
# self.quaternion = kwargs.get('quaternion', Quaternion())
self.Z = np.mat([0.0, 0.0, 0.0]).transpose()
self.StateMap = np.mat([0.0, 0.0, 0.0]).transpose()
self.ProcessCovariance = np.mat( [
[ 0.005, 0., 0., 0.],
[ 0., 0.005, 0., 0.],
[ 0., 0., 0.005, 0.],
[ 0., 0., 0., 0.005],
])
self.MeasurementCovariance = 10*np.mat( [
[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.]
])
self.ProcessJacobian = matlib.zeros( [len(self.stateList), len(self.stateList)] )
self.MeasurementJacobian = matlib.zeros( [len(self.measurementList), len(self.stateList) ])
def calc_d2r_closed(self):
u"""This function calculates the curvature of a closed spline."""
n = self.npnts
inda = [i-1 for i in range(n)]
indb = [i for i in range(n)]
inda[0] = n-1
pnl_dx = [pnl.vec.x/pnl.length for pnl in self.pnls]
pnl_dy = [pnl.vec.y/pnl.length for pnl in self.pnls]
del_dx = [0.]*n
del_dy = [0.]*n
for i in range(n):
del_dx[i] = pnl_dx[indb[i]]-pnl_dx[inda[i]]
del_dy[i] = pnl_dy[indb[i]]-pnl_dy[inda[i]]
A = zeros((n, n))
B = zeros((n, 2))
for i in range(n):
sA = self.pnls[inda[i]].length
sB = self.pnls[indb[i]].length
if i-1 < 0:
A[i, n-1] = sA/6
else:
A[i, i-1] = sA/6
A[i, i] = (sA+sB)/3
if i+1 > n-1:
A[i, 0] = sB/6
else:
A[i,i+1] = sB/6
B[i, 0] = del_dx[i]
B[i, 1] = del_dy[i]
X = solve(A, B)
d2x = [X[i, 0] for i in range(n)]
d2y = [X[i, 1] for i in range(n)]
d2r = [Vector2D(d2x[i], d2y[i]) for i in range(self.npnts)]
return d2r
def em_step(num_states, num_obs, pi_vec, z_mat, t_mat, obs_all):
# given initial parameters, run em from the obs
# obs are 2d with each row being a trajectory
# returns estimated observation and transition matrices
num_steps = obs_all.shape[1]
t_mat = npmat.matrix(t_mat)
gamma_sum = np.zeros((num_states,))
xi_sum = np.zeros((num_states, num_states))
obs_sum = np.zeros((num_obs, num_states))
for t in range(obs_all.shape[0]):
obs = obs_all[t, :]
# compute alpha
alpha = npmat.zeros((num_states, num_steps))
alpha[:, 0] = npmat.matrix((pi_vec * z_mat[obs[0], :])[:, np.newaxis])
for i in range(1, len(obs)):
alpha[:, i] = npmat.matrix(((t_mat * alpha[:, i - 1]).getA()[:, 0] * z_mat[obs[i], :])[:, np.newaxis])
asum = alpha[:, i].sum()
alpha[:, i] /= asum
# print('alpha\n' + str(alpha))
# compute beta
beta = npmat.zeros((num_states, num_steps))
beta[:, num_steps - 1] = npmat.ones((num_states, 1))
for i in range(len(obs) - 2, -1, -1):
beta[:, i] = t_mat.getT() * npmat.matrix(
(z_mat[obs[i + 1], :] * beta[:, i + 1].getA()[:, 0])[:, np.newaxis])
bsum = beta[:, i].sum()
beta[:, i] /= bsum
# print(beta)
# compute gamma
gamma = alpha.getA() * beta.getA()
gamma = gamma / (gamma.sum(axis=0)[np.newaxis, :])
# print(gamma)
# compute xi (transposed from the paper)
xi = np.zeros((num_states, num_states, num_steps - 1))
for i in range(num_steps - 1):
xi[:, :, i] = np.transpose(alpha[:, i].getA()) * t_mat.getA() * (z_mat[obs[i + 1], :] *
beta[:, i + 1].getA()[0])[:, np.newaxis]
xsum = xi[:, :, i].sum()
xi[:, :, i] /= xsum
# print('xi[:,:,i]' + str(xi[:,:,i]))
# print('np.transpose(alpha[:,i].getA()): ' + str(np.transpose(alpha[:,i].getA())))
# print('t_mat: ' + str(t_mat))
# print('result of *: ' + str(np.transpose(alpha[:,i].getA()) * t_mat.getA()))
# add to the sums
gamma_sum += gamma.sum(axis=1)
xi_sum += xi.sum(axis=2)
for z in range(num_obs):
obs_sum[z, :] += gamma[:, obs == z].sum(axis=1)
# print('gamma_sum\n' + str(gamma_sum))
# print('xi_sum\n' + str(xi_sum))
# print('obs_sum\n' + str(obs_sum))
# finally compute estimates
est_z_mat = obs_sum / obs_sum.sum(axis=0)[np.newaxis, :]
est_t_mat = xi_sum / xi_sum.sum(axis=0)[np.newaxis, :]
return est_z_mat, est_t_mat;
def echo_encode(original_audio: Audio,
mark: bytes,
alpha: float = 0.7,
m: tuple = (150, 200)) -> Audio:
"""
用回声隐藏进行隐写。
优点：透明性强，写入的数据并非噪声，鲁棒性好，抗压缩性好。
缺点：容量小，这里默认是8192个取样点写入一个比特。
:param original_audio: 原音频，为一个Audio对象
:param mark: 水印，必须为一个bytes对象
:param alpha: 衰退率，理论上这个值越大水印鲁棒性越好，但生成的音频回音会更强
:param m: 回音的延迟，分别为比特0和比特1的延迟
:return: 隐写后的音频，为一个Audio对象
"""
if not isinstance(mark, bytes):
raise MarkFormatError("Mark must be a bytes object.")
if alpha > 1 or alpha < 0:
raise MarkFormatError("Alpha must be in [0,1].")
bits = np.mat(get_all_bits(mark))
bits_len = bits.shape[1]
original_samples_reg = np.matrix(original_audio.get_reshaped_samples())
samples_len = original_samples_reg.shape[1]
channels = original_audio.channels
if 8192 * bits_len > samples_len:
raise MarkTooLargeError("Mark too large for echo encoding.")
fragment_len = samples_len // bits_len
encoded_len = bits_len * fragment_len
kernel0 = np.concatenate((mt.zeros(
(channels, m[0])), alpha * original_samples_reg), 1)
kernel1 = np.concatenate((mt.zeros(
(channels, m[1])), alpha * original_samples_reg),
1) # 这里是两个回声核，0代表隐藏信息的0，1代表隐藏信息的1
direct_sig = np.reshape(
np.matrix(mt.ones((fragment_len, 1))) * bits, (encoded_len, 1), 'F')
smooth_length = int(
np.floor(fragment_len / 4) - np.floor(fragment_len / 4) % 4)
tp = np.matrix(
fftconvolve(np.array(direct_sig)[:, 0],
np.hanning(smooth_length))) # 这里用的是汉宁窗口，其他窗口的效果类似
window = tp[0, smooth_length // 2:tp.shape[1] - smooth_length // 2 +
1] / np.max(np.abs(tp)) # window 用于平滑
mixer = mt.ones((channels, 1)) * window # 这里的 channels 很关键，需要写入所有的音轨
encoded_samples_reg = original_samples_reg[:, :encoded_len] + \
np.multiply(kernel1[:, :encoded_len], mixer) + \
np.multiply(kernel0[:, :encoded_len], abs(mixer - 1)) # 这里是回音合成
new_samples_reg = np.concatenate(
(encoded_samples_reg, original_samples_reg[:, encoded_len:]), 1)
new_audio = original_audio.spawn(new_samples_reg)
new_audio.key = {
'type': 'SINGLE_ECHO',
'key': {
'm': m,
'fragment_len': fragment_len,
'bits_len': bits_len
}
}
return new_audio
def __init__(self, name):
self.name = ''
self.position = matlib.zeros(shape=(3, 1))
self.position_camera = matlib.zeros(shape=(3, 1))
self.old_position_measurement = matlib.zeros(shape=(3, 1))
self.position_delayed = matlib.zeros(shape=(3, 1))
self.camera_flag = 0
self.vel = matlib.zeros((3, 1))
def localadapting(Graph, node):
I = mat.eye(Graph.N)
X = mat.zeros((Graph.N, Graph.N))
for i in range(Graph.N):
e = mat.zeros(Graph.N)
e[0, i] = 1
node.X[:, i] = node.X[:, i] - Graph.stepsize * np.multiply(
node.X[:, i], e) + Graph.stepsize * np.multiply(Graph.Y, e)
def __init__(self, name):
self.name = ""
self.position = matlib.zeros(shape=(3, 1))
self.position_camera = matlib.zeros(shape=(3, 1))
self.old_position_measurement = matlib.zeros(shape=(3, 1))
self.position_delayed = matlib.zeros(shape=(3, 1))
self.camera_flag = 0
self.vel = matlib.zeros((3, 1))
def getOrthColumns(m):
'''
Constructs the orthogonally complementing columns of the input.
Input of the form pxr is assumed to have r<=p,
and have either full column rank r or rank 0 (scalar or matrix)
Output is of the form px(p-r), except:
a) if M square and full rank p, returns scalar 0
b) if rank(M)=0 (zero matrix), returns I_p
(Note you cannot pass scalar zero, because dimension info would be
missing.)
Return type is as input type.
'''
if type(m) == type(asarray(m)):
m = mat(m)
output = 'array'
else: output = 'matrix'
p, r = m.shape
# first catch the stupid input case
if p < r: raise ValueError, 'need at least as many rows as columns'
# we use lstsq(M, ones) just to exploit its rank-finding algorithm,
rk = lstsq(m, ones(p).T)[2]
# first the square and full rank case:
if rk == p: result = zeros((p,0)) # note the shape! hopefully octave-like
# then the zero-matrix case (within machine precision):
elif rk == 0: result = eye(p)
# now the rank-deficient case:
elif rk < r:
raise ValueError, 'sorry, matrix does not have full column rank'
# (what's left should be ok)
else:
# we have to watch out for zero rows in M,
# if they are in the first p-r positions!
# so the (probably inefficient) algorithm:
# 1. check the rank of each row
# 2. if zero, then also put a zero row in c
# 3. if not, put the next unit vector in c-row
idr = eye(r)
idpr = eye(p-r)
c = empty([0,r]) # starting point
co = empty([0, p-r]) # will hold orth-compl.
idrcount = 0
for row in range(p):
# (must be ones() instead of 1 because of 2d-requirement
if lstsq( m[row,:], ones(1) )[2] == 0 or idrcount >= r:
c = r_[ c, zeros(r) ]
co = r_[ co, idpr[row-idrcount, :] ]
else: # row is non-zero, and we haven't used all unit vecs
c = r_[ c, idr[idrcount, :] ]
co = r_[ co, zeros(p-r) ]
idrcount += 1
# earlier non-general (=bug) line: c = mat(r_[eye(r), zeros((p-r, r))])
# and: co = mat( r_[zeros((r, p-r)), eye(p-r)] )
# old:
# result = ( eye(p) - c * (M.T * c).I * M.T ) * co
result = co - c * solve(m.T * c, m.T * co)
if output == 'array': return result.A
else: return result
def LagkJointMomentsFromMRAP (H, K=0, L=1):
"""
Returns the lag-L joint moments of a marked rational
arrival process.
Parameters
----------
H : list/cell of matrices of shape(M,M), length(N)
The H0...HN matrices of the MRAP to check
K : int, optional
The dimension of the matrix of joint moments to
compute. If K=0, the MxM joint moments will be
computed. The default value is 0
L : int, optional
The lag at which the joint moments are computed.
The default value is 1
prec : double, optional
Numerical precision to check if the input is valid.
The default value is 1e-14
Returns
-------
Nm : list/cell of matrices of shape(K+1,K+1), length(N)
The matrices containing the lag-L joint moments,
starting from moment 0.
"""
if butools.checkInput and not CheckMRAPRepresentation (H):
raise Exception("LagkJointMomentsFromMRAP: Input is not a valid MRAP representation!")
if K==0:
K = H[0].shape[0]-1
M = len(H)-1
H0 = H[0]
sumH = ml.zeros(H[0].shape)
for i in range(M):
sumH += H[i+1]
H0i = la.inv(-H0)
P = H0i*sumH
pi = DRPSolve(P)
Pw = ml.eye(H0.shape[0])
H0p = [ml.matrix(Pw)]
for i in range(1,K+1):
Pw *= i*H0i
H0p.append(ml.matrix(Pw))
Pl = la.matrix_power (P, L-1)
Nm = []
for m in range(M):
Nmm = ml.zeros ((K+1,K+1))
for i in range(K+1):
for j in range(K+1):
Nmm[i,j] = np.sum (pi * H0p[i] * H0i * H[m+1] * Pl * H0p[j])
Nm.append(ml.matrix(Nmm))
return Nm
def velocity_matrix(ra: MatrixVector,
rb: MatrixVector,
rc: MatrixVector,
betm: float = 1.0,
tol: float = 1e-12):
if ra.shape != rb.shape:
return ValueError()
numi = rc.shape[0]
numj = ra.shape[1]
ra = ra.repeat(numi, axis=0)
rb = rb.repeat(numi, axis=0)
rc = rc.repeat(numj, axis=1)
a = rc - ra
b = rc - rb
a.x = a.x / betm
b.x = b.x / betm
am = a.return_magnitude()
bm = b.return_magnitude()
# Velocity from Bound Vortex
adb = elementwise_dot_product(a, b)
abm = multiply(am, bm)
dm = multiply(abm, abm + adb)
axb = elementwise_cross_product(a, b)
axbm = axb.return_magnitude()
chki = (axbm == 0.0)
chki = logical_and(axbm >= -tol, axbm <= tol)
veli = elementwise_multiply(axb, divide(am + bm, dm))
veli.x[chki] = 0.0
veli.y[chki] = 0.0
veli.z[chki] = 0.0
# Velocity from Trailing Vortex A
axx = MatrixVector(zeros(a.shape, dtype=float), a.z, -a.y)
axxm = axx.return_magnitude()
chka = (axxm == 0.0)
vela = elementwise_divide(axx, multiply(am, am - a.x))
vela.x[chka] = 0.0
vela.y[chka] = 0.0
vela.z[chka] = 0.0
# Velocity from Trailing Vortex B
bxx = MatrixVector(zeros(b.shape, dtype=float), b.z, -b.y)
bxxm = bxx.return_magnitude()
chkb = (bxxm == 0.0)
velb = elementwise_divide(bxx, multiply(bm, bm - b.x))
velb.x[chkb] = 0.0
velb.y[chkb] = 0.0
velb.z[chkb] = 0.0
return veli, vela, velb
def fit(self, *p_list):
A = mat.zeros((6, 6))
Y = mat.zeros((6, 1))
for x, y, z in p_list:
Q = np.asmatrix(self._order(x, y)).T
A += Q * Q.T
Y += z * Q
self.coef = np.array(np.ravel(A.I * Y))
return self
def fit(self, * p_list):
A = mat.zeros((6, 6))
Y = mat.zeros((6, 1))
for x, y, z in p_list:
Q = np.asmatrix(self._order(x, y)).T
A += Q * Q.T
Y += z * Q
self.coef = np.array(np.ravel(A.I * Y))
return self
def DMRAPFromMoments (moms, Nm):
"""
Creates a discrete marked rational arrival process that
has the same marginal and lag-1 joint moments as given
(see [1]_).
Parameters
----------
moms : vector of doubles
The list of marginal moments. To obtain a discrete
marked rational process of order M, 2*M-1 marginal
moments are required.
Nm : list of matrices, shape (M,M)
The list of lag-1 joint moment matrices. The
length of the list determines K, the number of arrival
types of the discrete rational process.
Returns
-------
H : list of matrices, shape (M,M)
The H0, H1, ..., HK matrices of the discrete marked
rational process
References
----------
.. [1] Andras Horvath, Gabor Horvath, Miklos Telek, "A
traffic based decomposition of two-class queueing
networks with priority service," Computer Networks
53:(8) pp. 1235-1248. (2009)
"""
v, H0 = MGFromMoments (moms)
H0i = la.inv(ml.eye(H0.shape[0])-H0)
Ge = ml.zeros(H0.shape)
G1 = ml.zeros(H0.shape)
H0ip = ml.eye(H0.shape[0])
for i in range(H0.shape[0]):
Ge[i,:] = v * H0ip
G1[:,i] = np.sum(H0ip, 1)
H0ip *= (i+1) * H0i
if i>0:
H0ip *= H0
Gei = la.inv(Ge)
G1i = la.inv(G1)
H = [H0]
for i in range(1,len(Nm)+1):
Nmi = Nm[i-1]
row1 = np.array([FactorialMomsFromMoms(Nmi[0,1:].A.flatten())])
col1 = np.array([FactorialMomsFromMoms(Nmi[1:,0].A.flatten())]).T
mid = JFactorialMomsFromJMoms(Nmi[1:,1:])
Nmi = np.bmat([[[[Nmi[0,0]]], row1 ], [col1, mid]])
H.append((ml.eye(H0.shape[0])-H0)*Gei*Nmi*G1i)
return H
def fccd(moving, fixed, threshold=0.1, maxit=10000):
if len(moving)<6:
raise "Moving too short"
if len(fixed)!=3:
raise "Fixed should have length 3"
lng=len(moving)
# Coordinates along COLUMNS
moving_coords=zeros((3, 3))
fixed_coords=zeros((3, 3))
it=0
while 1:
if it==maxit:
# Stop - max iterations reached
return "MAXIT", rmsd, it
for i in range(2, lng-2):
it=it+1
center=moving[i]
# move to pivot origin
for j in range(0, 3):
index=-(3-j)
v=moving[index]-center
moving_coords[:,j]=array([v.get_array()]).transpose()
for j in range(0, 3):
v=fixed[j]-center
fixed_coords[:,j]=array([v.get_array()]).transpose()
# Do SVD
#a=matrixmultiply(fixed_coords, transpose(moving_coords))
a = dot(fixed_coords, moving_coords.transpose())
u, d, vt=svd(a)
# Check reflection
if (det(u)*det(vt))<0:
u=dot(u, S)
# Calculate rotation
rot_matrix=dot(u, vt)
assert(det(rot_matrix)>0)
# Apply rotation
for j in range(i+1, len(moving)):
v=moving[j]-center
v = dot(rot_matrix, v.get_array())
v = Vector(array(v)[0])
v=v+center
moving[j]=v
# Calculate RMSD
rmsd=0.0
for j in range(1, 4):
m=moving[-j]
f=fixed[-j]
n=(m-f).norm()
rmsd+=n*n
rmsd=sqrt(rmsd/3.0)
if rmsd<threshold:
# stop - RMSD threshold reached
return "SUCCESS", rmsd, it
def MRAPFromMoments (moms, Nm):
"""
Creates a marked rational arrival process that has the same
marginal and lag-1 joint moments as given (see [1]_).
Parameters
----------
moms : vector of doubles
The list of marginal moments. To obtain a marked
rational process of order M, 2*M-1 marginal moments
are required.
Nm : list of matrices, shape (M,M)
The list of lag-1 joint moment matrices. The
length of the list determines K, the number of arrival
types of the rational process.
Returns
-------
H : list of matrices, shape (M,M)
The H0, H1, ..., HK matrices of the marked rational
process
Notes
-----
There is no guarantee that the returned matrices define
a valid stochastic process. The joint densities may be
negative.
References
----------
.. [1] Andras Horvath, Gabor Horvath, Miklos Telek, "A
traffic based decomposition of two-class queueing
networks with priority service," Computer Networks
53:(8) pp. 1235-1248. (2009)
"""
v, H0 = MEFromMoments (moms)
H0i = la.inv(-H0)
Ge = ml.zeros(H0.shape)
G1 = ml.zeros(H0.shape)
H0ip = ml.eye(H0.shape[0])
for i in range(H0.shape[0]):
Ge[i,:] = v * H0ip
G1[:,i] = np.sum(H0ip, 1)
H0ip *= (i+1) * H0i
Gei = la.inv(Ge)
G1i = la.inv(G1)
return [-H0*Gei*Nm[i-1]*G1i if i>0 else H0 for i in range(len(Nm)+1)]
def EMForMoG(datam, n, pl, muvl, covml, max_iters=1):
assert len(pl)==len(muvl) and len(muvl)==len(covml)
d=datam.shape[0]
N=datam.shape[1]
#start EM posteriorm n*N
posteriorm=matlib.zeros((n, N), dtype=np.float32)
for it in xrange(max_iters):
print 'Iter: '+str(it)
#create Gaussian kernels
NormalFl=[makeNormalF(muv, covm) for muv, covm in zip(muvl, covml)]
#probability
px=matlib.zeros((1, N), dtype=np.float32)
posteriorm.fill(0)
#caculate posteriorm
for j in xrange(N):
cur_data=datam[:, j]
for i in xrange(n):
#print i, j
posteriorm[i, j]=pl[i]*NormalFl[i](cur_data)
px[0, j]+=posteriorm[i, j]
# print 'px:', px
posteriorm/=px
#update parameters
#soft num n*1
softnum=matlib.sum(posteriorm, 1)
print softnum
softnum_inv=1.0/softnum
pl=np.array((softnum/N)).reshape(-1).tolist()
mum=datam*posteriorm.T*matlib.diag(np.array(softnum_inv).reshape(-1))
muvl=[mum[:, k] for k in range(n)]
mum=[]#release
for k in range(n):
datam_temp=datam-muvl[k]
covml[k]=softnum_inv[k, 0]*datam_temp*matlib.diag(np.array(posteriorm[k, :]).reshape(-1))\
*datam_temp.T
return pl, muvl, covml
def computeCovariance(self):
"""
Computes the time-lagged covariance matrix :math:`C^{\\tau}` with
:math:`c_{ij}^{\\tau} = \\frac{1}{N-\\tau-1}\\sum_{t=1}^{N-\\tau}x_{ti}x_{(t+\\tau)j}`
"""
self.m_prinComp.m_dataImporter.rewind()
dataChunk = np.asarray(self.m_prinComp.m_dataImporter.get_data(self.m_prinComp.param_chunkSize), dtype=np.float32)
dimData = self.m_prinComp.m_dataImporter.get_shape_inFile()
shapeDataChunk = dataChunk.shape
m = shapeDataChunk[0]
self.m_covMatTimeLag = matlib.zeros([shapeDataChunk[1], shapeDataChunk[1]], dtype = np.float64)
normalizedPCs = matlib.zeros([shapeDataChunk[1], shapeDataChunk[1]], dtype = np.float64)
if 1 < m:
pcs = self.m_prinComp.computePCs(dataChunk, shapeDataChunk[1])
del dataChunk
normalizedPCs = self.m_prinComp.normalizePCs(pcs)
del pcs
self.m_covMatTimeLag += np.dot(normalizedPCs[0:(m - self.param_timeLag), :].T,
normalizedPCs[self.param_timeLag:m, :])
lastRowsChunkBefore = normalizedPCs[(m-self.param_timeLag):m, :]
del normalizedPCs
while self.m_prinComp.m_dataImporter.has_more_data():
dataChunk = np.asarray(self.m_prinComp.m_dataImporter.get_data(self.m_prinComp.param_chunkSize), dtype=np.float32)
shapeDataChunk = dataChunk.shape
m = shapeDataChunk[0]
pcs = self.m_prinComp.computePCs(dataChunk, shapeDataChunk[1])
del dataChunk
normalizedPCs = self.m_prinComp.normalizePCs(pcs)
del pcs
self.m_covMatTimeLag += np.dot(matlib.asmatrix(lastRowsChunkBefore).T,
matlib.asmatrix(normalizedPCs[0:self.param_timeLag, :]))
if 1 < m:
self.m_covMatTimeLag += np.dot(normalizedPCs[0:(m - self.param_timeLag), :].T,
normalizedPCs[self.param_timeLag:m, :])
lastRowsChunkBefore = normalizedPCs[(m-self.param_timeLag):m, :]
del normalizedPCs
self.m_covMatTimeLag *= 1.0 / (dimData[0] - self.param_timeLag - 1.0)
def __init__(self, intWindow, visibilityTimeout, tfl, sigma):
self.tfl = tfl # MAYBE JUST INITIALIZE INSTEAD OF PASSING IN
self.sigma = sigma
self.intWindow = intWindow
self.vCc = ml.zeros((3,1))
self.wGCc = ml.zeros((3,1))
self.vTt = ml.zeros((3,1)) #DEBUG
self.wGTt = ml.zeros((3,1)) #DEBUG
self.watchdogTimer = rospy.Timer(rospy.Duration.from_sec(5),self.timeout,oneshot=True) # large initial duration for slow startup
self.visibilityTimeout = visibilityTimeout
self.estimatorOn = False
self.tBuff = collections.deque()
self.etaBuff = collections.deque()
self.sigmaBuff = collections.deque()
self.fBuff = collections.deque()
def SimilarityMatrix (A1, A2):
"""
Returns the matrix that transforms A1 to A2.
Parameters
----------
A1 : matrix, shape (N,N)
The smaller matrix
A2 : matrix, shape (M,M)
The larger matrix (M>=N)
Returns
-------
B : matrix, shape (N,M)
The matrix satisfying `A_1\,B = B\,A_2`
Notes
-----
For the existence of a (unique) solution the larger
matrix has to inherit the eigenvalues of the smaller one.
"""
if A1.shape[0]!=A1.shape[1] or A2.shape[0]!=A2.shape[1]:
raise Exception("SimilarityMatrix: The input matrices must be square!")
N1 = A1.shape[0]
N2 = A2.shape[1]
if N1>N2:
raise Exception("SimilarityMatrix: The first input matrix must be smaller than the second one!")
[R1,Q1]=la.schur(A1,'complex')
[R2,Q2]=la.schur(A2,'complex')
Q1 = ml.matrix(Q1)
Q2 = ml.matrix(Q2)
c1 = ml.matrix(np.sum(Q2.H,1))
c2 = np.sum(Q1.H,1)
I = ml.eye(N2)
X = ml.zeros((N1,N2), dtype=complex)
for k in range(N1-1,-1,-1):
M = R1[k,k]*I-R2
if k==N1:
m = ml.zeros((1,N2))
else:
m = -R1[k,k+1:]*X[k+1:,:]
X[k,:] = Linsolve(np.hstack((M,c1)),np.hstack((m,c2[k])))
return (Q1*X*ml.matrix(Q2).H ).real
def getNode(self,p, V, E, last=False):
"""
This function calculates the velocity for the robot with RRT.
The inputs are (given in order):
p = the current x-y position of the robot
E = edges of the tree (2 x No. of nodes on the tree)
V = points of the tree (2 x No. of vertices)
last = True, if the current region is the last region
= False, if the current region is NOT the last region
"""
pose = mat(p).T
#dis_cur = distance between current position and the next point
dis_cur = vstack((V[1,E[1,self.E_current_column]],V[2,E[1,self.E_current_column]]))- pose
heading = E[1,self.E_current_column] # index of the current heading point on the tree
if norm(dis_cur) < 1.5*self.radius: # go to next point
if not heading == shape(V)[1]-1:
self.E_current_column = self.E_current_column + 1
dis_cur = vstack((V[1,E[1,self.E_current_column]],V[2,E[1,self.E_current_column]]))- pose
Node = zeros([2,1])
Node[0,0] = V[1,E[1,self.E_current_column]]
Node[1,0] = V[2,E[1,self.E_current_column]]
#Vel[0:2,0] = dis_cur/norm(dis_cur)*0.5 #TUNE THE SPEED LATER
return Node
def test_matrices(self):
# test all supported matrices for type and value
self.assertEqual(type(self.out['o']).__name__, 'matrix')
self.assertEqual(type(self.out['o'][0,0]).__name__, 'uint8')
self.assertTrue((self.out['o'] == mat('0, 1, 1; 0, 0, 1')).all())
self.assertEqual(type(self.out['p']).__name__, 'matrix')
self.assertEqual(type(self.out['p'][0,0]).__name__, 'int16')
self.assertTrue((self.out['p'] == mat('1, 2, 3; 4, 5, 6')).all())
self.assertEqual(type(self.out['q']).__name__, 'matrix')
self.assertEqual(type(self.out['q'][0,0]).__name__, 'int32')
self.assertTrue((self.out['q'] == mat('11, 12, 13; 14, 15, 16')).all())
self.assertEqual(type(self.out['r']).__name__, 'matrix')
self.assertEqual(type(self.out['r'][0,0]).__name__, 'float64')
self.assertTrue((abs(self.out['r']-mat('1.5, 1.6, 1.7; 2.3, 2.4, 2.5')) < self.eps).all())
self.assertEqual(type(self.out['s']).__name__, 'matrix')
self.assertEqual(type(self.out['s'][0,0]).__name__, 'complex128')
step = complex(0.5, -0.5)
ref_cvec = zeros((3, 2), complex)
ref_cvec[0,0] = complex(0.0, 0.0)
for i in range(1,3):
ref_cvec[i,0] = ref_cvec[i-1,0]+step
ref_cvec[0,1] = complex(1.5, -1.5)
for i in range(1,3):
ref_cvec[i,1] = ref_cvec[i-1,1]+step
self.assertTrue((abs(self.out['s']-ref_cvec) < self.eps).all())
def get_cjoint_jac(self):
from numpy.matlib import zeros
from jacobians import serialKinematicJacobianPassive as jacobian
n_endjoints = len(self.chains)
from serialmechanism import n_pjoints
n_pjnts = n_pjoints(l_from_l_of_l(self.pjoints))
J = zeros((6 * n_endjoints, n_pjnts))
# J = [J0 -J1 0 ...
# J0 0 -J2 ...
# ...
# J0 0 ... -Jb]
# Put J0 more times in the first columns.
# J0 = jacobian for main branch
# n_points0 = number of passive joints in main branch
J0 = jacobian(self.chains[0])
n_pjoints0 = J0.shape[1]
for i in range(n_endjoints - 1):
J[(6 * i):(6 * i + 6), :n_pjoints0] = J0
# Put -J1, -J2, ... in the diagonal of the non-yet-filled rest part
# of J.
np = n_pjoints0
for i in range(1, n_endjoints):
npi = n_pjoints(self.chains[i])
Ji = jacobian(self.chains[i])
J[(6 * i):(6 * i + 6), np:(np + npi)] = -Ji
np += npi
def test_vectors(self):
# test all supported vectors for type and value
self.assertEqual(type(self.out['i']).__name__, 'matrix')
self.assertEqual(type(self.out['i'][0,0]).__name__, 'uint8')
self.assertTrue((self.out['i'] == mat('0; 1; 1; 0; 0; 1')).all())
self.assertEqual(type(self.out['j']).__name__, 'str')
self.assertEqual(self.out['j'], 'abc')
self.assertEqual(type(self.out['k']).__name__, 'matrix')
self.assertEqual(type(self.out['k'][0,0]).__name__, 'int16')
self.assertTrue((self.out['k'] == mat('10; 11; 12; 13; 14; 15; 16; 17; 18; 19')).all())
self.assertEqual(type(self.out['l']).__name__, 'matrix')
self.assertEqual(type(self.out['l'][0,0]).__name__, 'int32')
self.assertTrue((self.out['l'] == mat('20; 21; 22; 23; 24; 25; 26; 27; 28; 29')).all())
self.assertEqual(type(self.out['m']).__name__, 'matrix')
self.assertEqual(type(self.out['m'][0,0]).__name__, 'float64')
self.assertTrue((abs(self.out['m']-mat('30; 31; 32; 33; 34; 35; 36; 37; 38; 39')) < self.eps).all())
self.assertEqual(type(self.out['n']).__name__, 'matrix')
self.assertEqual(type(self.out['n'][0,0]).__name__, 'complex128')
step = complex(0.5, -0.5)
ref_cvec = zeros((10, 1), complex)
ref_cvec[0,0] = complex(0.0, 0.0)
for i in range(1, 10):
ref_cvec[i,0] = ref_cvec[i-1,0]+step
self.assertTrue((abs(self.out['n']-ref_cvec) < self.eps).all())
def learn_triplets_cooccur_mat(triplets_file_path):
files = glob.glob(triplets_file_path)
np_voc = vocabulary.Vocabulary()
vp_voc = vocabulary.Vocabulary()
np_voc.load('../mat/np1.voc')
vp_voc.load('../mat/np2.voc')
num_np = np_voc.size()
num_vp = vp_voc.size()
cooccur_mat = zeros([num_np, num_vp])
for file_in in files:
with open(file_in, 'r') as f:
for line in f:
if(line[0] != '<'):
line = (line[:-2]).lower()
triplets = line.split('|')
np1 = cleansing.clean(triplets[0].split())
vp = cleansing.clean(triplets[1].split())
np2 = cleansing.clean(triplets[2].split())
for w in np2:
vp.append(w)
np1_new = [w for w in np1 if np_voc.contain(w)]
vp_new = [w for w in vp if vp_voc.contain(w)]
pairs = [(np_voc.get_word_index(u), vp_voc.get_word_index(v)) for u in np1_new for v in vp_new]
for pair in pairs:
cooccur_mat[pair[0], pair[1]] += 1
return cooccur_mat
def build_energy_surface_threaded_by_angle(system):
step_number = 99
theta2_begin = -1.0 * np.pi
theta2_end = 2.0 * np.pi
theta3_begin = -1.0 * np.pi
theta3_end = 2.0 * np.pi
theta_angles, phi_angles = system['theta_angle'], system['phi_angle']
N, M, E0, U0, V = system['N'], system['M'], system['E0'], system['U0'], system['hopping_matrix']
surface = matlib.zeros((step_number, step_number), dtype=float)
for i, th2 in enumerate(np.linspace(theta2_begin, theta2_end, step_number)):
theta_angles[0] = 0.0
theta_angles[1] = th2
logging.info('Step {} / {}'.format(i, step_number))
with Pool(2) as pool:
chunker = partial(build_surface_chunk, theta_angles=theta_angles, phi_angles=phi_angles, N=N, M=M, E0=E0,
U0=U0, V=V)
result = pool.map(chunker, [th3 for th3 in np.linspace(theta3_begin, theta3_end, step_number)])
for j, energy, _, _ in enumerate(result):
surface[i, j] = energy
# Normalize energies and return result
return surface - surface.min()
def main():
"""Comportement si on exécute le programme directement:
On lit le fichier matrices.txt pour obtenir l'information
sur les matrices, on exécute ensuite la recherche du
parenthésage optimal avec l'algorithme de programmation dynamique,
et on écrit dans resultat.txt les matrices m, frontieres,
et ensuite le résultat de la multiplication effectuée à l'aide
du parenthésage optimal.
"""
n, dimensions, matrices = lireFichierMatrice("matrices.txt")
with open("resultat.txt", "w") as resultat:
frontieres = matlib.zeros((n, n), dtype=int)
m = trouverParenthesageOptimalDynamique(n, dimensions, frontieres)
c = multiplierChaineMatrice(frontieres, matrices, 1, n)
for line in m.tolist():
resultat.write(reduce(lambda x, y: x+y,
map(lambda x: str(x)+" ", line)))
resultat.write("\n")
for line in frontieres.tolist():
resultat.write(reduce(lambda x, y: x+y,
map(lambda x: str(x)+" ", line)))
resultat.write("\n")
for line in c.tolist():
resultat.write(reduce(lambda x, y: x+y,
map(lambda x: str(x)+" ", line)))
resultat.write("\n")
def _backprop_output(nnet, evaluated):
ret = []
for out_ind in xrange(nnet.n_out):
to_add = m.zeros((nnet.n_hidden + 1, nnet.n_out))
to_add[:, out_ind] = np.asmatrix(nnet.W2)[:, out_ind]
ret.append(to_add * sigma_p(evaluated.xo)[out_ind, 0])
return ret
def matrix(self, shape=None):
"""
This function returns the object as a matrix of DFT or iDFT resp.
"""
N=self.N
prodN=np.prod(N)
if shape is not None:
dim=np.prod(np.array(shape))
elif hasattr(self, 'shape'):
dim=np.prod(np.array(shape))
else:
raise ValueError('Missing shape of the DFT.')
proddN=dim*prodN
ZN_input=Grid.get_ZNl(N, fft_form=0)
ZN_output=Grid.get_ZNl(N, fft_form='c')
if self.inverse:
DFTcoef=lambda k, l, N: np.exp(2*np.pi*1j*np.sum(k*l/N))
else:
DFTcoef=lambda k, l, N: np.exp(-2*np.pi*1j*np.sum(k*l/N))/np.prod(N)
DTM=np.zeros([self.pN(), self.pN()], dtype=np.complex128)
for ii, kk in enumerate(itertools.product(*tuple(ZN_output))):
for jj, ll in enumerate(itertools.product(*tuple(ZN_input))):
DTM[ii, jj]=DFTcoef(np.array(kk, dtype=np.float),
np.array(ll), N)
DTMd=npmatlib.zeros([proddN, proddN], dtype=np.complex128)
for ii in range(dim):
DTMd[prodN*ii:prodN*(ii+1), prodN*ii:prodN*(ii+1)]=DTM
return DTMd
def TestOutput7(test_scores,test_vad_ts,test_features_ts,baseline1,baseline2,model): [window_matrix,labels_matrix,score_matrix] = PrepareData(test_scores,test_vad_ts,test_features_ts) labels_matrix_val, labels_matrix_aro, labels_matrix_dom = \ labels_matrix[:,0], labels_matrix[:,1], labels_matrix[:,2] bl_features = numpy.concatenate((window_matrix,score_matrix),axis=1) # Baseline outputs b1_output = baseline1.predict(bl_features) b2_output = baseline2.predict(window_matrix) # model output l1_reg = model[0] l2_reg = model[1] layer1_output = numpy.matrix(l1_reg.predict(bl_features)) num_feats = layer1_output.shape[1] layer2_input = matlib.zeros((layer1_output.shape[0],layer1_output.shape[1])) # this will have layer2_input[:,0] = numpy.multiply(layer1_output[:,0],score_matrix) layer2_input[:,1] = numpy.divide(layer1_output[:,1],(score_matrix+.01*numpy.ones(score_matrix.shape))) model_output = l2_reg.predict(layer2_input) # printing results for supplied test split print 'Results for iteration' PrintResults7(labels_matrix_val,b1_output,b2_output,model_output) return labels_matrix_val,b1_output,b2_output,model_output
