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 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